HyperGraphElementAction* VertexSE3DrawAction::operator()(HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params_){ if (typeid(*element).name()!=_typeName) return 0; initializeDrawActionsCache(); refreshPropertyPtrs(params_); if (! _previousParams) return this; if (_show && !_show->value()) return this; VertexSE3* that = static_cast<VertexSE3*>(element); glColor3f(POSE_VERTEX_COLOR); glPushMatrix(); glMultMatrixd(that->estimate().matrix().data()); opengl::drawArrow2D(_triangleX->value(), _triangleY->value(), _triangleX->value()*.3f); drawCache(that->cacheContainer(), params_); drawUserData(that->userData(), params_); if(_showId && _showId->value()){ float scale = _idSize ? _idSize->value() : 1.f; drawId(std::to_string(that->id()), scale); } glPopMatrix(); return this; }
void ViewerState::addCloudSelected(){ if(listItem) { cerr << "adding a frame" << endl; int index = pwnGMW->listWidget->row(listItem); cerr << "index: " << endl; std::string fname = listAssociations[index]->baseFilename()+"_depth.pgm"; DepthImage depthImage; if (!depthImage.load(fname.c_str(), true)){ cerr << " skipping " << fname << endl; newCloudAdded = false; *addCloud = 0; return; } DepthImage scaledDepthImage; DepthImage::scale(scaledDepthImage, depthImage, reduction); imageRows = scaledDepthImage.rows(); imageCols = scaledDepthImage.cols(); correspondenceFinder->setSize(imageRows, imageCols); Frame * frame=new Frame(); converter->compute(*frame, scaledDepthImage, sensorOffset); OptimizableGraph::DataContainer* dc =listAssociations[index]->dataContainer(); cerr << "datacontainer: " << dc << endl; VertexSE3* v = dynamic_cast<VertexSE3*>(dc); cerr << "vertex of the frame: " << v->id() << endl; DrawableFrame* drawableFrame = new DrawableFrame(globalT, drawableFrameParameters, frame); drawableFrame->_vertex = v; Eigen::Isometry3f priorMean; Matrix6f priorInfo; bool hasImu =extractAbsolutePrior(priorMean, priorInfo, drawableFrame); if (hasImu && drawableFrameVector.size()==0){ globalT.linear() = priorMean.linear(); cerr << "!!!! i found an imu for the first vertex, me happy" << endl; drawableFrame->setTransformation(globalT); } drawableFrameVector.push_back(drawableFrame); pwnGMW->viewer_3d->addDrawable(drawableFrame); } newCloudAdded = true; *addCloud = 0; _meHasNewFrame = true; }