返回

[代码实践] G2O 学习记录(一):2D 位姿图优化

Ceres 学习记录(一) 我学习了一下 Ceres 优化库的基本使用方法,在这篇文章中会结合同样的问题作为例子,学习一下 G2O 的基本使用方法,并和 Ceres 在使用以及性能上作对比。

待求解问题

这里用的例子和 Ceres 学习记录(一) 的一样,是我在学习深蓝学院的激光slam课程第六次作业中提供的例子,这里简单介绍一下。主要是给定一系列节点和边然后进行图优化。这里的节点是机器人一系列位置(x, y, yaw),边则是前后两帧相对位姿观测值以及信息矩阵。优化过程主要是降低相对位姿观测值和由实际前后位姿计算出的预测值之间产生的误差。问题中节点的边的结构如下:

/**
 * Custom Edge and Vertex 
 */
struct myEdge
{
  int xi,xj;    // 对应节点的索引
  Eigen::Vector3d measurement;  // 两个节点之间的相对位姿的观测值
                                // 分别为:dx, dy, dtheta
  Eigen::Matrix3d infoMatrix;   // 本次观测的信息矩阵
};

typedef Eigen::Vector3d myVertex;   // 机器人位姿,x, y, theta

G2O 使用

G2O 的安装和正常 CMake 项目的安装流程一样,这里不再赘述,前置要求可以参考:g2o - General Graph Optimization

基本过程

相对于 Ceres 而言,G2O 是专门为了求解图优化问题而开发的,因此它已经给我们定义好了许多可以直接使用的节点和边的类型。使用 G2O 来求解图优化的问题的步骤大致如下:

  1. 确定要使用的节点和边的类型:
    • 使用 G2O 自带的节点和边
    • 自定义节点和边
  2. 确定块求解器和线性求解器类型
  3. 根据优化方法选择相应的优化器
  4. 进行优化器设定
  5. 添加节点和边
  6. 进行优化

具体代码片段如下:


/// 获取我们需要优化的节点和边
std::vector<myVertex> Vertexs;
std::vector<myEdge> Edges;

/// 确定块优化器和线性优化器,这里选择最基本的两个类型
using BlockSolverType =\
        g2o::BlockSolver<g2o::BlockSolverTraits<-1,-1>> ;
using LinearSolverType =\
        g2o::LinearSolverEigen<BlockSolverType::PoseMatrixType> ;

/// 确定优化方法,这里选择高斯牛顿法
g2o::SparseOptimizer optimizer;
auto linearSolver = g2o::make_unique<LinearSolverType>();
linearSolver->setBlockOrdering(false);
g2o::OptimizationAlgorithmGaussNewton* solver =\
            new g2o::OptimizationAlgorithmGaussNewton(
            g2o::make_unique<BlockSolverType>(std::move(linearSolver)));

optimizer.setAlgorithm(solver);
optimizer.setVerbose(true);

/// 添加节点
for (size_t i = 0; i < vertexes.size(); ++i) {
    
    const myVertex& v_vec = vertexes[i];
    MyG2OVertex* v = new MyG2OVertex(); /// 构造一个节点
    v->setId(i);                        /// 设定索引,要和边对应
    v->setEstimate(v_vec);              /// 设定初值
    optimizer.addVertex(v);             /// 将节点加入优化器中
}

for (const auto& edge: edges) {

    MyG2OEdge* e = new MyG2OEdge;                   // 构造一条边
    e->vertices()[0] = optimizer.vertex(edge.xi);   // 设定这条边对应的两个节点
                                                    // 的索引,注意前后顺序
    e->vertices()[1] = optimizer.vertex(edge.xj);
    e->setMeasurement(edge.measurement);            // 设定观测值
    e->setInformation(edge.infoMatrix);             // 设定信息矩阵
    optimizer.addEdge(e);                           // 将边加入优化器中

}

/// 固定第一帧节点作为初值
MyG2OVertex* firstVertex = dynamic_cast<MyG2OVertex*>(optimizer.vertex(0));
firstVertex->setFixed(true);

