ORBSlam2的位姿优化算法

版权声明：本文为博主原创文章，未经博主允许不得转载。 https://blog.csdn.net/chishuideyu/article/details/76013854

ORBSlam2的位姿优化算法

ORBSlam2中使用了对极约束，三角测量，PnP投影和BundleAdjustment算法
本文着重讲解在ORBSlam2中使用到的BundleAdjustment优化算法。

1. 全局BundleAdjustment算法

在ORBSlam2的Tracking线程中，进行相机状态初始化的时候，当使用对极约束时，求解了第一帧和第二帧之间的单应矩阵和基本矩阵，并通过三角测量得到两帧匹配点的三维位置之后，使用了全局BA算法进行了一次细致的位姿优化。
这一过程比较单调，就是普通的BundleAdjustment优化过程
先设定帧顶点，再设置MapPoint顶点，然后添加边信息。
执行BA迭代优化再替换原本的数据。

2. 局部BundleAdjustment算法

在ORBSlam2的LocalMapping线程中，在进行局部建图的时候，需要进行一次局部BundleAdjustment优化。

局部BA优化的过程是，通过当前取得的关键帧找到附近的局部关键帧，局部地图点和可以观察到这些局部地图点但是不是局部关键帧的关键帧作为固定关键帧。

我们通过固定附近的fixFK，统一优化附近的局部关键帧和当前关键帧的位姿，如果局部关键帧为空，那么不调整局部关键帧。

1. 构造局部关键帧列表
   
2. 构造在局部关键帧中看到的局部地图点列表
   
3. 构造固定关键帧列表。可以查看到局部MapPoint，但不是局部关键帧的关键帧是固定关键帧。
   
4. 设置顶点
5. 进行优化，这里的优化过程不同于上面的全局BA优化。
   首先进行5次迭代优化
   
   然后检查优化正确点的观测值，将异常点排除在外，继续进行一次10次迭代优化
   

3. 位姿调整函数PoseOptimization

在Tracking线程中，每一次进行位姿优化的时候，每进行过一次PnP投影操作将地图点投影到当前平面上之后，都会进行一次PoseOptimization位姿优化，通过BA优化重投影误差。

1. 在这里个方法中，先是利用g2o的设置了每个帧顶点
   

2. 再将每个地图点作为顶点保存起来作为优化的参数，要注意的是，这里信息矩阵不是单位矩阵，而是单位矩阵和关键点所在高斯模糊金字塔的层数的乘积，并且为了防止错误的关键点扰乱其他关键点的优化，从而使用了Huber核函数来使鲁棒性更强
   

3. 再使用四次最小二乘的优化
   

4. 最后再将原本的位姿替换为优化后的位姿，并返回正常值的数量。