目录
4.1 设置"允许接受关键帧"的状态标志LocalMapping::SetAcceptKeyFrames函数解析
4.2 查看列表中是否有等待被插入的关键帧LocalMapping::CheckNewKeyFrames函数
4.3 处理列表中的关键帧，包括计算BoW、更新观测、描述子、共视图，插入到地图等LocalMapping::ProcessNewKeyFrame
4.4 根据地图点的观测情况剔除质量不好的地图点LocalMapping::MapPointCulling
4.5当前关键帧与相邻关键帧通过三角化产生新的地图点，使得跟踪更稳 LocalMapping::CreateNewMapPoints
4.6 若已经处理完队列中的最后的一个关键帧，检查并融合当前关键帧与相邻关键帧帧（两级相邻）中重复的地图点LocalMapping::SearchInNeighbors
4.7 检测并剔除当前帧相邻的关键帧中冗余的关键帧LocalMapping::KeyFrameCulling
4.10 等待处理的关键帧列表为空且还没终止当前线程的时候的操作
一、线程作用
处理追踪线程传来的关键帧，剔除质量不好的地图点、新增观测地图点、去除冗余关键帧，为回环检测线程传送关键帧。
二、局部建图线程主要流程
Step 1 告诉Tracking，LocalMapping正处于繁忙状态，请不要给我发送关键帧打扰我
Step 2 处理列表中的关键帧，包括计算BoW、更新观测、描述子、共视图，插入到地图等
Step 3 根据地图点的观测情况剔除质量不好的地图点
Step 4 当前关键帧与相邻关键帧通过三角化产生新的地图点，使得跟踪更稳
Step 5 检查并融合当前关键帧与相邻关键帧帧（两级相邻）中重复的地图点
Step 6 当局部地图中的关键帧大于2个的时候进行局部地图的BA
Step 7 检测并剔除当前帧相邻的关键帧中冗余的关键帧
Step 8 将当前帧加入到闭环检测队列中
三、局部建图线程主函数
// 线程主函数 void LocalMapping::Run() { // 标记状态，表示当前run函数正在运行，尚未结束 mbFinished = false; // 主循环 while(1) { // Tracking will see that Local Mapping is busy // Step 1 告诉Tracking，LocalMapping正处于繁忙状态，请不要给我发送关键帧打扰我 // LocalMapping线程处理的关键帧都是Tracking线程发来的 SetAcceptKeyFrames(false); // Check if there are keyframes in the queue // 等待处理的关键帧列表不为空 if(CheckNewKeyFrames()) { // BoW conversion and insertion in Map // Step 2 处理列表中的关键帧，包括计算BoW、更新观测、描述子、共视图，插入到地图等 ProcessNewKeyFrame(); // Check recent MapPoints // Step 3 根据地图点的观测情况剔除质量不好的地图点 MapPointCulling(); // Triangulate new MapPoints // Step 4 当前关键帧与相邻关键帧通过三角化产生新的地图点，使得跟踪更稳 CreateNewMapPoints(); // 已经处理完队列中的最后的一个关键帧 if(!CheckNewKeyFrames()) { // Find more matches in neighbor keyframes and fuse point duplications // Step 5 检查并融合当前关键帧与相邻关键帧帧（两级相邻）中重复的地图点 SearchInNeighbors(); } // 终止BA的标志 mbAbortBA = false; // 已经处理完队列中的最后的一个关键帧，并且闭环检测没有请求停止LocalMapping if(!CheckNewKeyFrames() && !stopRequested()) { // Local BA // Step 6 当局部地图中的关键帧大于2个的时候进行局部地图的BA if(mpMap->KeyFramesInMap()>2) // 注意这里的第二个参数是按地址传递的,当这里的 mbAbortBA 状态发生变化时，能够及时执行/停止BA Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpMap); // Check redundant local Keyframes // Step 7 检测并剔除当前帧相邻的关键帧中冗余的关键帧 // 冗余的判定：该关键帧的90%的地图点可以被其它关键帧观测到 KeyFrameCulling(); } // Step 8 将当前帧加入到闭环检测队列中 // 注意这里的关键帧被设置成为了bad的情况,这个需要注意 mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame); } else if(Stop()) // 当要终止当前线程的时候 { // Safe area to stop while(isStopped() && !CheckFinish()) { // 如果还没有结束利索,那么等 // usleep(3000); std::this_thread::sleep_for(std::chrono::milliseconds(3)); } // 然后确定终止了就跳出这个线程的主循环 if(CheckFinish()) break; } // 查看是否有复位线程的请求 ResetIfRequested(); // Tracking will see that Local Mapping is not busy SetAcceptKeyFrames(true); // 如果当前线程已经结束了就跳出主循环 if(CheckFinish()) break; //usleep(3000); std::this_thread::sleep_for(std::chrono::milliseconds(3)); } // 设置线程已经终止 SetFinish(); }跟踪线程确定为关键帧的帧会一帧一帧的传入局部建图线程，当一帧进入局部建图线程时，我们先将局部建图线程的状态mbFinished置为false，即局部建图线程未完成。
四、调用函数解析
4.1 设置"允许接受关键帧"的状态标志LocalMapping::SetAcceptKeyFrames函数解析
// 设置"允许接受关键帧"的状态标志 void LocalMapping::SetAcceptKeyFrames(bool flag) { unique_lock<mutex> lock(mMutexAccept); mbAcceptKeyFrames=flag; }由于追踪线程和局部建图线程是同步进行的，因此需要一帧一帧进行，即当一帧传入追踪线程时，追踪线程计算位姿，计算完一帧的位姿后，将该关键帧送入局部建图线程，但这存在一个一次送多了这种情况，因为同一时间一进程只能处理一帧。
因此当局部建图线程传入一帧后，将设置"允许接受关键帧"的状态标志设置为false，表示局部建图线程已经有关键帧进行处理，请等我处理完，我会告诉你处理完了，再给我下一帧。这个函数起到了防止局部建图线程帧累计堆叠的现象。
4.2 查看列表中是否有等待被插入的关键帧LocalMapping::CheckNewKeyFrames函数
Tracking线程向LocalMapping中插入关键帧是先插入到该队列中。
std::list<KeyFrame*> mlNewKeyFrames;bool LocalMapping::CheckNewKeyFrames() { unique_lock<mutex> lock(mMutexNewKFs); return(!mlNewKeyFrames.empty()); }我们判断该队列是否为空，即追踪线程传给局部建图线程的关键帧是否为空，若为空，局部建图线程等待关键帧的到来，若不为空，我们进行局部建图线程。
4.3 处理列表中的关键帧，包括计算BoW、更新观测、描述子、共视图，插入到地图等LocalMapping::ProcessNewKeyFrame
4.3.1 函数最终目的
①从mlNewKeyFrames（追踪线程传来的关键帧）拿出一帧mpCurrentKeyFrame进行处理。
②计算该关键帧mpCurrentKeyFrame的词袋向量
③处理关键帧的地图点：更新地图点平均观测方向、更新该点的平均观测方向和观测距离范围、更新地图点的最佳描述子。
④更新关键帧间的连接关系：
更新下列变量：
@mConnectedKeyFrameWeights：当前关键帧的共视信息，记录当前关键帧共视关键帧的信息（哪一帧和当前关键帧有共视，共视程度是多少）
@mvpOrderedConnectedKeyFrames：对mConnectedKeyFrameWeights中超过共视阈值的关键帧（按照共视程度从大到小排序）
@mvOrderedWeights：对mConnectedKeyFrameWeights中超过共视阈值的关键帧的共视程度（按照共视程度从大到小排序）
@mpParent：共视程度最高的那个关键帧
@mspChildrens：建立双向关系，将当前帧的共视程度最高的那个关键帧的子关键帧添加当前帧
⑤将该关键帧mpCurrentKeyFrame插入地图
4.3.1 函数解析
先从跟踪线程传给局部线程的关键帧队列中取走一帧mlNewKeyFrames.front进行处理（本函数以及之后的函数），这帧放入mpCurrentKeyFrame局部变量中。并将此关键帧移出队列mlNewKeyFrames（我要处理了，并且这帧要处理完）。
计算该帧的BoW向量（也就是说，只要是关键帧它的BoW向量都会被计算）。
ORB-SLAM2 ---- Frame::ComputeBoW函数
https://blog.csdn.net/qq_41694024/article/details/128007040
// 获取当前关键帧的具体的地图点 vector<MapPoint*> KeyFrame::GetMapPointMatches() { unique_lock<mutex> lock(mMutexFeatures); return mvpMapPoints; }获得所有的地图点存入vpMapPointMatches向量中。（追踪线程会生成一部分地图点）。遍历每一个地图点。
如果这个地图点不是来自当前帧的观测（比如来自局部地图点），为当前地图点添加观测。
/** * @brief 给地图点添加观测 * * 记录哪些 KeyFrame 的那个特征点能观测到该 地图点 * 并增加观测的相机数目nObs，单目+1，双目或者rgbd+2 * 这个函数是建立关键帧共视关系的核心函数，能共同观测到某些地图点的关键帧是共视关键帧 * @param pKF KeyFrame * @param idx MapPoint在KeyFrame中的索引 */ void MapPoint::AddObservation(KeyFrame* pKF, size_t idx) { unique_lock<mutex> lock(mMutexFeatures); // mObservations:观测到该MapPoint的关键帧KF和该MapPoint在KF中的索引 // 如果已经添加过观测，返回 if(mObservations.count(pKF)) return; // 如果没有添加过观测，记录下能观测到该MapPoint的KF和该MapPoint在KF中的索引 mObservations[pKF]=idx; if(pKF->mvuRight[idx]>=0) nObs+=2; // 双目或者rgbd else nObs++; // 单目 }// 观测到该MapPoint的KF和该MapPoint在KF中的索引 std::map<KeyFrame*,size_t> mObservations;obs 被观测到的相机数目，单目+1，双目或RGB-D则+2因此pMP->AddObservation(mpCurrentKeyFrame, i);这行代码的作用就是更改mObservations变量，使该地图点能被该帧观测并且添加该地图点的观测次数obs。
由于该地图点的被观察次数可能改变，因此我们需要更新该点的平均观测方向和观测距离范围。
/** * @brief 更新地图点的平均观测方向、观测距离范围 * */ void MapPoint::UpdateNormalAndDepth() { // Step 1 获得观测到该地图点的所有关键帧、坐标等信息 map<KeyFrame*,size_t> observations; KeyFrame* pRefKF; cv::Mat Pos; { unique_lock<mutex> lock1(mMutexFeatures); unique_lock<mutex> lock2(mMutexPos); if(mbBad) return; observations=mObservations; // 获得观测到该地图点的所有关键帧 pRefKF=mpRefKF; // 观测到该点的参考关键帧（第一次创建时的关键帧） Pos = mWorldPos.clone(); // 地图点在世界坐标系中的位置 } if(observations.empty()) return; // Step 2 计算该地图点的平均观测方向 // 能观测到该地图点的所有关键帧，对该点的观测方向归一化为单位向量，然后进行求和得到该地图点的朝向 // 初始值为0向量，累加为归一化向量，最后除以总数n cv::Mat normal = cv::Mat::zeros(3,1,CV_32F); int n=0; for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++) { KeyFrame* pKF = mit->first; cv::Mat Owi = pKF->GetCameraCenter(); // 获得地图点和观测到它关键帧的向量并归一化 cv::Mat normali = mWorldPos - Owi; normal = normal + normali/cv::norm(normali); n++; } cv::Mat PC = Pos - pRefKF->GetCameraCenter(); // 参考关键帧相机指向地图点的向量（在世界坐标系下的表示） const float dist = cv::norm(PC); // 该点到参考关键帧相机的距离 const int level = pRefKF->mvKeysUn[observations[pRefKF]].octave; // 观测到该地图点的当前帧的特征点在金字塔的第几层 const float levelScaleFactor = pRefKF->mvScaleFactors[level]; // 当前金字塔层对应的尺度因子，scale^n，scale=1.2，n为层数 const int nLevels = pRefKF->mnScaleLevels; // 金字塔总层数，默认为8 { unique_lock<mutex> lock3(mMutexPos); // 使用方法见PredictScale函数前的注释 mfMaxDistance = dist*levelScaleFactor; // 观测到该点的距离上限 mfMinDistance = mfMaxDistance/pRefKF->mvScaleFactors[nLevels-1]; // 观测到该点的距离下限 mNormalVector = normal/n; // 获得地图点平均的观测方向 } }这里我之前有说过，不再阐述，主要是修改几个点。
mfMaxDistance ：能看到该地图点的最大距离（用于判定地图点的有效深度的一个变量）
mfMinDistance ：能看到该地图点的最短距离（用于判定地图点的有效深度的一个变量）
mNormalVector ：该地图点的平均观测方向（有很多帧都能观测到该地图点，求一个观测到该地图点的平均角度）
更新能观测到该点的最佳描述子：
ORB-SLAM2 --- MapPoint::ComputeDistinctiveDescriptors 函数解析
mlpRecentAddedMapPoints.push_back(pMP);更新关键帧间的连接关系（共视图）：函数十分重要，在下面链接单独讲述：
@mConnectedKeyFrameWeights：当前关键帧的共视信息，记录当前关键帧共视关键帧的信息（哪一帧和当前关键帧有共视，共视程度是多少）
@mvpOrderedConnectedKeyFrames：对mConnectedKeyFrameWeights中超过共视阈值的关键帧（按照共视程度从大到小排序）
@mvOrderedWeights：对mConnectedKeyFrameWeights中超过共视阈值的关键帧的共视程度（按照共视程度从大到小排序）
@mpParent：共视程度最高的那个关键帧
@mspChildrens：建立双向关系，将当前帧的共视程度最高的那个关键帧的子关键帧添加当前帧
⑤将该关键帧mpCurrentKeyFrame插入地图最后将该关键mpCurrentKeyFrame帧插入地图。
4.4 根据地图点的观测情况剔除质量不好的地图点LocalMapping::MapPointCulling
4.4.1 函数解析
检查新增地图点，根据地图点的观测情况剔除质量不好的新增的地图点。
更新mlpRecentAddedMapPoints变量：处理所有新增地图点，确定保留或舍弃。
4.5当前关键帧与相邻关键帧通过三角化产生新的地图点，使得跟踪更稳 LocalMapping::CreateNewMapPoints
4.5.1 函数解析
本函数主要是根据词典向量匹配局部建图线程传来的关键帧与其共视程度最高的几帧进行匹配，利用这些匹配生成地图点，并将地图点存储在mlpRecentAddedMapPoints向量中待检验。
4.6 若已经处理完队列中的最后的一个关键帧，检查并融合当前关键帧与相邻关键帧帧（两级相邻）中重复的地图点LocalMapping::SearchInNeighbors
4.6.1 函数解析
本函数进行融合地图点，替换地图点，主要是对关键帧mpCurrentKeyFrame的一级、二级共视关键帧进行操作，并将废弃地图点设置为坏点。
4.7 检测并剔除当前帧相邻的关键帧中冗余的关键帧LocalMapping::KeyFrameCulling
4.7.1 函数解析
已经处理完队列中的最后的一个关键帧，并且闭环检测没有请求停止LocalMapping的时候如果我们当局部地图中的关键帧大于2个的时候进行局部地图的BA，局部BA后可能存在冗余的关键帧，我们通过此函数检测并剔除当前帧相邻的关键帧中冗余的关键帧。
简而言之，是一个去除冗余关键帧的操作：
4.8 将当前帧加入到闭环检测队列中
进行完如上操作后，将该关键帧mpCurrentKeyFrame加入回环检测的关键帧队列mlpLoopKeyFrameQueue中：
// 将某个关键帧加入到回环检测的过程中,由局部建图线程调用 void LoopClosing::InsertKeyFrame(KeyFrame *pKF) { unique_lock<mutex> lock(mMutexLoopQueue); // 注意：这里第0个关键帧不能够参与到回环检测的过程中,因为第0关键帧定义了整个地图的世界坐标系 if(pKF->mnId!=0) mlpLoopKeyFrameQueue.push_back(pKF); }
4.9 当要终止当前线程的时候
判断：
①mbStopped是否为true，为true表示可以并终止localmapping 线程。
②检查是否已经有外部线程请求终止当前线程
我们等待3000ms，然后确定终止了就跳出这个线程的主循环中止该线程。
4.10 等待处理的关键帧列表为空且还没终止当前线程的时候的操作
查看是否有复位线程的请求：
// 检查是否有复位线程的请求 void LocalMapping::ResetIfRequested() { unique_lock<mutex> lock(mMutexReset); // 执行复位操作:清空关键帧缓冲区,清空待cull的地图点缓冲 if(mbResetRequested) { mlNewKeyFrames.clear(); mlpRecentAddedMapPoints.clear(); // 恢复为false表示复位过程完成 mbResetRequested=false; } }清空关键帧缓冲区，清空待cull的地图点缓冲。
设置允许跟踪线程可以传入局部建图线程关键帧（等待下一轮操作）。
// 设置"允许接受关键帧"的状态标志 void LocalMapping::SetAcceptKeyFrames(bool flag) { unique_lock<mutex> lock(mMutexAccept); mbAcceptKeyFrames=flag; }等待3000ms。
扫一扫
847
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
