图优化g2o教程简介

云端舞步

已于 2023-07-31 12:22:48 修改

阅读量1.5k

点赞数 4

分类专栏： VINS-FUSION 文章标签：人工智能自动驾驶计算机视觉数据结构 c++

于 2023-07-21 18:52:51 首次发布

本文链接：https://blog.csdn.net/u010196944/article/details/131845522

版权

VINS-FUSION 专栏收录该内容

14 篇文章 19 订阅

订阅专栏

写在前面：时至今日，网络上随便一搜能找到各种关于g2o的教程，但大多数文章让你看完后，有一种不知道自己懂没有懂的感觉。本文想从g2o是干什么的、怎么干的(定义顶点和边的一些疑问点)，在弄清楚这些之后，如何对g2o进行封装接口，实现为自己所用。能够实现最后一步也说明比较好的掌握了g2o的使用。当然这样做还有好处就是可以很方便切换不同优化库的使用（如换成ceres），且程序框架比较清晰，遵从高聚低耦思想。

一、g2o是干什么的

机器人和计算机视觉的很多问题，如SLAM、BA，都可以表达为由图表示的非线性误差函数的最小化问题。例如基于图的slam问题，其状态变量是机器人在环境中的位置和机器人能观测到的地标点的位置。因此一个传感器测量数据仅仅依赖于和它相关联的两个状态变量中的位置信息。里程计测量信息也仅仅和其前后两次的状态变量中的位置信息相关。同样，对于一个地图点的位置也仅仅和能够看到它的那几帧对应的相机位姿相关。
这些问题都可以通过图来表示，图的每一个节点（Node）都表示一个可以优化的变量，两个变量之间的每一个边（Edge）代表了和它相连的两个成对的节点。这类问题，基本上都采用了优化方法提供一个可接受的数值解来解决这样的应用问题，比如 Gauss-Newton, Levenberg-Marquardt (LM), Gauss-Seidel relaxation，或variants of gradient descent。
因此，提出一种非线性最小二乘问题的通用框架，统称为g2o（general graph optimization）。
总结，其实就干了两件事情：
1.构建图，将机器人位姿作为顶点，位姿间约束关系作为边。
2.优化图，调整机器人的位姿（顶点）来满足边的约束，使得误差最小。

二、g2o是怎么干的

1.g2o框架

图1 Class diagram of g2o

（1）图的核心

看g2o系统框图箭头流向，最左侧的SparseOptimizer是整个图的核心。

（2）顶点和边

往上看SparseOptimizer是一个优化图OptimizableGraph，也是一个超图，包含许多顶点和边。其中，顶点依次继承顶点OptimizableGraph::Vertex、BaseVertex<D, T>，边依次继承OptimizableGraph::Edge、（单边BaseUnaryEdge<D, E,VertexXi>、双边BaseBinaryEdge<D, E,VertexXi,VertexXj>、多边BaseMultiEdge<D,E> E,VertexXi,VertexXj>）。顶点和边，在使用过程中是最重要的，且也是最应该注意的，下面会举例说明。

（3）配置SparseOptimizer的优化算法

往下看SparseOptimizer有一个优化算法OptimizableAlgorithm。优化算法继承于OptimizationWithHessian 来实现的，其中OptimizationWithHessian 继承的迭代策略有Gauss-Newton（高斯牛顿法，简称GN）, Levernberg-Marquardt（简称LM法）, Powell's dogleg。

（4）求解器

同时OptimizationWithHessian有一个求解器Solver，这个Solver是一个块求解器BlockSolver，包含许多稀疏块矩阵SpaseBlockMatrix<T>求解和线性求解器LinearSolver，其中线性求解器继承有PCG, CSparse, Choldmod求解策略。
通过这四步，在脑海中大致形成一张g2o的框架图，这很重要！！

2.示例说明

图1中通过红圈描述了实际编程时，使用g2o的步骤1-5，为了更直观展示g2o使用全过程，以一个简单曲线拟合的例子来做说明。
问题描述：找到一条函数曲线去拟合上图中的这些散点，使得所有点均匀的分散在这个拟合曲线的两侧，散点如下图2所示(注意出现一个离散野点，后期叙述有用)。

