视觉SLAM 十四讲——后端设计II(Pose Graph, Factor Graph)
主要内容
1. 位姿图
1.1） 引出
BA问题中，特征点在优化问题中占据了绝大部分，因此在优化过程中，倾向于把收敛的特征点固定住，只把它们看做位姿估计的约束，而不再实际的优化它们的位置。
当机器人在更大范围的时间和空间中运动时，从减小计算量的角度出发有一下解决方案：
a 滑动窗口法，丢弃一些历史数据；
b 位姿图，舍弃对路标点的优化，只保留相机位姿。
1.2） Pose Graph优化
1.2.1） 构建误差方程：
其中，第一项表示i和j时刻之间的一个运动变换，这个是已知的，可以通过前后两帧图像计算得到的；后面两项分别为i时刻和j时刻的位姿。
1.2.2） 推到误差关于优化变量的导数（雅克比矩阵）：
其中两项分别如下所示：
由于se(3)上的左右雅克比Jr形式过去复杂，如果误差接近于0，可近似为如下形式：
(会有一定的损失)
其中，伴随矩阵求解如下：
1.2.3) 构造最优化问题
所有的位姿顶点和位姿-位姿边构成了一个图优化，本质上是一个最小二乘问题，优化变量为各个顶点的位姿，边来自于位姿观测约束。优化方法可以选用高斯牛顿法、列文伯格-马夸尔特方法等求解。
位姿图总结：
一般认为，位姿图是是结构最简单的图之一。在不假设机器人如何运动的前提下，很难再进一步讨论它的稀疏性。例如直线往前运动，形成带状的位姿图，是稀疏的，又如形成大量的小型回环需要优化，从而变成像试验中“球”那样比较稠密的地图。
当位姿点比较多，且整体位姿图是稠密的，那么整体计算也是比较耗费时间的，相比前端VO，因此在PTAM中，将前端和后端分开，分别运行与两个线程中，历史上称为跟踪（Tracking）和建图（Mapping）。
位姿图参考链接
g2o Error in `g2o_viewer': realloc(): invalid pointer: 0x00007f6911493820
2. 因子图
2.1） 参考文献
[1] Koller D , Friedman N . Probabilistic Graphical Models: Principles and Techniques[M]. MIT Press, 2009.
[2] Kaess M, Ranganathan A, Dellaert F. iSAM: Incremental smoothing and mapping[J]. IEEE Transactions on Robotics, 2008, 24(6): 1365-1378.
[3] Kaess M, Johannsson H, Roberts R, et al. iSAM2: Incremental smoothing and mapping using the Bayes tree[J]. The International Journal of Robotics Research, 2012, 31(2): 216-235.
[4] Rosen D M, Kaess M, Leonard J J. RISE: An incremental trust-region method for robust online sparse least-squares estimation[J]. IEEE Transactions on Robotics, 2014, 30(5): 1091-1108.
2.2） 贝叶斯网络
贝叶斯网络（Bayes Network）是一种概率图，由随机变量的节点和表达随机变量条件独立性的边组成，形成一个有向无环图。
在SLAM中，运动方程和观测方程恰好表示了状态变量之间的条件概率，因此，SLAM可以自然地表达成一个动态贝叶斯网络（Dynamic Bayes Network, DBN）。
在贝叶斯网络中，各线条表示了它们之间的关系，箭头表示依赖关系，比如x1指向x2，可以用条件概率来表示P(x2|x1)，SLAM中运动方程和观测方程可以用条件概率表示，即可以用贝叶斯网络来表达。
后端优化的目标是在这些既有约束之上，通过调整贝叶斯网络中随机变量的取值，使整个后验概率达到最大。可以使用概率图模型中的算法进行求解。
2.3） 因子图概念
因子图是一种无向图，由两种节点组成，表示优化变量的变量节点，以及表示因子的因子节点。
对因子图的优化，就是调整各变量的值，使它们的因子之乘积最大化——对应着一个优化问题，在该问题中，把各个因子的条件概率去高斯分布，则可显示的表达因子图优化的目标函数。
运动方程和观测方程作为因子存在于图中，除此之外，对于某些相机的位姿可能具有先验信息（GPS测量的位置或者速度信息等），那么就可以对这些待优化变量位姿添加先验因子。且由于不同的先验信息具有不同的概率分布，因此因子图也可以定义许多不同的因子，比如里程计，IMU的测量等。
2.4） 增量特性
Kaess等人提出的iSAM（incremental Smooth and Mapping）中，对因子图进行了更加细致的处理，使得它可以增量式地处理后端优化。
2.4.1） 原有问题：
随着机器人运动，新的节点和边加入到图中，使得整体规模不断增长。是否每次新加一个节点，就需要重新计算一遍所有节点的更新量？
2.4.2） 增量特性:
新加节点，受影响的节点可以近似看成只有最后一个与之相连的节点（实际情况，新增节点还是会对之前的估计产生影响的，只是对最近的数据影响最大，对较远的数据影响很小，可以忽略掉，节省计算量，避免增加新节点对整个图进行优化）
按回环检测增加新节点，受影响范围为回环开始到当前帧这一段中的所有节点，也就是整段轨迹都有可能被重新调整，回环以外的节点是不受影响的。
2.4.3） 实际实现
在每次优化中保存中间结果，而当新的变量和因子加入时，首先分析它们因子图之间的连接和影响关系，考虑之前存储的信息有哪些可以继续利用，哪些必须重新计算，最后处理对增量的优化。
实际中，尽管有增量分析，但必须清楚的一点这里受影响的节点是近似的，当图的规模发生一定程度的改变时，需要再做一次全图优化。
因子图参考链接：
3. 位姿图优化——g2o原生位姿图实现
3.1） 数据说明：
位姿图数据是由g2o自带的create sphere 程序仿真生成的
真实轨迹为一个球体，由从下往上的多个层组成
仿真程序生成了t-1到t时刻的边，称为odometry边（里程计），此外，又生成了层与层之间的边，称为loop closure (回环)
在每条边上添加噪声，根据里程计边的噪声，重新设置节点的初始值，这样就生成了带有累计误差的位姿图数据。
3.2） 位姿图信息读取
包括各个位姿点的位姿信息；（ID + 平移向量+ 四元数）
两两位姿点之间的量测信息以及R阵信息（其中R阵是非对角线元素是一样的，所以给的是三角阵）
3.3） 图优化问题构建
求解器的设置，优化问题构建
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 ); // 设置求解器
位姿顶点采用g2o::VertexSE3：
其中顶点数据初始化采用read函数读文件数据实现
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); }
边的类型采用g2o::EdgeSE3：
其中需要注意边类型中：
computeError 误差函数计算（两测的两两顶点的位姿变化和实际位姿变化的残差计算）
linearizeOplus线性化函数实现（雅克比矩阵求解）：computeEdgeSE3Gradient函数（细节需要在查看一下？？？？？）
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); }
优化结果保存：调用优化类g2o::SparseOptimizer实现的save函数实现：
optimizer.save("result.g2o");
3.4） 测试结果如下：（上面一幅图为原始结果，下面一幅图为优化以后的结果）
read total 2500 vertices, 9799 edges. prepare optimizing ... calling optimizing ... iteration= 0 chi2= 1023011093.967642 time= 0.284138 cumTime= 0.284138 edges= 9799 schur= 0 lambda= 805.622433 levenbergIter= 1 iteration= 1 chi2= 385118688.233189 time= 0.226302 cumTime= 0.51044 edges= 9799 schur= 0 lambda= 537.081622 levenbergIter= 1 iteration= 2 chi2= 166223726.693659 time= 0.224116 cumTime= 0.734555 edges= 9799 schur= 0 lambda= 358.054415 levenbergIter= 1 iteration= 3 chi2= 86610874.269316 time= 0.235826 cumTime= 0.970382 edges= 9799 schur= 0 lambda= 238.702943 levenbergIter= 1 iteration= 4 chi2= 40582782.710189 time= 0.2299 cumTime= 1.20028 edges= 9799 schur= 0 lambda= 159.135295 levenbergIter= 1 iteration= 5 chi2= 15055383.753040 time= 0.226908 cumTime= 1.42719 edges= 9799 schur= 0 lambda= 101.425210 levenbergIter= 1 iteration= 6 chi2= 6715193.487654 time= 0.23027 cumTime= 1.65746 edges= 9799 schur= 0 lambda= 37.664667 levenbergIter= 1 iteration= 7 chi2= 2171949.168383 time= 0.226018 cumTime= 1.88348 edges= 9799 schur= 0 lambda= 12.554889 levenbergIter= 1 iteration= 8 chi2= 740566.827049 time= 0.229358 cumTime= 2.11284 edges= 9799 schur= 0 lambda= 4.184963 levenbergIter= 1 iteration= 9 chi2= 313641.802464 time= 0.225855 cumTime= 2.33869 edges= 9799 schur= 0 lambda= 2.583432 levenbergIter= 1 iteration= 10 chi2= 82659.743578 time= 0.231751 cumTime= 2.57044 edges= 9799 schur= 0 lambda= 0.861144 levenbergIter= 1 iteration= 11 chi2= 58220.369189 time= 0.228829 cumTime= 2.79927 edges= 9799 schur= 0 lambda= 0.287048 levenbergIter= 1 iteration= 12 chi2= 52214.188561 time= 0.236754 cumTime= 3.03603 edges= 9799 schur= 0 lambda= 0.095683 levenbergIter= 1 iteration= 13 chi2= 50948.580336 time= 0.230022 cumTime= 3.26605 edges= 9799 schur= 0 lambda= 0.031894 levenbergIter= 1 iteration= 14 chi2= 50587.776729 time= 0.278807 cumTime= 3.54485 edges= 9799 schur= 0 lambda= 0.016436 levenbergIter= 1 iteration= 15 chi2= 50233.038802 time= 0.238338 cumTime= 3.78319 edges= 9799 schur= 0 lambda= 0.010957 levenbergIter= 1 iteration= 16 chi2= 49995.082839 time= 0.256058 cumTime= 4.03925 edges= 9799 schur= 0 lambda= 0.007305 levenbergIter= 1 iteration= 17 chi2= 48876.738967 time= 0.453673 cumTime= 4.49292 edges= 9799 schur= 0 lambda= 0.009298 levenbergIter= 2 iteration= 18 chi2= 48806.625522 time= 0.237011 cumTime= 4.72993 edges= 9799 schur= 0 lambda= 0.006199 levenbergIter= 1 iteration= 19 chi2= 47790.891373 time= 0.456466 cumTime= 5.1864 edges= 9799 schur= 0 lambda= 0.008265 levenbergIter= 2 iteration= 20 chi2= 47713.626582 time= 0.242076 cumTime= 5.42848 edges= 9799 schur= 0 lambda= 0.005510 levenbergIter= 1 iteration= 21 chi2= 46869.323688 time= 0.494889 cumTime= 5.92336 edges= 9799 schur= 0 lambda= 0.007347 levenbergIter= 2 iteration= 22 chi2= 46802.585510 time= 0.242211 cumTime= 6.16558 edges= 9799 schur= 0 lambda= 0.004898 levenbergIter= 1 iteration= 23 chi2= 46128.758042 time= 0.456365 cumTime= 6.62194 edges= 9799 schur= 0 lambda= 0.006489 levenbergIter= 2 iteration= 24 chi2= 46069.133541 time= 0.59885 cumTime= 7.22079 edges= 9799 schur= 0 lambda= 0.004326 levenbergIter= 1 iteration= 25 chi2= 45553.862165 time= 0.544436 cumTime= 7.76523 edges= 9799 schur= 0 lambda= 0.005595 levenbergIter= 2 iteration= 26 chi2= 45511.762617 time= 0.282814 cumTime= 8.04804 edges= 9799 schur= 0 lambda= 0.003730 levenbergIter= 1 iteration= 27 chi2= 45122.762999 time= 0.465221 cumTime= 8.51326 edges= 9799 schur= 0 lambda= 0.004690 levenbergIter= 2 iteration= 28 chi2= 45095.174401 time= 0.236706 cumTime= 8.74997 edges= 9799 schur= 0 lambda= 0.003127 levenbergIter= 1 iteration= 29 chi2= 44811.248504 time= 0.470759 cumTime= 9.22073 edges= 9799 schur= 0 lambda= 0.003785 levenbergIter= 2 saving optimization results ...
4. 位姿图优化——g2o 李代数实现
4.1） g2o求解配置
注意 :BlockSolver<g2o::BlockSolverTraits<6,6>>的维度情况
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 ); // 设置求解器
4.2） 顶点实现及添加
4.2.1） 顶点实现
其中，需要注意，顶点更新函数，以及定点信息从文件中输入和输出的函数
// 李代数顶点 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; } };
4.2.2） 定点信息读入
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); }
4.3） 边实现及添加
4.3.1） 李代数边实现
其中需要关注 g2o::BaseBinaryEdge<6, SE3, VertexSE3LieAlgebra, VertexSE3LieAlgebra> 类模板参数中，包含边的维度及类型，以及 两个顶点类型
残差计算函数computeError实现（和书上的公式一致）
线性化函数 linearizeOplus（雅克比矩阵求解）：（和书上的公式一致）
这边需要注意公式中Jr^-1的求解实现：（书上的近似公式）
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; }
注：雅克比矩阵计算方式：a: 不提供雅克比计算函数，g2o自动计算数值雅克比；b： 提供完整的或近似的雅克比计算过程（当前实现是提供近似的雅克比计算过程）
边信息的读入以及 协方差阵信息的读入及设置
边信息的输出，包括连接顶点的ID，边对应的残差信息（对应平移和四元数形式），量测信息的协方差矩阵
// 两个李代数节点之边 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(); } };
4.3.2） 边的信息添加
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); }
4.4）最终优化结果写入文件：
// 因为用了自定义顶点且没有向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();
4.5） 优化结果如下：
Starting: /home/qifarui/code/slambook-master/ch11/build/pose_graph_g2o_lie sphere.g2o read total 2500 vertices, 9799 edges. prepare optimizing ... calling optimizing ... iteration= 0 chi2= 781963143.389706 time= 0.369662 cumTime= 0.369662 edges= 9799 schur= 0 lambda= 6706.585223 levenbergIter= 1 iteration= 1 chi2= 236521032.458036 time= 0.334284 cumTime= 0.703946 edges= 9799 schur= 0 lambda= 2235.528408 levenbergIter= 1 iteration= 2 chi2= 142934798.398777 time= 0.323039 cumTime= 1.02699 edges= 9799 schur= 0 lambda= 745.176136 levenbergIter= 1 iteration= 3 chi2= 84490229.050136 time= 0.342605 cumTime= 1.36959 edges= 9799 schur= 0 lambda= 248.392045 levenbergIter= 1 iteration= 4 chi2= 42690811.624642 time= 0.326033 cumTime= 1.69562 edges= 9799 schur= 0 lambda= 82.797348 levenbergIter= 1 iteration= 5 chi2= 17934045.397014 time= 0.330691 cumTime= 2.02632 edges= 9799 schur= 0 lambda= 27.599116 levenbergIter= 1 iteration= 6 chi2= 8366798.477212 time= 0.338484 cumTime= 2.3648 edges= 9799 schur= 0 lambda= 18.399411 levenbergIter= 1 iteration= 7 chi2= 3482772.310310 time= 0.328993 cumTime= 2.69379 edges= 9799 schur= 0 lambda= 7.575799 levenbergIter= 1 iteration= 8 chi2= 1729487.328109 time= 0.329889 cumTime= 3.02368 edges= 9799 schur= 0 lambda= 5.050533 levenbergIter= 1 iteration= 9 chi2= 782974.656516 time= 0.312246 cumTime= 3.33593 edges= 9799 schur= 0 lambda= 1.717118 levenbergIter= 1 iteration= 10 chi2= 480960.530109 time= 0.302879 cumTime= 3.63881 edges= 9799 schur= 0 lambda= 1.144745 levenbergIter= 1 iteration= 11 chi2= 240895.040158 time= 0.301409 cumTime= 3.94022 edges= 9799 schur= 0 lambda= 0.734938 levenbergIter= 1 iteration= 12 chi2= 168428.093802 time= 0.31991 cumTime= 4.26013 edges= 9799 schur= 0 lambda= 0.380169 levenbergIter= 1 iteration= 13 chi2= 148772.984808 time= 0.343125 cumTime= 4.60325 edges= 9799 schur= 0 lambda= 0.163337 levenbergIter= 1 iteration= 14 chi2= 144381.312705 time= 0.333406 cumTime= 4.93666 edges= 9799 schur= 0 lambda= 0.054446 levenbergIter= 1 iteration= 15 chi2= 141820.019983 time= 0.338513 cumTime= 5.27517 edges= 9799 schur= 0 lambda= 0.018149 levenbergIter= 1 iteration= 16 chi2= 138280.601565 time= 0.379272 cumTime= 5.65444 edges= 9799 schur= 0 lambda= 0.012099 levenbergIter= 1 iteration= 17 chi2= 134737.613765 time= 0.311259 cumTime= 5.9657 edges= 9799 schur= 0 lambda= 0.008066 levenbergIter= 1 iteration= 18 chi2= 131592.574358 time= 0.32877 cumTime= 6.29447 edges= 9799 schur= 0 lambda= 0.005377 levenbergIter= 1 iteration= 19 chi2= 129088.012127 time= 0.328451 cumTime= 6.62292 edges= 9799 schur= 0 lambda= 0.003585 levenbergIter= 1 iteration= 20 chi2= 127881.146473 time= 0.326231 cumTime= 6.94915 edges= 9799 schur= 0 lambda= 0.002137 levenbergIter= 1 iteration= 21 chi2= 127607.121623 time= 0.333534 cumTime= 7.28269 edges= 9799 schur= 0 lambda= 0.000712 levenbergIter= 1 iteration= 22 chi2= 127578.889888 time= 0.33181 cumTime= 7.6145 edges= 9799 schur= 0 lambda= 0.000237 levenbergIter= 1 iteration= 23 chi2= 127578.158794 time= 0.326293 cumTime= 7.94079 edges= 9799 schur= 0 lambda= 0.000079 levenbergIter= 1 iteration= 24 chi2= 127578.157859 time= 0.456538 cumTime= 8.39733 edges= 9799 schur= 0 lambda= 0.000053 levenbergIter= 1 iteration= 25 chi2= 127578.157859 time= 0.395115 cumTime= 8.79244 edges= 9799 schur= 0 lambda= 0.000035 levenbergIter= 1 iteration= 26 chi2= 127578.157859 time= 1.46339 cumTime= 10.2558 edges= 9799 schur= 0 lambda= 0.001501 levenbergIter= 4 iteration= 27 chi2= 127578.157859 time= 0.781467 cumTime= 11.0373 edges= 9799 schur= 0 lambda= 0.002002 levenbergIter= 2 iteration= 28 chi2= 127578.157859 time= 0.35155 cumTime= 11.3888 edges= 9799 schur= 0 lambda= 0.001334 levenbergIter= 1 iteration= 29 chi2= 127578.157859 time= 2.98557 cumTime= 14.3744 edges= 9799 schur= 0 lambda= 48074889408591.265625 levenbergIter= 10 saving optimization results ...
结果分析：
a: 从结果中可以看出，算法在23次迭代的时候，基本收敛，总体误差保持不变。
b: 而前一个实验中，g2o原生优化的方法，总体误差一直在收敛。（因为定义的误差计算方式不一样，因此不能直接比较两次的数值大小）
c: 用g2o_viewer result_lie.g2o 打开李代数优化以后的结果，点击优化按钮，查看用g2o原生的整体误差度量方法，评价当前方案优化出来的结果如下：
从该结果中可以看出，前面实验用 30 次迭代优化的总体误差为44811.248504； 而本次实验在 23 次 迭代就收敛到 44360.492972的水平，说明用李代数实现的方法可以在更少的迭代次数下得到了更好的结果。
loaded result_lie.g2o with 2500 vertices and 9799 measurements graph is fixed by node 2499 # Using CHOLMOD poseDim -1 landMarkDim -1 blockordering 0 Preparing (no marginalization of Landmarks) iteration= 0 chi2= 44360.492972 time= 0.223326 cumTime= 0.223326 edges= 9799 schur= 0 iteration= 1 chi2= 44360.454460 time= 0.163356 cumTime= 0.386682 edges= 9799 schur= 0 iteration= 2 chi2= 44360.454460 time= 0.165951 cumTime= 0.552633 edges= 9799 schur= 0 iteration= 3 chi2= 44360.454460 time= 0.169297 cumTime= 0.72193 edges= 9799 schur= 0 iteration= 4 chi2= 44360.454460 time= 0.217875 cumTime= 0.939805 edges= 9799 schur= 0 iteration= 5 chi2= 44360.454460 time= 0.219421 cumTime= 1.15923 edges= 9799 schur= 0 iteration= 6 chi2= 44360.454460 time= 0.185975 cumTime= 1.3452 edges= 9799 schur= 0 iteration= 7 chi2= 44360.454460 time= 0.169911 cumTime= 1.51511 edges= 9799 schur= 0 iteration= 8 chi2= 44360.454460 time= 0.175694 cumTime= 1.69081 edges= 9799 schur= 0 iteration= 9 chi2= 44360.454460 time= 0.159107 cumTime= 1.84991 edges= 9799 schur= 0
5. 因子图优化实现——gtsam使用
5.1） 初始值变量gtsam::Values给定
读取顶点信息，设置初始值变量，通过insert函数设置id和位姿信息（gtsam::Pose3）
5.2） 因子图构建（gtsam::NonlinearFactorGraph）
读取文件中边的信息，往因子图中增加量测因子，并设置因子的噪声大小（协方差阵）
量测因子构建：new gtsam::BetweenFactor<gtsam::Pose3> ，参数为连接的两个变量ID，以及量测信息（gtsam::Pose3），以及量测噪声阵（gtsam::SharedNoiseModel）
注：g2o的信息矩阵定义方式与gtsam不同
5.3） 给定初值值，在因子图中增加先验因子
先验因子构建：gtsam::PriorFactor<gtsam::Pose3>：参数为变量键值，变量值以及噪声模型
此处噪声模型：gtsam::noiseModel::Diagonal::Variances
5.4） 因子图优化优化器设置
其中不同的求解方法对应这不同的类型，构建该类型需要的参数：构建的因子图，给定的初始值以及该求解方法的配置信息
注：在稀疏化处理方面，gtsam可以选择使用QR分解或者CHOLESKY分解，具体差别可以查看参数，源代码进行了解（由于单纯的位姿图没有太多的稀疏性可以利用，所以这里区别不大。）
5.5） 调用优化器 函数optimize进行优化，通过返回值返回优化优化值
5.6） 从最终优化结果中输出位姿图文件
其中包括变量信息和因子信息的输出，包括其中的量测噪声信息。
5.7）整体代码如下
#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 ) // 添加一个因子 BetweenFactor ); graph->push_back ( factor ); // 指针push 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 ) // PriorFactor ); 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; }
5.8） 结果如下：
Starting: /home/qifarui/code/slambook-master/ch11/build/pose_graph_gtsam sphere.g2o reading from g2o file read total 2500 vertices, 9799 edges. Adding prior to g2o file optimizing the factor graph Initial error: 4.7724e+09 newError: 6.07118e+08 errorThreshold: 6.07118e+08 > 0 absoluteDecrease: 4165284178.46 >= 1e-05 relativeDecrease: 0.872785675288 >= 1e-05 newError: 5568201.60391 errorThreshold: 5568201.60391 > 0 absoluteDecrease: 601549707.176 >= 1e-05 relativeDecrease: 0.990828467546 >= 1e-05 newError: 66469.3394102 errorThreshold: 66469.3394102 > 0 absoluteDecrease: 5501732.2645 >= 1e-05 relativeDecrease: 0.988062691666 >= 1e-05 newError: 63794.2086834 errorThreshold: 63794.2086834 > 0 absoluteDecrease: 2675.13072678 >= 1e-05 relativeDecrease: 0.0402460856467 >= 1e-05 newError: 63793.9289983 errorThreshold: 63793.9289983 > 0 absoluteDecrease: 0.279685093155 >= 1e-05 relativeDecrease: 4.38417685441e-06 < 1e-05 converged errorThreshold: 63793.9289983 <? 0 absoluteDecrease: 0.279685093155 <? 1e-05 relativeDecrease: 4.38417685441e-06 <? 1e-05 iterations: 5 >? 20 Optimization complete initial error: 4772402087.24 final error: 63793.9289982 done. write to g2o ... done.
从结果中可以看出：
算法在经过5次迭代以后收敛，整体误差每次迭代逐步减小，从最初的4772402087.24减小到 63793.9289982（该绝对值和g2o的整体误差直接比较没有意义）
用g2o_viewer result_gtsam.g2o查看误差情况： 44360.747407，和李代数实现收敛基本一致，但是，该方法收敛速更快，迭代的次数更少。
注：
本例程为体现出gtsam的增量特性（如果将这些位姿点按时间顺序，逐步放入，可能对起增量特性进行验证和了解）
浙公网安备 33010602011771号