g2o basics

Create an optimizer #

First define the block solver type and linear solver type:

typedef BlockSolver<BlockSolverTraits<-1, -1> > SlamBlockSolver;
typedef LinearSolverEigen<SlamBlockSolver::PoseMatrixType> SlamLinearSolver;

Then create an optimizer object.

  SparseOptimizer optimizer;

Set the solver #

  auto linearSolver = std::make_unique<SlamLinearSolver>();
  linearSolver->setBlockOrdering(false);

  OptimizationAlgorithmGaussNewton* solver = 
  new OptimizationAlgorithmGaussNewton(std::make_unique<SlamBlockSolver>(std::move(linearSolver)));

  optimizer.setAlgorithm(solver);

BlockSolver The optimization problem in SLAM is typically sparse and can be efficiently represented and solved using block matrices. BlockSolver handles the sparse structure of the problem. The template arguments <-1, -1> indicate that the size of the blocks is dynamic, which is common in SLAM problems where the number of poses and landmarks can vary.

LinearSolver: linear solver solves the linear part of the optimization problem. The choice of linear solver can affect the efficiency and accuracy of the optimization.

(Optional) Parameters #

In the context of SLAM (Simultaneous Localization and Mapping) and optimization frameworks like g2o, a sensor offset refers to the relative position and orientation of a sensor with respect to the robot’s coordinate frame. Essentially, it’s the transformation needed to map measurements taken by the sensor back to the robot’s frame of reference.

This is important because sensors are rarely located at the exact center of a robot. For example, if a sensor is mounted 20 cm in front of a robot’s center and 10 cm to the right, and it detects an object 1 meter directly in front of it, you need to account for that offset to understand where the object is in relation to the robot’s center.

  SE2 sensorOffsetTransf(0.2, 0.1, -0.1);

  ParameterSE2Offset* sensorOffset = new ParameterSE2Offset;
  sensorOffset->setOffset(sensorOffsetTransf);
  sensorOffset->setId(0);
  optimizer.addParameter(sensorOffset);