图2 离散点

为了使问题简单，给定拟合函数为：

$y = A*sin(w *x + \varphi )$

那问题就变得很简单了，可以将上述问题转化为找到一组参数 ${A, w, \varphi }$ ，使得所有离散点到函数图像的误差最小。于是数学上可以描述为：

$Min\sum_{i }^{N}(y_{i}-A*sin(w*x_{i}+\varphi ))$

到这里一般就有两种方式求解了，求函数解析解：最小二乘法，数值解：优化方法，对上述函数函数求导，通过导数为负，不断迭代使损失函数最小时，确定为找到的最优参数 ${A, w, \varphi }$ 。

具体最小二乘和优化相关方法详细介绍，请参见非线性优化相关资料。

于是g2o就派上用场了，按照图1通过红圈描述的g2o编程步骤1-5。

typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block; // 每个误差项优化变量维度为3，误差值维度为1

注意：如果在某些应用场景，优化变量维度或者误差变量维度不能确定的，可以使用，当然维度已知也可以使用了。
typedef g2o::BlockSolver< g2o::BlockSolverTraits<Eigen::Dynamic, Eigen::Dynamic> Block;

第1步：创建一个线性求解器LinearSolver

Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();

第2步：创建BlockSolver。并用上面定义的线性求解器初始化

Block* solver_ptr = new Block( linearSolver );

第3步：创建总求解器solver。并从GN, LM, DogLeg 中选一个，再用上述块求解器BlockSolver初始化

g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );

第4步：创建终极大boss 稀疏优化器（SparseOptimizer）

g2o::SparseOptimizer optimizer; // 图模型

optimizer.setAlgorithm( solver ); // 设置求解器

optimizer.setVerbose( true ); // 打开调试输出

第5步：定义图的顶点和边。并添加到SparseOptimizer中

int numPoints = 100;
Eigen::Vector2d *points = new Eigen::Vector2d[numPoints];

CurveFittingVertex* v = new CurveFittingVertex(); //往图中增加顶点

v->setEstimate( Eigen::Vector3d(0,0,0) ); //初始化顶点估计值
v->setId(0); //顶点编号
optimizer.addVertex(v);

for ( int i=0; i<N; i++ ) // 往图中增加边
{
    CurveFittingEdge* edge = new CurveFittingEdge;
    edge->setVertex(0, v); // 设置连接的顶点
    edge->setMeasurement(point[i]); // 观测数值
    edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma)); // 信息矩阵：协方差矩阵之逆
    g2o::RobustKernelHuber *robust_kernel_huber = new g2o::RobustKernelHuber; //设置鲁棒核函数
    robust_kernel_huber->setDelta(0.1); //设置delta大小
    edge->setRobustKernel(robust_kernel_huber); //向边添加鲁棒核函数

    optimizer.addEdge(edge);
}

主要成员变量函数和变量：

//成员变量
_measuremen 		//存储观测值
_error				//存储computeError() 函数计算的误差
_vertices[]			//存储顶点信息，
                    //比如二元边的话，_vertices[] 的大小为2，
                    //存储顺序和调用setVertex(int, vertex) 是设定的int 有关（0 或1）

//成员函数
setId(int)			//来定义边的编号（决定了在H矩阵中的位置）
setMeasurement(type) 	//函数来定义观测值
setVertex(int, vertex) 	//来定义顶点
setInformation() 		//来定义协方差矩阵的逆
setRobustKernel(robust_kernel_huber) //向边添加鲁棒核函数

第6步：设置优化参数，开始执行优化

optimizer.initializeOptimization();

optimizer.optimize(100);

通过上述步骤，g2o就可以正常完成优化了。但这里重点说明一下1.自定义顶点和边，这也是文章开始就说过顶点和边的重要性了；2.信息矩阵的设置；3.鲁棒核函数的设置。

根据步骤第5步，定义了顶点和边，顶点我们知道就是待优化的状态变量，边为观测约束，下面我们就看下，在顶点、边类中需要完成什么操作。

（1）构造顶点CurveFittingVertex