/// 设定结束条件
g2o::SparseOptimizerTerminateAction* terminateAction =\
                     new g2o::SparseOptimizerTerminateAction;
terminateAction->setGainThreshold(1e-6);    /// 结束迭代的阈值
terminateAction->setMaxIterations(10);      /// 最大迭代次数
optimizer.addPostIterationAction(terminateAction);

/// 进行优化
optimizer.initializeOptimization();
optimizer.optimize(10);

/// 可选:将优化器中优化后的结果更新到原来的节点中
for (size_t i = 0; i < vertexes.size(); ++i) {
    vertexes[i] = dynamic_cast<MyG2OVertex*>(optimizer.vertex(i))->estimate();
}

G2O 节点和边

关于这次的具体问题我们可以使用 G2O 自带的节点和边 g2o::VertexSE2g2o::EdgeSE2。关于 G2O 中可以直接使用的节点和边可以参考:g2o/types。不过为了学习怎么自定义节点和边,这次我们还是采用自定义的方法。

首先来看节点,通过以下代码介绍:

/// 继承 G2O 中的基础节点类型,参数分别为:节点维度和数据类型
class MyG2OVertex : public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    /// 重置
    virtual void setToOriginImpl() override {
        _estimate << 0, 0, 0;
    }

    /// 更新
    virtual void oplusImpl(const double *update) override {
        _estimate += Eigen::Vector3d(update);
    }

    virtual bool read(std::istream &in) {}
    virtual bool write(std::ostream &out) const {}

};

节点的定义比较简单,只需要继承 G2O 的基础节点类型,并且给出几个关键虚函数的实现即可:

  • setToOriginImpl():重置节点的实现,通常情况下各个参数置 0 即可
  • oplusImpl():更新节点方法的实现,这里由于我们使用的节点和更新量都是 Eigen::Vector3d,直接相加即可
  • read(std::istream &in); & write(std::ostream &out);:读盘和写盘,还没有深入了解,不过大部分时间不需要

接下来看边的实现,同样直接看代码:

/// 继承 G2O 中的基础边类型,这里使用二元边
/// 参数分别为:误差维度,第一个节点数据类型,第二个节点数据类型
class MyG2OEdge : \
public g2o::BaseBinaryEdge<3, Eigen::Vector3d, MyG2OVertex, MyG2OVertex> 
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    /// 计算误差
    virtual void computeError() override;

    /// (可选)计算雅克比,
    virtual void linearizeOplus() override;  

    virtual bool read(std::istream& is) {}
    virtual bool write(std::ostream& os) const {}  
};

先看一下声明,同样也是需要继承 G2O 中的一个基础边类型,并给出几个关键函数的实现:

  • computeError(): 在之前介绍基本过程时有提到,再添加边之前需要设定好该边连接的节点的索引以及观测值和信息矩阵,这个函数则是使用该边连接的节点和观测值来计算误差
  • linearizeOplus():计算雅克比矩阵,这个函数是可选的,如果给出了则进行解析求导,不给则进行数值求导
  • read(std::istream &in); & write(std::ostream &out);:读盘和写盘

接下来看一下实现:

