SLAM后端：位姿图优化（Pose Graph）

BA优化时间跟特征点数量有关，特征点数量越多BA消耗时间越长。折衷做法是，在进行几次优化后，将特征点位置固定，不再优化特征点，只优化相机位姿。位姿图优化是一中方法。场景如下：

                                   

三角形节点表示相机位姿，用表示，蓝色的边表示两相机位姿之间的相对运动，表示之间的相对位姿变化。图优化的目的是为了优化，方法是利用与之间的相对关系构建最小二乘问题进行优化求解。

首先根据两帧的位姿可以得到相对位姿变换：

                                                                             

李群表达式：

                                                                               

实际的，即值根据i,j两帧图像利用2D-2D（对极几何）得到。优化的目的就是让

                                                                       

可以写成：

                    

求解该优化问题一般采用LM法，需要计算偏导数并求解H，以通过增量方程求解优化量。

给分别左乘以一个扰动：

                                               

套用伴随性质（附在后文）得到：

                                  

泰勒展开合并得到：

                                   

则的偏导数如下：

                                                                 

得到偏导数即可求解增量方程解决优化问题。

伴随性质：

旋转群伴随性质：

                                                                    

变换群伴随性质：

                                                               

其中：