【随记】g2o使用，解决2d路径回环误差为例

在slam中经常使用g2o来解决优化问题，例如BA和位姿图优化。由于网上教程需要的知识储备较多，大多数和slam问题相关，本文不涉及复杂公式，本文只是通过一个简单任务直观了解g2o在做什么的文章。

好了，上图表示从0点出发经过1，2，3点，估计世界坐标分别为$X_0=(0,0)$，$X_1=(1,1)$，$X_2=(2,0)$，$X_3=(1,-1)$，再回到$X_0$，但是由于误差原因，回到$X_0$时，得到的估计世界坐标为(0.5, -0.5)，我们记为${\hat{X}}_0=(0.5,-0.5)$，所谓估计就表示不是真实的坐标，是通过估计得到。

那么初始0点和最后的0点误差就为(0.5, -0.5)，我们不能认为整个路径误差就是由点3到点0产生，事实上误差更可能是整个路径累计下来的，所以不能简单的将最后的0点观察值改(0.5, -0.5)为(0, 0)。而位姿图优化要做的就是均摊误差，结合上图就是这么以调整各个顶点位置代价，使得起始0点不要和最后0点不要差那么多。

简单提一下上面问题如何表示为数学优化问题：

$$e=X_0-{\hat{X}}_0$$
表示误差$e$的值为初始0点和最后的0点估计世界坐标之差。而最后的0点估计世界坐标可以表示为路径上顶点与顶点之间的位置（slam中为6自由度位姿，本例只有2维)变换
$${\hat{X}}_0=T_{03}^{-1}{T}_{32}^{-1}{T}_{21}^{-1}{T}_{10}^{-1}{X}_0$$

所以最终要优化的是：
$$\underset{T}{\min }{X}_0-T_{03}^{-1}{T}_{32}^{-1}{T}_{21}^{-1}{T}_{10}^{-1}{X}_0$$

顶点在g2o中表示：
本文问题是解决2d路径问题，上图顶点也表示其为2维，所以在g2o中，顶点的数据类型为2，类型使用g2o::Vector2 表示。这样的顶点g2o中已经定义好了，不需要自定义：

class G2O_TYPES_SLAM2D_API VertexPointXY : public BaseVertex<2, Vector2> {
...
}

