/**轉(zhuǎn)載自https://blog.csdn.net/xiaoxiaowenqiang/article/details/79281122
* This file is part of ORB-SLAM2.
*
* LocalMapping作用是將Tracking中送來的關(guān)鍵幀放在mlNewKeyFrame列表中；
* 處理新關(guān)鍵幀，地圖點(diǎn)檢查剔除湖蜕，生成新地圖點(diǎn)雨涛，Local BA市埋，關(guān)鍵幀剔除尝哆。
* 主要工作在于維護(hù)局部地圖，也就是SLAM中的Mapping针肥。
*
*
* Tracking線程 只是判斷當(dāng)前幀是否需要加入關(guān)鍵幀衰抑，并沒有真的加入地圖象迎，
* 因?yàn)門racking線程的主要功能是局部定位，
*
* 而處理地圖中的關(guān)鍵幀，地圖點(diǎn)砾淌，包括如何加入啦撮，
* 如何刪除的工作是在LocalMapping線程完成的
*
* 建圖
* 處理新的關(guān)鍵幀 KeyFrame 完成局部地圖構(gòu)建
* 插入關(guān)鍵幀 ------>? 處理地圖點(diǎn)(篩選生成的地圖點(diǎn) 生成地圖點(diǎn))? -------->? 局部 BA最小化重投影誤差? -調(diào)整-------->? 篩選 新插入的 關(guān)鍵幀
*
* mlNewKeyFrames? ? list 列表隊(duì)列存儲(chǔ)關(guān)鍵幀
* 1】檢查隊(duì)列
*? ? ? CheckNewKeyFrames();
*
* 2】處理新關(guān)鍵幀 Proces New Key Frames
* ProcessNewKeyFrame(); 更新地圖點(diǎn)MapPoints 和 關(guān)鍵幀 KepFrame 的關(guān)聯(lián)關(guān)系? UpdateConnections() 更新關(guān)聯(lián)關(guān)系
*
* 3】剔除 地圖點(diǎn) MapPoints
*? ? ? 刪除地圖中新添加的但 質(zhì)量不好的 地圖點(diǎn)
*? ? ? a)? IncreaseFound 共視點(diǎn)? / IncreaseVisible? 投影在圖像上 < 25%
*? ? ? b) 觀測(cè)到該 點(diǎn)的關(guān)鍵幀太少
*
* 4】生成 地圖點(diǎn) MapPoints
* 運(yùn)動(dòng)過程中和共視程度比較高的 關(guān)鍵幀 通過三角化 恢復(fù)出的一些地圖點(diǎn)
*
* 5】地圖點(diǎn)融合 MapPoints
*? ? ? 檢測(cè)當(dāng)前關(guān)鍵幀和相鄰 關(guān)鍵幀(兩級(jí)相鄰) 中 重復(fù)的 地圖點(diǎn) 留下觀測(cè)幀高的 地圖點(diǎn)
*
* 6】局部 BA 最小化重投影誤差
*? ? ? 和當(dāng)前關(guān)鍵幀相鄰的關(guān)鍵幀 中相匹配的 地圖點(diǎn)對(duì) 局部 BA最小化重投影誤差優(yōu)化點(diǎn)坐標(biāo) 和 位姿
*
* 7】關(guān)鍵幀剔除
*? ? ? 其90%以上的 地圖點(diǎn) 能夠被其他 共視 關(guān)鍵幀(至少3個(gè)) 觀測(cè)到，認(rèn)為該關(guān)鍵幀多余汪厨，可以刪除
*
*/
#include "LocalMapping.h"
#include "LoopClosing.h"
#include "ORBmatcher.h"
#include "Optimizer.h"
#include<mutex>
namespace ORB_SLAM2
{
LocalMapping::LocalMapping(Map *pMap, const float bMonocular):
? ? mbMonocular(bMonocular), mbResetRequested(false), mbFinishRequested(false), mbFinished(true), mpMap(pMap),
? ? mbAbortBA(false), mbStopped(false), mbStopRequested(false), mbNotStop(false), mbAcceptKeyFrames(true)
{
}
void LocalMapping::SetLoopCloser(LoopClosing* pLoopCloser)
{
? ? mpLoopCloser = pLoopCloser;
}
void LocalMapping::SetTracker(Tracking *pTracker)
{
? ? mpTracker=pTracker;
}
void LocalMapping::Run()
{
? ? mbFinished = false;
? ? while(1)
? ? {
// Tracking will see that Local Mapping is busy
// 步驟1：設(shè)置進(jìn)程間的訪問標(biāo)志 告訴Tracking線程赃春，LocalMapping線程正在處理新的關(guān)鍵幀，處于繁忙狀態(tài)
? ? ? ? ? ? ? // LocalMapping線程處理的關(guān)鍵幀都是Tracking線程發(fā)過的
? ? ? ? ? ? ? // 在LocalMapping線程還沒有處理完關(guān)鍵幀之前Tracking線程最好不要發(fā)送太快
SetAcceptKeyFrames(false);
// Check if there are keyframes in the queue
// 等待處理的關(guān)鍵幀列表不為空
if(CheckNewKeyFrames())
{
? ? // BoW conversion and insertion in Map
// 步驟2：計(jì)算關(guān)鍵幀特征點(diǎn)的詞典單詞向量BoW映射劫乱，將關(guān)鍵幀插入地圖
? ? ProcessNewKeyFrame();
? ? // Check recent MapPoints
? // 剔除ProcessNewKeyFrame函數(shù)中引入的不合格MapPoints
// 步驟3：對(duì)新添加的地圖點(diǎn)融合 對(duì)于 ProcessNewKeyFrame 和 CreateNewMapPoints 中 最近添加的MapPoints進(jìn)行檢查剔除 ? ?
? //? MapPointCulling();
? ? // Triangulate new MapPoints
// 步驟4： 創(chuàng)建新的地圖點(diǎn) 相機(jī)運(yùn)動(dòng)過程中與相鄰關(guān)鍵幀通過三角化恢復(fù)出一些新的地圖點(diǎn)MapPoints ? ?
? ? CreateNewMapPoints();
? ? MapPointCulling();// 從上面 移到下面
? ? ? // 已經(jīng)處理完隊(duì)列中的最后的一個(gè)關(guān)鍵幀
? ? if(!CheckNewKeyFrames())
? ? {
// Find more matches in neighbor keyframes and fuse point duplications
// 步驟5：相鄰幀地圖點(diǎn)融合 檢查并融合當(dāng)前關(guān)鍵幀與相鄰幀（兩級(jí)相鄰）重復(fù)的MapPoints
SearchInNeighbors();
? ? }
? ? mbAbortBA = false;
// 已經(jīng)處理完隊(duì)列中的最后的一個(gè)關(guān)鍵幀织中，并且閉環(huán)檢測(cè)沒有請(qǐng)求停止LocalMapping
? ? if(!CheckNewKeyFrames() && !stopRequested())
? ? {
// 步驟6：局部地圖優(yōu)化 Local BA
if(mpMap->KeyFramesInMap() > 2)
? ? Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpMap);
// 步驟7： 關(guān)鍵幀融合 檢測(cè)并剔除當(dāng)前幀相鄰的關(guān)鍵幀中冗余的關(guān)鍵幀 Check redundant local Keyframes
? ? ? ? ? ? ? ? // 剔除的標(biāo)準(zhǔn)是：該關(guān)鍵幀的90%的MapPoints可以被其它關(guān)鍵幀觀測(cè)到
// Tracking中先把關(guān)鍵幀交給LocalMapping線程
// 并且在Tracking中InsertKeyFrame函數(shù)的條件比較松，交給LocalMapping線程的關(guān)鍵幀會(huì)比較密
? ? ? ? ? ? ? ? ? ? ? ? // 在這里再刪除冗余的關(guān)鍵幀
KeyFrameCulling();
? ? }
// 步驟8：將當(dāng)前幀加入到閉環(huán)檢測(cè)隊(duì)列中
? ? mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);
}
// 步驟9：等待線程空閑 完成一幀關(guān)鍵幀的插入融合工作
else if(Stop())
{
? ? // Safe area to stop
? ? while(isStopped() && !CheckFinish())
? ? {
usleep(3000);
? ? }
? ? if(CheckFinish())//檢查 是否完成
break;
}
? ? ? ? ? ? ? // 檢查重置
ResetIfRequested();
// Tracking will see that Local Mapping is not busy
// 步驟10：告訴 Tracking 線程? Local Mapping 線程 空閑 可一處理接收 下一個(gè) 關(guān)鍵幀
SetAcceptKeyFrames(true);
if(CheckFinish())
? ? break;
usleep(3000);
? ? }
? ? SetFinish();
}
/**
* @brief 插入關(guān)鍵幀
*
* 將關(guān)鍵幀插入到地圖中衷戈，以便將來進(jìn)行局部地圖優(yōu)化
* 這里僅僅是將關(guān)鍵幀插入到列表中進(jìn)行等待
* @param pKF KeyFrame
*/
void LocalMapping::InsertKeyFrame(KeyFrame *pKF)
{
? ? unique_lock<mutex> lock(mMutexNewKFs);
? ? // 將關(guān)鍵幀插入到 等待處理的關(guān)鍵幀列表中
? ? mlNewKeyFrames.push_back(pKF);
? ? mbAbortBA=true;// BA優(yōu)化停止
}
/**
* @brief? ? 查看列表中是否有等待 被處理的關(guān)鍵幀
* @return 如果存在狭吼，返回true
*/
bool LocalMapping::CheckNewKeyFrames()
{
? ? unique_lock<mutex> lock(mMutexNewKFs);
? ? return(!mlNewKeyFrames.empty());// 等待處理的關(guān)鍵幀列表是否為空
}
? /*
? a. 根據(jù)詞典 計(jì)算當(dāng)前關(guān)鍵幀Bow，便于后面三角化恢復(fù)新地圖點(diǎn)殖妇；
? b. 將TrackLocalMap中跟蹤局部地圖匹配上的地圖點(diǎn)綁定到當(dāng)前關(guān)鍵幀
? ? ? （在Tracking線程中只是通過匹配進(jìn)行局部地圖跟蹤刁笙，優(yōu)化當(dāng)前關(guān)鍵幀姿態(tài)），
? ? ? 也就是在graph 中加入當(dāng)前關(guān)鍵幀作為node谦趣，并更新edge采盒。
? ? ? 而CreateNewMapPoint()中則通過當(dāng)前關(guān)鍵幀，在局部地圖中添加與新的地圖點(diǎn)蔚润；
? c. 更新加入當(dāng)前關(guān)鍵幀之后關(guān)鍵幀之間的連接關(guān)系，包括更新Covisibility圖和Essential圖
? （最小生成樹spanning tree尺栖，共視關(guān)系好的邊subset of edges from covisibility graph
? ? with high covisibility (θ=100)嫡纠， 閉環(huán)邊）。
? ? */
? /**
* @brief 處理列表中的關(guān)鍵幀
*
* - 計(jì)算Bow延赌，加速三角化新的MapPoints
* - 關(guān)聯(lián)當(dāng)前關(guān)鍵幀至MapPoints除盏，并更新MapPoints的平均觀測(cè)方向和觀測(cè)距離范圍
* - 插入關(guān)鍵幀，更新Covisibility圖和Essential圖
* @see VI-A keyframe insertion
*/
void LocalMapping::ProcessNewKeyFrame()
{ ?
// 步驟1：從緩沖隊(duì)列中取出一幀待處理的關(guān)鍵幀
? ? ? ? ? ? // Tracking線程向LocalMapping中插入關(guān)鍵幀存在該隊(duì)列中
? ? {
unique_lock<mutex> lock(mMutexNewKFs);
// 從列表中獲得一個(gè)等待被插入的關(guān)鍵幀
mpCurrentKeyFrame = mlNewKeyFrames.front();
mlNewKeyFrames.pop_front();// 出隊(duì)
? ? }
? ? // Compute Bags of Words structures
// 步驟2：計(jì)算該關(guān)鍵幀特征點(diǎn)的Bow映射關(guān)系? ?
? ? //? 根據(jù)詞典 計(jì)算當(dāng)前關(guān)鍵幀Bow挫以，便于后面三角化恢復(fù)新地圖點(diǎn)? ?
? ? mpCurrentKeyFrame->ComputeBoW();// 幀描述子 用字典單詞線性表示的 向量
? ? // Associate MapPoints to the new keyframe and update normal and descriptor
? ? // 當(dāng)前關(guān)鍵幀? TrackLocalMap中跟蹤局部地圖 匹配上的 地圖點(diǎn)
// 步驟3：跟蹤局部地圖過程中新匹配上的MapPoints和當(dāng)前關(guān)鍵幀綁定
? ? ? // 在TrackLocalMap函數(shù)中將局部地圖中的MapPoints與當(dāng)前幀進(jìn)行了匹配者蠕，
? ? ? // 但沒有對(duì)這些匹配上的MapPoints與當(dāng)前幀進(jìn)行關(guān)聯(lián)? ?
? ? const vector<MapPoint*> vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();
? ? for(size_t i=0; i<vpMapPointMatches.size(); i++)
? ? {
MapPoint* pMP = vpMapPointMatches[i];// 每一個(gè)與當(dāng)前關(guān)鍵幀匹配好的地圖點(diǎn)
if(pMP)//地圖點(diǎn)存在
{
? ? if(!pMP->isBad())
? ? {
? ? ? // 為當(dāng)前幀在tracking過重跟蹤到的MapPoints更新屬性
if(!pMP->IsInKeyFrame(mpCurrentKeyFrame))//下視野內(nèi)
{
? ? pMP->AddObservation(mpCurrentKeyFrame, i);// 地圖點(diǎn)添加關(guān)鍵幀
? ? pMP->UpdateNormalAndDepth();// 地圖點(diǎn)更新 平均觀測(cè)方向 和 觀測(cè)距離深度
? ? pMP->ComputeDistinctiveDescriptors();// 加入關(guān)鍵幀后，更新地圖點(diǎn)的最佳描述子
}
else // 雙目追蹤時(shí)插入的點(diǎn) 可能不在 幀上this can only happen for new stereo points inserted by the Tracking
{
? // 將雙目或RGBD跟蹤過程中新插入的MapPoints放入mlpRecentAddedMapPoints掐松，等待檢查
? ? ? ? ? ? ? ? ? ? ? ? ? // CreateNewMapPoints函數(shù)中通過三角化也會(huì)生成MapPoints
? ? ? ? ? ? ? ? ? ? ? ? ? // 這些MapPoints都會(huì)經(jīng)過MapPointCulling函數(shù)的檢驗(yàn)
? ? mlpRecentAddedMapPoints.push_back(pMP);
? ? // 候選待檢查地圖點(diǎn)存放在mlpRecentAddedMapPoints
}
? ? }
}
? ? }? ?
? ? // Update links in the Covisibility Graph
// 步驟4：更新關(guān)鍵幀間的連接關(guān)系踱侣，Covisibility圖和Essential圖(tree)
? ? mpCurrentKeyFrame->UpdateConnections();
? ? // Insert Keyframe in Map
// 步驟5：將該關(guān)鍵幀插入到地圖中
? ? mpMap->AddKeyFrame(mpCurrentKeyFrame);
}
/**
* @brief 剔除ProcessNewKeyFrame(不在幀上的地圖點(diǎn) 進(jìn)入待查list)和
*? ? ? ? ? ? CreateNewMapPoints(兩幀三角變換產(chǎn)生的新地圖點(diǎn)進(jìn)入 待查list)
*? ? ? ? ? ? 函數(shù)中引入的質(zhì)量不好的MapPoints
* @see VI-B recent map points culling
*/
// 對(duì)于ProcessNewKeyFrame和CreateNewMapPoints中最近添加的MapPoints進(jìn)行檢查剔除
void LocalMapping::MapPointCulling()
{
? ? // Check Recent Added MapPoints
? ? list<MapPoint*>::iterator lit = mlpRecentAddedMapPoints.begin();//待檢測(cè)的地圖點(diǎn) 迭代器
? ? const unsigned long int nCurrentKFid = mpCurrentKeyFrame->mnId;//當(dāng)前關(guān)鍵幀id
? ? //? 從添加該地圖點(diǎn)的關(guān)鍵幀算起的 初始三個(gè)關(guān)鍵幀，
? ? // 第一幀不算大磺，后面兩幀看到該地圖點(diǎn)的幀數(shù)抡句，對(duì)于單目<=2，對(duì)于雙目和RGBD<=3杠愧；
? ? int nThObs;
? ? if(mbMonocular)
nThObs = 2;
? ? else
nThObs = 3;
? ? const int cnThObs = nThObs;
// 遍歷等待檢查的地圖點(diǎn)MapPoints
? ? while(lit !=mlpRecentAddedMapPoints.end())
? ? {
MapPoint* pMP = *lit;//? 新添加的地圖點(diǎn)
if(pMP->isBad())
{
// 步驟1：已經(jīng)是壞點(diǎn)的MapPoints直接從檢查鏈表中刪除 ?
? ? lit = mlpRecentAddedMapPoints.erase(lit);
}
//? 跟蹤（匹配上）到該地圖點(diǎn)的普通幀幀數(shù)（IncreaseFound）< 應(yīng)該觀測(cè)到該地圖點(diǎn)的普通幀數(shù)量（25%*IncreaseVisible）：
// 該地圖點(diǎn)雖在視野范圍內(nèi)待榔，但很少被普通幀檢測(cè)到。 剔除
else if(pMP->GetFoundRatio()<0.25f )
{
// 步驟2：將不滿足VI-B條件的MapPoint剔除
// VI-B 條件1：
// 跟蹤到該MapPoint的Frame數(shù)相比預(yù)計(jì)可觀測(cè)到該MapPoint的Frame數(shù)的比例需大于25%
// IncreaseFound() / IncreaseVisible(該地圖點(diǎn)在視野范圍內(nèi)) < 25%流济，注意不一定是關(guān)鍵幀锐锣。 ?
? ? pMP->SetBadFlag();
? ? lit = mlpRecentAddedMapPoints.erase(lit);//從待查? list中刪除
}
//
// 初始三個(gè)關(guān)鍵幀 地圖點(diǎn)觀測(cè)次數(shù) 不能太少
// 而且單目的要求更嚴(yán)格腌闯，需要三幀都看到
else if(( (int)nCurrentKFid - (int)pMP->mnFirstKFid) >=2 && pMP->Observations() <= cnThObs)
{
? // 步驟3：將不滿足VI-B條件的MapPoint剔除
? ? ? ? ? ? // VI-B 條件2：從該點(diǎn)建立開始，到現(xiàn)在已經(jīng)過了不小于2幀雕憔，
? ? ? ? ? ? // 但是觀測(cè)到該點(diǎn)的關(guān)鍵幀數(shù)卻不超過cnThObs幀姿骏，那么該點(diǎn)檢驗(yàn)不合格
? ? pMP->SetBadFlag();
? ? lit = mlpRecentAddedMapPoints.erase(lit);//從待查? list中刪除
}
else if(((int)nCurrentKFid - (int)pMP->mnFirstKFid ) >= 3)
// 步驟4：從建立該點(diǎn)開始，已經(jīng)過了3幀(前三幀地圖點(diǎn)比較寶貴需要特別檢查)橘茉，放棄對(duì)該MapPoint的檢測(cè) ?
? ? lit = mlpRecentAddedMapPoints.erase(lit);//從待查? list中刪除
else
? ? lit++;
? ? }
}
/**
* @brief 相機(jī)運(yùn)動(dòng)過程中和共視程度比較高的關(guān)鍵幀通過三角化恢復(fù)出一些MapPoints
*? 根據(jù)當(dāng)前關(guān)鍵幀恢復(fù)出一些新的地圖點(diǎn)工腋，不包括和當(dāng)前關(guān)鍵幀匹配的局部地圖點(diǎn)（已經(jīng)在ProcessNewKeyFrame中處理）
*? 先處理新關(guān)鍵幀與局部地圖點(diǎn)之間的關(guān)系，然后對(duì)局部地圖點(diǎn)進(jìn)行檢查畅卓，
*? 最后再通過新關(guān)鍵幀恢復(fù) 新的局部地圖點(diǎn)：CreateNewMapPoints()
*
* 步驟1：在當(dāng)前關(guān)鍵幀的 共視關(guān)鍵幀 中找到 共視程度 最高的前nn幀 相鄰幀vpNeighKFs
* 步驟2：遍歷和當(dāng)前關(guān)鍵幀 相鄰的 每一個(gè)關(guān)鍵幀vpNeighKFs
* 步驟3：判斷相機(jī)運(yùn)動(dòng)的基線在（兩針間的相機(jī)相對(duì)坐標(biāo)）是不是足夠長(zhǎng)
* 步驟4：根據(jù)兩個(gè)關(guān)鍵幀的位姿計(jì)算它們之間的基本矩陣 F =? inv(K1 轉(zhuǎn)置) * t12 叉乘 R12 * inv(K2)
* 步驟5：通過幀間詞典向量加速匹配擅腰，極線約束限制匹配時(shí)的搜索范圍身害，進(jìn)行特征點(diǎn)匹配
* 步驟6：對(duì)每對(duì)匹配點(diǎn) 2d-2d 通過三角化生成3D點(diǎn),和 Triangulate函數(shù)差不多
*? 步驟6.1：取出匹配特征點(diǎn)
*? 步驟6.2：利用匹配點(diǎn)反投影得到視差角? 用來決定使用三角化恢復(fù)(視差角較大) 還是 直接2-d點(diǎn)反投影(視差角較小)
*? 步驟6.3：對(duì)于雙目巍糯，利用雙目基線 深度 得到視差角
*? 步驟6.4：視差角較大時(shí) 使用 三角化恢復(fù)3D點(diǎn)
*? 步驟6.4：對(duì)于雙目 視差角較小時(shí) 二維點(diǎn) 利用深度值 反投影 成 三維點(diǎn)? ? 單目的話直接跳過
*? 步驟6.5：檢測(cè)生成的3D點(diǎn)是否在相機(jī)前方
*? 步驟6.6：計(jì)算3D點(diǎn)在當(dāng)前關(guān)鍵幀下的重投影誤差? 誤差較大跳過
*? 步驟6.7：計(jì)算3D點(diǎn)在 鄰接關(guān)鍵幀 下的重投影誤差 誤差較大跳過
*? 步驟6.9：三角化生成3D點(diǎn)成功抓艳，構(gòu)造成地圖點(diǎn) MapPoint
*? 步驟6.9：為該MapPoint添加屬性
*? 步驟6.10：將新產(chǎn)生的點(diǎn)放入檢測(cè)隊(duì)列 mlpRecentAddedMapPoints? 交給 MapPointCulling() 檢查生成的點(diǎn)是否合適
* @see?
*/
void LocalMapping::CreateNewMapPoints()
{
? ? // Retrieve neighbor keyframes in covisibility graph
? ? int nn = 10;// 雙目/深度 共視幀 數(shù)量
? ? if(mbMonocular)
nn=20;//單目
// 步驟1：在當(dāng)前關(guān)鍵幀的 共視關(guān)鍵幀 中找到 共視程度 最高的nn幀 相鄰幀vpNeighKFs
? ? const vector<KeyFrame*> vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn);
? ? ORBmatcher matcher(0.6,false);// 描述子匹配器
? ? ? ? ? ? // 當(dāng)前關(guān)鍵幀 旋轉(zhuǎn)平移矩陣向量
? ? cv::Mat Rcw1 = mpCurrentKeyFrame->GetRotation();// 世界---> 當(dāng)前關(guān)鍵幀
? ? cv::Mat Rwc1 = Rcw1.t();// 當(dāng)前關(guān)鍵幀---> 世界
? ? cv::Mat tcw1 = mpCurrentKeyFrame->GetTranslation();
? ? cv::Mat Tcw1(3,4,CV_32F);// 世界---> 當(dāng)前關(guān)鍵幀 變換矩陣
? ? Rcw1.copyTo(Tcw1.colRange(0,3));
? ? tcw1.copyTo(Tcw1.col(3));
? ? // 得到當(dāng)前當(dāng)前關(guān)鍵幀在世界坐標(biāo)系中的坐標(biāo)
? ? cv::Mat Ow1 = mpCurrentKeyFrame->GetCameraCenter();
? ? ? ? ? // 相機(jī)內(nèi)參數(shù)
? ? const float &fx1 = mpCurrentKeyFrame->fx;
? ? const float &fy1 = mpCurrentKeyFrame->fy;
? ? const float &cx1 = mpCurrentKeyFrame->cx;
? ? const float &cy1 = mpCurrentKeyFrame->cy;
? ? const float &invfx1 = mpCurrentKeyFrame->invfx;
? ? const float &invfy1 = mpCurrentKeyFrame->invfy;
? ? const float ratioFactor = 1.5f*mpCurrentKeyFrame->mfScaleFactor;//匹配點(diǎn) 查找范圍
? ? int nnew=0;
? ? // Search matches with epipolar restriction and triangulate
// 步驟2：遍歷和當(dāng)前關(guān)鍵幀 相鄰的 每一個(gè)關(guān)鍵幀vpNeighKFs ? ?
? ? for(size_t i=0; i<vpNeighKFs.size(); i++)
? ? {
if(i>0 && CheckNewKeyFrames())
? ? return;
KeyFrame* pKF2 = vpNeighKFs[i];//關(guān)鍵幀
// Check first that baseline is not too short
? // 鄰接的關(guān)鍵幀在世界坐標(biāo)系中的坐標(biāo)
cv::Mat Ow2 = pKF2->GetCameraCenter();
? // 基線向量战惊，兩個(gè)關(guān)鍵幀間的相機(jī)相對(duì)坐標(biāo)
cv::Mat vBaseline = Ow2-Ow1;
? // 基線長(zhǎng)度
const float baseline = cv::norm(vBaseline);
// 步驟3：判斷相機(jī)運(yùn)動(dòng)的基線是不是足夠長(zhǎng)
if(!mbMonocular)
{
? ? if(baseline < pKF2->mb)
? ? continue;// 如果是立體相機(jī)淹父，關(guān)鍵幀間距太小時(shí)不生成3D點(diǎn)
}
else// 單目相機(jī)
{
? // 鄰接關(guān)鍵幀的場(chǎng)景深度中值
? ? const float medianDepthKF2 = pKF2->ComputeSceneMedianDepth(2);//中值深度
? ? // baseline與景深的比例
? ? const float ratioBaselineDepth = baseline/medianDepthKF2;
? ? ? ? ? ? ? ? ? // 如果特別遠(yuǎn)(比例特別小)喂走，那么不考慮當(dāng)前鄰接的關(guān)鍵幀蜕该，不生成3D點(diǎn)
? ? if(ratioBaselineDepth < 0.01)
continue;
}
// Compute Fundamental Matrix
// 步驟4：根據(jù)兩個(gè)關(guān)鍵幀的位姿計(jì)算它們之間的基本矩陣
? ? ? // 根據(jù)兩關(guān)鍵幀的姿態(tài)計(jì)算兩個(gè)關(guān)鍵幀之間的基本矩陣
? ? ? // F =? inv(K1 轉(zhuǎn)置)*E*inv(K2) = inv(K1 轉(zhuǎn)置) * t12 叉乘 R12 * inv(K2)
cv::Mat F12 = ComputeF12(mpCurrentKeyFrame,pKF2);
// Search matches that fullfil epipolar constraint
// 步驟5：通過幀間詞典向量加速匹配蔗彤，極線約束限制匹配時(shí)的搜索范圍俩莽，進(jìn)行特征點(diǎn)匹配
vector<pair<size_t,size_t> > vMatchedIndices;// 特征匹配候選點(diǎn)
matcher.SearchForTriangulation(mpCurrentKeyFrame,pKF2,F12,vMatchedIndices,false);
? ? ? ? // 相鄰關(guān)鍵幀 旋轉(zhuǎn)平移矩陣向量
cv::Mat Rcw2 = pKF2->GetRotation();
cv::Mat Rwc2 = Rcw2.t();
cv::Mat tcw2 = pKF2->GetTranslation();
cv::Mat Tcw2(3,4,CV_32F);// 轉(zhuǎn)換矩陣
Rcw2.copyTo(Tcw2.colRange(0,3));
tcw2.copyTo(Tcw2.col(3));
// 相機(jī)內(nèi)參
const float &fx2 = pKF2->fx;
const float &fy2 = pKF2->fy;
const float &cx2 = pKF2->cx;
const float &cy2 = pKF2->cy;
const float &invfx2 = pKF2->invfx;
const float &invfy2 = pKF2->invfy;
// Triangulate each match
// 三角化每一個(gè)匹配點(diǎn)對(duì)
// 步驟6：對(duì)每對(duì)匹配點(diǎn) 2d-2d 通過三角化生成3D點(diǎn),和 Triangulate函數(shù)差不多
const int nmatches = vMatchedIndices.size();
for(int ikp=0; ikp<nmatches; ikp++)
{
// 步驟6.1：取出匹配特征點(diǎn) ?
? ? const int &idx1 = vMatchedIndices[ikp].first; // 當(dāng)前匹配對(duì)在當(dāng)前關(guān)鍵幀中的索引
? ? const int &idx2 = vMatchedIndices[ikp].second;// 當(dāng)前匹配對(duì)在鄰接關(guān)鍵幀中的索引
? ? //當(dāng)前關(guān)鍵幀 特征點(diǎn) 和 右圖像匹配點(diǎn)橫坐標(biāo)
? ? const cv::KeyPoint &kp1 = mpCurrentKeyFrame->mvKeysUn[idx1];
? ? const float kp1_ur=mpCurrentKeyFrame->mvuRight[idx1];
? ? bool bStereo1 = kp1_ur >= 0;//右圖像匹配點(diǎn)橫坐標(biāo)>=0是雙目/深度相機(jī)
? ? // 鄰接關(guān)鍵幀 特征點(diǎn) 和 右圖像匹配點(diǎn)橫坐標(biāo)
? ? const cv::KeyPoint &kp2 = pKF2->mvKeysUn[idx2];
? ? const float kp2_ur = pKF2->mvuRight[idx2];
? ? bool bStereo2 = kp2_ur >= 0;
// 步驟6.2：利用匹配點(diǎn)反投影得到視差角? ?
? ? // Check parallax between rays
? ? // 相機(jī)歸一化平面上的點(diǎn)坐標(biāo)
? ? cv::Mat xn1 = (cv::Mat_<float>(3,1) << (kp1.pt.x - cx1)*invfx1, (kp1.pt.y - cy1)*invfy1, 1.0);
? ? cv::Mat xn2 = (cv::Mat_<float>(3,1) << (kp2.pt.x - cx2)*invfx2, (kp2.pt.y - cy2)*invfy2, 1.0);
? ? // 由相機(jī)坐標(biāo)系轉(zhuǎn)到世界坐標(biāo)系旺坠，得到視差角余弦值
? ? cv::Mat ray1 = Rwc1*xn1;// 相機(jī)坐標(biāo)系 ------> 世界坐標(biāo)系
? ? cv::Mat ray2 = Rwc2*xn2;
? ? // 向量a × 向量b / （向量a模 × 向量吧模） = 夾角余弦值
? ? const float cosParallaxRays = ray1.dot(ray2) / (cv::norm(ray1)*cv::norm(ray2));
? ? // 加1是為了讓cosParallaxStereo隨便初始化為一個(gè)很大的值
? ? float cosParallaxStereo = cosParallaxRays+1;
? ? float cosParallaxStereo1 = cosParallaxStereo;
? ? float cosParallaxStereo2 = cosParallaxStereo;
// 步驟6.3：對(duì)于雙目，利用雙目基線 深度 得到視差角
? ? if(bStereo1)
cosParallaxStereo1 = cos(2*atan2(mpCurrentKeyFrame->mb/2,mpCurrentKeyFrame->mvDepth[idx1]));
? ? else if(bStereo2)
cosParallaxStereo2 = cos(2*atan2(pKF2->mb/2,pKF2->mvDepth[idx2]));
? ? // 得到雙目觀測(cè)的視差角
? ? cosParallaxStereo = min(cosParallaxStereo1,cosParallaxStereo2);
// 步驟6.4：三角化恢復(fù)3D點(diǎn)
? ? cv::Mat x3D;
? // cosParallaxRays>0 && (bStereo1 || bStereo2 || cosParallaxRays<0.9998)表明視差角正常
? ? ? ? ? ? ? ? ? // cosParallaxRays < cosParallaxStereo表明視差角很小
? ? ? ? ? ? ? ? ? // 視差角度小時(shí)用三角法恢復(fù)3D點(diǎn)扮超，視差角大時(shí)用雙目恢復(fù)3D點(diǎn)（雙目以及深度有效）
? ? if(cosParallaxRays < cosParallaxStereo && cosParallaxRays>0 && (bStereo1 || bStereo2 || cosParallaxRays<0.9998))
? ? {
// Linear Triangulation Method
? ? ? // p1 = k × [R1 t1] × D? ? ? k逆 × p1 =? [R1 t1] × D? ? x1 = T1 × D? ? x1叉乘x1 = x1叉乘T1 × D = 0
? ? ? // p2 = k × [ R2 t2]? × D? ? k逆 × p2 =? [R2 t2] × D? ? x2 = T2 × D? ? x2叉乘x2 = x2叉乘T2 × D = 0
? ? ? //
? ? ? //p = ( x,y,1)
? ? ? //其叉乘矩陣為
? ? ? //? 叉乘矩陣 = [0? -1? y;? ? ? ? ? ? ? ? T0
? ? ? //? ? ? ? ? ? ? ? ? ? ? 1? 0? -x;? ? ? ? ? *? T1? *D? ===>( y * T2 - T1 ) *D = 0
? ? ? //? ? ? ? ? ? ? ? ? ? ? -y? x? 0 ] ? ? T2? ? ? ? ? ? ? ? ? ( x * T2 - T0 ) *D = 0
? ? ? //一個(gè)點(diǎn)兩個(gè)方程? 兩個(gè)點(diǎn) 四個(gè)方程? A × D =0? 求三維點(diǎn) D? 對(duì) A奇異值分解
cv::Mat A(4,4,CV_32F);
A.row(0) = xn1.at<float>(0)*Tcw1.row(2) - Tcw1.row(0);
A.row(1) = xn1.at<float>(1)*Tcw1.row(2) - Tcw1.row(1);
A.row(2) = xn2.at<float>(0)*Tcw2.row(2) - Tcw2.row(0);
A.row(3) = xn2.at<float>(1)*Tcw2.row(2) - Tcw2.row(1);
cv::Mat w,u,vt;
cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);
x3D = vt.row(3).t();
if(x3D.at<float>(3)==0)
? ? continue;
// Euclidean coordinates
x3D = x3D.rowRange(0,3) / x3D.at<float>(3);//其次點(diǎn)坐標(biāo) 除去尺度
? ? }
? //? 步驟6.4：對(duì)于雙目 視差角較小時(shí) 二維點(diǎn) 利用深度值 反投影 成 三維點(diǎn)
? ? else if(bStereo1 && cosParallaxStereo1<cosParallaxStereo2)// 雙目 視差角 小
? ? {
x3D = mpCurrentKeyFrame->UnprojectStereo(idx1);// 二維點(diǎn) 反投影 成 三維點(diǎn)? ? ? ? ? ? ?
? ? }
? ? else if(bStereo2 && cosParallaxStereo2 < cosParallaxStereo1)
? ? {
x3D = pKF2->UnprojectStereo(idx2);
? ? }
? ? ? // 單目 視差角 較小時(shí) 生成不了三維點(diǎn)
? ? else
continue; //沒有雙目/深度 且兩針視角差太小? 三角測(cè)量也不合適 得不到三維點(diǎn) No stereo and very low parallax
? ? cv::Mat x3Dt = x3D.t();
// 步驟6.5：檢測(cè)生成的3D點(diǎn)是否在相機(jī)前方
? ? //Check triangulation in front of cameras
? ? float z1 = Rcw1.row(2).dot(x3Dt) + tcw1.at<float>(2);// 只算z坐標(biāo)值
? ? if(z1<= 0)
continue;
? ? float z2 = Rcw2.row(2).dot(x3Dt)+tcw2.at<float>(2);
? ? if(z2 <= 0)
continue;
? ? ? ? // 步驟6.6：計(jì)算3D點(diǎn)在當(dāng)前關(guān)鍵幀下的重投影誤差
? ? //Check reprojection error in first keyframe
? ? const float &sigmaSquare1 = mpCurrentKeyFrame->mvLevelSigma2[kp1.octave];//誤差 分布參數(shù)
? ? const float x1 = Rcw1.row(0).dot(x3Dt) + tcw1.at<float>(0);//相機(jī)歸一化坐標(biāo)
? ? const float y1 = Rcw1.row(1).dot(x3Dt) + tcw1.at<float>(1);
? ? const float invz1 = 1.0/z1;
? ? if(!bStereo1)
? ? {// 單目
float u1 = fx1*x1*invz1 + cx1;//像素坐標(biāo)
float v1 = fy1*y1*invz1 + cy1;
float errX1 = u1 - kp1.pt.x;
float errY1 = v1 - kp1.pt.y;
if((errX1*errX1+errY1*errY1) > 5.991*sigmaSquare1)
? ? continue;//投影誤差過大 跳過
? ? }
? ? else
? ? {// 雙目 / 深度 相機(jī)? 有右圖像匹配點(diǎn)橫坐標(biāo)差值
float u1 = fx1*x1*invz1+cx1;
float u1_r = u1 - mpCurrentKeyFrame->mbf * invz1;//左圖像坐標(biāo)值 - 視差 =? 右圖像匹配點(diǎn)橫坐標(biāo)
float v1 = fy1*y1*invz1+cy1;
float errX1 = u1 - kp1.pt.x;
float errY1 = v1 - kp1.pt.y;
float errX1_r = u1_r - kp1_ur;
// 基于卡方檢驗(yàn)計(jì)算出的閾值（假設(shè)測(cè)量有一個(gè)一個(gè)像素的偏差）
if((errX1*errX1 + errY1*errY1 + errX1_r*errX1_r) > 7.8*sigmaSquare1)
? ? continue;//投影誤差過大 跳過
? ? }
? ? ? ? // 步驟6.7：計(jì)算3D點(diǎn)在 鄰接關(guān)鍵幀 下的重投影誤差
? ? //Check reprojection error in second keyframe
? ? const float sigmaSquare2 = pKF2->mvLevelSigma2[kp2.octave];
? ? const float x2 = Rcw2.row(0).dot(x3Dt)+tcw2.at<float>(0);
? ? const float y2 = Rcw2.row(1).dot(x3Dt)+tcw2.at<float>(1);
? ? const float invz2 = 1.0/z2;
? ? if(!bStereo2)
? ? {// 單目
float u2 = fx2*x2*invz2+cx2;
float v2 = fy2*y2*invz2+cy2;
float errX2 = u2 - kp2.pt.x;
float errY2 = v2 - kp2.pt.y;
if((errX2*errX2+errY2*errY2)>5.991*sigmaSquare2)
? ? continue;//投影誤差過大 跳過
? ? }
? ? else
? ? {// 雙目 / 深度 相機(jī)? 有右圖像匹配點(diǎn)橫坐標(biāo)差值
float u2 = fx2*x2*invz2+cx2;
float u2_r = u2 - mpCurrentKeyFrame->mbf*invz2;//左圖像坐標(biāo)值 - 視差 =? 右圖像匹配點(diǎn)橫坐標(biāo)
float v2 = fy2*y2*invz2+cy2;
float errX2 = u2 - kp2.pt.x;
float errY2 = v2 - kp2.pt.y;
float errX2_r = u2_r - kp2_ur;
if((errX2*errX2+errY2*errY2+errX2_r*errX2_r)>7.8*sigmaSquare2)
? ? continue;//投影誤差過大 跳過
? ? }
? ? ? ? ? // 步驟6.8：檢查尺度連續(xù)性
? ? //Check scale consistency
? ? cv::Mat normal1 = x3D-Ow1;//? 世界坐標(biāo)系下取刃，3D點(diǎn)與相機(jī)間的向量，方向由相機(jī)指向3D點(diǎn)
? ? float dist1 = cv::norm(normal1);// 模長(zhǎng)
? ? cv::Mat normal2 = x3D-Ow2;
? ? float dist2 = cv::norm(normal2);
? ? if(dist1==0 || dist2==0)
continue;// 模長(zhǎng)為0 跳過
? ? ? ? ? ? ? ? ? // ratioDist是不考慮金字塔尺度下的距離比例
? ? const float ratioDist = dist2/dist1;
? // 金字塔尺度因子的比例
? ? const float ratioOctave = mpCurrentKeyFrame->mvScaleFactors[kp1.octave] / pKF2->mvScaleFactors[kp2.octave];
? ? /*if(fabs(ratioDist-ratioOctave)>ratioFactor)
continue;*/
? ? // 深度比值和 兩幅圖像下的金字塔層級(jí)比值應(yīng)該相差不大
? ? // |ratioDist/ratioOctave |<ratioFactor
? ? if(ratioDist * ratioFactor<ratioOctave || ratioDist > ratioOctave*ratioFactor)
continue;
? // 步驟6.9：三角化生成3D點(diǎn)成功出刷，構(gòu)造成地圖點(diǎn) MapPoint
? ? // Triangulation is succesfull
? ? MapPoint* pMP = new MapPoint(x3D,mpCurrentKeyFrame,mpMap);
? ? ? ? ? ? // 步驟6.9：為該MapPoint添加屬性：
? ? ? // a.觀測(cè)到該MapPoint的關(guān)鍵幀
? ? pMP->AddObservation(mpCurrentKeyFrame,idx1); // 地圖點(diǎn) 添加觀測(cè)幀?
? ? pMP->AddObservation(pKF2,idx2);//
? ? mpCurrentKeyFrame->AddMapPoint(pMP,idx1);// 關(guān)鍵幀 添加地圖點(diǎn)
? ? pKF2->AddMapPoint(pMP,idx2);
? ? ? ? ? ? ? // b.該MapPoint的描述子
? ? pMP->ComputeDistinctiveDescriptors();
? ? ? ? ? ? ? // c.該MapPoint的平均觀測(cè)方向和深度范圍
? ? pMP->UpdateNormalAndDepth();
? ? ? ? ? ? ? // d.地圖添加地圖點(diǎn)
? ? mpMap->AddMapPoint(pMP);
? ? ? ? ? ? // 步驟6.10：將新產(chǎn)生的點(diǎn)放入檢測(cè)隊(duì)列 mlpRecentAddedMapPoints
? ? ? ? ? ? ? ? ? // 這些MapPoints都會(huì)經(jīng)過MapPointCulling函數(shù)的檢驗(yàn)
? ? mlpRecentAddedMapPoints.push_back(pMP);
? ? nnew++;
}
? ? }
}
/**
* @brief? ? 檢查并融合 當(dāng)前關(guān)鍵幀 與 相鄰幀（一級(jí)二級(jí)相鄰幀）重復(fù)的地圖點(diǎn) MapPoints
* 步驟1：獲得當(dāng)前關(guān)鍵幀在covisibility幀連接圖中權(quán)重排名前nn的一級(jí)鄰接關(guān)鍵幀(按觀測(cè)到當(dāng)前幀地圖點(diǎn)次數(shù)選取)
* 步驟2：獲得當(dāng)前關(guān)鍵幀在 其一級(jí)相鄰幀 在 covisibility圖中權(quán)重排名前5 的二級(jí)鄰接關(guān)鍵幀
* 步驟3：將當(dāng)前幀的 地圖點(diǎn)MapPoints 分別與 其一級(jí)二級(jí)相鄰幀的 地圖點(diǎn) MapPoints 進(jìn)行融合(保留觀測(cè)次數(shù)最高的)
* 步驟4：找到一級(jí)二級(jí)相鄰幀所有的地圖點(diǎn)MapPoints 與當(dāng)前幀 的? 地圖點(diǎn)MapPoints 進(jìn)行融合
* 步驟5：更新當(dāng)前幀 地圖點(diǎn) MapPoints 的描述子璧疗，深度，觀測(cè)主方向等屬性
* 步驟5：更新當(dāng)前 與其它幀的連接關(guān)系 (? 觀測(cè)到互相的地圖點(diǎn)的次數(shù)等信息 )
* @return? 無
*/
void LocalMapping::SearchInNeighbors()
{
? ? // Retrieve neighbor keyframes
// 步驟1：獲得當(dāng)前關(guān)鍵幀在covisibility圖中權(quán)重排名前nn的一級(jí)鄰接關(guān)鍵幀
? ? ? ? ? // 找到當(dāng)前幀一級(jí)相鄰與二級(jí)相鄰關(guān)鍵幀
? ? int nn = 10;
? ? if(mbMonocular)
nn=20;//單目 多找一些
? // 一級(jí)相鄰
? ? const vector<KeyFrame*> vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn);
? ? vector<KeyFrame*> vpTargetKFs;// 最后合格的一級(jí)二級(jí)相鄰關(guān)鍵幀
? ? // 遍歷每一個(gè) 一級(jí)相鄰幀
? ? for(vector<KeyFrame*>::const_iterator vit=vpNeighKFs.begin(), vend=vpNeighKFs.end(); vit!=vend; vit++)
? ? {
KeyFrame* pKFi = *vit;// 一級(jí)相鄰關(guān)鍵幀
if(pKFi->isBad() || pKFi->mnFuseTargetForKF == mpCurrentKeyFrame->mnId)//壞幀? 或者 已經(jīng)加入過
? ? continue;// 跳過
vpTargetKFs.push_back(pKFi);// 加入 最后合格的相鄰關(guān)鍵幀
pKFi->mnFuseTargetForKF = mpCurrentKeyFrame->mnId;// 已經(jīng)做過相鄰匹配? 標(biāo)記已經(jīng)加入
// 步驟2：獲得當(dāng)前關(guān)鍵幀在 其一級(jí)相鄰幀的? covisibility圖中權(quán)重排名前5的二級(jí)鄰接關(guān)鍵幀
? ? ? ? // 二級(jí)相鄰
// Extend to some second neighbors
const vector<KeyFrame*> vpSecondNeighKFs = pKFi->GetBestCovisibilityKeyFrames(5);
// 遍歷每一個(gè) 二級(jí)相鄰幀
for(vector<KeyFrame*>::const_iterator vit2=vpSecondNeighKFs.begin(), vend2=vpSecondNeighKFs.end(); vit2!=vend2; vit2++)
{
? ? KeyFrame* pKFi2 = *vit2;// 二級(jí)相鄰關(guān)鍵幀
? ? if(pKFi2->isBad() || pKFi2->mnFuseTargetForKF==mpCurrentKeyFrame->mnId || pKFi2->mnId==mpCurrentKeyFrame->mnId)
continue;// 二級(jí)相鄰關(guān)鍵幀是壞幀 在一級(jí)時(shí)已經(jīng)加入 或者 又找回來了找到當(dāng)前幀了 跳過
? ? vpTargetKFs.push_back(pKFi2);
}
? ? }
// 步驟3：將當(dāng)前幀的 地圖點(diǎn)MapPoints 分別與 其一級(jí)二級(jí)相鄰幀的 地圖點(diǎn) MapPoints 進(jìn)行融合
? ? // Search matches by projection from current KF in target KFs
? ? ORBmatcher matcher;
? ? vector<MapPoint*> vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();//與當(dāng)前幀 匹配的地圖點(diǎn)
? ? // vector<KeyFrame*>::iterator
? ? for(auto? vit=vpTargetKFs.begin(), vend=vpTargetKFs.end(); vit!=vend; vit++)
? ? {
KeyFrame* pKFi = *vit;//其一級(jí)二級(jí)相鄰幀
// 投影當(dāng)前幀的MapPoints到相鄰關(guān)鍵幀pKFi中馁龟，在附加區(qū)域搜索匹配關(guān)鍵點(diǎn)崩侠，并判斷是否有重復(fù)的MapPoints
// 1.如果MapPoint能匹配關(guān)鍵幀的特征點(diǎn)，并且該點(diǎn)有對(duì)應(yīng)的MapPoint坷檩，那么將兩個(gè)MapPoint合并（選擇觀測(cè)數(shù)多的）
// 2.如果MapPoint能匹配關(guān)鍵幀的特征點(diǎn)却音，并且該點(diǎn)沒有對(duì)應(yīng)的MapPoint，那么為該點(diǎn)添加MapPoint
matcher.Fuse(pKFi,vpMapPointMatches);
? ? }
// 步驟4：將一級(jí)二級(jí)相鄰幀所有的地圖點(diǎn)MapPoints 與當(dāng)前幀（的MapPoints）進(jìn)行融合
? ? ? ? ? ? // 遍歷每一個(gè)一級(jí)鄰接和二級(jí)鄰接關(guān)鍵幀 找到所有的地圖點(diǎn)
? ? // Search matches by projection from target KFs in current KF
? ? // 用于存儲(chǔ)一級(jí)鄰接和二級(jí)鄰接關(guān)鍵幀所有MapPoints的集合
? ? vector<MapPoint*> vpFuseCandidates;// 一級(jí)二級(jí)相鄰幀所有地圖點(diǎn)
? ? vpFuseCandidates.reserve(vpTargetKFs.size() * vpMapPointMatches.size());// 幀數(shù)量 × 每一幀地圖點(diǎn)數(shù)量
? ? ? ? ? ? // vector<KeyFrame*>::iterator
? ? for(auto vitKF=vpTargetKFs.begin(), vendKF=vpTargetKFs.end(); vitKF!=vendKF; vitKF++)
? ? {
KeyFrame* pKFi = *vitKF;//其一級(jí)二級(jí)相鄰幀
vector<MapPoint*> vpMapPointsKFi = pKFi->GetMapPointMatches();//地圖點(diǎn)
// vector<MapPoint*>::iterator
for(auto vitMP=vpMapPointsKFi.begin(), vendMP=vpMapPointsKFi.end(); vitMP!=vendMP; vitMP++)
{
? ? MapPoint* pMP = *vitMP;//? 一級(jí)二級(jí)相鄰幀 的每一個(gè)地圖點(diǎn)
? ? if(!pMP)
continue;
? ? if(pMP->isBad() || pMP->mnFuseCandidateForKF == mpCurrentKeyFrame->mnId)
continue;
? ? // 加入集合淌喻，并標(biāo)記 已經(jīng)加入
? ? pMP->mnFuseCandidateForKF = mpCurrentKeyFrame->mnId; //標(biāo)記 已經(jīng)加
? ? vpFuseCandidates.push_back(pMP); // 加入 一級(jí)二級(jí)相鄰幀 地圖點(diǎn) 集合
}
? ? }
? ? ? ? ? ? //一級(jí)二級(jí)相鄰幀 所有的 地圖點(diǎn) 與當(dāng)前幀 融合
? ? ? ? ? ? // 投影 地圖點(diǎn)MapPoints到當(dāng)前幀上僧家，在附加區(qū)域搜索匹配關(guān)鍵點(diǎn)，并判斷是否有重復(fù)的地圖點(diǎn)
? ? ? ? // 1.如果MapPoint能匹配當(dāng)前幀的特征點(diǎn)裸删，并且該點(diǎn)有對(duì)應(yīng)的MapPoint八拱，那么將兩個(gè)MapPoint合并（選擇觀測(cè)數(shù)多的）
? ? ? ? // 2.如果MapPoint能匹配當(dāng)前幀的特征點(diǎn)，并且該點(diǎn)沒有對(duì)應(yīng)的MapPoint，那么為該點(diǎn)添加MapPoint
? ? matcher.Fuse(mpCurrentKeyFrame,vpFuseCandidates);
// 步驟5：更新當(dāng)前幀MapPoints的描述子肌稻，深度清蚀，觀測(cè)主方向等屬性
? ? // Update points
? ? vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();//當(dāng)前幀 所有的 匹配地圖點(diǎn)
? ? for(size_t i=0, iend=vpMapPointMatches.size(); i<iend; i++)
? ? {
MapPoint* pMP=vpMapPointMatches[i];//當(dāng)前幀 每個(gè)關(guān)鍵點(diǎn)匹配的地圖點(diǎn)
if(pMP)//存在
{
? ? if(!pMP->isBad())//非 壞點(diǎn)
? ? {
pMP->ComputeDistinctiveDescriptors();// 更新 地圖點(diǎn)的描述子(在所有觀測(cè)在的描述子中選出最好的描述子)
pMP->UpdateNormalAndDepth();? ? ? ? ? // 更新平均觀測(cè)方向和觀測(cè)距離
? ? }
}
? ? }
// 步驟5：更新當(dāng)前幀的MapPoints后 更新與其它幀的連接關(guān)系 觀測(cè)到互相的地圖點(diǎn)的次數(shù)等信息
? ? ? ? ? ? // 更新covisibility圖
? ? // Update connections in covisibility graph
? ? mpCurrentKeyFrame->UpdateConnections();
}
/**
* @brief? ? 關(guān)鍵幀剔除
*? 在Covisibility Graph 關(guān)鍵幀連接圖 中的關(guān)鍵幀，
*? 其90%以上的地圖點(diǎn)MapPoints能被其他關(guān)鍵幀（至少3個(gè)）觀測(cè)到爹谭，
*? 則認(rèn)為該關(guān)鍵幀為冗余關(guān)鍵幀枷邪。
* @param? pKF1 關(guān)鍵幀1
* @param? pKF2 關(guān)鍵幀2
* @return 兩個(gè)關(guān)鍵幀之間的基本矩陣 F
*/
void LocalMapping::KeyFrameCulling()
{
? ? // Check redundant keyframes (only local keyframes)
? ? // A keyframe is considered redundant if the 90% of the MapPoints it sees, are seen
? ? // in at least other 3 keyframes (in the same or finer scale)
? ? // We only consider close stereo points
// 步驟1：根據(jù)Covisibility Graph 關(guān)鍵幀連接 圖提取當(dāng)前幀的 所有共視關(guān)鍵幀(關(guān)聯(lián)幀) ?
? ? vector<KeyFrame*> vpLocalKeyFrames = mpCurrentKeyFrame->GetVectorCovisibleKeyFrames();
? ? ? ? ? ? // vector<KeyFrame*>::iterator
? ? ? ? ? // 對(duì)所有的局部關(guān)鍵幀進(jìn)行遍歷 ? ?
? ? for(auto? vit=vpLocalKeyFrames.begin(), vend=vpLocalKeyFrames.end(); vit!=vend; vit++)
? ? {
KeyFrame* pKF = *vit;// 當(dāng)前幀的每一個(gè) 局部關(guān)聯(lián)幀
if(pKF->mnId == 0)//第一幀關(guān)鍵幀為 初始化世界關(guān)鍵幀 跳過
? ? continue;
// 步驟2：提取每個(gè)共視關(guān)鍵幀的 地圖點(diǎn) MapPoints
const vector<MapPoint*> vpMapPoints = pKF->GetMapPointMatches();// 局部關(guān)聯(lián)幀 匹配的 地圖點(diǎn)
int nObs = 3;
const int thObs=nObs; //3
int nRedundantObservations=0;
int nMPs=0;
// 步驟3：遍歷該局部關(guān)鍵幀的MapPoints，判斷是否90%以上的MapPoints能被其它關(guān)鍵幀（至少3個(gè)）觀測(cè)到
for(size_t i=0, iend=vpMapPoints.size(); i<iend; i++)
{
? ? MapPoint* pMP = vpMapPoints[i];// 該局部關(guān)鍵幀的 地圖點(diǎn) MapPoints
? ? if(pMP)
? ? {
if(!pMP->isBad())
{
? ? if(!mbMonocular)// 雙目/深度
? ? {? // 對(duì)于雙目诺凡，僅考慮近處的MapPoints东揣，不超過mbf * 35 / fx
if(pKF->mvDepth[i] > pKF->mThDepth || pKF->mvDepth[i] < 0)
? ? continue;
? ? }
? ? nMPs++;
? ? // 地圖點(diǎn) MapPoints 至少被三個(gè)關(guān)鍵幀觀測(cè)到
? ? if(pMP->Observations() > thObs)// 觀測(cè)幀個(gè)數(shù) > 3
? ? {
const int &scaleLevel = pKF->mvKeysUn[i].octave;// 金字塔層數(shù)
const map<KeyFrame*, size_t> observations = pMP->GetObservations();// 局部 觀測(cè)關(guān)鍵幀地圖
int nObs=0;
for(map<KeyFrame*, size_t>::const_iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
{
? ? KeyFrame* pKFi = mit->first;
? ? if(pKFi==pKF)// 跳過 原地圖點(diǎn)的幀
continue;
? ? const int &scaleLeveli = pKFi->mvKeysUn[mit->second].octave;// 金字塔層數(shù)
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? // 尺度約束，要求MapPoint在該局部關(guān)鍵幀的特征尺度大于（或近似于）其它關(guān)鍵幀的特征尺度
? ? if(scaleLeveli <= scaleLevel+1)
? ? {
nObs++;
if(nObs >= thObs)
? ? break;
? ? }
}
if(nObs >= thObs)
{// 該MapPoint至少被三個(gè)關(guān)鍵幀觀測(cè)到
? ? nRedundantObservations++;
}
? ? }
}
? ? }
}?
// 步驟4：該局部關(guān)鍵幀90%以上的MapPoints能被其它關(guān)鍵幀（至少3個(gè)）觀測(cè)到腹泌，則認(rèn)為是冗余關(guān)鍵幀
if(nRedundantObservations > 0.9*nMPs)
? ? pKF->SetBadFlag();
? ? }
}
/**
* @brief? ? 根據(jù)兩關(guān)鍵幀的姿態(tài)計(jì)算兩個(gè)關(guān)鍵幀之間的基本矩陣
*? ? ? ? ? ? ? ? F =? inv(K1 轉(zhuǎn)置)*E*inv(K2) = inv(K1 轉(zhuǎn)置)*t叉乘R*inv(K2)
* @param? pKF1 關(guān)鍵幀1
* @param? pKF2 關(guān)鍵幀2
* @return 兩個(gè)關(guān)鍵幀之間的基本矩陣 F
*/
cv::Mat LocalMapping::ComputeF12(KeyFrame *&pKF1, KeyFrame *&pKF2)
{
? ? // Essential Matrix: t12叉乘R12
? ? // Fundamental Matrix: inv(K1轉(zhuǎn)置)*E*inv(K2) ?
? ? cv::Mat R1w = pKF1->GetRotation();? // Rc1w
? ? cv::Mat t1w = pKF1->GetTranslation();
? ? cv::Mat R2w = pKF2->GetRotation();? // Rc2w
? ? cv::Mat t2w = pKF2->GetTranslation();// t c2 w
? ? cv::Mat R12 = R1w*R2w.t();// R12 =Rc1w *? Rwc2 // c2 -->w --->c1
? ? cv::Mat t12 = -R12*t2w + t1w; // tw2? + t1w? ? // c2 -->w --->c1
? ? // t12 的叉乘矩陣
? ? cv::Mat t12x = SkewSymmetricMatrix(t12);
? ? const cv::Mat &K1 = pKF1->mK;
? ? const cv::Mat &K2 = pKF2->mK;
? ? return K1.t().inv()*t12x*R12*K2.inv();
}
/**
* @brief? ? 請(qǐng)求停止局部建圖線程? 設(shè)置停止標(biāo)志
* @return 無
*/
void LocalMapping::RequestStop()
{
? ? unique_lock<mutex> lock(mMutexStop);
? ? mbStopRequested = true;//局部建圖 請(qǐng)求停止
? ? unique_lock<mutex> lock2(mMutexNewKFs);
? ? mbAbortBA = true;//停止BA 優(yōu)化
}
/**
* @brief? ? 停止局部建圖線程? 設(shè)置停止標(biāo)志
* @return 無
*/
bool LocalMapping::Stop()
{
? ? unique_lock<mutex> lock(mMutexStop);
? ? if(mbStopRequested && !mbNotStop)
? ? {
mbStopped = true;
cout << "局部建圖停止 Local Mapping STOP" << endl;
return true;
? ? }
? ? return false;
}
/**
* @brief? ? 檢查局部建圖線程 是否停止
* @return 是否停止標(biāo)志
*/
bool LocalMapping::isStopped()
{
? ? unique_lock<mutex> lock(mMutexStop);
? ? return mbStopped;
}
/**
* @brief? 返回 請(qǐng)求停止局部建圖線程? 標(biāo)志
* @return 返回 請(qǐng)求停止局部建圖線程? 標(biāo)志
*/
bool LocalMapping::stopRequested()
{
? ? unique_lock<mutex> lock(mMutexStop);
? ? return mbStopRequested;
}
/**
* @brief? ? 釋放局部建圖線程?
* @return? 無
*/
void LocalMapping::Release()
{
? ? unique_lock<mutex> lock(mMutexStop);
? ? unique_lock<mutex> lock2(mMutexFinish);
? ? if(mbFinished)
return;
? ? mbStopped = false;
? ? mbStopRequested = false;
? ? // list<KeyFrame*>::iterator
? ? for(auto lit = mlNewKeyFrames.begin(), lend=mlNewKeyFrames.end(); lit!=lend; lit++)
delete *lit;//刪除關(guān)鍵幀
? ? mlNewKeyFrames.clear();
? ? cout << "局部建圖釋放 Local Mapping RELEASE" << endl;
}
/**
* @brief? ? 返回 可以接收新的一個(gè)關(guān)鍵幀標(biāo)志
* @return 是否 可以接收新的一個(gè)關(guān)鍵幀
*/
bool LocalMapping::AcceptKeyFrames()
{
? ? unique_lock<mutex> lock(mMutexAccept);
? ? return mbAcceptKeyFrames;
}
/**
* @brief? ? 設(shè)置 可以接收新的一個(gè)關(guān)鍵幀標(biāo)志
* @return 無
*/
void LocalMapping::SetAcceptKeyFrames(bool flag)
{
? ? unique_lock<mutex> lock(mMutexAccept);
? ? mbAcceptKeyFrames=flag;
}
/**
* @brief? ? 設(shè)置不要停止標(biāo)志
* @return? 成功與否
*/
bool LocalMapping::SetNotStop(bool flag)
{
? ? unique_lock<mutex> lock(mMutexStop);
? ? if(flag && mbStopped)//? 在已經(jīng)停止的情況下 設(shè)置不要停止? 錯(cuò)誤
return false;
? ? mbNotStop = flag;
? ? return true;
}
/**
* @brief? ? 停止 全局優(yōu)化 BA
* @return 是否 可以接收新的一個(gè)關(guān)鍵幀
*/
void LocalMapping::InterruptBA()
{
? ? mbAbortBA = true;
}
/**
* @brief? 計(jì)算向量的 叉乘矩陣? ? 變叉乘為 矩陣乘
* @return 該向量的叉乘矩陣
*/
cv::Mat LocalMapping::SkewSymmetricMatrix(const cv::Mat &v)
{
// 向量 t=(a1 a2 a3) t叉乘A
// 等于 向量t的叉乘矩陣 * A
//? t的叉乘矩陣
//|0? ? -a3? a2 |
//|a3? ? 0? -a1|
//|-a2? a1? ? 0 |
? ? return (cv::Mat_<float>(3,3) <<? ? ? ? ? ? 0, -v.at<float>(2), v.at<float>(1),
? ? v.at<float>(2),? ? ? ? ? ? ? 0,-v.at<float>(0),
? ? -v.at<float>(1),? v.at<float>(0),? ? ? ? ? ? ? 0);
}
/**
* @brief? ? 請(qǐng)求重置
* @return? 無
*/
void LocalMapping::RequestReset()
{
? ? {
unique_lock<mutex> lock(mMutexReset);
mbResetRequested = true;
? ? }
? ? while(1)
? ? {
{
? ? unique_lock<mutex> lock2(mMutexReset);
? ? if(!mbResetRequested)
break;
}
usleep(3000);
? ? }
}
/**
* @brief? ? 重置線程
* @return? 無
*/
void LocalMapping::ResetIfRequested()
{
? ? unique_lock<mutex> lock(mMutexReset);
? ? if(mbResetRequested)
? ? {
mlNewKeyFrames.clear();
mlpRecentAddedMapPoints.clear();
mbResetRequested=false;
? ? }
}
void LocalMapping::RequestFinish()
{
? ? unique_lock<mutex> lock(mMutexFinish);
? ? mbFinishRequested = true;
}
bool LocalMapping::CheckFinish()
{
? ? unique_lock<mutex> lock(mMutexFinish);
? ? return mbFinishRequested;
}
void LocalMapping::SetFinish()
{
? ? unique_lock<mutex> lock(mMutexFinish);
? ? mbFinished = true;? ?
? ? unique_lock<mutex> lock2(mMutexStop);
? ? mbStopped = true;
}
bool LocalMapping::isFinished()
{
? ? unique_lock<mutex> lock(mMutexFinish);
? ? return mbFinished;
}
} //namespace ORB_SLAM
