VSLAM(8)：后端---Pose Graph优化和Factor Graph优化

原创已于 2023-07-30 20:38:26 修改 · 1.1k 阅读

7 ·

CC 4.0 BY-SA版权

文章标签：

#计算机视觉 #人工智能

于 2023-07-25 19:17:36 首次发布

视觉SLAM14讲专栏收录该内容

10 篇文章

订阅专栏

本文详细介绍了SLAM后端优化中的两种关键方法：位姿图（PoseGraph）优化和因子图优化。位姿图优化通过去掉特征点，仅保留位姿节点，以提高实时性和减少计算负担；因子图则利用贝叶斯网络和概率论，通过因子节点表示变量间的约束，实现更通用的优化。文章还提供了g2o和gtsam库在实际代码中的应用示例。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

前一章主要介绍了BA图优化的后端方法，这种方法可以同时优化相机位姿和路标点位置。这里的路标点其实就是特征点，往往数量巨大，对存储和计算都会带来巨大的负担，并且会不断累加，导致在大场景下无法实时化。

特征点的数量远大于位姿节点的数量，一个关键帧的特征点往往会有几百个关键帧。对于BA优化的方法，即使利用稀疏性，达到实时也只能有几万个点。那么为了解决实时性的问题，可以适当丢弃一些历史数据，想Sliding Window（滑动窗口法）；或则直接选择不对路标点进行优化，只保留位姿之间的约束条件，使用Pose Graph。

一，位姿图

1.1 Pose Graph的意义

BA优化的主要计算压力来自于对特征点的优化，因为随着特征点的不断增加，计算效率就会不断降低。实际上，在经过数次观测后，那些收敛的特征点，空间位置估计就会在某一个值附近保持不动；而发散的外点通常就看不到了。所以可不可以假设一个特征点，经过几次优化后就可以不再考虑，只把他们看作位姿估计的约束，而不再去对他的位置进行优化。

那如果考虑算法更快，完全不去考虑路标点的优化，只管轨迹优化呢？构建一个只有轨迹而没有特征点的图优化，节点为各个时刻的位姿，节点之间的边为由相邻两帧之间的特征匹配的结果作为初值。与BA优化不同的是，一旦初始估计完成后，就不再去考虑优化特征点位置，而只关心所有的相机位姿之间的联系。过程如下所示：

在Pose Graph问题中，每个节点就是相机此时的位姿，用它的李代数（ $\xi _1 , \dots , \xi _n$ ）表示；图的边就是根据相邻帧估计的位姿节点之间相对运动，这个估计结果可能来自直接发或则特征点法。在实际的SLAM框架中，也可能会有其他的约束条件，比如来自闭环检测的约束。这个运动在李代数上的定义为：

而按照李群的写法为：

就像之前在视觉里程计中说的1，这个估计本身就会存在误差，所以等式不会精确的成立。所以可以设最小二乘误差，利用优化方法求解最优值。误差 $e_{ij}$ 可以定义为：

可以看到误差定义式子中 $\Delta T^{-1}_{ij}$ 和 $\xi _{ij}$ 代表相邻帧估计结果,即相对位姿，可以通过IMU,lidar或则是camera传感器，使用一定的算法得到，例如ego-emotion，scan-registration，ICP等，是已知量，而待优化量则是前后两个时刻的相机位姿 $\xi _i$ 和 $\xi _j$ ，因此对这两项求导来得到雅可比矩阵。按照李代数的求导方式，分别给 $\xi _i$ 和 $\xi _j$ 各一个左扰动： $\delta \xi _i$ 和 $\delta \xi _j$ ，于是误差变为：

如果想利用BCH公式来近似的话，需要将中间的两个扰动项移到式子左边或则右边。这里使用下面的伴随性质：

就有：

因此，按照李代数上的求导法则，对 $T_i$ 的雅可比矩阵就是：

对 $T_j$ 的雅可比矩阵就是：

得到雅可比矩阵后，剩下就是定义目标函数，使用图优化的方法求解。总体目标函数为：

其中 $\Sigma _{i,j} ^{-1}$ 代表节点i和j之间的信息矩阵（information matrix），信息矩阵用来代表边的不确定度，信息矩阵越大代表这条边在优化的过程中越重要。

