SLAM的全稱是同時建圖與定位，在視覺SLAM中，這裏的建圖指的就是生成和管理MapPoint，MapPoint與激光SLAM中點雲不同的是，點雲是沒有特徵可言的，而MapPoint具有標識其唯一性的屬性，如，mDescriptor、Visible、mnFound、nObs等. 本次將對orb-slam源碼中涉及MapPoint的地方進行梳理.
文章目錄
1、創建新的MapPoints
生成新的MapPoints(即，建圖)存在於下面三個地方
- 初始化時，使用H矩陣或者F矩陣得到R，t後，使用線性三角化生成新的MapPoints
- tracking線程，CreateNewKeyFrame() 會爲雙目和RGBD添加MapPoint
- local mapping線程，CreateNewMapPoints() 通過線性三角化方式生成新的MapPoint
2、爲當前幀添加MapPoints
tracking線程主要任務就是爲當前幀跟蹤儘可能多的MapPoints，只有這樣約束纔夠多，估計的位姿就越準確. 爲當前幀添加MapPoints有下面三個地方：
- TrackReferenceKeyFrame() 函數，通過 SearchByBoW()，上一幀中包含了MapPoints，對這些MapPoints進行tracking，由此增加當前幀的MapPoints
- TrackWithMotionModel() 函數，通過 SearchByProjection()，將上一幀的MapPoints投影到當前幀(根據速度模型可以估計當前幀的
Tcw)，在投影點附近根據描述子距離選取匹配，以及最終的方向投票機制進行剔除 - TrackLocalMap() 函數，SearchLocalPoints() 函數，通過 SearchByProjection()，只對除了 <新增加的，即 TrackReferenceKeyFrame() 函數中匹配上的> MapPoint進行 SearchByProjection()，以向
mCurrentFrame增加新的MapPoint.
3、mDescriptor屬性
不僅關鍵點有描述子，MapPoint也有描述子
一個MapPoint可能被很多關鍵幀觀測到，那麼該MapPoint的描述子取哪一個呢？orb-slam使用ComputeDistinctiveDescriptors() 函數選擇最具代表性的描述子，因此當MapPoint添加了新的關鍵幀觀測後，需要判斷是否更新當前點的最適合的描述子.
選取規則是，先獲得當前點的所有描述子，然後計算描述子之間的兩兩距離，最好的描述子與其他描述子應該具有最小的距離中值.
4、MapPoint的法向量和深度
orb關鍵點具有旋轉不變性和尺度不變性，它在提取的時候是有尺度的，不同的金字塔層表示不同的尺度，層數越高，分辨率越低，即表示該關鍵點是在更低的分辨率下提出出來的，就表示這個MapPoint距離相機光心的距離越近. 同理，金字塔層數越低，分辨率越高，才能識別出遠點.
一個MapPoint會被許多相機觀測到因此每次插入對某個關鍵幀的Observation後，需要調用 UpdateNormalAndDepth() 函數，
- 由該MapPoint的3D位置到所有 <該MapPoint的觀測到關鍵幀光心> 向量均值確定法向量
- 由它在參考關鍵幀（創建該MapPoint時指定）對應的關鍵點的金字塔層，更新該MapPoint的最大深度
mfMaxDistance和最小深度mfMinDistance（貌似後兩個屬性，只跟MapPoint的3D位置和參考關鍵幀有關，插入新的關鍵幀觀測不會改變這倆屬性吧？）. 如下：
{
unique_lock<mutex> lock3(mMutexPos);
// 另見PredictScale函數前的註釋
mfMaxDistance = dist*levelScaleFactor; // 觀測到該點的距離下限
mfMinDistance = mfMaxDistance/pRefKF->mvScaleFactors[nLevels-1]; // 觀測到該點的距離上限
mNormalVector = normal/n; // 獲得平均的觀測方向
}
最大深度mfMaxDistance和最小深度mfMinDistance這兩個屬性會在 PredictScale() 時用到，如下：
4、Visible屬性
該屬性表示，能觀測到該MapPoint的圖像幀數目計數器.
1） 在tracking線程跟蹤局部地圖時，使用 SearchLocalPoints() 向當前幀增加新的MapPoints時，
- 首先對當前幀已經匹配上MapPoints各自的
Visible屬性增加計數 - 然後搜索局部地圖，只要該MapPoint滿足到當前幀的投影條件，就增加一次計數，如下：
if(mCurrentFrame.isInFrustum(pMP,0.5)){
pMP->IncreaseVisible();
nToMatch++;
}
滿足上面的投影條件就增加計數，但這些MapPoint並不一定能和當前幀的特徵點匹配上，這就是 SearchByProjection() 函數要做的事情. 例如：有一個MapPoint（記爲M），在某一幀F的視野範圍內，但並不表明該點M可以和F這一幀的某個特徵點能匹配上
注意，這裏並沒有對這些匹配上的MapPoints與當前幀進行關聯，僅僅是把匹配上的MapPoints添加到當前幀mvpMapPoints屬性中（因爲不是關鍵幀，所以只做單向關聯）
3）在local mapping中fuse當前關鍵幀新生成的MapPoints時、以及形成閉環時，會調用 Replace() 函數進行替換，會對觀測更多的MapPoint增加計數，增加的數目爲原來那個MapPoint的計數值.
5、mnFound屬性
相比Visible屬性，mnFound的要求就要嚴的多.
1）TrackLocalMap()函數中，在PoseOptimization()優化位姿之後，對非mvbOutlier的點執行IncreaseFound()，如下：
if(!mCurrentFrame.mvbOutlier[i]){
mCurrentFrame.mvpMapPoints[i]->IncreaseFound();
......
}
2）在local mapping中fuse當前關鍵幀新生成的MapPoints時、以及形成閉環時，會調用 Replace() 函數進行替換，會對觀測更多的MapPoint增加計數，增加的數目爲原來那個MapPoint的計數值.
Visible與mnFound作用：
local mapping線程中的 MapPointCulling() 函數，會根據VI-B條件1，能找到該點的幀不應該少於理論上觀測到該點的幀的1/4，如果低於閾值，調用 SetBadFlag() 函數，擦除該MapPoint.
6、nObs屬性
記錄哪些KeyFrame的那個特徵點能觀測到該MapPoint，單目+1，雙目或者grbd+2. 注意，該屬性只對關鍵幀有效，如下：
void MapPoint::AddObservation(KeyFrame* pKF, size_t idx) {
unique_lock<mutex> lock(mMutexFeatures);
if(mObservations.count(pKF))
return;
......
}
這個函數是建立關鍵幀共視關係的核心函數（參考下一篇博客），能共同觀測到某些MapPoints的關鍵幀是共視關鍵幀.
6.1 AddObservation 增加計數
1）在tracking線程，CreateNewKeyFrame() 函數中對雙目和RGBD相機，需要生成新的MapPoints，這一步跟 updateLastFrame() 函數內容相似，但這裏生成的MapPoints不再是臨時的MapPoints（mlpTemporalPoints），而是添加到關鍵幀裏面，同時MapPoint也會添加對該關鍵幀的觀測、計算該MapPoint的平均觀測方向、觀測距離範圍、最佳描述子，並加入到mpMap中（因爲這是新生成的MapPoint）.
除了上面這種情況和初始化外，tracking線程跟蹤過程中，都只與已存在地圖中的MapPoints進行匹配，並不進行關聯（因爲不是關鍵幀，所以只做單向關聯），只有在該普通幀確定爲關鍵幀時，才進行關聯，關聯這步是發生在local mapping線程中ProcessNewKeyFrame() 函數，即下一步要說的就是它.
2）local mapping線程，ProcessNewKeyFrame() 函數，由於mpCurrentKeyFrame中一些MapPoints在 TrackLocalMap() 函數中的MapPoints與當前關鍵幀進行了匹配，但沒有對這些匹配上的MapPoints與當前幀進行關聯，所以在這裏添加其對mpCurrentKeyFrame的觀測.
if(!pMP->IsInKeyFrame(mpCurrentKeyFrame)){
// 添加觀測
pMP->AddObservation(mpCurrentKeyFrame, i);
// 獲取該點的平均觀測方向和觀測距離範圍
pMP->UpdateNormalAndDepth();
// 加入關鍵幀後，更新3D點的最佳描述子
pMP->ComputeDistinctiveDescriptors();
}
else /** @todo this can only happen for new stereo points inserted by*/
{
// 將雙目或RGBD跟蹤過程中新插入的MapPoints放入mlpRecentAddedMapPoints，等待檢查
// CreateNewMapPoints函數中通過三角化也會生成MapPoints
// 這些MapPoints都會經過MapPointCulling函數的檢驗
mlpRecentAddedMapPoints.push_back(pMP);
}
3）local mapping線程，CreateNewMapPoints() 函數中，通過三角化生成新的3D點（注意，這裏還不能叫MapPoint），這些3D點需要通過平行、重投影誤差、尺度一致性等檢查後，才建立一個對應3D點的MapPoint對象，然後添加對該關鍵幀的觀測、計算該MapPoint的平均觀測方向、觀測距離範圍、最佳描述子，最後加入到mlpRecentAddedMapPoints列表中（還要繼續檢查）.
4）在local mapping中fuse當前關鍵幀新生成的MapPoints時、以及形成閉環時，會調用 Replace() 函數進行替換，會對觀測更多的MapPoint，讓該MapPoint替換掉原來MapPoint對應的KeyFrame，讓原來MapPoint對應的KeyFrame用pMP替換掉原來的MapPoint，詳細解釋見下面mpReplaced屬性.
6.2 EraseObservation 減少計數
整個擦除過程分三步進行：
- 減少
nObs屬性，與增加相反，單目-1，雙目或者grbd -2，並在mObservations屬性擦出對該關鍵幀的觀測 - 如果要擦出的關鍵幀是這個MapPoint的參考關鍵幀(
mpRefKF），則需要重新設置參考關鍵幀 - 如果少於
2個關鍵幀觀測到該MapPoint，則刪除該MapPoint，即通過 MapPoint::SetBadFlag() 實現
上面三步的代碼如下：
mObservations.erase(pKF);
// 如果該keyFrame是參考幀，該Frame被刪除後重新指定RefFrame
if(mpRefKF==pKF)
mpRefKF=mObservations.begin()->first; //重設參考關鍵幀
// 如果少於2個關鍵幀觀測到該MapPoint，則刪除該MapPoint*/
if(nObs<=2)
bBad=true;
1）LocalBundleAdjustment() 函數會對誤差比較大的邊，在關鍵幀中剔除對該MapPoint的觀測（KeyFrame::EraseMapPointMatch()），同時在MapPoint中剔除對該關鍵幀的觀測（MapPoint::EraseObservation() ）實現，如下
if(!vToErase.empty())
{
for(size_t i=0;i<vToErase.size();i++)
{
KeyFrame* pKFi = vToErase[i].first;
MapPoint* pMPi = vToErase[i].second;
pKFi->EraseMapPointMatch(pMPi);
pMPi->EraseObservation(pKFi);
}
}
2）在擦除關鍵幀的時候，記得要解除關鍵幀和MapPoints的觀測關係，即 KeyFrame::SetBadFlag() 函數要做的事之一.
7、mbBad屬性
設置true，即爲壞點，相當於擦除了該MappPoint（並沒有delete，實際上要等程序結束，調用析構函數才能回收分配的內存）.
成員函數 MapPoint::EraseObservation() 、Replace() 函數（詳細解釋參見mpReplaced屬性）和 MapPoint::SetBadFlag() 函數會設置該屬性
8、mpReplaced屬性
Replace()函數會對該屬性操作，有兩個地方會對該屬性進行修改：
1）在形成閉環的時候，會更新KeyFrame與MapPoint之間的關係，<閉環幀的中MapPoints> 對應的描述子與 <當前幀的MapPoints> 對應的描述子相近(即二者匹配上了)，就認爲這兩個MapPoint是同一個MapPoint(因爲它們是來自同一個特徵點)，那麼就使用閉環幀對應的MapPoints替換當前幀中MapPoints
2）local mapping線程中，由於當前關鍵幀產生新的MapPoints點後，這些MapPoints有可能會被其他關鍵幀找到，所以在SearchInNeighbors() 函數中，分別與一級二級相鄰幀(的MapPoints)中重複的進行合併，將會出現下面兩種情況：
- 如果MapPoint能匹配關鍵幀的特徵點，並且該點有對應的MapPoint，那麼將兩個MapPoint合併（選擇觀測數多的）
- 如果MapPoint能匹配關鍵幀的特徵點，並且該點沒有對應的MapPoint，那麼爲該點添加MapPoint，如下：
if(pMPinKF)// 如果這個點有對應的MapPoint
{
if(!pMPinKF->isBad())// 如果這個MapPoint不是bad，選擇哪一個呢？
{
if(pMPinKF->Observations() > pMP->Observations()) /** @attention 選擇觀測數更多的*/
pMP->Replace(pMPinKF);
else
pMPinKF->Replace(pMP);
}
}
else// 如果這個點沒有對應的MapPoint
{
pMP->AddObservation(pKF,bestIdx);
pKF->AddMapPoint(pMP,bestIdx);
}
第一種情況比較複雜，其宗旨是使用選擇觀測數更多的MapPoint替換較少的MapPoint，下面是替換過程的實現代碼，
當if條件成立時很容易理解，即pMP不在this的mObservations中的關鍵幀（等同於說，這個關鍵幀不曾觀測到該MapPoint），直接替換即可.
當if條件不成立時，注意，下面要說的this和pMP的值都是在if條件不成立時才成立！！this是指在local mapping中當前關鍵幀新生成的MapPoint，該MapPoint對應的關鍵點記爲a，pKF表示當前關鍵幀（obs除了生成該MapPoint那兩個關鍵幀外，還有在local mapping線程創建完新的MapPoints後，tracking線程利用這個MapPoint創建了新的關鍵幀），pMP表示的一定不是當前關鍵幀在local mapping新生成的MapPoint（因爲if不成立），而是來自當前關鍵幀的一二級相鄰關鍵幀生成過的MapPoint，而這個MapPoint又正好已經被當前關鍵幀（pKF）匹配上了（匹配時，假設當前關鍵幀使用的是關鍵點b），爲啥這兩個不同的關鍵點會衝突呢，是因爲這倆貨都跟當前關鍵幀的一二級相鄰關鍵幀中同一個關鍵點相似，設爲關鍵點c，我們再來梳理一下，a與c的描述子很像，b又與c的描述子很像，但描述子不具備傳遞性，你想想那麼描述子的向量維度是那麼高！！！隨便就能找到這樣三個向量，a與c很像，b又與c很像，但是a與b不像. 寫了這麼多，還不如一張圖來的清晰，見下圖 v
for(map<KeyFrame*,size_t>::iterator mit=obs.begin(), mend=obs.end(); mit!=mend; mit++)
{
// Replace measurement in keyframe
KeyFrame* pKF = mit->first;
if(!pMP->IsInKeyFrame(pKF))
{
pKF->ReplaceMapPointMatch(mit->second, pMP);// 讓KeyFrame用pMP替換掉原來的MapPoint
pMP->AddObservation(pKF,mit->second);// 讓MapPoint替換掉對應的KeyFrame
}
else
{
// 產生衝突，即pKF中有兩個特徵點a,b（這兩個特徵點的描述子是近似相同的），這兩個特徵點對應兩個MapPoint爲this,pMP
// 然而在fuse的過程中pMP的觀測更多，需要替換this，因此保留b與pMP的聯繫，去掉a與this的聯繫
pKF->EraseMapPointMatch(mit->second);
}
}
Replace() 這個函數同樣會設置mbBad屬性爲true，但MapPoint的內存不會被釋放，故保留mpReplaced屬性，用於保存替換該MapPoint的那個MapPoint.
保留mpReplaced屬性的作用是，由於tracking中需要用到mLastFrame，這裏檢查並更新上一幀中被替換的MapPoints(使用替換後的). 這個鬼費了這麼大勁，就這麼點用麼…可能主要用在閉環那裏吧（防止閉環出現之後造成跟丟），local mapping線程用處不大吧？？
9、mpRefKF
在生成該mapPoint的時候確定（構造函數），以及上面說到的 EraseObservation() 時，如果擦除的關鍵幀正好是其參考關鍵幀，那麼就使用mObservations中的第一個代替之（這麼隨意嗎）.
10、mnFirstKFid
基本同上，但是與上面不同的是，在 EraseObservation() 函數並不會進行代替.
這個屬性主要用在local mapping中的 MapPointCulling() 函數，即該mapPoints生成之後，必須滿足能被接下來的兩個關鍵幀觀測到（算上創建這個MapPoint的關鍵幀，一共三個），才能保證不會被剔除
11、MapPoints融合
當local mapping中的當前關鍵幀mpCurrentKeyFrame是最新關鍵幀時，也就是mlNewKeyFrames在 ProcessNewKeyFrame() 之後變成空時， 就會調用 SearchInNeighbors() 函數進行MapPoints的融合，這一步的重要性不言而喻，當我們用SLAM建圖時，出現較多旋轉時，經常能看到當前圖像有很大一塊區域都無法生成新的MapPoint，如果一直這麼旋轉下去，隨着跟蹤到的MapPoint越來越少，那麼跟蹤將失敗，一般我們看到這種情況，一定會調整機器人運動方式，即增加平移，減少旋轉，只有這樣，跟蹤才能在新的MapPoint生成中維持下去，那麼問題來了，過去那些因爲旋轉導致的關鍵幀有很大一塊區域沒有跟蹤到的MapPoint該如何處理，這就是fuse要解決的問題啊！
將當前關鍵幀的所有MapPoints，向當前關鍵幀的所有一級相鄰（Covisibility Graph中的前20個）和二級相鄰關鍵幀（與一級相鄰關鍵幀的共視的前5個）上投影，在投影點附近搜索匹配特徵點，如果找到匹配的特徵點，那麼判斷該特徵點是否已經對應MapPoint，
- 如果是，那麼將兩個MapPoint點進行合併，注意觀測它的關鍵幀也要合併
- 如果否，那麼就添加這個MapPoint與關鍵幀還有特徵點之間的關係
上面fuse過程，我放在了 orb-slam2代碼總結（四）特徵點匹配 這篇博客內細說. fuse過程是雙向的，上面一步完成了之後，接下來會選擇一二級相鄰的關鍵幀的MapPoints向當前關鍵幀進行投影，操作方法跟上面一樣.
<完>
@leatherwang