表在g2o中表示：
上图中，边连接着2个顶点，其值为位置变换T，比如在顶点0到顶点1的测量值为(1, 1)。所以在g2o中，边连接着2个顶点(VertexPointXY)，并且测量值的维度为2，类型我们使用g2o::Vector2 `表示。这样的边g2o中也已经定义好了，不需要自定义：

class G2O_TYPES_SLAM2D_API EdgePointXY : public BaseBinaryEdge<2, Vector2, VertexPointXY, VertexPointXY> {
...
}

所以用g2o就可以形象表示为优化边的误差，在EdgePointXY的定义中，计算误差：

 void computeError(){
   const VertexPointXY* v1 = static_cast<const VertexPointXY*>(_vertices[0]);
   const VertexPointXY* v2 = static_cast<const VertexPointXY*>(_vertices[1]);
   _error = (v2->estimate()-v1->estimate())-_measurement;
 }

误差来自2个顶点之差(也就是上图中的T)减去测量值T，所以要优化的值实际上是顶点，而顶点更新来自设置好的优化算法，每次会得到更新值，而更新值如何应用在VertexPointXY中：

virtual void oplusImpl(const number_t* update){
  _estimate[0] += update[0];
  _estimate[1] += update[1];
}

其中update来自求解器给出，比如LinearSolverDense，LinearSolverEigen, LinearSolverCSparse各个求解器效率、准确性和依赖库不一样，根据实际情况选择，本文使用LinearSolverEigen。由于顶点是2为vector，可以直接相加，而在slam中位姿矩阵是不能直接相加的，所以要么转换为乘法或者用李代数表示为加法。

下面是完整代码：

#include <stdio.h>
#include <iostream>
#include <g2o/core/block_solver.h>
#include <g2o/solvers/eigen/linear_solver_eigen.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/types/slam2d/types_slam2d.h>
#include <Eigen/Core>

typedef struct{
    int s, e;
    Eigen::Vector2d pose;
} Edge;
typedef g2o::BlockSolver< g2o::BlockSolverTraits<2, 2> >  SlamBlockSolver;
typedef g2o::LinearSolverEigen<SlamBlockSolver::PoseMatrixType> SlamLinearSolver;

int main(int argc, const char * argv[]) {

    std::vector<Edge> edgeData = {
        {0, 1, {1, 1}},
        {1, 2, {1, -1}},
        {2, 3, {-1, -1}},
        {3, 0, {-0.5, 0.5}},
    };
    
    std::unique_ptr<SlamLinearSolver> linearSolver = g2o::make_unique<SlamLinearSolver>();
    linearSolver->setBlockOrdering(false);
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(g2o::make_unique<SlamBlockSolver>(std::move(linearSolver)));

    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);
    optimizer.setVerbose(true);

    auto maxEdge = std::max_element(edgeData.begin(), edgeData.end(), [](const Edge& a, const Edge& b){
        return std::max(a.e, a.s) < std::max(b.e, b.s);
    });
    int maxIndex = std::max(maxEdge->s, maxEdge->e);

    for(int i = 0; i < maxIndex+1; i++){
        g2o::VertexPointXY *v = new g2o::VertexPointXY();
        v->setId(i);
        v->setEstimate(g2o::Vector2());
        if(i == 0){
            v->setFixed(true);
        }
        optimizer.addVertex(v);
    }

    for(const auto& pData: edgeData){
        g2o::EdgePointXY* edge = new g2o::EdgePointXY();
        edge->setVertex( 0, optimizer.vertex(pData.s));
        edge->setVertex( 1, optimizer.vertex(pData.e));
        edge->setInformation(  Eigen::Matrix< double, 2,2 >::Identity() );// 信息矩阵表示2维上侧重哪一维，xy是一样重要的，所以就是单位矩阵，但是在6维的位姿中，有可能更侧重优化旋转或者位移，就需要设置信息矩阵
        edge->setMeasurement(pData.pose );
        optimizer.addEdge(edge);
    }
    
    optimizer.initializeOptimization();
    optimizer.optimize(500);
    for(int i = 0; i < maxIndex+1; i++){
        g2o::VertexPointXY* vertex = dynamic_cast<g2o::VertexPointXY*>(optimizer.vertex( i ));
        g2o::Vector2 pose = vertex->estimate();
        std::cout << i << ":\n" << pose << std::endl ;
    }
    return 0;
}

本文使用g2o版本比slam14讲中版本更新，上述代码与旧版本不一样的地方是新版本优化器初始化地方使用智能指针取代了指针

输出：

iteration= 0	 chi2= 0.125000	 time= 0.00019975	 cumTime= 0.00019975	 edges= 4	 schur= 0	 lambda= 0.000007	 levenbergIter= 1
iteration= 1	 chi2= 0.125000	 time= 3.4167e-05	 cumTime= 0.000233917	 edges= 4	 schur= 0	 lambda= 0.000004	 levenbergIter= 1
iteration= 2	 chi2= 0.125000	 time= 3.1708e-05	 cumTime= 0.000265625	 edges= 4	 schur= 0	 lambda= 0.000009	 levenbergIter= 1
0:
0
0
1:
0.875
1.125
2:
1.75
0.25
3:
 0.625
-0.625

结果可视化：

本文给出简单2d的路径优化，熟悉后修改顶点和边的类型即可拓展到6维的位姿图优化，以及更复杂的BA优化。只看了一周这些算法，尽可能用自己理解解释，但还是感觉没讲太清楚，希望可以帮助读者。

本文代码GitHub链接

参考

本文只是简单入门理解，需要更熟悉g2o使用，请参考文章：
SLAM图优化库g2o --来自计算机视觉life公众号，很详细
g2o使用总结 – 各个点和边类型介绍
第六讲 图优化工具g2o的入门 --来自高博，6维位姿图的优化和g2o使用
第七讲 添加回环检测 – 来自高博一个完整示例，如何回环检测加优化