Pose Gragh的优势：求解问题存在稀疏性，可以加快求解速度，并且对于初始状态的变化较为鲁棒。

Pose Gragh的缺点：对外点(outliers，亲切一点就是错的离谱的结果)不具有鲁棒性，另外，优化估计Rotation的过程是一个非凸优化问题，容易造成导致局部最优问题，且不保证全局最优问题。

重磅论文精读！SLAM中姿态估计的图优化方法比较_问题_约束_Pose (sohu.com)这个文章了使用不同框架，在相同的而数据集（INTEL, MIT两个数据集）上求解Pose Graph问题的对比。

1.2 Pose Graph代码实践

这里给了一个带噪声的球形位姿图，使用g2o_viewer查看如下左图所示，是由g2o的create sphere函数创造的。而经过Pose Graph优化后的结果就左图所示。

位姿图优化的代码使用了g2o库自带的SE3和自己定义的SE3来实现。因为g2o自带的默认旋转向量以四元数的形式表示。代码分别如下：

#include <iostream>
#include <fstream>
#include <string>

#include <g2o/types/slam3d/types_slam3d.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
using namespace std;

/************************************************
 * 本程序演示如何用g2o solver进行位姿图优化
 * sphere.g2o是人工生成的一个Pose graph，我们来优化它。
 * 尽管可以直接通过load函数读取整个图，但我们还是自己来实现读取代码，以期获得更深刻的理解
 * 这里使用g2o/types/slam3d/中的SE3表示位姿，它实质上是四元数而非李代数.
 * **********************************************/

int main( int argc, char** argv )
{
    if ( argc != 2 )
    {
        cout<<"Usage: pose_graph_g2o_SE3 sphere.g2o"<<endl;
        return 1;
    }
    ifstream fin( argv[1] );
    if ( !fin )
    {
        cout<<"file "<<argv[1]<<" does not exist."<<endl;
        return 1;
    }

    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,6>> Block;  // 6x6 BlockSolver
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverCholmod<Block::PoseMatrixType>(); // 线性方程求解器
    Block* solver_ptr = new Block( linearSolver );      // 矩阵块求解器
    // 梯度下降方法，从GN, LM, DogLeg 中选
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );
    g2o::SparseOptimizer optimizer;     // 图模型
    optimizer.setAlgorithm( solver );   // 设置求解器

    int vertexCnt = 0, edgeCnt = 0; // 顶点和边的数量
    while ( !fin.eof() )
    {
        string name;
        fin>>name;
        if ( name == "VERTEX_SE3:QUAT" )
        {
            // SE3 顶点
            g2o::VertexSE3* v = new g2o::VertexSE3();
            int index = 0;
            fin>>index;
            v->setId( index );
            v->read(fin);
            optimizer.addVertex(v);
            vertexCnt++;
            if ( index==0 )
                v->setFixed(true);
        }
        else if ( name=="EDGE_SE3:QUAT" )
        {
            // SE3-SE3 边
            g2o::EdgeSE3* e = new g2o::EdgeSE3();
            int idx1, idx2;     // 关联的两个顶点
            fin>>idx1>>idx2;
            e->setId( edgeCnt++ );
            e->setVertex( 0, optimizer.vertices()[idx1] );
            e->setVertex( 1, optimizer.vertices()[idx2] );
            e->read(fin);
            optimizer.addEdge(e);
        }
        if ( !fin.good() ) break;
    }
    
    cout<<"read total "<<vertexCnt<<" vertices, "<<edgeCnt<<" edges."<<endl;
    
    cout<<"prepare optimizing ..."<<endl;
    optimizer.setVerbose(true);
    optimizer.initializeOptimization();
    cout<<"calling optimizing ..."<<endl;
    optimizer.optimize(30);
    
    cout<<"saving optimization results ..."<<endl;
    optimizer.save("result.g2o");

    return 0;
}

以及自己定义边和节点的代码：

#include <iostream>
#include <fstream>
#include <string>
#include <Eigen/Core>

#include <g2o/core/base_vertex.h>
#include <g2o/core/base_binary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>

