void ViewerState::polishOldThings(){ if(drawableFrameVector.size() > 40) { pwnGMW->viewer_3d->drawableList().erase(pwnGMW->viewer_3d->drawableList().begin()); DrawableFrame* firstDrawableFrame = drawableFrameVector.front(); if (0 && firstDrawableFrame->frame()){ delete firstDrawableFrame->frame(); } delete firstDrawableFrame; drawableFrameVector.erase(drawableFrameVector.begin()); } }
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; }
int main (int argc, char** argv) { // Handle input int vz_applyTransform, vz_pointStep; int pwn_chunkStep; float vz_pointSize, vz_alpha; float pwn_scaleFactor, pwn_scale; string pwn_configFilename, pwn_logFilename, pwn_sensorType; string oc_benchmarkFilename, oc_trajectoryFilename, oc_groundTruthFilename, oc_associationsFilename; string directory; g2o::CommandArgs arg; arg.param("vz_pointSize", vz_pointSize, 1.0f, "Size value of the points to visualize"); arg.param("vz_transform", vz_applyTransform, 1, "Apply absolute transform to the point clouds"); arg.param("vz_alpha", vz_alpha, 1.0f, "Alpha channel value of the points to visualize"); arg.param("vz_pointStep", vz_pointStep, 1, "Step value at which points are drawn"); arg.param("pwn_chunkStep", pwn_chunkStep, 30, "Number of cloud composing a registered scene"); arg.param("pwn_scaleFactor", pwn_scaleFactor, 0.001f, "Scale factor at which the depth images were saved"); arg.param("pwn_scale", pwn_scale, 4.0f, "Scale factor the depth images are loaded"); arg.param("pwn_configFilename", pwn_configFilename, "pwn.conf", "Specify the name of the file that contains the parameter configurations of the pwn structures"); arg.param("pwn_logFilename", pwn_logFilename, "pwn.log", "Specify the name of the file that will contain the trajectory computed in boss format"); arg.param("pwn_sensorType", pwn_sensorType, "kinect", "Sensor type: xtion640/xtion320/kinect/kinectFreiburg1/kinectFreiburg2/kinectFreiburg3"); arg.param("oc_benchmarkFilename", oc_benchmarkFilename, "gicp_benchmark.txt", "Specify the name of the file that will contain the results of the benchmark"); arg.param("oc_trajectoryFilename", oc_trajectoryFilename, "gicp_trajectory.txt", "Specify the name of the file that will contain the trajectory computed"); arg.param("oc_associationsFilename", oc_associationsFilename, "gicp_associations.txt", "Specify the name of the file that contains the images associations"); arg.param("oc_groundTruthFilename", oc_groundTruthFilename, "groundtruth.txt", "Specify the name of the file that contains the ground truth trajectory"); arg.paramLeftOver("directory", directory, ".", "Directory where the program will find the input needed files and the subfolders depth and rgb containing the images", true); arg.parseArgs(argc, argv); // Create GUI QApplication application(argc,argv); QWidget* mainWindow = new QWidget(); mainWindow->setWindowTitle("GICP Odometry "); QHBoxLayout* hlayout = new QHBoxLayout(); mainWindow->setLayout(hlayout); PWNQGLViewer* viewer = new PWNQGLViewer(mainWindow); hlayout->addWidget(viewer); viewer->init(); viewer->setAxisIsDrawn(true); viewer->show(); mainWindow->showMaximized(); mainWindow->show(); // Init odometry controller GICPOdometryController *gicpOdometryController = new GICPOdometryController(pwn_configFilename.c_str(), pwn_logFilename.c_str()); gicpOdometryController->fileInitialization(oc_groundTruthFilename.c_str(), oc_associationsFilename.c_str(), oc_trajectoryFilename.c_str(), oc_benchmarkFilename.c_str()); gicpOdometryController->setScaleFactor(pwn_scaleFactor); gicpOdometryController->setScale(pwn_scale); gicpOdometryController->setSensorType(pwn_sensorType); gicpOdometryController->setChunkStep(pwn_chunkStep); // Compute odometry bool sceneHasChanged = false; Frame *frame = 0, *groundTruthReferenceFrame = 0; Isometry3f groundTruthPose; GLParameterTrajectory *groundTruthTrajectoryParam = new GLParameterTrajectory(0.02f, Vector4f(0.0f, 1.0f, 0.0f, 0.6f)); GLParameterTrajectory *trajectoryParam = new GLParameterTrajectory(0.02f, Vector4f(1.0f, 0.0f, 0.0f, 0.6f)); std::vector<Isometry3f> trajectory, groundTruthTrajectory; std::vector<Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > trajectoryColors, groundTruthTrajectoryColors; DrawableTrajectory *drawableGroundTruthTrajectory = new DrawableTrajectory(Isometry3f::Identity(), groundTruthTrajectoryParam, &groundTruthTrajectory, &groundTruthTrajectoryColors); DrawableTrajectory *drawableTrajectory = new DrawableTrajectory(Isometry3f::Identity(), trajectoryParam, &trajectory, &trajectoryColors); viewer->addDrawable(drawableGroundTruthTrajectory); viewer->addDrawable(drawableTrajectory); bool finished = false; while (mainWindow->isVisible()) { sceneHasChanged = false; if (sceneHasChanged) viewer->updateGL(); application.processEvents(); if (finished) { continue; } if (gicpOdometryController->loadFrame(frame)) { if(gicpOdometryController->counter() == 1) { gicpOdometryController->getGroundTruthPose(groundTruthPose, atof(gicpOdometryController->timestamp().c_str())); // Add first frame to draw groundTruthReferenceFrame = new Frame(); *groundTruthReferenceFrame = *frame; GLParameterFrame *groundTruthReferenceFrameParams = new GLParameterFrame(); groundTruthReferenceFrameParams->setStep(vz_pointStep); groundTruthReferenceFrameParams->setShow(true); groundTruthReferenceFrameParams->parameterPoints()->setColor(Vector4f(1.0f, 0.0f, 1.0f, vz_alpha)); DrawableFrame *drawableGroundTruthReferenceFrame = new DrawableFrame(groundTruthPose, groundTruthReferenceFrameParams, groundTruthReferenceFrame); GLParameterFrame *frameParams = new GLParameterFrame(); frameParams->setStep(vz_pointStep); frameParams->setShow(true); DrawableFrame *drawableFrame = new DrawableFrame(gicpOdometryController->globalPose(), frameParams, frame); viewer->addDrawable(drawableGroundTruthReferenceFrame); viewer->addDrawable(drawableFrame); sceneHasChanged = true; } // Compute current transformation if (gicpOdometryController->processFrame()) { gicpOdometryController->getGroundTruthPose(groundTruthPose, atof(gicpOdometryController->timestamp().c_str())); // Add frame to draw GLParameterFrame *frameParams = new GLParameterFrame(); frameParams->setStep(vz_pointStep); frameParams->setShow(true); DrawableFrame *drawableFrame = new DrawableFrame(gicpOdometryController->globalPose(), frameParams, frame); viewer->addDrawable(drawableFrame); // Add trajectory pose groundTruthTrajectory.push_back(groundTruthPose); groundTruthTrajectoryColors.push_back(Vector4f(0.0f, 1.0f, 0.0f, 0.6f)); trajectory.push_back(gicpOdometryController->globalPose()); trajectoryColors.push_back(Vector4f(1.0f, 0.0f, 0.0f, 0.6f)); drawableGroundTruthTrajectory->updateTrajectoryDrawList(); drawableTrajectory->updateTrajectoryDrawList(); // Write results gicpOdometryController->writeResults(); sceneHasChanged = true; } // Remove old frames if (viewer->drawableList().size() > 23) { DrawableFrame *d = dynamic_cast<DrawableFrame*>(viewer->drawableList()[3]); if (d) { Frame *f = d->frame(); if (gicpOdometryController->referenceFrame() != f && gicpOdometryController->currentFrame() != f) { viewer->erase(3); delete d; delete f; } } } frame = 0; } else { finished = true; } } return 0; }