void MyG2OEdge::computeError() {

        MyG2OVertex* vtx1 = static_cast<MyG2OVertex*>(_vertices[0]);
        MyG2OVertex* vtx2 = static_cast<MyG2OVertex*>(_vertices[1]);
        Eigen::Vector3d measurement{ _measurement };
        Eigen::Vector3d v1 = vtx1->estimate();
        Eigen::Vector3d v2 = vtx2->estimate();

        // calculate error from translation and rotation respectively
        //  and combine them together
        Eigen::Matrix3d X1 = PoseToTrans(v1);
        Eigen::Matrix3d X2 = PoseToTrans(v2);
        Eigen::Matrix3d Z = PoseToTrans(measurement);
        
        Eigen::Matrix2d Ri = X1.block(0, 0, 2, 2);
        Eigen::Matrix2d Rj = X2.block(0, 0, 2, 2);
        Eigen::Matrix2d Rij = Z.block(0, 0, 2, 2);

        Eigen::Vector2d ti{ v1(0), v1(1) };
        Eigen::Vector2d tj{ v2(0), v2(1) };
        Eigen::Vector2d tij{ measurement(0), measurement(1) };

        Eigen::Matrix2d dRiT_dtheta;       //  derivative of Ri^T over theta
        dRiT_dtheta(0, 0) = -1 * Ri(1, 0); //  cosX -> -sinX
        dRiT_dtheta(0, 1) =  1 * Ri(0, 0); //  sinX ->  cosX
        dRiT_dtheta(1, 0) = -1 * Ri(0, 0); // -sinX -> -cosX
        dRiT_dtheta(1, 1) = -1 * Ri(1, 0); //  cosX -> -sinX

        // calcuate error & normalize error on theta
        _error.segment<2>(0) =\
                 Rij.transpose() * (Ri.transpose() * (tj - ti) - tij);
        _error(2) = v2(2) - v1(2) - measurement(2);
        if (_error(2) > M_PI) {
            _error(2) -= 2 * M_PI;
        } else if (_error(2) < -1 * M_PI) {
            _error(2) += 2 * M_PI;
        }
}

void MyG2OEdge::linearizeOplus() {

    MyG2OVertex* vtx1 = static_cast<MyG2OVertex*>(_vertices[0]);
    MyG2OVertex* vtx2 = static_cast<MyG2OVertex*>(_vertices[1]);
    Eigen::Vector3d measurement{ _measurement };
    Eigen::Vector3d v1 = vtx1->estimate();
    Eigen::Vector3d v2 = vtx2->estimate();

    // calculate error from translation and rotation respectively 
    // and combine them together
    Eigen::Matrix3d X1 = PoseToTrans(v1);
    Eigen::Matrix3d X2 = PoseToTrans(v2);
    Eigen::Matrix3d Z = PoseToTrans(measurement);
    
    Eigen::Matrix2d Ri = X1.block(0, 0, 2, 2);
    Eigen::Matrix2d Rj = X2.block(0, 0, 2, 2);
    Eigen::Matrix2d Rij = Z.block(0, 0, 2, 2);

    Eigen::Vector2d ti{ v1(0), v1(1) };
    Eigen::Vector2d tj{ v2(0), v2(1) };
    Eigen::Vector2d tij{ measurement(0), measurement(1) };

    Eigen::Matrix2d dRiT_dtheta;       //  derivative of Ri^T over theta
    dRiT_dtheta(0, 0) = -1 * Ri(1, 0); //  cosX -> -sinX
    dRiT_dtheta(0, 1) =  1 * Ri(0, 0); //  sinX ->  cosX
    dRiT_dtheta(1, 0) = -1 * Ri(0, 0); // -sinX -> -cosX
    dRiT_dtheta(1, 1) = -1 * Ri(1, 0); //  cosX -> -sinX

    Eigen::Matrix3d Ai;
    Eigen::Matrix3d Bi;
    Ai.setZero();
    Ai.block(0, 0, 2, 2) = -Rij.transpose() * Ri.transpose();
    Ai.block(0, 2, 2, 1) = Rij.transpose() * dRiT_dtheta * (tj - ti);
    Ai(2, 2) = -1.0;

    Bi.setIdentity();
    Bi.block(0, 0, 2, 2) = Rij.transpose() * Ri.transpose();

    _jacobianOplusXi = Ai;
    _jacobianOplusXj = Bi;
}

具体求误差和雅克比的方法在之前的博客中已经介绍过,这里不再赘述,可以参考:基于图优化的激光 SLAM 方法,针对 G2O 的部分主要有:

  • computeError() 最后要更新的变量的是 _error,其数据类型在声明边类型的时候已经给出,这里是 Eigen::Vector3d
  • linearizeOplus() 最后要更新的变量是 _jacobianOplusXi_jacobianOplusXj (因为是二元边所以有两个雅克比矩阵),数据类型也是固定的,这里是 Eigen::Matrix3d 可以直接使用