#include <sophus/se3.h>
#include <sophus/so3.h>
using namespace std;
using Sophus::SE3;
using Sophus::SO3;

/************************************************
 * 本程序演示如何用g2o solver进行位姿图优化
 * sphere.g2o是人工生成的一个Pose graph，我们来优化它。
 * 尽管可以直接通过load函数读取整个图，但我们还是自己来实现读取代码，以期获得更深刻的理解
 * 本节使用李代数表达位姿图，节点和边的方式为自定义
 * **********************************************/

typedef Eigen::Matrix<double,6,6> Matrix6d;

// 给定误差求J_R^{-1}的近似
Matrix6d JRInv( SE3 e )
{
    Matrix6d J;
    J.block(0,0,3,3) = SO3::hat(e.so3().log());
    J.block(0,3,3,3) = SO3::hat(e.translation());
    J.block(3,0,3,3) = Eigen::Matrix3d::Zero(3,3);
    J.block(3,3,3,3) = SO3::hat(e.so3().log());
    J = J*0.5 + Matrix6d::Identity();
    return J;
}
// 李代数顶点
typedef Eigen::Matrix<double, 6, 1> Vector6d;
class VertexSE3LieAlgebra: public g2o::BaseVertex<6, SE3>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    bool read ( istream& is )
    {
        double data[7];
        for ( int i=0; i<7; i++ )
            is>>data[i];
        setEstimate ( SE3 (
                Eigen::Quaterniond ( data[6],data[3], data[4], data[5] ),
                Eigen::Vector3d ( data[0], data[1], data[2] )
        ));
    }

    bool write ( ostream& os ) const
    {
        os<<id()<<" ";
        Eigen::Quaterniond q = _estimate.unit_quaternion();
        os<<_estimate.translation().transpose()<<" ";
        os<<q.coeffs()[0]<<" "<<q.coeffs()[1]<<" "<<q.coeffs()[2]<<" "<<q.coeffs()[3]<<endl;
        return true;
    }
    
    virtual void setToOriginImpl()
    {
        _estimate = Sophus::SE3();
    }
    // 左乘更新
    virtual void oplusImpl ( const double* update )
    {
        Sophus::SE3 up (
            Sophus::SO3 ( update[3], update[4], update[5] ),
            Eigen::Vector3d ( update[0], update[1], update[2] )
        );
        _estimate = up*_estimate;
    }
};

// 两个李代数节点之边
class EdgeSE3LieAlgebra: public g2o::BaseBinaryEdge<6, SE3, VertexSE3LieAlgebra, VertexSE3LieAlgebra>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    bool read ( istream& is )
    {
        double data[7];
        for ( int i=0; i<7; i++ )
            is>>data[i];
        Eigen::Quaterniond q ( data[6], data[3], data[4], data[5] );
        q.normalize();
        setMeasurement (
            Sophus::SE3 ( q, Eigen::Vector3d ( data[0], data[1], data[2] ) ) 
        );
        //?
        for ( int i=0; i<information().rows() && is.good(); i++ )
            for ( int j=i; j<information().cols() && is.good(); j++ )
            {
                is >> information() ( i,j );
                if ( i!=j )
                    information() ( j,i ) =information() ( i,j );
            }
        return true;
    }
    bool write ( ostream& os ) const
    {
        VertexSE3LieAlgebra* v1 = static_cast<VertexSE3LieAlgebra*> (_vertices[0]);
        VertexSE3LieAlgebra* v2 = static_cast<VertexSE3LieAlgebra*> (_vertices[1]);
        os<<v1->id()<<" "<<v2->id()<<" ";
        SE3 m = _measurement;
        Eigen::Quaterniond q = m.unit_quaternion();
        os<<m.translation().transpose()<<" ";
        os<<q.coeffs()[0]<<" "<<q.coeffs()[1]<<" "<<q.coeffs()[2]<<" "<<q.coeffs()[3]<<" ";
        // information matrix 
        for ( int i=0; i<information().rows(); i++ )
            for ( int j=i; j<information().cols(); j++ )
            {
                os << information() ( i,j ) << " ";
            }
        os<<endl;
        return true;
    }

    // 误差计算与书中推导一致
    virtual void computeError()
    {
        Sophus::SE3 v1 = (static_cast<VertexSE3LieAlgebra*> (_vertices[0]))->estimate();
        Sophus::SE3 v2 = (static_cast<VertexSE3LieAlgebra*> (_vertices[1]))->estimate();
        _error = (_measurement.inverse()*v1.inverse()*v2).log();
    }
    
    // 雅可比计算
    virtual void linearizeOplus()
    {
        Sophus::SE3 v1 = (static_cast<VertexSE3LieAlgebra*> (_vertices[0]))->estimate();
        Sophus::SE3 v2 = (static_cast<VertexSE3LieAlgebra*> (_vertices[1]))->estimate();
        Matrix6d J = JRInv(SE3::exp(_error));
        // 尝试把J近似为I？
        _jacobianOplusXi = - J* v2.inverse().Adj();
        _jacobianOplusXj = J*v2.inverse().Adj();
    }
};

