void ViewerState::initialGuessSelected(){ cerr << "initial guess" << endl; DrawableFrame* current = drawableFrameVector.back(); DrawableFrame* reference = drawableFrameVector[drawableFrameVector.size()-2]; current->setTransformation(reference->transformation()); newCloudAdded = true; wasInitialGuess = true; *initialGuessViewer = 0; }
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; }
void ViewerState::optimizeSelected(){ DrawableFrame* current = drawableFrameVector.back(); DrawableFrame* reference = drawableFrameVector[drawableFrameVector.size()-2]; cerr << "optimizing" << endl; cerr << "current=" << current->frame() << endl; cerr << "reference= " << reference->frame() << endl; // cerr computing initial guess based on the frame positions, just for convenience Eigen::Isometry3d delta = reference->_vertex->estimate().inverse()*current->_vertex->estimate(); for(int c=0; c<4; c++) for(int r=0; r<3; r++) initialGuess.matrix()(r,c) = delta.matrix()(r,c); Eigen::Isometry3f odometryMean; Matrix6f odometryInfo; bool hasOdometry = extractRelativePrior(odometryMean, odometryInfo, reference, current); if (hasOdometry) initialGuess=odometryMean; Eigen::Isometry3f imuMean; Matrix6f imuInfo; bool hasImu = extractAbsolutePrior(imuMean, imuInfo, current); initialGuess.matrix().row(3) << 0,0,0,1; if(!wasInitialGuess) { aligner->clearPriors(); aligner->setOuterIterations(al_outerIterations); aligner->setReferenceFrame(reference->frame()); aligner->setCurrentFrame(current->frame()); aligner->setInitialGuess(initialGuess); aligner->setSensorOffset(sensorOffset); if (hasOdometry) aligner->addRelativePrior(odometryMean, odometryInfo); if (hasImu) aligner->addAbsolutePrior(reference->transformation(), imuMean, imuInfo); aligner->align(); } Eigen::Isometry3f localTransformation =aligner->T(); if (aligner->inliers()<1000 || aligner->error()/aligner->inliers()>10){ cerr << "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" << endl; cerr << "aligner: monster failure: inliers = " << aligner->inliers() << endl; cerr << "aligner: monster failure: error/inliers = " << aligner->error()/aligner->inliers() << endl; cerr << "Local transformation: " << t2v(aligner->T()).transpose() << endl; localTransformation = initialGuess; sleep(1); } cout << "Local transformation: " << t2v(aligner->T()).transpose() << endl; globalT = reference->transformation()*aligner->T(); // Update cloud drawing position. current->setTransformation(globalT); current->setLocalTransformation(aligner->T()); // Show zBuffers. refScn->clear(); currScn->clear(); QImage refQImage; QImage currQImage; DepthImageView div; div.computeColorMap(300, 2000, 128); div.convertToQImage(refQImage, aligner->correspondenceFinder()->referenceDepthImage()); div.convertToQImage(currQImage, aligner->correspondenceFinder()->currentDepthImage()); refScn->addPixmap((QPixmap::fromImage(refQImage)).scaled(QSize((int)refQImage.width()/(ng_scale*3), (int)(refQImage.height()/(ng_scale*3))))); currScn->addPixmap((QPixmap::fromImage(currQImage)).scaled(QSize((int)currQImage.width()/(ng_scale*3), (int)(currQImage.height()/(ng_scale*3))))); pwnGMW->graphicsView1_2d->show(); pwnGMW->graphicsView2_2d->show(); wasInitialGuess = false; newCloudAdded = false; *initialGuessViewer = 0; *optimizeViewer = 0; }