ORB-SLAM2的LOCAL MAPPING代码阅读

首先给出泡泡机器人的ORB-SLAM2的源码详解链接：【泡泡机器人公开课】第三十六课：ORB-SLAM2源码详解 by 吴博

这里主要介绍ORB LOCALMAPPING模块

主要流程

泡泡机器人里面吴博给出的localmapping流程图

成员变量

    bool mbMonocular;

    //localmapping中调用的其它三大类
    Map* mpMap;
    LoopClosing* mpLoopCloser;
    Tracking* mpTracker;

    std::list<KeyFrame*> mlNewKeyFrames;
    KeyFrame* mpCurrentKeyFrame;
    std::list<MapPoint*> mlpRecentAddedMapPoints;

    //该线程的各种标志变量
    bool mbResetRequested;
    bool mbFinishRequested;
    bool mbFinished;
    bool mbAbortBA;
    bool mbAcceptKeyFrames;
    bool mbStopped;
    bool mbStopRequested;
    bool mbNotStop;

    //线程同步互斥量
    std::mutex mMutexStop;
    std::mutex mMutexReset;
    std::mutex mMutexNewKFs;
    std::mutex mMutexAccept;
    std::mutex mMutexFinish;

重要函数

  //构造函数，传入ORBSLAM的Map类和是否是MONOCULAR传感器的标志bMonocular，用于判断后面计算时使用关键帧的帧数
LocalMapping::LocalMapping(Map *pMap, const float bMonocular)

//设置LoopClosing成员，相当于在LocalMapping里面调用外部的LoopClosing类
void LocalMapping::SetLoopCloser(LoopClosing* pLoopCloser)

//设置Tracking成员，相当于在LocalMapping里面调用外部的Tracking类
void LocalMapping::SetTracker(Tracking *pTracker)

//该模块的主线程，在system里面调用（mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run,mpLocalMapper)）
void LocalMapping::Run()

/**
 * @brief 插入关键帧
 *
 * 将关键帧插入到地图中，以便将来进行局部地图优化
 * 这里仅仅是将关键帧插入到列表中进行等待
 * @param pKF KeyFrame
 */
void LocalMapping::InsertKeyFrame(KeyFrame *pKF)

/**
 * @brief 查看列表中是否有等待被插入的关键帧
 * @return 如果存在，返回true
 */
bool LocalMapping::CheckNewKeyFrames()

/**
 * @brief 处理列表中的关键帧
 * 
 * - 计算Bow，加速三角化新的MapPoints
 * - 关联当前关键帧至MapPoints，并更新MapPoints的平均观测方向和观测距离范围
 * - 插入关键帧，更新Covisibility图和Essential图
 * @see VI-A keyframe insertion
 */
void LocalMapping::ProcessNewKeyFrame()

/**
 * @brief 剔除ProcessNewKeyFrame和CreateNewMapPoints函数中引入的质量不好的MapPoints
 * @see VI-B recent map points culling
 */
void LocalMapping::MapPointCulling()

/**
 * 相机运动过程中和共视程度比较高的关键帧通过三角化恢复出一些MapPoints
 */
void LocalMapping::CreateNewMapPoints()

/**
 * 检查并融合当前关键帧与相邻帧（两级相邻）重复的MapPoints
 */
void LocalMapping::SearchInNeighbors()

/**
 * 根据两关键帧的姿态计算两个关键帧之间的基本矩阵
 * @param  pKF1 关键帧1
 * @param  pKF2 关键帧2
 * @return      基本矩阵
 */
cv::Mat LocalMapping::ComputeF12(KeyFrame *&pKF1, KeyFrame *&pKF2)

/**
 * @brief 关键帧剔除
 * 
 * 在Covisibility Graph中的关键帧，其90%以上的MapPoints能被其他关键帧（至少3个）观测到，则认为该关键帧为冗余关键帧。
 * @see VI-E Local Keyframe Culling
 */
void LocalMapping::KeyFrameCulling()

本征矩阵和基础矩阵介绍

下面简单介绍一下本征矩阵和基础矩阵


其中E是本征矩阵，如果相机内参是$K$,那么有

$$F=K'^{-T}EK$$

Bundle Adjustment

首先介绍相机模型，假设有$N$帧已经去畸变的图像和$M$个点，其中第$i$ 帧图像和第$j$个点$u_{ij}=(x_{ij},y_{ij})^T$ 和对应的三维点$P_{ij}=(X_{ij},Y_{ij},Z_{ij})^{T}$，在世界坐标系下，写作齐次坐标$\hat u_{ij}=(x_{ij},y_{ij},1)^{T}$ 和对应的三维点$\hat P_{ij}=(X_{ij},Y_{ij},Z_{ij},1)^{T}$根据相机模型有

$$\hat u_{ij}=sK[R_{jWC}|T_{jWC}]\hat P_{ij}$$
其中s是尺度，将等式左边最后一项化为1， $K$是当前相机内参，$R_{WC}，T_{WC}$ 分别是第 $i$ 帧图像拍摄出相机的旋转矩阵和平移向量。

最后Bundle adjustment是一个优化问题，解决

$$\min \limits_{a_j,b_j} \sum_i \sum_j v_{ij}d(Q(a_j,b_i),x_{ij})^{2}$$
其中 $v_{ij}$表示是否可被观测，如果能够被观测到则 $v_{ij}=1$，否则 $v_{ij}=0$, $a_j$是相机的参数， $b_j$是3D点投影到相平面的参数。 $d(.)$函数表示距离函数，可以使欧氏距离、无穷范数等等，这里一般使用的是 Huber 损失函数
$$L_{Huber}(a)=\left\{ \begin{aligned} &\frac{1}{2}a^2,for |a|<\delta\\ &\delta(|a|-\frac{1}{2}\delta), |a| \ge \delta \end{aligned} \right.$$

注：$Q$的几何表达见上面的相机模型，但是在优化中一般需要用到求导之类的操作，直接使用旋转矩阵并不合适，一般在使用过程中使用对应的Li代数来进行运算，$R$和$T$可以被参数化为$a_j$.