int main ( int argc, char** argv )
{
    if ( argc != 2 )
    {
        cout<<"Usage: pose_graph_g2o_SE3_lie sphere.g2o"<<endl;
        return 1;
    }
    ifstream fin ( argv[1] );
    if ( !fin )
    {
        cout<<"file "<<argv[1]<<" does not exist."<<endl;
        return 1;
    }

    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,6>> Block;  // BlockSolver为6x6
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverCholmod<Block::PoseMatrixType>(); // 线性方程求解器
    Block* solver_ptr = new Block ( linearSolver );     // 矩阵块求解器
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );
    // 试试G-N或Dogleg？
    // g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( solver_ptr );
    // g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton ( solver_ptr );
    
    g2o::SparseOptimizer optimizer;     // 图模型
    optimizer.setAlgorithm ( solver );  // 设置求解器

    int vertexCnt = 0, edgeCnt = 0; // 顶点和边的数量
    
    vector<VertexSE3LieAlgebra*> vectices;
    vector<EdgeSE3LieAlgebra*> edges;
    while ( !fin.eof() )
    {
        string name;
        fin>>name;
        if ( name == "VERTEX_SE3:QUAT" )
        {
            // 顶点
            VertexSE3LieAlgebra* v = new VertexSE3LieAlgebra();
            int index = 0;
            fin>>index;
            v->setId( index );
            v->read(fin);
            optimizer.addVertex(v);
            vertexCnt++;
            vectices.push_back(v);
            if ( index==0 )
                v->setFixed(true);
        }
        else if ( name=="EDGE_SE3:QUAT" )
        {
            // SE3-SE3 边
            EdgeSE3LieAlgebra* e = new EdgeSE3LieAlgebra();
            int idx1, idx2;     // 关联的两个顶点
            fin>>idx1>>idx2;
            e->setId( edgeCnt++ );
            e->setVertex( 0, optimizer.vertices()[idx1] );
            e->setVertex( 1, optimizer.vertices()[idx2] );
            e->read(fin);
            optimizer.addEdge(e);
            edges.push_back(e);
        }
        if ( !fin.good() ) break;
    }

    cout<<"read total "<<vertexCnt<<" vertices, "<<edgeCnt<<" edges."<<endl;

    cout<<"prepare optimizing ..."<<endl;
    optimizer.setVerbose(true);
    optimizer.initializeOptimization();
    cout<<"calling optimizing ..."<<endl;
    optimizer.optimize(30);

    cout<<"saving optimization results ..."<<endl;
    // 因为用了自定义顶点且没有向g2o注册，这里保存自己来实现
    // 伪装成 SE3 顶点和边，让 g2o_viewer 可以认出
    ofstream fout("result_lie.g2o");
    for ( VertexSE3LieAlgebra* v:vectices )
    {
        fout<<"VERTEX_SE3:QUAT ";
        v->write(fout);
    }
    for ( EdgeSE3LieAlgebra* e:edges )
    {
        fout<<"EDGE_SE3:QUAT ";
        e->write(fout);
    }
    fout.close();
    return 0;
}

二，因子图

2.1 Bayes Network

