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′−TEK

Bundle Adjustment

首先介绍相机模型，假设有N帧已经去畸变的图像和M个点，其中第i 帧图像和第j个点uij=(xij,yij)T 和对应的三维点Pij=(Xij,Yij,Zij)T，在世界坐标系下，写作齐次坐标u^ij=(xij,yij,1)T 和对应的三维点P^ij=(Xij,Yij,Zij,1)T根据相机模型有

u^ij=sK[RjWC|TjWC]P^ij

其中s是尺度，将等式左边最后一项化为1，K是当前相机内参，RWC，TWC 分别是第i 帧图像拍摄出相机的旋转矩阵和平移向量。

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

minaj,bj∑i∑jvijd(Q(aj,bi),xij)2

其中vij表示是否可被观测，如果能够被观测到则vij=1，否则vij=0,aj是相机的参数，bj是3D点投影到相平面的参数。d(.)函数表示距离函数，可以使欧氏距离、无穷范数等等，这里一般使用的是 Huber 损失函数

LHuber(a)=⎧⎩⎨⎪⎪⎪⎪12a2,for|a|<δδ(|a|−12δ),|a|≥δ

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