g2o学习笔记（一）：曲线拟合

前言：g2o对于slam等问题来说十分的重要，将一个slam问题构建成图结构表达的优化问题是目前大多数的slam算法的通用做法。之前在学习了高博对g2o的讲解后至今感觉意犹未尽，表现在对于简单问题能够理解其含义而在slam问题中构建的各种复杂的优化问题则感到无力，因此决定按照g2o源码给出的实例系统的学习g2o库的使用。

预备知识：《视觉slam14讲》ch6部分

博文风格：由于这是第一篇博客，先介绍撰写博客时的思路。首先介绍每篇博客需要求解的问题定义，然后介绍使用到的g2o接口最后便是源码剖析。

曲线拟合

1. 问题定义

给定一系列点，拟合非线性曲线

已知：一系列点的坐标,拟合曲线

待求：参数

2. g2o相关API介绍

2.1 节点

BaseVertex 类，模板类 

头文件：#include "g2o/core/base_vertex.h"

自定义的节点继承自该基类，由于需要估计的参数为三个因此模板类具化为g2o::BaseVertex<3,Eigen::Vector3d>

由于在该类定义时部分函数为虚函数，需要在继承的子类中实现具体函数及含义如下：

• 构造函数（继承时并不继承构造函数）
• 读取函数，暂时没有用 
• 写入函数，暂时没用 
• 设置优化的起点，暂时没用
• 更新优化变量

virtual bool read(std::istream& /*is*/)
virtual bool write(std::ostream& /*os*/) const
virtual void setToOriginImpl()
virtual void oplusImpl(const double* update)

在本例中仅仅用到了更新优化变量函数，而对于欧式空间中的优化问题只需要在状态中加上更新量即可，因此实现如下：

 virtual void oplusImpl(const double* update)
{
    Eigen::Vector3d::ConstMapType v(update);// 每次的更新量
    _estimate += v;// 直接加上跟新量即可
}

而整体继承类实现如下

class VertexParams : public g2o::BaseVertex<3,Eigen::Vector3d>
{
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
        VertexParams()
        {
            
        }
        virtual bool read(std::istream& /*is*/)
        {
            cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
            return false;
        }
        virtual bool write(std::ostream& /*os*/) const
        {
            cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
            return false;
        }
        virtual void setToOriginImpl()
        {
            std::cout<<"not implemented!"<<std::endl;
        }
        virtual void oplusImpl(const double* update)
        {
            Eigen::Vector3d::ConstMapType v(update);
            _estimate += v;
        }
};

2.2 边

BaseUnaryEdge类，模板类

头文件：#include "g2o/core/base_unary_edge.h"

 自定义边继承自该基类，由于约束项为2d点因此模板类具化为 g2o::BaseUnaryEdge<1,Eigen::Vector2d,VertexParams>

该类需要在子类中实现的虚函数如下：

• 构造函数（继承时并不继承构造函数）
• 读取函数，暂时没有用 
• 写入函数，暂时没用 
• 误差计算函数

virtual bool read(std::istream& /*is*/)
virtual bool write(std::ostream& /*os*/) const
void computeError()

在本例中使用到的主要函数为误差计算函数，即计算当前拟合曲线和约束点之间的误差，因此实现如下

void computeError()
{
    // 需要优化的节点
    const VertexParams* params = static_cast<const VertexParams*>(vertex(0));
    // 节点参数
    const double& a = params->estimate()(0);
    const double& b = params->estimate()(1);
    const double& lambda = params->estimate()(2);
    // 计算误差
    double fval = a * exp(-lambda * measurement()(0)) + b;
    // 记录误差
    _error(0) = fval - measurement()(1);
}

整体实现代码

class EdgePointOnCurve : public g2o::BaseUnaryEdge<1,Eigen::Vector2d,VertexParams>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    EdgePointOnCurve(){}
    virtual bool read(std::istream& /*is*/)
    {
        cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
        return false;
    }
    virtual bool write(std::ostream& /*os*/) const
    {
        cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
        return false;
    }
    void computeError()
    {
        const VertexParams* params = static_cast<const VertexParams*>(vertex(0));
        const double& a = params->estimate()(0);
        const double& b = params->estimate()(1);
        const double& lambda = params->estimate()(2);
        double fval = a * exp(-lambda * measurement()(0)) + b;
        _error(0) = fval - measurement()(1);
    }

};

2.3 优化流程

2.3.1 重命名

由于模板类名称太长，一般会采用typedef重命名，同时定义求解器的类型.其中第一行的3代表一个节点的维度为3，而2代表一条边的维度为2.

    typedef g2o::BlockSolver<g2o::BlockSolverTraits<3,2> > MyBlockSolver;
    typedef g2o::LinearSolverDense<MyBlockSolver::PoseMatrixType> MyLinearSolver;

2.3.2 设置求解器

   // 设置求解器
    g2o::SparseOptimizer optimizer;
    optimizer.setVerbose(false);

    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(g2o::make_unique<MyBlockSolver>(g2o::make_unique<MyLinearSolver>()));

    optimizer.setAlgorithm(solver);

2.3.3 构造图结构 

    // 1.添加待优化的节点
    VertexParams* params = new VertexParams();
    params->setId(0);
    params->setEstimate(Eigen::Vector3d(1,1,1));
    
    optimizer.addVertex(params);
    // 2.添加约束
    for(int i=0;i<numPoints;i++)
    {
        EdgePointOnCurve* e = new EdgePointOnCurve();
        e->setInformation(Eigen::Matrix<double,1,1>::Identity());
        e->setVertex(0,params);
        e->setMeasurement(points[i]);
        optimizer.addEdge(e);
    }

2.3.4 求解

    // 执行优化
    optimizer.initializeOptimization();
    optimizer.setVerbose(verbose);
    optimizer.optimize(maxIterations);

3. 代码实现

由于实现代码较为冗长因此给出源码github链接

4. 小结

通过对曲线拟合的简单例子了解到了g2o库的调用流程

1. 自定义边、节点类继承自模板类，重新实现部分虚函数
2. 初始化求解器，选择优化方法
3. 构造图结构，往图中添加节点和边
4. 调用函数进行求解

大部分的g2o教程到此便结束了，但是学习g2o的目的并非为了拟合曲线而是求解slam中各种复杂的问题，因此后面将进行进一步深入的讨论。