//继承BaseVertex类，构造顶点
class CurveFittingVertex : public g2o::BaseVertex<3, Eigen::Vector3d> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    CurveFittingVertex() = default;
    //读盘函数
    bool read(std::istream & /*is*/) override {
        cout << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
        return false;
    }
    //写盘函数
    bool write(std::ostream & /*os*/) const override {
        cout << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
        return false;
    }

    //顶点重置函数，设置顶点原始值
    void setToOriginImpl() override {
        cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
    }

    //顶点更新函数，更新优化之后的顶点
    void oplusImpl(const double *update) override {
        Eigen::Vector3d v(update);
        _estimate += v;
    }
};

构造的顶点继承BaseVertex<D, T>，其中D表示顶的维度，T表示顶点类型。

构造边CurveFittingEdge

//从BaseUnaryEdge继承得到一元边
class CurveFittingEdge : public g2o::BaseUnaryEdge<1, Eigen::Vector2d, CurveFittingVertex> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    CurveFittingEdge () = default;

    bool read(std::istream & /*is*/) override {
        cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
        return false;
    }

    bool write(std::ostream & /*os*/) const override {
        cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
        return false;
    }

    //边的误差计算
    void computeError() override {
        const CurveFittingVertex*params = dynamic_cast<const CurveFittingVertex *>(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;
        double fval = a*sin(lambda*measurement()(0) + b);
        _error(0) = std::abs(fval - measurement()(1));
    }

    /*
    计算雅克比矩阵，即当前误差对顶点的偏导数
    若定义了该函数，表示自己完成雅克比矩阵；无，则表示自动求导
    virtual void linearizeOplus()
    {
        cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl; 
    }
    */ 
};

构造的边继承BaseUnaryEdge<D, E, VertexXi>，其中D测量值维度，E表示测量值类型，VertexXi表示顶点类型。

如果二元边，BaseBinaryEdge<D, E, VertexXi, VertexXj>，VertexXi, VertexXj分别表示两个顶点的类型。

g2o库已定义顶点见附录

（2）设置信息矩阵

要弄清楚信息矩阵，还得从g2o的优化的目标函数入手，其形式如下：

$Min\sum_{i}^{N}e_{k}(x_{k}, z_{k})^{T}\Omega _{k}e_{k}(x_{k}, z_{k})$

其中， $\Omega$ 就表示信息矩阵。

信息矩阵 $\Omega$ 是协方差矩阵的逆，是一个对称矩阵。它的每个元素 $\Omega_{i,j}$ 作为 $e_{i}e_{j}$ 的系数，可以看成我们对 $e_{i}$ ， $e_{j}$ 这个误差项相关性的一个预计。最简单的是把 $\Omega$ 设成对角矩阵，对角阵元素的大小表明我们对此项误差的重视程度即权重。

edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma));

（3）设置鲁棒核函数

引入核函数的原因，是为了平衡误差，不让误差的二范数度量增加的过快。因为由于噪声等异常原因，SLAM中可能给出错误的边。把一条原本不应该加到图中的边加入优化，会怎么样？可能会使优化结果朝向异常点偏移，致使优化误差变大。

于是就有了核函数的存在。核函数保证每条边的误差不会太大，掩盖掉其他的边。具体的方式是，把原先误差的二范数度量，替换成一个增长没有那么快的函数，同时保证自己的光滑性质（不然没法求导啊！）。因为它们使得整个优化结果更为鲁棒，所以又叫它们为robust kernel（鲁棒核函数）。

Huber核如下：

