ORB-SLAM2系统中的优化函数

2024-02-28 00:27:30　　点击量：

SLAM系统中，一般的做法是通过P3P或者EPnP等方法进行投影估算出相机位姿，进而构建最小二乘对估计的相机位姿进行优化，从而得到精度比较高的位姿。优化完后可以剔除一些outlier点，这样的话后边再进行投影和优化的时候精度会有所保证。ORB-SLAM系统当中的优化分为下面5个，从优化的范围层次逐步扩大。，

一些说明：PoseOptimization优化的是单帧图像的位姿。相机在运动的时候每个图像帧可以对应三维世界中多个地图点，那么此时我们可以知道三维世界中的多个3D地图点，也可以知道图像帧中对应的2D像素点坐标，就可以通过3D-2D的投影关系估计出相机的位姿。PoseOptimization就是对单个图像帧中的多个地图点建立多个一元连接边，构成图进行优化，优化单个图像帧的SE3位姿。

调用时机：当一个图像帧中的特征点和3D空间中多个地图点匹配，并且有一个“初始位姿”的时候，就可以通过位姿优化函数来对当前帧的位姿进行优化。ORB-SLAM系统当中在Tracking线程中多次调用了位姿优化函数，可以试想一下，毕竟Tracking线程的主要业务就是进行跟踪，一帧一帧图像的位姿描述了相机的轨迹。系统的前两帧图像的位姿都初始化为单位矩阵，在通过2D点匹配建立关联关系后，通过三角化创建了地图点，此时才可以对图像帧的位姿进行优化更新。只要3D-2D匹配关系发生变化，就需要对位姿进行优化。

ORB-SLAM系统中下面几个函数里边都调用了位姿优化函数：

1）Tracking::TrackReferenceKeyFrame()

2）Tracking::TrackWithMotionModel()

3）Tracking::TrackLocalMap()

4）Tracking::Relocalization()

代码：

一些说明：对单目相机来说具有尺度不确定性，当相机运动一段时间后不可避免会产生尺度漂移累积误差增大，当进行闭环检测的时候如果尺度不一致就不能进行很好的对应。相比于SE3来说，相似变换可以提供尺度的表达。那么这时候就可以借助相似变换群sim3来求解当前关键帧和闭环候选帧之间的sim3位姿，同时sim3位姿也可以和SE3进行转换。

要优化的两帧都能观测到多个相同的地图点，并且已经知道有哪些地图点是一致的。此时可以构造一个超定方程组，并对其求解最小化误差优化。优化帧间变化的SIM3位姿与地图的VertexSBAPointXYZ位姿。

调用时机：单目闭环检测中检测出闭环后，对当前关键帧和闭环帧之间计算sim3位姿，当计算出sim3位姿后，就需要调用sim3位姿优化函数。

ORB-SLAM当中在LoopClosing::ComputetSim3()中调用了OptimizeSim3函数。

代码：

一些说明：局部BA优化，优化的是局部关键帧的位姿和这些局部关键帧可以观测到的地图点的3D坐标。相机在运动过程中，当前时刻的图像帧何其前后的若干个帧是能观测到一些相同的路标点的，也就是说这些帧之间有共视关系。这些共视帧和他们的所有地图点放在一块就体现了“局部”的意思。将局部的所有地图点与可以观测到他们的关键帧放在一起进行优化，将关键帧的SE3位姿和地图点的3D坐标作为待优化量添加到顶点当中。

调用时机：LocalMapping线程当中调用。LocalMapping线程中处理的就是当前关键帧和其共视关键帧、以及这些关键帧能够观测到的地图点之间的连接关系，当然少不了对局部关键帧位姿和地图点3D坐标的更新。

代码：

一些说明：函数名称是OptimizeEssentialGraph，那么只有形成了EssentialGraph之后才需要进行优化。EssentialGraph中不但有一般的共视关键帧之间的边，还有闭环边和共视图边。所以其中加入了帧间闭环的考虑，也加入了纠正后的帧的SIM3位姿。所有共视的关键帧、地图点、一般的边和闭环边放在一起进行优化，优化的目标是关键帧的sim3位姿和地图点的坐标。将优化后的sim3位姿转换为SE3，就得到了相机的位姿，并对地图点的坐标进行投影得到更新后的坐标。

调用时机：LoopClosing线程当中通过sim3调整完关键帧和其闭环帧之间位姿后调用。

代码：

一些说明：这个是ORB-SLAM系统当中最大的优化，将全局地图当中所有的关键帧和地图点都放进来一起进行优化。对关键帧的位姿和地图点3D坐标进行优化。

调用时机：LoopClosing线程当中所有工作完成后创建线程进行整体优化。函数调用关系为：RunGlobalBundleAdjustment-->Optimizer::GlobalBundleAdjustemnt-->BundleAdjustment

代码：

RunGlobalBundleAdjustment函数：

Optimizer::GlobalBundleAdjustemnt函数：

Optimizer::BundleAdjustment函数：

ORB-SLAM当中的优化，在论文的附录中有进行解释。