视觉定位与建图14讲：后端深入解析之姿势图与因子图(Visual SLAM Part XIV - Backend Design: 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） 实际实现
在每次优化中保存中间结果，而当新的变量和因子加入时，首先分析它们因子图之间的连接和影响关系，考虑之前存储的信息有哪些可以继续利用，哪些必须重新计算，最后处理对增量的优化。
实际中，尽管有增量分析，但必须清楚的一点这里受影响的节点是近似的，当图的规模发生一定程度的改变时，需要再做一次全图优化。
因子图参考链接：
gtsam：从入门到使用
GTSAM 库的学习建议
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 sch
上一篇： 深入浅出视觉SLAM(第二部)：图优化理论详解与g2o应用实践
下一篇： 图优化基础：极大似然估计