相对于 Ceres 而言,由于是针对图优化专门开发的,所以函数接口比较优化,数据都是可以使用更新的。而 Ceres 则经常给定的参数是一个通用的指针,需要使用 Eigen::Map 进行映射。并且 G2O 在加入边时可以加入信息矩阵,所以在计算误差和雅克比不需要考虑信息矩阵的位置。

性能比较

自动求导

首先试一下不给出 linearizeOplus() 的实现进行自动求导,结果如下:

Edges:3995
initError: 3.08592e+08
Use G2O Guassian Newton Method.
iteration= 0     chi2= 139936504.849390  time= 0.718313  cumTime= 0.718313       edges= 3995    schur= 0
iteration= 1     chi2= 13461.335501      time= 0.65867   cumTime= 1.37698        edges= 3995    schur= 0
iteration= 2     chi2= 10344.667469      time= 0.659928  cumTime= 2.03691        edges= 3995    schur= 0
iteration= 3     chi2= 10344.665262      time= 0.659581  cumTime= 2.69649        edges= 3995    schur= 0
Takes :3.07313 Seconds.
FinalError: 10344.7

使用了 3 次迭代,耗时 3 秒左右。

解析求导

然后给出 linearizeOplus() 的实现,结果如下:

Edges:3995
initError: 3.08592e+08
Use G2O Guassian Newton Method.
iteration= 0     chi2= 139937196.168651  time= 0.361189  cumTime= 0.361189       edges= 3995    schur= 0
iteration= 1     chi2= 13466.439812      time= 0.297259  cumTime= 0.658448       edges= 3995    schur= 0
iteration= 2     chi2= 10344.667472      time= 0.295245  cumTime= 0.953693       edges= 3995    schur= 0
iteration= 3     chi2= 10344.665262      time= 0.296346  cumTime= 1.25004        edges= 3995    schur= 0
Takes :1.63204 Seconds.
FinalError: 10344.7

同样使用了 3 次迭代,耗时 1.6 秒左右,可以看出比自动求导快了不少。

使用 G2O 自带的节点和边

最后试一下使用 G2O 自带的节点和边 g2o::VertexSE2g2o::EdgeSE2,结果如下:

Edges:3995
initError: 3.08592e+08
Use G2O Guassian Newton Method.
iteration= 0     chi2= 139937196.168749  time= 0.0911207         cumTime= 0.0911207      edges= 3995     schur= 0
iteration= 1     chi2= 13466.439812      time= 0.0390052         cumTime= 0.130126       edges= 3995     schur= 0
iteration= 2     chi2= 10344.667472      time= 0.0390674         cumTime= 0.169193       edges= 3995     schur= 0
iteration= 3     chi2= 10344.665262      time= 0.0389506         cumTime= 0.208144       edges= 3995     schur= 0
Takes :0.270036 Seconds.
FinalError: 10344.7

同样使用了 3 次迭代,但是耗时仅在 0.3 秒左右,速度大概是自己实现的版本的 5 倍左右。考虑了一下主要的原因有可能是节点的存储方式有区别,这里我主要是以学习为主所以没有很考虑效率,存储时直接以向量 x, y, theta的方式存储,因此在计算误差和雅克比需要经常进行转换以及构造旋转矩阵和平移向量等,这里会比较耗费资源,而 G2O 中的节点是以李群 SE2 的方式来存储,所以计算误差和雅克比可以直接使用。

而我们在 Ceres 学习记录(一) 也使用过 Ceres 进行类似测试,比较一下可以发现,都使用自动求导时, Ceres 的效率会比 G2O 的数值求导方法稍微高一点,而使用解析求导时,G2O 会更快一点,尤其是在使用自带的节点和边的情况。当然还和具体的编程方式有关系,这里我本身生成很多临时变量比较没有注重效率,所以可能这个结果比较的意义也不是很大,仅供参考。

Built with Hugo
Theme Stack designed by Jimmy