SLAM书中对因子图优化介绍只是简单的介绍，因为涉及到概率图理论，比较复杂，具体的理论参考引用2-5。

因子图优化基于概率论，将SLAM问题表达为一个动态贝叶斯网络（Dynamic Bayes Network，DBN）。这个问题的定义有点像之前的滤波器角度，从概率的角度来看待SLAM问题，将运动方程和观测方程构成状态变量之间的条件概率。如下图所示，其中圆圈表示贝叶斯网络的节点，红色和蓝色线表示状态变量之间的关系，箭头表示依赖关系。例如，从状态变量 $x_1$ 指向观测量 $z_1$ ，那么就代表观测量 $z_1$ 是依赖于状态量 $x_1$ ，用概率表示就是 $P(x_2|x_1)$ 。包括：

相机的位姿形成的节点： $x_1, x_2,\dots$ ;
输入量节点： $u$ ；
路标节点： $l$ ；
观测数据节点： $z$ 。

这样贝叶斯网络就表达了所有变量，以及各个方程给出的变量之间的条件概率关系。那么有了这个约束，后端优化的目标就是希望通过调整贝叶斯网络中随机变量的取值，让整个后验概率最大。

整个后验概率由条件概率公式推出，所以是一连串的因子乘积形式，因此可以将整个贝叶斯网络转换为一个因子图形式。

2.2 因子图--Factor Graph

因子图是一种无向图，由两种节点组成：表示优化变量的变量节点节点，以及表示因子的因子节点。将图11-5用因子图表示就是如下的结构：

其中，圆圈代表优化变量节点，表示SLAM问题待优化的量，即相机位姿x和路标点坐标l。而方框代表因子节点，表示待优化变量之间的关系，来自于运动方程和观测方程。优化因子图，就是调整变量的值，使它们的因子乘积最大化。

考虑运动方程为：

其中 $w_k$ 表示运动噪声，服从高斯分布 $w_k \sim N(0, R_k)$ 。所以对状态变量 $x_k$ 满足如下的高斯分布：

$P(x_k|x_{k-1}) \sim N(f(x_{k-1},u_k), R_k)$

而对于观测方程类似的高斯分布：

$P(z_{kj}|x_k,l_j) \sim N(h(x_k,l_j), Q_{kj})$

通过假设高斯分布，显式的表达出了因子图优化的目标函数。剩下的就是将这个问题转为我们熟悉的最优化问题，最大化后验概率就等于是最小化这个后验概率的负对数函数，然后就可以用熟悉的非线性优化方法来求解最优值。类似于图优化问题，因子可以使用单元的，二元的或则多元的，这取决于他和几个变量节点有关。

下图是一个实际问题中的因子图，其中红色嗲表我们的观测因子，浅蓝色代表运动因子，而深蓝色则是添加的回环检测出的回环因子。绿色代表先验因子，如GPS给出的定位信息。因为可能表达成概率分布的信息有很多种，于是因子图也可以定义许多不同的因子，例如轮速计的测量，IMU的测量等，所以因子图优化是一种更通用的优化方式。

2.3 增量特性

引用6种，Karess等人提出了iSAM（inremental Smooth and Mapping），对因子图进行处理，使其可以增量式地初始后端优化，来避免对于每一个新加入的节点重新计算一边所有的节点的更新量（包括雅可比矩阵的求取和更新方程的计算）。

当我们按照里程计的方式向因子图种添加新的节点时，其实收到影响的节点只有最后一个与之相连的节点，而早先节点的估计值，可以近似的（其实新增节点会对之前的估计产生印象，这个影响对于会随着距离新节点的距离增加而逐渐减小，所以默认忽略较远的数据）看成没有发生变化。这样的话，就可以在每次新增节点时，不必对整个图进行优化。

对于回环检测添加的因子节点，受影响的范围应该考虑为整个回环开始到当前帧的所有节点。这样依然是没有对整个图进行优化，只考虑了回环内的节点，对于回环外的点不考虑。

具体怎么操作还是需要去看原论文iSAM和iSAM2等，放在参考文献中。

2.4 代码实践

#include <iostream>
#include <fstream>
#include <string>
#include <Eigen/Core>

#include <sophus/se3.h>
#include <sophus/so3.h>

