gicp_demo.cpp File Reference

#include <stdint.h>
#include <iostream>
#include <random>
#include "g2o/core/block_solver.h"
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/core/solver.h"
#include "g2o/core/sparse_optimizer.h"
#include "g2o/solvers/dense/linear_solver_dense.h"
#include "g2o/stuff/sampler.h"
#include "g2o/types/icp/types_icp.h"

Include dependency graph for gicp_demo.cpp:

Go to the source code of this file.

Functions
int	main ()

Function Documentation

main()

int main

(

)

Definition at line 44 of file gicp_demo.cpp.

           {
  double euc_noise = 0.01;  // noise in position, m
  //  double outlier_ratio = 0.1;
 
  SparseOptimizer optimizer;
  optimizer.setVerbose(false);
 
  // variable-size block solver
  g2o::OptimizationAlgorithmLevenberg* solver =
      new g2o::OptimizationAlgorithmLevenberg(std::make_unique<BlockSolverX>(
          std::make_unique<
              LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>>()));
 
  optimizer.setAlgorithm(solver);
 
  vector<Vector3d> true_points;
  for (size_t i = 0; i < 1000; ++i) {
    true_points.push_back(
        Vector3d((g2o::Sampler::uniformRand(0., 1.) - 0.5) * 3,
                 g2o::Sampler::uniformRand(0., 1.) - 0.5,
                 g2o::Sampler::uniformRand(0., 1.) + 10));
  }
 
  // set up two poses
  int vertex_id = 0;
  for (size_t i = 0; i < 2; ++i) {
    // set up rotation and translation for this node
    Vector3d t(0, 0, i);
    Quaterniond q;
    q.setIdentity();
 
    Eigen::Isometry3d cam;  // camera pose
    cam = q;
    cam.translation() = t;
 
    // set up node
    VertexSE3* vc = new VertexSE3();
    vc->setEstimate(cam);
 
    vc->setId(vertex_id);  // vertex id
 
    cerr << t.transpose() << " | " << q.coeffs().transpose() << endl;
 
    // set first cam pose fixed
    if (i == 0) vc->setFixed(true);
 
    // add to optimizer
    optimizer.addVertex(vc);
 
    vertex_id++;
  }
 
  // set up point matches
  for (size_t i = 0; i < true_points.size(); ++i) {
    // get two poses
    VertexSE3* vp0 =
        dynamic_cast<VertexSE3*>(optimizer.vertices().find(0)->second);
    VertexSE3* vp1 =
        dynamic_cast<VertexSE3*>(optimizer.vertices().find(1)->second);
 
    // calculate the relative 3D position of the point
    Vector3d pt0, pt1;
    pt0 = vp0->estimate().inverse() * true_points[i];
    pt1 = vp1->estimate().inverse() * true_points[i];
 
    // add in noise
    pt0 += Vector3d(g2o::Sampler::gaussRand(0., euc_noise),
                    g2o::Sampler::gaussRand(0., euc_noise),
                    g2o::Sampler::gaussRand(0., euc_noise));
 
    pt1 += Vector3d(g2o::Sampler::gaussRand(0., euc_noise),
                    g2o::Sampler::gaussRand(0., euc_noise),
                    g2o::Sampler::gaussRand(0., euc_noise));
 
    // form edge, with normals in varioius positions
    Vector3d nm0, nm1;
    nm0 << 0, i, 1;
    nm1 << 0, i, 1;
    nm0.normalize();
    nm1.normalize();
 
    Edge_V_V_GICP* e  // new edge with correct cohort for caching
        = new Edge_V_V_GICP();
 
    e->setVertex(0, vp0);  // first viewpoint
    e->setVertex(1, vp1);  // second viewpoint
 
    EdgeGICP meas;
    meas.pos0 = pt0;
    meas.pos1 = pt1;
    meas.normal0 = nm0;
    meas.normal1 = nm1;
 
    e->setMeasurement(meas);
    //        e->inverseMeasurement().pos() = -kp;
 
    meas = e->measurement();
    // use this for point-plane
    e->information() = meas.prec0(0.01);
 
    // use this for point-point
    //    e->information().setIdentity();
 
    //    e->setRobustKernel(true);
    // e->setHuberWidth(0.01);
 
    optimizer.addEdge(e);
  }
 
  // move second cam off of its true position
  VertexSE3* vc =
      dynamic_cast<VertexSE3*>(optimizer.vertices().find(1)->second);
  Eigen::Isometry3d cam = vc->estimate();
  cam.translation() = Vector3d(0, 0, 0.2);
  vc->setEstimate(cam);
 
  optimizer.initializeOptimization();
  optimizer.computeActiveErrors();
  cout << "Initial chi2 = " << FIXED(optimizer.chi2()) << endl;
 
  optimizer.setVerbose(true);
 
  optimizer.optimize(5);
 
  cout << endl << "Second vertex should be near 0,0,1" << endl;
  cout << dynamic_cast<VertexSE3*>(optimizer.vertices().find(0)->second)
              ->estimate()
              .translation()
              .transpose()
       << endl;
  cout << dynamic_cast<VertexSE3*>(optimizer.vertices().find(1)->second)
              ->estimate()
              .translation()
              .transpose()
       << endl;
}

References g2o::OptimizableGraph::addEdge(), g2o::OptimizableGraph::addVertex(), g2o::OptimizableGraph::chi2(), g2o::SparseOptimizer::computeActiveErrors(), g2o::BaseVertex< D, T >::estimate(), g2o::Sampler::gaussRand(), g2o::BaseEdge< D, E >::information(), g2o::SparseOptimizer::initializeOptimization(), g2o::BaseEdge< D, E >::measurement(), g2o::EdgeGICP::normal0, g2o::EdgeGICP::normal1, g2o::SparseOptimizer::optimize(), g2o::EdgeGICP::pos0, g2o::EdgeGICP::pos1, g2o::EdgeGICP::prec0(), g2o::SparseOptimizer::setAlgorithm(), g2o::BaseVertex< D, T >::setEstimate(), g2o::OptimizableGraph::Vertex::setFixed(), g2o::OptimizableGraph::Vertex::setId(), g2o::BaseEdge< D, E >::setMeasurement(), g2o::SparseOptimizer::setVerbose(), g2o::HyperGraph::Edge::setVertex(), g2o::Sampler::uniformRand(), and g2o::HyperGraph::vertices().