$f(n)=\left\{\begin{matrix} \frac{1}{2}e^{2}, & if\left | e\leqslant \delta \right |\\ \delta(\left | e \right |-\frac{1}{2}\delta) & else\end{matrix}\right.$

g2o::RobustKernelHuber *robust_kernel_huber = new g2o::RobustKernelHuber; 
robust_kernel_huber->setDelta(0.1); //设置delta大小
edge->setRobustKernel(robust_kernel_huber); //向边添加鲁棒核函数

（4）验证

通过信息矩阵和核函数的介绍，可以利用图1散点中出现的野点做实验验证，（1）假设野点不存在，优化后参数值；（2）加入野点后，优化后参数值；（3）假设已知野点的边，改变该边信息矩阵权重，优化后参数值；（4）添加鲁棒核函数后，优化后参数的值。

设置待优化参数真值 ${A, w, \varphi }$ ：2，0.4，1。

（1）假设野点不存在

优化后参数值： ${A, w, \varphi }$ ：2.00123, 0.402314, 0.999973，优化后曲线如图3所示

图3 无野点

（2）加入野点后

加入野点如图2所示，优化后参数值： ${A, w, \varphi }$ ：1.99425, 0.344419, 1.006，精度有所下降，下降的精度会随着野点的数量和偏离程度变化。优化后曲线如图4所示

图4 加入野点（3, 2.5）

（3）加入信息矩阵

加入信息矩阵，并设置野点边（3, 2.5）权重为其它边的1/100，优化后参数值： ${A, w, \varphi }$ ：1.99723, 0.398168, 1.00027，可以看出精度立即提上来了，优化后曲线如图5所示

e->setInformation(Eigen::Matrix<double, 1, 1>::Identity());

if (i == 3) {
    e->setInformation(Eigen::Matrix<double, 1, 1>::Identity()*1/(10*10));
}

图5 加入信息矩阵

（4）加入核函数

加入核函数，设置delta为0.3(设置大小可以根据误差e的数值类型设定，不能设置太小，太小会增加迭代步数)，优化后参数值： ${A, w, \varphi }$ ：2.00103, 0.40047, 1.00018，可以看出精度和未加野点相近，可好的去除了野点的影响。优化后曲线如图6所示

图6 加入核函数

从图5-6可以看出，加入信息矩阵，通过设置不同边的权重，可以达到调节不同测量约束的权重的目的，毕竟不同测量约束精度不可能完全一致；而合适的设置鲁棒核函数delta，可以让你的优化结果起到鲁棒的作用，避免被一些异常测量给带偏，两者都可以起到提高精度、鲁棒性的作用。

三、封装g2o

有了前面介绍g2o是干什么的和怎么干的，并通过一个简单的曲线拟合例子做了说明，接下来我们将g2o用于通用slam优化。

其优化顶点变量：

$x^{_{k}}=\left \{ p_{b_{k}}^{w},v_{b_{k}}^{w},q_{b_{k}}^{w},b_{a},b_{g} \right \}$

约束边：

（1）imu预积分双向边约束

（2）gnss单向边

自定义顶点Vertex

struct PRVAG {
    static const int INDEX_POS = 0;
    static const int INDEX_ORI = 3;
    static const int INDEX_VEL = 6;
    static const int INDEX_B_A = 9;
    static const int INDEX_B_G = 12;

    PRVAG() {}

    explicit PRVAG(const double *data) {
        pos = Eigen::Vector3d(data[INDEX_POS + 0], data[INDEX_POS + 1], data[INDEX_POS + 2]);
        ori = Sophus::SO3d::exp(
              Eigen::Vector3d(data[INDEX_ORI + 0], data[INDEX_ORI + 1], data[INDEX_ORI + 2])
        );
        vel = Eigen::Vector3d(data[INDEX_VEL + 0], data[INDEX_VEL + 1], data[INDEX_VEL + 2]);
        b_a = Eigen::Vector3d(data[INDEX_B_A + 0], data[INDEX_B_A + 1], data[INDEX_B_A + 2]);
        b_g = Eigen::Vector3d(data[INDEX_B_G + 0], data[INDEX_B_G + 1], data[INDEX_B_G + 2]);
    }

    void write(double *data) const {}

    double time = 0.0;

    Eigen::Vector3d pos = Eigen::Vector3d::Zero();
    Sophus::SO3d ori = Sophus::SO3d();
    Eigen::Vector3d vel = Eigen::Vector3d::Zero();
    Eigen::Vector3d b_a = Eigen::Vector3d::Zero();
    Eigen::Vector3d b_g = Eigen::Vector3d::Zero();
};

class VertexPRVAG : public g2o::BaseVertex<15, PRVAG> {
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    virtual void setToOriginImpl() override {
        _estimate = PRVAG();
    }

    virtual void oplusImpl(const double *update) override {         //  顶点更新
        _estimate.pos  +=  Eigen::Vector3d(
            update[PRVAG::INDEX_POS + 0],  update[PRVAG::INDEX_POS + 1],  update[PRVAG::INDEX_POS + 2]  
        );
        _estimate.ori    =  _estimate.ori * Sophus::SO3d::exp(
            Eigen::Vector3d(
                update[PRVAG::INDEX_ORI + 0],  update[PRVAG::INDEX_ORI + 1],  update[PRVAG::INDEX_ORI + 2]  
            )
        );
        _estimate.vel += Eigen::Vector3d(
                update[PRVAG::INDEX_VEL + 0],  update[PRVAG::INDEX_VEL + 1],  update[PRVAG::INDEX_VEL + 2]  
        );
        Eigen::Vector3d d_b_a_i(
                update[PRVAG::INDEX_B_A + 0],  update[PRVAG::INDEX_B_A + 1],  update[PRVAG::INDEX_B_A + 2]  
        );
        Eigen::Vector3d d_b_g_i(
                update[PRVAG::INDEX_B_G + 0],  update[PRVAG::INDEX_B_G + 1],  update[PRVAG::INDEX_B_G + 2]  
        );

        _estimate.b_a += d_b_a_i ;
        _estimate.b_g += d_b_g_i;               

        updateDeltaBiases(d_b_a_i, d_b_g_i);
    }

    bool isUpdated(void) const { return _is_updated; }

    void updateDeltaBiases(
        const Eigen::Vector3d &d_b_a_i, 
        const Eigen::Vector3d &d_b_g_i
    ) {
        std::lock_guard<std::mutex> l(_m);

        _is_updated = true;

        _d_b_a_i += d_b_a_i;
        _d_b_g_i += d_b_g_i;
    }

    void getDeltaBiases(Eigen::Vector3d &d_b_a_i, Eigen::Vector3d &d_b_g_i) {
        std::lock_guard<std::mutex> l(_m);

        d_b_a_i = _d_b_a_i;
        d_b_g_i = _d_b_g_i;

        _d_b_a_i = _d_b_g_i = Eigen::Vector3d::Zero();

        _is_updated = false;
    }

    virtual bool read(std::istream &in) { return true; }

    virtual bool write(std::ostream &out) const { return true; }

private:
    std::mutex _m;
    bool _is_updated = false;

    Eigen::Vector3d _d_b_a_i = Eigen::Vector3d::Zero();
    Eigen::Vector3d _d_b_g_i = Eigen::Vector3d::Zero();
};

gnss边

class EdgePRVAGPriorPos : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexPRVAG> {
  public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW

	EdgePRVAGPriorPos() 
	 : g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexPRVAG>() {
	}

	virtual void computeError() override {
		const g2o::VertexPRVAG* v = static_cast<const g2o::VertexPRVAG*>(_vertices[0]);

		const Eigen::Vector3d &estimate = v->estimate().pos;

		_error = estimate - _measurement;
	}

    virtual void setMeasurement(const Eigen::Vector3d& m) override {
		_measurement = m;
	}

	virtual bool read(std::istream& is) override {}

	virtual bool write(std::ostream& os) const override {}
};

未完待续。。。

参考文章

1.非线性优化库g2o使用教程
2.深入理解图优化与g2o：图优化篇

3.SLAM图优化g2o

4.多传感器融合

附录

VertexSE2 : public BaseVertex<3, SE2>  //2D pose Vertex, (x,y,theta)
VertexSE3 : public BaseVertex<6, Isometry3>  //6d vector (x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion)
VertexPointXY : public BaseVertex<2, Vector2>
VertexPointXYZ : public BaseVertex<3, Vector3>
VertexSBAPointXYZ : public BaseVertex<3, Vector3>

// SE3 Vertex parameterized internally with a transformation matrix and externally with its exponential map
VertexSE3Expmap : public BaseVertex<6, SE3Quat>

// SBACam Vertex, (x,y,z,qw,qx,qy,qz),(x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion.
// qw is assumed to be positive, otherwise there is an ambiguity in qx,qy,qz as a rotation
VertexCam : public BaseVertex<6, SBACam>

// Sim3 Vertex, (x,y,z,qw,qx,qy,qz),7d vector,(x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion.
VertexSim3Expmap : public BaseVertex<7, Sim3>