#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>

using namespace std;
using Sophus::SE3;
using Sophus::SO3;

/************************************************
 * 本程序演示如何用 gtsam 进行位姿图优化
 * sphere.g2o 是人工生成的一个 Pose graph，我们来优化它。
 * 与 g2o 相似，在 gtsam 中添加的是因子，相当于误差
 * **********************************************/

int main ( int argc, char** argv )
{
    if ( argc != 2 )
    {
        cout<<"Usage: pose_graph_gtsam sphere.g2o"<<endl;
        return 1;
    }
    ifstream fin ( argv[1] );
    if ( !fin )
    {
        cout<<"file "<<argv[1]<<" does not exist."<<endl;
        return 1;
    }

    gtsam::NonlinearFactorGraph::shared_ptr graph ( new gtsam::NonlinearFactorGraph );  // gtsam的因子图
    gtsam::Values::shared_ptr initial ( new gtsam::Values ); // 初始值
    // 从g2o文件中读取节点和边的信息
    int cntVertex=0, cntEdge = 0;
    cout<<"reading from g2o file"<<endl;
    
    while ( !fin.eof() )
    {
        string tag;
        fin>>tag;
        if ( tag == "VERTEX_SE3:QUAT" )
        {
            // 顶点
            gtsam::Key id;
            fin>>id;
            double data[7];
            for ( int i=0; i<7; i++ ) fin>>data[i];
            // 转换至gtsam的Pose3
            gtsam::Rot3 R = gtsam::Rot3::Quaternion ( data[6], data[3], data[4], data[5] );
            gtsam::Point3 t ( data[0], data[1], data[2] );
            initial->insert ( id, gtsam::Pose3 ( R,t ) );       // 添加初始值
            cntVertex++;
        }
        else if ( tag == "EDGE_SE3:QUAT" )
        {
            // 边，对应到因子图中的因子
            gtsam::Matrix m = gtsam::I_6x6;     // 信息矩阵
            gtsam::Key id1, id2;
            fin>>id1>>id2;
            double data[7];
            for ( int i=0; i<7; i++ ) fin>>data[i];
            gtsam::Rot3 R = gtsam::Rot3::Quaternion ( data[6], data[3], data[4], data[5] );
            gtsam::Point3 t ( data[0], data[1], data[2] );
            for ( int i=0; i<6; i++ )
                for ( int j=i; j<6; j++ )
                {
                    double mij;
                    fin>>mij;
                    m ( i,j ) = mij;
                    m ( j,i ) = mij;
                }
                
            // g2o的信息矩阵定义方式与gtsam不同，这里对它进行修改
            gtsam::Matrix mgtsam = gtsam::I_6x6;
            mgtsam.block<3,3> ( 0,0 ) = m.block<3,3> ( 3,3 ); // cov rotation
            mgtsam.block<3,3> ( 3,3 ) = m.block<3,3> ( 0,0 ); // cov translation
            mgtsam.block<3,3> ( 0,3 ) = m.block<3,3> ( 0,3 ); // off diagonal
            mgtsam.block<3,3> ( 3,0 ) = m.block<3,3> ( 3,0 ); // off diagonal
            
            gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information ( mgtsam );        // 高斯噪声模型
            gtsam::NonlinearFactor::shared_ptr factor ( 
                new gtsam::BetweenFactor<gtsam::Pose3> ( id1, id2, gtsam::Pose3 ( R,t ), model ) // 添加一个因子
            );
            graph->push_back ( factor );
            cntEdge++;
        }
        if ( !fin.good() )
            break;
    }
    
    cout<<"read total "<<cntVertex<<" vertices, "<<cntEdge<<" edges."<<endl;
    // 固定第一个顶点，在gtsam中相当于添加一个先验因子 
    gtsam::NonlinearFactorGraph graphWithPrior = *graph;
    gtsam::noiseModel::Diagonal::shared_ptr priorModel = 
        gtsam::noiseModel::Diagonal::Variances (
            ( gtsam::Vector ( 6 ) <<1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6 ).finished() 
        );
    gtsam::Key firstKey = 0;
    for ( const gtsam::Values::ConstKeyValuePair& key_value: *initial )
    {
        cout<<"Adding prior to g2o file "<<endl;
        graphWithPrior.add ( gtsam::PriorFactor<gtsam::Pose3> ( 
            key_value.key, key_value.value.cast<gtsam::Pose3>(), priorModel ) 
        );
        break;
    }

    // 开始因子图优化，配置优化选项
    cout<<"optimizing the factor graph"<<endl;
    // 我们使用 LM 优化
    gtsam::LevenbergMarquardtParams params_lm;
    params_lm.setVerbosity("ERROR");
    params_lm.setMaxIterations(20);
    params_lm.setLinearSolverType("MULTIFRONTAL_QR");
    gtsam::LevenbergMarquardtOptimizer optimizer_LM( graphWithPrior, *initial, params_lm );
    
    // 你可以尝试下 GN
    // gtsam::GaussNewtonParams params_gn;
    // params_gn.setVerbosity("ERROR");
    // params_gn.setMaxIterations(20);
    // params_gn.setLinearSolverType("MULTIFRONTAL_QR");
    // gtsam::GaussNewtonOptimizer optimizer ( graphWithPrior, *initial, params_gn );
    
    gtsam::Values result = optimizer_LM.optimize();
    cout<<"Optimization complete"<<endl;
    cout<<"initial error: "<<graph->error ( *initial ) <<endl;
    cout<<"final error: "<<graph->error ( result ) <<endl;

    cout<<"done. write to g2o ... "<<endl;
    // 写入 g2o 文件，同样伪装成 g2o 中的顶点和边，以便用 g2o_viewer 查看。
    // 顶点咯
    ofstream fout ( "result_gtsam.g2o" );
    for ( const gtsam::Values::ConstKeyValuePair& key_value: result )
    {
        gtsam::Pose3 pose = key_value.value.cast<gtsam::Pose3>();
        gtsam::Point3 p = pose.translation();
        gtsam::Quaternion q = pose.rotation().toQuaternion();
        fout<<"VERTEX_SE3:QUAT "<<key_value.key<<" "
            <<p.x() <<" "<<p.y() <<" "<<p.z() <<" "
            <<q.x()<<" "<<q.y()<<" "<<q.z()<<" "<<q.w()<<" "<<endl;
    }
    // 边咯 
    for ( gtsam::NonlinearFactor::shared_ptr factor: *graph )
    {
        gtsam::BetweenFactor<gtsam::Pose3>::shared_ptr f = dynamic_pointer_cast<gtsam::BetweenFactor<gtsam::Pose3>>( factor );
        if ( f )
        {
            gtsam::SharedNoiseModel model = f->noiseModel();
            gtsam::noiseModel::Gaussian::shared_ptr gaussianModel = dynamic_pointer_cast<gtsam::noiseModel::Gaussian>( model );
            if ( gaussianModel )
            {
                // write the edge information 
                gtsam::Matrix info = gaussianModel->R().transpose() * gaussianModel->R();
                gtsam::Pose3 pose = f->measured();
                gtsam::Point3 p = pose.translation();
                gtsam::Quaternion q = pose.rotation().toQuaternion();
                fout<<"EDGE_SE3:QUAT "<<f->key1()<<" "<<f->key2()<<" "
                    <<p.x() <<" "<<p.y() <<" "<<p.z() <<" "
                    <<q.x()<<" "<<q.y()<<" "<<q.z()<<" "<<q.w()<<" ";
                gtsam::Matrix infoG2o = gtsam::I_6x6;
                infoG2o.block(0,0,3,3) = info.block(3,3,3,3); // cov translation
                infoG2o.block(3,3,3,3) = info.block(0,0,3,3); // cov rotation
                infoG2o.block(0,3,3,3) = info.block(0,3,3,3); // off diagonal
                infoG2o.block(3,0,3,3) = info.block(3,0,3,3); // off diagonal
                for ( int i=0; i<6; i++ )
                    for ( int j=i; j<6; j++ )
                    {
                        fout<<infoG2o(i,j)<<" ";
                    }
                fout<<endl;
            }
        }
    }
    fout.close();
    cout<<"done."<<endl;
}