# Tracking.cpp
追蹤的簡單流程
### Tracking::Tracking()
追蹤流程
在[System::System()](#System::System())中引用
```
mpTracker = new Tracking(this,
mpVocabulary, // 詞袋
mpFrameDrawer, // 幀繪製器
mpMapDrawer, // 地圖繪製器
mpMap, // 地圖類
mpKeyFrameDatabase, // 關鍵幀數據庫
strSettingsFile, // 配置文件
mSensor); // 感測器類別
```
#### 函式流程:
1. 根據配置文件路徑讀取配置文件
2. 從配置文件讀取相機內參[知識點1]
3. 根據內參構造內參矩陣
4. 從配置文件讀取畸變參數[知識點2]
5. 從配置文件讀取ORB特徵相關參數[ORB特徵提取器 知識點1 2 3]
6. 構建ORB特徵提取器
7. 構建尺度變數[知識點3]
#### 知識點:
1. 座標系
四個坐標系：世界坐標系、相機坐標系、圖像坐標系、像素坐標系。
* 世界坐標系
為參考坐標系，描述世界中的任何一點P（Xw,Yw,Zw)
* 相機坐標系
相機運動的一個坐標系，通過世界坐標系的變換(旋轉R,平移T)計算得到。因此主要是將世界坐標系描述的點坐標P(Xw,Yw,Zw)轉換成相機坐標系下描述P(Xc,Yc,Zc)，用來計算在成像坐標系的坐標。
R為旋轉矩陣
* 圖像坐標系
從相機坐標係到圖像坐標系，屬於透視投影關係，從3D轉換到2D。此時P點已由上面經過世界坐標系轉換成相機坐標系下表述P(Xc,Yc,Zc)，接下來需要將相機坐標轉換到圖像坐標，需要用到相機的內參：
* 像素坐標系
像素座標系和圖像座標系都在成像平面上，只是各自的原點和單位不一樣。圖像座標系的原點爲相機光軸(相機中心點)與成像平面的交點。圖像座標系的單位是mm，屬於物理單位，而像素座標系的單位是pixel，我們平常描述一個像素點都是幾行幾列。所以這二者之間的轉換如下：其中dx和dy表示每一列和每一行分別代表多少mm，即1 pixel=dx mm。
從世界坐標系轉到像素坐標系可以寫成:
相機內參就如公式上所述
2. 畸變參數
可分為兩種，徑向畸變與切向畸變。徑向畸變發生在相機坐標系轉像物理坐標系的過程中。切向畸變：產生的原因是透鏡不完全平行於圖像。
徑向畸變可用光心(x,y)與距離r用泰勒展開來描述
畸變係數為k1,k2,k3。k1最重要，k3影響最小因此有時候為可捨棄的係數。
切向畸變可用兩個公式表示
因此總共有5個參數(k1,k2,k3,p1,p2)可用來去畸變。
3. 特徵金字塔
對一張圖像進行連續的等比縮放(一般是縮小)，把多次縮放之後的圖像加上原圖，稱為圖像金字塔
其中Level 0表示原圖，Level 1則為按照縮放因子f進行第一次縮放的結果，Level 2則是在Level 1 的基礎上，按照放因子f再次進行縮放之後的結果。這樣一次循環疊加，形成了上面的圖像金字塔。使用圖像金字塔，可以提取到圖像各個尺寸的特徵點，這樣增加了算法的魯棒性。
當金字塔的層數越高，圖像的面積越小，能提取到的特徵點數目就越小，所以分配策略就是根據圖像的面積來定，將總特徵點數目根據面積比例均攤到每層圖像上。假設需要提取的特徵點數目為N，金字塔總共有m層，第0層圖像的寬為W，高為H，對應的面積C=H*W，圖像金字塔縮放因子為s，0<s<1，在ORB-SLAM2中，m=8,s=1/1.2。
整個金字塔總的圖像面積為：
單位面積應該分配的特徵點數量為：
第0層應該分配的特徵點數量為：
第i層應該分配的特徵點數量為：
在ORB-SLAM2 的代碼裡，不是按照面積均攤的，而是按照面積的開方來均攤特徵點的，也就是將上述公式中的s平方換成s即可。
#### 流程詳解
1. 根據配置文件路徑讀取配置文件
```
cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ);
```
2. 從配置文件讀取相機內參
```
float fx = fSettings["Camera.fx"];
float fy = fSettings["Camera.fy"];
float cx = fSettings["Camera.cx"];
float cy = fSettings["Camera.cy"];
```
3. 根據內參構造內參矩陣
```
// |fx 0 cx|
// K = |0 fy cy|
// |0 0 1 |
cv::Mat K = cv::Mat::eye(3,3,CV_32F);
K.at<float>(0,0) = fx;
K.at<float>(1,1) = fy;
K.at<float>(0,2) = cx;
K.at<float>(1,2) = cy;
K.copyTo(mK);
```
4. 從配置文件讀取畸變參數
```
// [k1 k2 p1 p2 k3]
// K3不一定要存在，因為畸變參數的重要性為k1>k2>k3
cv::Mat DistCoef(4,1,CV_32F);
DistCoef.at<float>(0) = fSettings["Camera.k1"];
DistCoef.at<float>(1) = fSettings["Camera.k2"];
DistCoef.at<float>(2) = fSettings["Camera.p1"];
DistCoef.at<float>(3) = fSettings["Camera.p2"];
const float k3 = fSettings["Camera.k3"];
if(k3!=0)
{
DistCoef.resize(5);
DistCoef.at<float>(4) = k3;
}
DistCoef.copyTo(mDistCoef);
```
5. 從配置文件讀取ORB特徵相關參數
```
// 每一幀提取的特徵點數為1000
int nFeatures = fSettings["ORBextractor.nFeatures"];
// 圖像建立金字塔時的變化尺度 1.2
float fScaleFactor = fSettings["ORBextractor.scaleFactor"];
// 尺度金字塔的層數 8
int nLevels = fSettings["ORBextractor.nLevels"];
// 提取fast特徵點的默認閾值 20
int fIniThFAST = fSettings["ORBextractor.iniThFAST"];
// 如果默認閾值提取不出足夠fast特徵點，則使用最小閾值 8
int fMinThFAST = fSettings["ORBextractor.minThFAST"];
```
6. 構建ORB特徵提取器
```
// tracking過程都會用到mpORBextractorLeft作為特徵點提取器
mpORBextractorLeft = new ORBextractor(nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);
// 如果是雙目，tracking過程中還會用用到mpORBextractorRight作為右目特徵點提取器
if(sensor==System::STEREO)
mpORBextractorRight = new ORBextractor(nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);
// 在單目初始化的時候，會用mpIniORBextractor來作為特徵點提取器，單目會用兩倍特徵點數目
if(sensor==System::MONOCULAR)
mpIniORBextractor = new ORBextractor(2*nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);
```
跳至[ORBextractor::ORBextractor](#ORBextractor::ORBextractor)
7. 構建尺度變數
```
if(sensor==System::STEREO || sensor==System::RGBD)
{
// 判斷一個3D點遠/近的閾值 mbf * 35 / fx
mThDepth = mbf*(float)fSettings["ThDepth"]/fx;
cout << endl << "Depth Threshold (Close/Far Points): " << mThDepth << endl;
}
if(sensor==System::RGBD)
{
// 深度相機disparity轉化為depth時的因子
mDepthMapFactor = fSettings["DepthMapFactor"];
if(fabs(mDepthMapFactor)<1e-5)
mDepthMapFactor=1;
else
mDepthMapFactor = 1.0f/mDepthMapFactor;
}
```
### Tracking::GrabImageMonocular()
構建影像並進行跟蹤
在[System::TrackMonocular()](#System::TrackMonocular())中引用
#### 定義
```
cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, //輸入影像
const double ×tamp) //輸入時間戳
```
#### 函式流程
1. 將影像轉為灰度影像
2. 構造Frame類
3. 跟踪
#### 流程詳解
1. 將影像轉為灰度影像
```
mImGray = im;
if(mImGray.channels()==3)
{
if(mbRGB)
cvtColor(mImGray,mImGray,CV_RGB2GRAY);
else
cvtColor(mImGray,mImGray,CV_BGR2GRAY);
}
else if(mImGray.channels()==4)
{
if(mbRGB)
cvtColor(mImGray,mImGray,CV_RGBA2GRAY);
else
cvtColor(mImGray,mImGray,CV_BGRA2GRAY);
}
```
2. 構造Frame類
```
// 沒有成功初始化的前一個狀態就是NO_IMAGES_YET
if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET)
mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
else
mCurrentFrame = Frame(mImGray,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
```
跳到[Frame::Frame()單目](#Frame::Frame()單目)
3. 跟踪
```
Track();
```
跳到[Tracking::Track()](#Tracking::Track())
### Tracking::Track()
追蹤線程
在[Tracking::GrabImageMonocular()](#Tracking::GrabImageMonocular())中引用
#### 函式流程
1. 初始化
2. 跟踪進入正常SLAM模式，有地圖更新
3. 純定位模式，只進行跟踪tracking，局部地圖不工作
4. 在跟踪得到當前幀初始姿態後，對局部地圖進行跟踪得到更多的匹配，並優化當前姿態
5. 跟踪成功，更新恆速運動模型
6. 清除觀測不到的地圖點
7. 清除恆速模型跟踪中 UpdateLastFrame中為當前幀臨時添加的MapPoints（
8. 檢測並插入關鍵幀，對於雙目或RGB-D會產生新的地圖點
9. 刪除那些在bundle adjustment中檢測為outlier的地圖點
10. 如果初始化後不久就跟踪失敗，並且relocation也沒有搞定，只能重新Reset
11. 記錄位姿信息，用於最後保存所有的軌跡
#### 流程詳解
1. 初始化
```
//確認是否初始化
if(mState==NO_IMAGES_YET)
{
mState = NOT_INITIALIZED;
}
// 儲存了Tracking最新的狀態，用於FrameDrawer中的繪製
mLastProcessedState=mState;
//鎖地圖點更新
unique_lock<mutex> lock(mpMap->mMutexMapUpdate);
//沒初始化的時候
if(mState==NOT_INITIALIZED)
{
if(mSensor==System::STEREO || mSensor==System::RGBD)
StereoInitialization();
else
MonocularInitialization(); //單目地圖初始化
mpFrameDrawer->Update(this);
if(mState!=OK)
return;
}
```
單目模式下，跳至[Tracking::MonocularInitialization()](#Tracking::MonocularInitialization())
2. 跟踪進入正常SLAM模式，有地圖更新
```
//如果已經完成初始化
else
{
// bOK為臨時變數，用於表示每個函數是否執行成功
bool bOK;
// mbOnlyTracking 等於false表示正常SLAM模式（定位+地圖更新），mbOnlyTracking等於true表示僅定位模式
// tracking 類構造時默認為false。在viewer中有個開關ActivateLocalizationMode，可以控制是否開啟mbOnlyTracking
if(!mbOnlyTracking) //非僅追踪模式
{
// 是否初始化成功
if(mState==OK)
{
//2.1. 檢查並更新上一幀被替換的MapPoints
// 局部建圖線程則可能會對原有的地圖點進行替換.在這裡進行檢查
CheckReplacedInLastFrame();
//2.2. 運動模型是空的或剛完成重定位，跟踪參考關鍵幀；否則恆速模型跟踪
// 第一個條件,如果運動模型為空,說明是剛初始化開始，或者已經跟丟了
// 第二個條件,如果當前幀緊緊地跟著在重定位的幀的後面，我們將重定位幀來恢復位姿
// mnLastRelocFrameId 上一次重定位的那一幀
if(mVelocity.empty() || mCurrentFrame.mnId<mnLastRelocFrameId+2)
{
// 用最近的關鍵幀來跟踪當前的普通幀
// 通過BoW的方式在參考幀中找當前幀特徵點的匹配點
// 優化每個特徵點都對應3D點重投影誤差即可得到位姿
bOK = TrackReferenceKeyFrame(); //使用關鍵幀追踪
}
else //先用當前幀追踪(恆速模式)，如果追踪失敗，則使用關鍵點幀追踪
{
// 用最近的普通幀來跟踪當前的普通幀
// 根據恆速模型設定當前幀的初始位姿
// 通過投影的方式在參考幀中找當前幀特徵點的匹配點
// 優化每個特徵點所對應3D點的投影誤差即可得到位姿
bOK = TrackWithMotionModel();
if(!bOK)
//根據恆速模型失敗了，只能根據參考關鍵幀來跟踪
bOK = TrackReferenceKeyFrame();
}
}
else //如果跟踪失敗，則進行重定位
{
// 如果跟踪狀態不成功,那麼就只能重定位了
// BOW搜索，EPnP求解位姿
bOK = Relocalization();
}
}
```
CheckReplacedInLastFrame(): 跳到[Tracking::CheckReplacedInLastFrame()](#Tracking::CheckReplacedInLastFrame())
TrackReferenceKeyFrame(): 跳到[Tracking::TrackReferenceKeyFrame()](#Tracking::TrackReferenceKeyFrame())
TrackWithMotionModel(): 跳到[Tracking::TrackWithMotionModel()](#Tracking::TrackWithMotionModel())
Tracking::Relocalization(): 跳到[Tracking::Relocalization()](#Tracking::Relocalization())
3. 純定位模式，只進行跟踪tracking，局部地圖不工作
```
else
{
// Localization Mode: Local Mapping is deactivated
if(mState==LOST)
{
// 3.1 如果跟丟了，只能重定位
bOK = Relocalization();
}
else
{
// mbVO是mbOnlyTracking為true時的才有的一個變量
// mbVO為false表示此幀匹配了很多的MapPoints，跟踪很正常 (注意有點反直覺)
// mbVO為true表明此幀匹配了很少的MapPoints，少於10個，要跪的節奏
if(!mbVO)
{
// 3.2 如果跟踪正常，使用恆速模型 或 參考關鍵幀跟踪
// In last frame we tracked enough MapPoints in the map
if(!mVelocity.empty())
{
bOK = TrackWithMotionModel();
// ? 為了和前面模式統一，這個地方是不是應該加上
// if(!bOK)
// bOK = TrackReferenceKeyFrame();
}
else
{
// 如果恆速模型不被滿足,那麼就只能夠通過參考關鍵幀來定位
bOK = TrackReferenceKeyFrame();
}
}
//mbVO為true，表明此幀匹配了很少（小於10）的地圖點，既做跟踪又做重定位
else
{
//MM=Motion Model,通過運動模型進行跟踪的結果
bool bOKMM = false;
//通過重定位方法來跟踪的結果
bool bOKReloc = false;
//運動模型中構造的地圖點
vector<MapPoint*> vpMPsMM;
//在追踪運動模型後發現的外點
vector<bool> vbOutMM;
//運動模型得到的位姿
cv::Mat TcwMM;
// 3.3. 當運動模型有效的時候,根據運動模型計算位姿
if(!mVelocity.empty())
{
bOKMM = TrackWithMotionModel();
// 將恆速模型跟踪結果暫存到這幾個變量中，因為後面重定位會改變這些變量
vpMPsMM = mCurrentFrame.mvpMapPoints;
vbOutMM = mCurrentFrame.mvbOutlier;
TcwMM = mCurrentFrame.mTcw.clone();
}
// 3.4. 使用重定位的方法來得到當前幀的位姿
bOKReloc = Relocalization();
// 3.5. 根據前面的恆速模型、重定位結果來更新狀態
if(bOKMM && !bOKReloc)
{
// 恆速模型成功、重定位失敗，重新使用之前暫存的恆速模型結果
mCurrentFrame.SetPose(TcwMM);
mCurrentFrame.mvpMapPoints = vpMPsMM;
mCurrentFrame.mvbOutlier = vbOutMM;
//? 疑似bug！這段代碼是不是重複增加了觀測次數？後面 TrackLocalMap 函數中會有這些操作
// 如果當前幀匹配的3D點很少，增加當前可視地圖點的被觀測次數
if(mbVO)
{
// 更新當前幀的地圖點被觀測次數
for(int i =0; i<mCurrentFrame.N; i++)
{
//如果這個特徵點形成了地圖點,並且也不是外點的時候
if(mCurrentFrame.mvpMapPoints[i] && !mCurrentFrame.mvbOutlier[i])
{
//增加能觀測到該地圖點的幀數
mCurrentFrame.mvpMapPoints[i]->IncreaseFound();
}
}
}
}
else if(bOKReloc)
{
// 只要重定位成功整個跟踪過程正常進行（重定位與跟踪，更相信重定位）
mbVO = false;
}
//有一個成功我們就認為執行成功了
bOK = bOKReloc || bOKMM;
}
}
}
```
4. 在跟踪得到當前幀初始姿態後，對局部地圖進行跟踪得到更多的匹配，並優化當前姿態
```
// 將最新的關鍵幀作為當前幀的參考關鍵幀
mCurrentFrame.mpReferenceKF = mpReferenceKF;
// 前面只是跟踪一幀得到初始位姿，這裡搜索局部關鍵幀、局部地圖點，和當前幀進行投影匹配，得到更多匹配的MapPoints後進行Pose優化
if(!mbOnlyTracking)
{
if(bOK)
bOK = TrackLocalMap();
}
else
{
// mbVO true means that there are few matches to MapPoints in the map. We cannot retrieve
// a local map and therefore we do not perform TrackLocalMap(). Once the system relocalizes
// the camera we will use the local map again.
// 重定位成功
if(bOK && !mbVO)
bOK = TrackLocalMap();
}
//根據上面的操作來判斷是否追踪成功
if(bOK)
mState = OK;
else
mState=LOST;
// Step 4：更新顯示線程中的圖像、特徵點、地圖點等信息
mpFrameDrawer->Update(this);
```
TrackLocalMap : 跳到[Tracking::TrackLocalMap()](#Tracking::TrackLocalMap())
5. 跟踪成功，更新恆速運動模型
```
//只有在成功追踪時才考慮生成關鍵幀的問題
if(bOK)
{
// Update motion model
if(!mLastFrame.mTcw.empty())
{
// 更新恆速運動模型 TrackWithMotionModel 中的mVelocity
cv::Mat LastTwc = cv::Mat::eye(4,4,CV_32F);
mLastFrame.GetRotationInverse().copyTo(LastTwc.rowRange(0,3).colRange(0,3));
mLastFrame.GetCameraCenter().copyTo(LastTwc.rowRange(0,3).col(3));
// mVelocity = Tcl = Tcw * Twl,表示上一幀到當前幀的變換， 其中 Twl = LastTwc
mVelocity = mCurrentFrame.mTcw*LastTwc;
}
else
//否則速度為空
mVelocity = cv::Mat();
//更新顯示中的位姿
mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw);
```
6. 清除觀測不到的地圖點
```
for(int i=0; i<mCurrentFrame.N; i++)
{
MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
if(pMP)
if(pMP->Observations()<1)
{
mCurrentFrame.mvbOutlier[i] = false;
mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
}
}
```
7. 清除恆速模型跟踪中 UpdateLastFrame中為當前幀臨時添加的MapPoints（僅雙目和rgbd）
```
// 步驟6中只是在當前幀中將這些MapPoints剔除，這裡從MapPoints數據庫中刪除
// 臨時地圖點僅僅是為了提高雙目或rgbd攝像頭的幀間跟踪效果，用完以後就扔了，沒有添加到地圖中
for(list<MapPoint*>::iterator lit = mlpTemporalPoints.begin(), lend = mlpTemporalPoints.end(); lit!=lend; lit++)
{
MapPoint* pMP = *lit;
delete pMP;
}
// 這裡不僅僅是清除mlpTemporalPoints，通過delete pMP還刪除了指針指向的MapPoint
// 不能夠直接執行這個是因為其中存儲的都是指針,之前的操作都是為了避免內存洩露
mlpTemporalPoints.clear();
```
8. 檢測並插入關鍵幀，對於雙目或RGB-D會產生新的地圖點
```
if(NeedNewKeyFrame())
CreateNewKeyFrame();
// We allow points with high innovation (considererd outliers by the Huber Function)
// pass to the new keyframe, so that bundle adjustment will finally decide
// if they are outliers or not. We don't want next frame to estimate its position
// with those points so we discard them in the frame.
// 作者這裡說允許在BA中被Huber核函數判斷為外點的傳入新的關鍵幀中，讓後續的BA來審判他們是不是真正的外點
// 但是估計下一幀位姿的時候我們不想用這些外點，所以刪掉
```
NeedNewKeyFrame(): 跳到[Tracking::NeedNewKeyFrame()](#Tracking::NeedNewKeyFrame())
9. 刪除那些在bundle adjustment中檢測為outlier的地圖點
```
for(int i=0; i<mCurrentFrame.N;i++)
{
// 這裡第一個條件還要執行判斷是因為, 前面的操作中可能刪除了其中的地圖點
if(mCurrentFrame.mvpMapPoints[i] && mCurrentFrame.mvbOutlier[i])
mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
}
}
```
10. 如果初始化後不久就跟踪失敗，並且relocation也沒有搞定，只能重新Reset
```
if(mState==LOST)
{
//如果地圖中的關鍵幀信息過少的話,直接重新進行初始化了
if(mpMap->KeyFramesInMap()<=5)
{
cout << "Track lost soon after initialisation, reseting..." << endl;
mpSystem->Reset();
return;
}
}
//確保已經設置了參考關鍵幀
if(!mCurrentFrame.mpReferenceKF)
mCurrentFrame.mpReferenceKF = mpReferenceKF;
// 保存上一幀的數據,當前幀變上一幀
mLastFrame = Frame(mCurrentFrame);
}
```
11. 記錄位姿信息，用於最後保存所有的軌跡
```
if(!mCurrentFrame.mTcw.empty())
{
// 計算相對姿態Tcr = Tcw * Twr, Twr = Trw^-1
cv::Mat Tcr = mCurrentFrame.mTcw*mCurrentFrame.mpReferenceKF->GetPoseInverse();
//保存各種狀態
mlRelativeFramePoses.push_back(Tcr);
mlpReferences.push_back(mpReferenceKF);
mlFrameTimes.push_back(mCurrentFrame.mTimeStamp);
mlbLost.push_back(mState==LOST);
}
else
{
// This can happen if tracking is lost
// 如果跟踪失敗，則相對位姿使用上一次值
mlRelativeFramePoses.push_back(mlRelativeFramePoses.back());
mlpReferences.push_back(mlpReferences.back());
mlFrameTimes.push_back(mlFrameTimes.back());
mlbLost.push_back(mState==LOST);
}
```
### Tracking::MonocularInitialization()
單目地圖初始化
在[Tracking::Track()](#Tracking::Track())中引用
#### 函式流程
1. 構建單目初始器
2. 如果當前幀特徵點太少，重新構造初始器
3. 尋找初始幀與當前幀的匹配特徵點對
4. 驗證匹配結果，如果匹配點太少就從新初始化
5. 用匹配結果計算H與F矩陣
6. 初始化成功後，刪除無法三角化的匹配點
7. 將初始化的第一幀作為世界座標中心
8. 創建初始化地圖點
#### 流程詳解
1. 構建單目初始器
```
if(!mpInitializer)
{
// Set Reference Frame
// 單目初始幀提取的特徵點數必須大於100，否則放棄該幀圖像
if(mCurrentFrame.mvKeys.size()>100)
{
// 步驟1：得到用於初始化的第一幀，初始化需要兩幀
mInitialFrame = Frame(mCurrentFrame);
// 記錄最近的一幀
mLastFrame = Frame(mCurrentFrame);
// mvbPrevMatched最大的情況就是所有特徵點都被跟踪上
mvbPrevMatched.resize(mCurrentFrame.mvKeysUn.size());
for(size_t i=0; i<mCurrentFrame.mvKeysUn.size(); i++)
mvbPrevMatched[i]=mCurrentFrame.mvKeysUn[i].pt;
// 這兩句是多餘的
if(mpInitializer)
delete mpInitializer;
// 由當前幀構造初始器 sigma:1.0 iterations:200
mpInitializer = new Initializer(mCurrentFrame,1.0,200);
//初始化為-1。表示沒有任何匹配，儲存匹配的ID
fill(mvIniMatches.begin(),mvIniMatches.end(),-1);
return;
}
}
```
2. 如果當前幀特徵點太少，重新構造初始器
只有連續兩幀的特徵點個數都大於100時，才能繼續進行初始化過程
```
else
{
//當當前幀特徵點小於100
if((int)mCurrentFrame.mvKeys.size()<=100)
{
delete mpInitializer;
mpInitializer = static_cast<Initializer*>(NULL);
fill(mvIniMatches.begin(),mvIniMatches.end(),-1);
return;
}
```
3. 尋找初始幀與當前幀的匹配特徵點對
尋找mInitialFrame與mCurrentFrame的匹配特徵點對
```
//建構特徵匹配器
ORBmatcher matcher(0.9,true);
int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);
```
ORBmatcher matcher 跳至[ORBmatcher::ORBmatcher()](#ORBmatcher::ORBmatcher())
matcher.SearchForInitialization 跳至[ORBmatcher::SearchForInitialization()](#ORBmatcher::SearchForInitialization())
4. 驗證匹配結果，如果匹配點太少就從新初始化
```
if(nmatches<100)
{
delete mpInitializer;
mpInitializer = static_cast<Initializer*>(NULL);
return;
}
```
5. 用匹配結果計算H與F矩陣
```
//旋轉矩陣
cv::Mat Rcw; // Current Camera Rotation
//平移矩陣
cv::Mat tcw; // Current Camera Translation
vector<bool> vbTriangulated; // Triangulated Correspondences (mvIniMatches)
// 通過H模型或F模型進行單目初始化，得到兩幀間相對運動、初始MapPoints
if(mpInitializer->Initialize(mCurrentFrame, mvIniMatches, Rcw, tcw, mvIniP3D, vbTriangulated))
{
```
跳至[Initializer::Initialize()](#Initializer::Initialize())
6. 初始化成功後，刪除無法三角化的匹配點
```
// 刪除那些無法進行三角化的匹配點
for(size_t i=0, iend=mvIniMatches.size(); i<iend;i++)
{
if(mvIniMatches[i]>=0 && !vbTriangulated[i])
{
mvIniMatches[i]=-1;
nmatches--;
}
}
```
7. 將初始化的第一幀作為世界座標中心
```
// Set Frame Poses
// 將初始化的第一幀作為世界坐標系，因此第一幀變換矩陣為單位矩陣
mInitialFrame.SetPose(cv::Mat::eye(4,4,CV_32F));
// 由Rcw和tcw構造Tcw,並賦值給mTcw，mTcw為世界坐標係到該幀的變換矩陣
cv::Mat Tcw = cv::Mat::eye(4,4,CV_32F);
Rcw.copyTo(Tcw.rowRange(0,3).colRange(0,3));
tcw.copyTo(Tcw.rowRange(0,3).col(3));
mCurrentFrame.SetPose(Tcw);
```
8. 創建初始化地圖點
```
// 步驟6：將三角化得到的3D點包裝成MapPoints
// Initialize函數會得到mvIniP3D，
// mvIniP3D是cv::Point3f類型的一個容器，是個存放3D點的臨時變量，
// CreateInitialMapMonocular將3D點包裝成MapPoint類型存入KeyFrame和Map中
CreateInitialMapMonocular();
}
```
跳到[Tracking::CreateInitialMapMonocular()](#Tracking::CreateInitialMapMonocular())
### Tracking::CreateInitialMapMonocular()
為單目攝影機初始化後，用三角測量得到的三維點生成地圖點。
在[Tracking::MonocularInitialization()](#Tracking::MonocularInitialization())中引用
#### 函式流程
1. 創立關鍵幀
2. 將初始關鍵幀轉為詞袋
3. 將關鍵幀插入到地圖中
4. 用初始化的3D點生成地圖點
5. 為該地圖點添加屬性
6. 在地圖中添加這個地圖點
7. 更新關鍵幀之間的連接關係
8. 全域的BA優化，優化所有姿態及三維點
9. 將MapPoints的中值深度歸一化
10. 將兩幀之間的變換歸一化
11. 將三維點歸一化
12. 將關鍵幀插入局部地圖，更新姿態與局部地圖點
#### 流程詳解
1. 創立關鍵幀
```
// Create KeyFrames
//將初始幀與當前幀都視為關鍵幀
KeyFrame* pKFini = new KeyFrame(mInitialFrame,mpMap,mpKeyFrameDB);
KeyFrame* pKFcur = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);
```
跳到[KeyFrame::KeyFrame()](#KeyFrame::KeyFrame)
2. 將初始關鍵幀轉為詞袋
```
// 步驟1：將初始關鍵幀的描述子轉為BoW
pKFini->ComputeBoW();
// 步驟2：將當前關鍵幀的描述子轉為BoW
pKFcur->ComputeBoW();
```
跳到[Frame::ComputeBoW()](#Frame::ComputeBoW())
3. 將關鍵幀插入到地圖中
```
// 凡是關鍵幀，都要插入地圖
mpMap->AddKeyFrame(pKFini);
mpMap->AddKeyFrame(pKFcur);
```
跳到[Map::AddKeyFrame()](#Map::AddKeyFrame())
4. 用初始化的3D點生成地圖點
```
// 遍歷所有匹配點
for(size_t i=0; i<mvIniMatches.size();i++)
{
//沒有匹配關係
if(mvIniMatches[i]<0)
continue;
//將3D點放到世界座標中
cv::Mat worldPos(mvIniP3D[i]);
//用3D點構造地圖點
MapPoint* pMP = new MapPoint(worldPos,pKFcur,mpMap);
```
跳到[MapPoint::MapPoint()](#MapPoint::MapPoint())
5. 為該地圖點添加屬性
```
// a.觀測到該MapPoint的關鍵幀
// b.該MapPoint的描述子
// c.該MapPoint的平均觀測方向和深度範圍
// 該KeyFrame的哪個特徵點可以觀測到哪個3D點
pKFini->AddMapPoint(pMP,i);
pKFcur->AddMapPoint(pMP,mvIniMatches[i]);
// a.表示該MapPoint可以被哪個KeyFrame的哪個特徵點觀測到(添加觀測關係)
pMP->AddObservation(pKFini,i);
pMP->AddObservation(pKFcur,mvIniMatches[i]);
// b.從眾多觀測到該MapPoint的特徵點中挑選區分讀最高的描述子
pMP->ComputeDistinctiveDescriptors();
// c.更新該MapPoint平均觀測方向以及觀測距離的範圍
pMP->UpdateNormalAndDepth();
//Fill Current Frame structure
mCurrentFrame.mvpMapPoints[mvIniMatches[i]] = pMP;
mCurrentFrame.mvbOutlier[mvIniMatches[i]] = false;
//Add to Map
```
AddMapPoint : 跳到[KeyFrame::AddMapPoint()](#KeyFrame::AddMapPoint())
AddObservation: 跳到[MapPoint::AddObservation()](#MapPoint::AddObservation())
ComputeDistinctiveDescriptors: 跳到[MapPoint::ComputeDistinctiveDescriptors()](#MapPoint::ComputeDistinctiveDescriptors())
6. 在地圖中添加這個地圖點
```
mpMap->AddMapPoint(pMP);
}
```
跳到[Map::AddMapPoint()](#Map::AddMapPoint())
7. 更新關鍵幀之間的連接關係
```
// Update Connections
// 更新關鍵幀間的連接關係，對於一個新創建的關鍵幀都會執行一次關鍵連接關係更新
// 在3D點和關鍵幀之間建立邊，每個邊有一個權重，邊的權重是該關鍵幀與當前幀公共3D點的個數
pKFini->UpdateConnections();
pKFcur->UpdateConnections();
```
跳到[KeyFrame::UpdateConnections()](#KeyFrame::UpdateConnections())
8. 全域的BA優化，優化所有姿態及三維點
```
// Bundle Adjustment
cout << "New Map created with " << mpMap->MapPointsInMap() << " points" << endl;
// 步驟5：BA優化
Optimizer::GlobalBundleAdjustemnt(mpMap,20);
```
9. 將MapPoints的中值深度歸一化
```
// Set median depth to 1
// 單目傳感器無法恢復真實的深度，這裡將點雲中值深度（歐式距離，不是指z）歸一化到1
// 評估關鍵幀場景深度，q=2表示中值
float medianDepth = pKFini->ComputeSceneMedianDepth(2);
float invMedianDepth = 1.0f/medianDepth;
//當平均深度小於0，或是在當前幀中被觀測到的地圖點小於100個，代表初始化失敗
if(medianDepth<0 || pKFcur->TrackedMapPoints(1)<100)
{
cout << "Wrong initialization, reseting..." << endl;
Reset();
return;
}
```
跳到[KeyFrame::ComputeSceneMedianDepth()](#KeyFrame::ComputeSceneMedianDepth())
10. 將兩幀之間的變換歸一化
```
// Scale initial baseline
cv::Mat Tc2w = pKFcur->GetPose();
// 根據點雲歸一化比例縮放平移量，x/z,y/z
Tc2w.col(3).rowRange(0,3) = Tc2w.col(3).rowRange(0,3)*invMedianDepth;
pKFcur->SetPose(Tc2w);
```
11. 將三維點歸一化
```
// Scale points
// 把3D點的尺度也歸一化到1
vector<MapPoint*> vpAllMapPoints = pKFini->GetMapPointMatches();
for(size_t iMP=0; iMP<vpAllMapPoints.size(); iMP++)
{
if(vpAllMapPoints[iMP])
{
MapPoint* pMP = vpAllMapPoints[iMP];
pMP->SetWorldPos(pMP->GetWorldPos()*invMedianDepth);
}
}
```
12. 將關鍵幀插入局部地圖，更新姿態與局部地圖點
```
mpLocalMapper->InsertKeyFrame(pKFini);
mpLocalMapper->InsertKeyFrame(pKFcur);
mCurrentFrame.SetPose(pKFcur->GetPose());
mnLastKeyFrameId=mCurrentFrame.mnId;
mpLastKeyFrame = pKFcur;
mvpLocalKeyFrames.push_back(pKFcur);
mvpLocalKeyFrames.push_back(pKFini);
mvpLocalMapPoints=mpMap->GetAllMapPoints();
mpReferenceKF = pKFcur;
mCurrentFrame.mpReferenceKF = pKFcur;
mLastFrame = Frame(mCurrentFrame);
//更新局部地圖點
mpMap->SetReferenceMapPoints(mvpLocalMapPoints);
mpMapDrawer->SetCurrentCameraPose(pKFcur->GetPose());
mpMap->mvpKeyFrameOrigins.push_back(pKFini);
mState=OK;// 初始化成功，至此，初始化過程完成
```
跳到[Map::SetReferenceMapPoints()](#Map::SetReferenceMapPoints())
### Tracking::UpdateLastFrame()
更新上一幀姿態，再上一幀中生成臨時地圖點
在[Tracking::TrackWithMotionModel()](#Tracking::TrackWithMotionModel())中引用
#### 函式流程
1. 更新最近一幀的姿態
2. 對於雙目或rgbd攝像頭，為上一幀生成新的臨時地圖點
#### 流程詳解
1. 更新最近一幀的姿態
```
// Update pose according to reference keyframe
KeyFrame* pRef = mLastFrame.mpReferenceKF;
//參考幀到上一幀的變換
cv::Mat Tlr = mlRelativeFramePoses.back();
//上一幀在世界座標下的姿態
mLastFrame.SetPose(Tlr*pRef->GetPose()); // Tlr*Trw = Tlw 1:last r:reference w:world
// 如果上一幀為關鍵幀，或者單目的情況，則退出
if(mnLastKeyFrameId==mLastFrame.mnId || mSensor==System::MONOCULAR)
return;
```
2. 對於雙目或rgbd攝像頭，為上一幀生成新的臨時地圖點
```
// 注意這些MapPoints不加入到Map中，在tracking的最後會刪除
// 跟踪過程中需要將將上一幀的MapPoints投影到當前幀可以縮小匹配範圍，加快當前幀與上一幀進行特徵點匹配
// 2.1. 得到上一幀有深度值的特徵點
vector<pair<float,int> > vDepthIdx;
vDepthIdx.reserve(mLastFrame.N);
for(int i=0; i<mLastFrame.N;i++)
{
float z = mLastFrame.mvDepth[i];
//是否存在
if(z>0)
{
//push(深度，特徵點ID)
vDepthIdx.push_back(make_pair(z,i));
}
}
//如果沒有有效深度的點，就退出
if(vDepthIdx.empty())
return;
// 2.2. 按照深度從小到大排序
sort(vDepthIdx.begin(),vDepthIdx.end());
// 2.3. 判斷是否要生成臨時地圖點
int nPoints = 0;
for(size_t j=0; j<vDepthIdx.size();j++)
{
int i = vDepthIdx[j].second;
bool bCreateNew = false;
//提取地圖點
MapPoint* pMP = mLastFrame.mvpMapPoints[i];
//如果這個點在上一幀中沒有地圖點存在
if(!pMP)
bCreateNew = true; //要生成地圖點
//如果地圖點沒有被觀測到
else if(pMP->Observations()<1)
{
bCreateNew = true; //要生成地圖點
}
//2.4. 從中找出不是地圖點的生成臨時地圖點
if(bCreateNew)
{
// 這些生成MapPoints後並沒有通過：
// a.AddMapPoint、
// b.AddObservation、
// c.ComputeDistinctiveDescriptors、
// d.UpdateNormalAndDepth添加屬性，
// 這些MapPoint僅僅為了提高雙目和RGBD的跟踪成功率
//從2維影像反投影到相機坐標系
cv::Mat x3D = mLastFrame.UnprojectStereo(i);
MapPoint* pNewMP = new MapPoint(x3D,mpMap,&mLastFrame,i);
mLastFrame.mvpMapPoints[i]=pNewMP; // 添加新的MapPoint
// 標記為臨時添加的MapPoint，之後在CreateNewKeyFrame之前會全部刪除
mlpTemporalPoints.push_back(pNewMP);
nPoints++;
}
else
{
//紀錄不需要創建地圖點的特徵點個數
nPoints++;
}
//停止新增臨時地圖點要滿足兩個條件
//1. 深度超過設定的閾值
//2. 不需要創建地圖點的特徵點個數>100，代表距離太遠不準確
if(vDepthIdx[j].first>mThDepth && nPoints>100)
break;
}
```
UnprojectStereo(): 跳到[KeyFrame::UnprojectStereo()](#KeyFrame::UnprojectStereo())
### Tracking::NeedNewKeyFrame()
判斷當前幀是否需要插入關鍵幀
在[Tracking::Track()](#Tracking::Track())中引用
#### 函式流程
1. 純視覺里程計模式(VO)下，不插入關鍵幀(不做local mapping)。
2. 如果局部地圖在被閉環檢測使用，不插入關鍵幀。
3. 如果距離上一次插入關鍵幀的時間太短，或是關鍵幀數量過多，不插入關鍵幀。
4. 得到參考關鍵幀跟踪到的地圖點數量
5. 查詢局部地圖線程是否繁忙
6. 對於雙目或RGBD攝影機，計算成功跟縱的地圖點
7. 判斷是否需要插入關鍵幀
#### 流程詳解
1. 純視覺里程計模式(VO)下，不插入關鍵幀(不做local mapping)。
```
// 由於插入關鍵幀過程中會生成MapPoint，因此用戶選擇重定位後地圖上的點雲和關鍵幀都不會再增加
if(mbOnlyTracking)
return false;
```
2. 如果局部地圖在被閉環檢測使用，不插入關鍵幀。
```
// If Local Mapping is freezed by a Loop Closure do not insert keyframes
// 如果局部地圖被閉環檢測使用，則不插入關鍵幀
if(mpLocalMapper->isStopped() || mpLocalMapper->stopRequested())
return false;
```
3. 如果距離上一次插入關鍵幀的時間太短，或是關鍵幀數量過多，不插入關鍵幀。
```
const int nKFs = mpMap->KeyFramesInMap();
// mCurrentFrame.mnId是當前幀的ID
// mnLastRelocFrameId是最近一次重定位幀的ID
// mMaxFrames等於圖像輸入的幀率
// 或距離上一次重定位超過1s，則考慮插入關鍵幀
if(mCurrentFrame.mnId<mnLastRelocFrameId+mMaxFrames && nKFs>mMaxFrames)
return false;
```
4. 得到參考關鍵幀跟踪到的地圖點數量
```
// 地圖點的最小觀測次數
int nMinObs = 3;
if(nKFs<=2)
nMinObs=2;
int nRefMatches = mpReferenceKF->TrackedMapPoints(nMinObs);
```
跳到[KeyFrame::TrackedMapPoints()](#KeyFrame::TrackedMapPoints)
5. 查詢局部地圖線程是否繁忙
```
// Local Mapping accept keyframes?
bool bLocalMappingIdle = mpLocalMapper->AcceptKeyFrames();
```
6. 對於雙目或RGBD攝影機，計算成功跟縱的地圖點
```
// 對於雙目或RGBD攝像頭，統計成功跟踪的近點的數量，如果跟踪到的近點太少，沒有跟踪到的近點較多，可以插入關鍵幀
int nNonTrackedClose = 0; //雙目或RGB-D中沒有跟踪到的近點
int nTrackedClose= 0; //雙目或RGB-D中成功跟踪的近點（三維點）
if(mSensor!=System::MONOCULAR)
{
for(int i =0; i<mCurrentFrame.N; i++)
{
// 深度值在有效範圍內
if(mCurrentFrame.mvDepth[i]>0 && mCurrentFrame.mvDepth[i]<mThDepth)
{
//是否有對應地圖點
if(mCurrentFrame.mvpMapPoints[i] && !mCurrentFrame.mvbOutlier[i])
nTrackedClose++;
else
nNonTrackedClose++;
}
}
}
// 雙目或RGBD情況下：跟踪到的地圖點中近點太少 同時 沒有跟踪到的三維點太多，可以插入關鍵幀了
// 單目時，為false
bool bNeedToInsertClose = (nTrackedClose<100) && (nNonTrackedClose>70);
```
7. 判斷是否需要插入關鍵幀
```
// Thresholds
// 7.1.：設定比例閾值，當前幀和參考關鍵幀跟踪到點的比例，比例越大，越傾向於增加關鍵幀
float thRefRatio = 0.75f;
// 關鍵幀只有一幀，那麼插入關鍵幀的閾值設置的低一點，插入頻率較高
if(nKFs<2)
thRefRatio = 0.4f;
//單目情況下插入關鍵幀的頻率很高
if(mSensor==System::MONOCULAR)
thRefRatio = 0.9f;
// Condition 1a: More than "MaxFrames" have passed from last keyframe insertion
// 7.2.：很長時間沒有插入關鍵幀，可以插入
const bool c1a = mCurrentFrame.mnId>=mnLastKeyFrameId+mMaxFrames;
// Condition 1b: More than "MinFrames" have passed and Local Mapping is idle
// 7.3.：滿足插入關鍵幀的最小間隔並且 localMapper 處於空閒狀態，可以插入
const bool c1b = (mCurrentFrame.mnId>=mnLastKeyFrameId+mMinFrames && bLocalMappingIdle);
//Condition 1c: tracking is weak
// 7.4.：在雙目，RGB-D的情況下當前幀跟踪到的點比參考關鍵幀的0.25倍還少，或者滿足 bNeedToInsertClose
const bool c1c = mSensor!=System::MONOCULAR && (mnMatchesInliers<nRefMatches*0.25 || bNeedToInsertClose) ;
// Condition 2: Few tracked points compared to reference keyframe. Lots of visual odometry compared to map matches.
// 7.5.：和參考幀相比當前跟踪到的點太少 或者滿足bNeedToInsertClose；同時跟踪到的內點還不能太少
const bool c2 = ((mnMatchesInliers<nRefMatches*thRefRatio|| bNeedToInsertClose) && mnMatchesInliers>15);
if((c1a||c1b||c1c)&&c2)
{
// If the mapping accepts keyframes, insert keyframe.
// Otherwise send a signal to interrupt BA
// 7.6.：local mapping空閒時可以直接插入，不空閒的時候要根據情況插入
if(bLocalMappingIdle)
{
//可以插入關鍵幀
return true;
}
else
{
mpLocalMapper->InterruptBA();
if(mSensor!=System::MONOCULAR)
{
//隊列中的關鍵幀數目不是很多,可以插入
if(mpLocalMapper->KeyframesInQueue()<3)
return true;
else
//隊列中緩衝的關鍵幀數目太多,暫時不能插入
return false;
}
else
//對於單目情況,就直接無法插入關鍵幀了
return false;
}
}
else
//不滿足上面的條件,自然不能插入關鍵幀
return false;
```
### Tracking::CreateNewKeyFrame()
插入關鍵幀
在[Tracking::Track()](#Tracking::Track())中引用
#### 函式流程
1. 將當前幀構造成關鍵幀
2. 將當前關鍵幀設置為當前幀的參考關鍵幀
3. 對於雙目或rgbd攝像頭，為當前幀生成新的地圖點；單目無操作
3.1. 得到當前幀有深度值的特徵點（不一定是地圖點）
3.2. 按照深度從小到大排序(深度值越近越準確)
3.3. 從中找出不是地圖點的生成臨時地圖點
3.4.：停止新建地圖點必須同時滿足以下條件：
* 當前的點的深度已經超過了設定的深度閾值（35倍基線）
* nPoints已經超過100個點，說明距離比較遠了，可能不准確，停掉退出
4. 插入關鍵幀
#### 流程詳解
1. 將當前幀構造成關鍵幀
```
// 如果局部建圖線程關閉了,就無法插入關鍵幀
if(!mpLocalMapper->SetNotStop(true))
return;
KeyFrame* pKF = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);
```
跳到[KeyFrame::KeyFrame()](#KeyFrame::KeyFrame())
2. 將當前關鍵幀設置為當前幀的參考關鍵幀
```
// 在 UpdateLocalKeyFrames 函數中會將與當前關鍵幀共視程度最高的關鍵幀設定為當前幀的參考關鍵幀
mpReferenceKF = pKF;
mCurrentFrame.mpReferenceKF = pKF;
```
3. 對於雙目或rgbd攝像頭，為當前幀生成新的地圖點；單目無操作
```
// 這段代碼和 Tracking::UpdateLastFrame 中的那一部分代碼功能相同
if(mSensor!=System::MONOCULAR)
{
// 根據Tcw計算mRcw、mtcw和mRwc、mOw
mCurrentFrame.UpdatePoseMatrices();
// 3.1.：得到當前幀有深度值的特徵點（不一定是地圖點）
vector<pair<float,int> > vDepthIdx;
vDepthIdx.reserve(mCurrentFrame.N);
for(int i=0; i<mCurrentFrame.N; i++)
{
float z = mCurrentFrame.mvDepth[i];
if(z>0)
{
// 第一個元素是深度,第二個元素是對應的特徵點的id
vDepthIdx.push_back(make_pair(z,i));
}
}
if(!vDepthIdx.empty())
{
// 3.2.：按照深度從小到大排序(深度值越近越準確)
sort(vDepthIdx.begin(),vDepthIdx.end());
// 3.3.：從中找出不是地圖點的生成臨時地圖點
// 處理的近點的個數
int nPoints = 0;
for(size_t j=0; j<vDepthIdx.size();j++)
{
int i = vDepthIdx[j].second; //ID
//這個特徵點是否要變成地圖點
bool bCreateNew = false;
// 如果這個點對應在上一幀中的地圖點沒有,或者創建後就沒有被觀測到,那麼就生成一個臨時的地圖點
MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
if(!pMP)
bCreateNew = true;
else if(pMP->Observations()<1)
{
bCreateNew = true;
mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint*>(NULL);
}
// 如果需要就新建地圖點，這裡的地圖點不是臨時的，是全局地圖中新建地圖點，用於跟踪
if(bCreateNew)
{
cv::Mat x3D = mCurrentFrame.UnprojectStereo(i);
MapPoint* pNewMP = new MapPoint(x3D,pKF,mpMap);
// 這些添加屬性的操作是每次創建地圖點後都要做的
pNewMP->AddObservation(pKF,i);
pKF->AddMapPoint(pNewMP,i);
pNewMP->ComputeDistinctiveDescriptors();
pNewMP->UpdateNormalAndDepth();
mpMap->AddMapPoint(pNewMP);
mCurrentFrame.mvpMapPoints[i]=pNewMP;
nPoints++;
}
else
{
// 因為從近到遠排序，記錄其中不需要創建地圖點的個數
nPoints++;
}
// 3.4.：停止新建地圖點必須同時滿足以下條件：
// 1、當前的點的深度已經超過了設定的深度閾值（35倍基線）
// 2、nPoints已經超過100個點，說明距離比較遠了，可能不准確，停掉退出
if(vDepthIdx[j].first>mThDepth && nPoints>100)
break;
}
}
}
```
mCurrentFrame.UpdatePoseMatrices(): 跳到[Tracking::CreateNewKeyFrame()](#Tracking::CreateNewKeyFrame())
4. 插入關鍵幀
```
// 關鍵幀插入到列表 mlNewKeyFrames中，等待local mapping線程臨幸
mpLocalMapper->InsertKeyFrame(pKF);
// 插入好了，允許局部建圖停止
mpLocalMapper->SetNotStop(false);
// 當前幀成為新的關鍵幀，更新
mnLastKeyFrameId = mCurrentFrame.mnId;
mpLastKeyFrame = pKF;
```
### Tracking::TrackReferenceKeyFrame()
用參考關鍵幀的地圖點來對當前普通幀進行跟踪
用最近的關鍵幀來跟踪當前的普通幀
通過BoW的方式在參考幀中找當前幀特徵點的匹配點
優化每個特徵點都對應3D點重投影誤差即可得到位姿
在[Tracking::Track()](#Tracking::Track())中引用
#### 函式流程
1. 將當前幀的描述子轉化為BoW向量
2. 通過詞袋BoW加速當前幀與參考幀之間的特徵點匹配
3. 將上一幀的姿態作為當前幀姿態的初始值
4. 通過優化3D-2D的重投影誤差來獲得姿態
5. 剔除優化後的匹配點中的外點
#### 流程詳解
1. 將當前幀的描述子轉化為BoW向量
```
mCurrentFrame.ComputeBoW();
ORBmatcher matcher(0.7,true);
vector<MapPoint*> vpMapPointMatches;
```
mCurrentFrame.ComputeBoW(): 跳到[Frame::ComputeBoW()](#Frame::ComputeBoW())
ORBmatcher matcher(): 跳到[ORBmatcher::ORBmatcher()](#ORBmatcher::ORBmatcher())
2. 通過詞袋BoW加速當前幀與參考幀之間的特徵點匹配
```
int nmatches = matcher.SearchByBoW(
mpReferenceKF, //參考關鍵幀
mCurrentFrame, //當前幀
vpMapPointMatches); //存儲匹配關係
// 匹配數目小於15，認為跟踪失敗
if(nmatches<15)
return false;
```
跳到[ORBmatcher::SearchByBoW()普通幀](#ORBmatcher::SearchByBoW()普通幀)
3. 將上一幀的姿態作為當前幀姿態的初始值
```
mCurrentFrame.mvpMapPoints = vpMapPointMatches;
mCurrentFrame.SetPose(mLastFrame.mTcw); // 用上一次的Tcw設置初值，在PoseOptimization可以收斂快一些
```
4. 通過優化3D-2D的重投影誤差來獲得姿態
```
Optimizer::PoseOptimization(&mCurrentFrame);
```
5. 剔除優化後的匹配點中的外點
```
//之所以在優化之後才剔除外點，是因為在優化的過程中就有了對這些外點的標記
int nmatchesMap = 0;
for(int i =0; i<mCurrentFrame.N; i++)
{
if(mCurrentFrame.mvpMapPoints[i])
{
//如果對應到的某個特徵點是外點
if(mCurrentFrame.mvbOutlier[i])
{
//清除它在當前幀中存在過的痕跡
MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
mCurrentFrame.mvbOutlier[i]=false;
pMP->mbTrackInView = false;
pMP->mnLastFrameSeen = mCurrentFrame.mnId;
nmatches--;
}
else if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)
//匹配的內點計數++
nmatchesMap++;
}
}
// 跟踪成功的數目超過10才認為跟踪成功，否則跟踪失敗
return nmatchesMap>=10;
```
### Tracking::CheckReplacedInLastFrame()
檢查上一幀的地圖點是否被替換
在[Tracking::Track()](#Tracking::Track())中引用(閉環線程)
#### 函式流程
1. 地圖點是否需要替換
2. 替換地圖點
#### 流程詳解
1. 地圖點是否需要替換
```
for(int i =0; i<mLastFrame.N; i++)
{
MapPoint* pMP = mLastFrame.mvpMapPoints[i];
if(pMP)
{
MapPoint* pRep = pMP->GetReplaced();
```
跳到[MapPoint::GetReplaced()](#MapPoint::GetReplaced())
2. 替換地圖點
```
if(pRep)
{
mLastFrame.mvpMapPoints[i] = pRep;
}
}
}
```
### Tracking::TrackWithMotionModel()
根據恆定速度模型用上一幀地圖點來對當前幀進行跟踪
在[Tracking::Track()](#Tracking::Track())中引用
#### 函式流程
1. 更新上一幀的姿態；對於雙目或RGB-D相機，還會根據深度值生成臨時地圖點
2. 根據之前估計的速度，用恆速模型得到當前幀的初始姿態。
3. 用上一幀地圖點進行投影匹配，如果匹配點不夠，則擴大搜索半徑再來一次
4. 利用3D-2D投影關係，優化當前幀姿態
5. 剔除地圖點中外點
6. 匹配超過10個點就認為跟踪成功
#### 流程詳解
1. 更新上一幀的姿態；對於雙目或RGB-D相機，還會根據深度值生成臨時地圖點
```
// 最小距離 < 0.9*次小距離 匹配成功，檢查旋轉
ORBmatcher matcher(0.9,true);
UpdateLastFrame();
```
ORBmatcher matcher(): 跳到[ORBmatcher::ORBmatcher()](#ORBmatcher::ORBmatcher())
UpdateLastFrame(): 跳到[Tracking::UpdateLastFrame()](#Tracking::UpdateLastFrame())
2. 根據之前估計的速度，用恆速模型得到當前幀的初始姿態。
```
//根據恆速模型估計當前幀初始姿態(mVelocity:上一幀到這一幀的變換；mLastFrame.mTcw:上一幀的姿態)
mCurrentFrame.SetPose(mVelocity*mLastFrame.mTcw);
// 清空當前幀的地圖點
fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast<MapPoint*>(NULL));
// Project points seen in previous frame
// 設置特徵匹配過程中的搜索半徑
int th;
if(mSensor!=System::STEREO)
th=15;//單目誤差比較大，範圍大一點
else
th=7;//雙目
```
3. 用上一幀地圖點進行投影匹配，如果匹配點不夠，則擴大搜索半徑再來一次
```
int nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,th,mSensor==System::MONOCULAR);
// If few matches, uses a wider window search
// 如果匹配點太少，則擴大搜索半徑再來一次
if(nmatches<20)
{
fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast<MapPoint*>(NULL));
nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,2*th,mSensor==System::MONOCULAR); // 2*th
}
// 如果還是不能夠獲得足夠的匹配點,那麼就認為跟踪失敗
if(nmatches<20)
return false;
```
跳到[ORBmatcher::SearchByProjection()](#ORBmatcher::SearchByProjection())
4. 利用3D-2D投影關係，優化當前幀姿態
```
// Optimize frame pose with all matches
Optimizer::PoseOptimization(&mCurrentFrame);
```
5. 剔除地圖點中外點
```
// Discard outliers
int nmatchesMap = 0;
for(int i =0; i<mCurrentFrame.N; i++)
{
if(mCurrentFrame.mvpMapPoints[i])
{
if(mCurrentFrame.mvbOutlier[i])
{
// 如果優化後判斷某個地圖點是外點，清除它的所有關係
MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
mCurrentFrame.mvbOutlier[i]=false;
pMP->mbTrackInView = false;
pMP->mnLastFrameSeen = mCurrentFrame.mnId;
nmatches--;
}
else if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)
// 累加成功匹配到的地圖點數目
nmatchesMap++;
}
}
if(mbOnlyTracking)
{
// 純定位模式下：如果成功追踪的地圖點非常少,那麼這裡的mbVO標誌就會置位
mbVO = nmatchesMap<10;
return nmatches>20;
}
```
6. 匹配超過10個點就認為跟踪成功
```
return nmatchesMap>=10;
```
### Tracking::Relocalization()
重定位模式
在[Tracking::Track()](#Tracking::Track())中引用
#### 函式流程
1. 計算當前幀特徵點的詞袋向量
2. 用詞袋找到與當前幀相似的候選關鍵幀
3. 遍歷所有的候選關鍵幀，通過詞袋進行快速匹配，用匹配結果初始化 PnP Solver
4. 通過一系列操作,直到找到能夠匹配上的關鍵幀
4.1. 通過EPnP算法估計姿態，迭代5次
4.2. 如果EPnP 計算出了位姿，對內點進行BA優化
4.3. 如果内点较少，则通过投影的方式对之前未匹配的点进行匹配，再进行优化求解
4.4. 如果BA后内点数还是比较少(<50)但是还不至于太少(>30)，可以挽救一下, 最后垂死挣扎
#### 流程詳解
1. 計算當前幀特徵點的詞袋向量
```
mCurrentFrame.ComputeBoW();
// Relocalization is performed when tracking is lost
// Track Lost: Query KeyFrame Database for keyframe candidates for relocalisation
```
2. 用詞袋找到與當前幀相似的候選關鍵幀
```
vector<KeyFrame*> vpCandidateKFs = mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame);
// 如果沒有候選關鍵幀，則退出
if(vpCandidateKFs.empty())
return false;
const int nKFs = vpCandidateKFs.size();
// We perform first an ORB matching with each candidate
// If enough matches are found we setup a PnP solver
ORBmatcher matcher(0.75,true);
//每個關鍵幀的解算器
vector<PnPsolver*> vpPnPsolvers;
vpPnPsolvers.resize(nKFs);
//每個關鍵幀和當前幀中特徵點的匹配關係
vector<vector<MapPoint*> > vvpMapPointMatches;
vvpMapPointMatches.resize(nKFs);
//放棄某個關鍵幀的標記
vector<bool> vbDiscarded;
vbDiscarded.resize(nKFs);
//有效的候選關鍵幀數目
int nCandidates=0;
```
跳到[KeyFrameDatabase::DetectRelocalizationCandidates()](#KeyFrameDatabase::DetectRelocalizationCandidates())
3. 遍歷所有的候選關鍵幀，通過詞袋進行快速匹配，用匹配結果初始化 PnP Solver
```
for(int i=0; i<nKFs; i++)
{
KeyFrame* pKF = vpCandidateKFs[i]; //候選關鍵幀
if(pKF->isBad())
vbDiscarded[i] = true;
else
{
// 當前幀和候選關鍵幀用BoW進行快速匹配，匹配結果記錄在 vvpMapPointMatches ，nmatches表示匹配的數目
int nmatches = matcher.SearchByBoW(pKF,mCurrentFrame,vvpMapPointMatches[i]);
// 如果和當前幀的匹配數小於15,那麼只能放棄這個關鍵幀
if(nmatches<15)
{
vbDiscarded[i] = true;
continue;
}
else
{
// 如果匹配數目夠用，用匹配結果初始化EPnPsolver
// 為什麼用EPnP? 因為計算複雜度低，精度高
PnPsolver* pSolver = new PnPsolver(mCurrentFrame,vvpMapPointMatches[i]);
pSolver->SetRansacParameters(
0.99, //用於計算RANSAC迭代次數理論值的概率
10, //最小內點數, 但是要注意在程序中實際上是min(給定最小內點數,最小集,內點數理論值),不一定使用這個
300, //最大迭代次數
4, //最小集(求解這個問題在一次採樣中所需要採樣的最少的點的個數,對於Sim3是3,EPnP是4),參與到最小內點數的確定過程中
0.5, //這個是表示(最小內點數/樣本總數);實際上的RANSAC正常退出的時候所需要的最小內點數其實是根據這個量來計算得到的
5.991); // 自由度為2的卡方檢驗的閾值,程序中還會根據特徵點所在的圖層對這個閾值進行縮放
vpPnPsolvers[i] = pSolver;
nCandidates++;
}
}
}
// Alternatively perform some iterations of P4P RANSAC
// Until we found a camera pose supported by enough inliers
// 這裡的 P4P RANSAC是Epnp，每次迭代需要4個點
// 是否已經找到相匹配的關鍵幀的標誌
bool bMatch = false;
ORBmatcher matcher2(0.9,true);
```
4. 通過一系列操作,直到找到能夠匹配上的關鍵幀
```
while(nCandidates>0 && !bMatch)
{
//遍歷當前所有的候選關鍵幀
for(int i=0; i<nKFs; i++)
{
// 忽略放棄的
if(vbDiscarded[i])
continue;
//內點標記
vector<bool> vbInliers;
//內點數
int nInliers;
// 表示RANSAC已經沒有更多的迭代次數可用 -- 也就是說數據不夠好，RANSAC也已經盡力了。 。 。
bool bNoMore;
// Step 4.1：通過EPnP算法估計姿態，迭代5次
PnPsolver* pSolver = vpPnPsolvers[i];
cv::Mat Tcw = pSolver->iterate(5,bNoMore,vbInliers,nInliers);
// If Ransac reachs max. iterations discard keyframe
// bNoMore 為true 表示已經超過了RANSAC最大迭代次數，就放棄當前關鍵幀
if(bNoMore)
{
vbDiscarded[i]=true;
nCandidates--;
}
// If a Camera Pose is computed, optimize
if(!Tcw.empty())
{
// Step 4.2：如果EPnP 計算出了位姿，對內點進行BA優化
Tcw.copyTo(mCurrentFrame.mTcw);
// EPnP 裡RANSAC後的內點的集合
set<MapPoint*> sFound;
const int np = vbInliers.size();
//遍歷所有內點
for(int j=0; j<np; j++)
{
if(vbInliers[j])
{
mCurrentFrame.mvpMapPoints[j]=vvpMapPointMatches[i][j];
sFound.insert(vvpMapPointMatches[i][j]);
}
else
mCurrentFrame.mvpMapPoints[j]=NULL;
}
// 只優化位姿,不優化地圖點的坐標，返回的是內點的數量
int nGood = Optimizer::PoseOptimization(&mCurrentFrame);
// 如果優化之後的內點數目不多，跳過了當前候選關鍵幀,但是卻沒有放棄當前幀的重定位
if(nGood<10)
continue;
// 刪除外點對應的地圖點
for(int io =0; io<mCurrentFrame.N; io++)
if(mCurrentFrame.mvbOutlier[io])
mCurrentFrame.mvpMapPoints[io]=static_cast<MapPoint*>(NULL);
// If few inliers, search by projection in a coarse window and optimize again
// Step 4.3：如果內點較少，則通過投影的方式對之前未匹配的點進行匹配，再進行優化求解
// 前面的匹配關係是用詞袋匹配過程得到的
if(nGood<50)
{
// 通過投影的方式將關鍵幀中未匹配的地圖點投影到當前幀中, 生成新的匹配
int nadditional = matcher2.SearchByProjection(
mCurrentFrame, //當前幀
vpCandidateKFs[i], //關鍵幀
sFound, //已經找到的地圖點集合，不會用於PNP
10, //窗口閾值，會乘以金字塔尺度
100); //匹配的ORB描述子距離應該小於這個閾值
// 如果通過投影過程新增了比較多的匹配特徵點對
if(nadditional+nGood>=50)
{
// 根據投影匹配的結果，再次採用3D-2D pnp BA優化位姿
nGood = Optimizer::PoseOptimization(&mCurrentFrame);
// If many inliers but still not enough, search by projection again in a narrower window
// the camera has been already optimized with many points
// Step 4.4：如果BA後內點數還是比較少(<50)但是還不至於太少(>30)，可以挽救一下, 最後垂死掙扎
// 重新執行上一步 4.3的過程，只不過使用更小的搜索窗口
// 這裡的位姿已經使用了更多的點進行了優化,應該更準，所以使用更小的窗口搜索
if(nGood>30 && nGood<50)
{
// 用更小窗口、更嚴格的描述子閾值，重新進行投影搜索匹配
sFound.clear();
for(int ip =0; ip<mCurrentFrame.N; ip++)
if(mCurrentFrame.mvpMapPoints[ip])
sFound.insert(mCurrentFrame.mvpMapPoints[ip]);
nadditional =matcher2.SearchByProjection(
mCurrentFrame, //當前幀
vpCandidateKFs[i], //候選的關鍵幀
sFound, //已經找到的地圖點，不會用於PNP
3, //新的窗口閾值，會乘以金字塔尺度
64); //匹配的ORB描述子距離應該小於這個閾值
// Final optimization
// 如果成功挽救回來，匹配數目達到要求，最後BA優化一下
if(nGood+nadditional>=50)
{
nGood = Optimizer::PoseOptimization(&mCurrentFrame);
//更新地圖點
for(int io =0; io<mCurrentFrame.N; io++)
if(mCurrentFrame.mvbOutlier[io])
mCurrentFrame.mvpMapPoints[io]=NULL;
}
//如果還是不能夠滿足就放棄了
}
}
}
// If the pose is supported by enough inliers stop ransacs and continue
// 如果對於當前的候選關鍵幀已經有足夠的內點(50個)了,那麼就認為重定位成功
if(nGood>=50)
{
bMatch = true;
// 只要有一個候選關鍵幀重定位成功，就退出循環，不考慮其他候選關鍵幀了
break;
}
}
}//一直運行,知道已經沒有足夠的關鍵幀,或者是已經有成功匹配上的關鍵幀
}
// 折騰了這麼久還是沒有匹配上，重定位失敗
if(!bMatch)
{
return false;
}
else
{
// 如果匹配上了,說明當前幀重定位成功了(當前幀已經有了自己的位姿)
// 記錄成功重定位幀的id，防止短時間多次重定位
mnLastRelocFrameId = mCurrentFrame.mnId;
return true;
}
```
### Tracking::TrackLocalMap()
局部地圖跟蹤，跟蹤完後的後處理。用當前幀的兩級共視關鍵幀，優化姿態。
在[Tracking::Track()](#Tracking::Track())中引用
#### 函式流程
1. 更新局部關鍵幀 mvpLocalKeyFrames 和局部地圖點 mvpLocalMapPoints
2. 在局部地圖中查找與當前幀匹配的地圖點
3. 更新局部所有局部地圖點後對姿態再次優化
4. 更新當前幀的局部地圖點被觀測程度，並統計跟踪局部地圖的效果
5. 根據跟蹤匹配數目及閉環情況，決定是否跟踪成功
#### 流程詳解
1. 更新局部關鍵幀 mvpLocalKeyFrames 和局部地圖點 mvpLocalMapPoints
```
UpdateLocalMap();
```
跳到[Tracking::UpdateLocalMap()](#Tracking::UpdateLocalMap())
2. 在局部地圖中查找與當前幀匹配的地圖點
```
SearchLocalPoints();
```
跳到[Tracking::SearchLocalPoints()](#Tracking::SearchLocalPoints())
3. 更新局部所有局部地圖點後對姿態再次優化
```
// 在這個函數之前，在Relocalization、TrackReferenceKeyFrame、TrackWithMotionModel中都有位姿優化，
Optimizer::PoseOptimization(&mCurrentFrame);
mnMatchesInliers = 0;
```
4. 更新當前幀的局部地圖點被觀測程度，並統計跟踪局部地圖的效果
```
//遍歷當前幀的所有地圖點
for(int i=0; i<mCurrentFrame.N; i++)
{
if(mCurrentFrame.mvpMapPoints[i])
{
// 如果當前幀的地圖點可以被當前幀觀測到
if(!mCurrentFrame.mvbOutlier[i])
{
//觀測統計量加1
mCurrentFrame.mvpMapPoints[i]->IncreaseFound();
//是否為純定位
if(!mbOnlyTracking)
{
// 該地圖點被其它關鍵幀觀測到過
if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)
mnMatchesInliers++;
}
else
// 記錄當前幀跟踪到的MapPoints，用於統計跟踪效果
mnMatchesInliers++;
}
else if(mSensor==System::STEREO)
mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint*>(NULL);
}
}
```
5. 根據跟蹤匹配數目及閉環情況，決定是否跟踪成功
```
if(mCurrentFrame.mnId<mnLastRelocFrameId+mMaxFrames && mnMatchesInliers<50)
return false;
// 正常狀態只要跟蹤的地圖點>30就成功
if(mnMatchesInliers<30)
return false;
else
return true;
```
### Tracking::UpdateLocalMap()
更新LocalMap
在[Tracking::TrackLocalMap()](#Tracking::TrackLocalMap())中引用
#### 函式流程
1. 設置參考地圖點，用於繪圖顯示局部地圖點
2. 用共視圖來更新局部關鍵幀與局部地圖點
#### 流程詳解
1. 設置參考地圖點，用於繪圖顯示局部地圖點
```
mpMap->SetReferenceMapPoints(mvpLocalMapPoints);
```
mvpLocalMapPoints 在 [Tracking::CreateInitialMapMonocular()](#Tracking::CreateInitialMapMonocular())，[Tracking::UpdateLocalPoints()](#Tracking::UpdateLocalPoints())更新
跳到[Map::SetReferenceMapPoints()](#Map::SetReferenceMapPoints())
2. 用共視圖來更新局部關鍵幀與局部地圖點
```
UpdateLocalKeyFrames();
UpdateLocalPoints();
```
UpdateLocalKeyFrames()跳到 : [Tracking::UpdateLocalKeyFrames()](#Tracking::UpdateLocalKeyFrames())
UpdateLocalPoints()跳到 : [Tracking::UpdateLocalPoints()](#Tracking::UpdateLocalPoints())
### Tracking::UpdateLocalKeyFrames()
更新局部關鍵幀
父關鍵幀：和當前關鍵幀共視程度最高的關鍵幀
子關鍵幀：是上述父關鍵幀的子關鍵幀
在[KeyFrame::UpdateConnections()](#KeyFrame::UpdateConnections()) 裡確定關鍵幀的父子關係（當前幀必須是關鍵幀）
在[Tracking::UpdateLocalMap()](#Tracking::UpdateLocalMap())中引用
#### 函式流程
1. 記錄所有能觀測到當前幀地圖點的關鍵幀
2. 將一級共視關鍵幀加入局部關鍵幀
3. 遍歷一級共視關鍵幀，尋找更多的局部關鍵幀
4. 將一級共視關鍵幀的子關鍵幀作為局部關鍵幀（將鄰居的孩子們拉攏入夥）
5. 將一級共視關鍵幀的父關鍵幀作為局部關鍵幀（將鄰居的父母們拉攏入夥）
6. 更新當前幀的參考關鍵幀，與自己共視程度最高的關鍵幀作為參考關鍵幀
#### 流程詳解
1. 記錄所有能觀測到當前幀地圖點的關鍵幀
```
// 遍歷當前幀的MapPoints
map<KeyFrame*,int> keyframeCounter;
for(int i=0; i<mCurrentFrame.N; i++)
{
if(mCurrentFrame.mvpMapPoints[i])
{
//取出當前幀的地圖點
MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
if(!pMP->isBad())
{
// 能觀測到當前幀MapPoints的關鍵幀
const map<KeyFrame*,size_t> observations = pMP->GetObservations();
//遍歷能觀測到的關鍵幀
for(map<KeyFrame*,size_t>::const_iterator it=observations.begin(), itend=observations.end(); it!=itend; it++)
// map[key] = value，當要插入的鍵存在時，會覆蓋鍵對應的原來的值。如果鍵不存在，則添加一組鍵值對
// it->first 是地圖點看到的關鍵幀，同一個關鍵幀看到的地圖點會累加到該關鍵幀計數
// 所以最後keyframeCounter 第一個參數表示某個關鍵幀，第2個參數表示該關鍵幀看到了多少當前幀(mCurrentFrame)的地圖點，也就是共視程度
keyframeCounter[it->first]++;
}
else
{
mCurrentFrame.mvpMapPoints[i]=NULL;
}
}
}
if(keyframeCounter.empty())
return;
// 存儲具有最多觀測次數（max）的關鍵幀
int max=0;
KeyFrame* pKFmax= static_cast<KeyFrame*>(NULL);
```
2. 將一級共視關鍵幀加入局部關鍵幀
```
// 先清空局部關鍵幀
mvpLocalKeyFrames.clear();
// 先申請3倍內存，不夠後面再加
mvpLocalKeyFrames.reserve(3*keyframeCounter.size());
//能觀測到當前幀地圖點的關鍵幀作為局部關鍵幀 (將鄰居拉攏入夥)
//遍歷所有關鍵幀(first:關鍵幀; second: )
for(map<KeyFrame*,int>::const_iterator it=keyframeCounter.begin(), itEnd=keyframeCounter.end(); it!=itEnd; it++)
{
KeyFrame* pKF = it->first;
// 如果設定為要刪除的，跳過
if(pKF->isBad())
continue;
// 尋找具有最大觀測數目的關鍵幀
if(it->second>max)
{
max=it->second;
pKFmax=pKF;
}
// 添加到局部關鍵幀的列表裡
mvpLocalKeyFrames.push_back(it->first);
// 用該關鍵幀的成員變量mnTrackReferenceForFrame 記錄當前幀的id
// 表示它已經是當前幀的局部關鍵幀了，可以防止重複添加局部關鍵幀
pKF->mnTrackReferenceForFrame = mCurrentFrame.mnId;
}
```
3. 遍歷一級共視關鍵幀，尋找更多的局部關鍵幀
```
for(vector<KeyFrame*>::const_iterator itKF=mvpLocalKeyFrames.begin(), itEndKF=mvpLocalKeyFrames.end(); itKF!=itEndKF; itKF++)
{
// Limit the number of keyframes
// 處理的局部關鍵幀不超過80幀
if(mvpLocalKeyFrames.size()>80)
break;
KeyFrame* pKF = *itKF;
// 一級共視關鍵幀的共視（前10個）關鍵幀，稱為二級共視關鍵幀（將鄰居的鄰居拉攏入夥）
// 如果共視幀不足10幀,那麼就返回所有具有共視關係的關鍵幀
const vector<KeyFrame*> vNeighs = pKF->GetBestCovisibilityKeyFrames(10);
// vNeighs 是按照共視程度從大到小排列
for(vector<KeyFrame*>::const_iterator itNeighKF=vNeighs.begin(), itEndNeighKF=vNeighs.end(); itNeighKF!=itEndNeighKF; itNeighKF++)
{
KeyFrame* pNeighKF = *itNeighKF;
if(!pNeighKF->isBad())
{
// mnTrackReferenceForFrame防止重複添加局部關鍵幀
if(pNeighKF->mnTrackReferenceForFrame!=mCurrentFrame.mnId)
{
mvpLocalKeyFrames.push_back(pNeighKF);
pNeighKF->mnTrackReferenceForFrame=mCurrentFrame.mnId;
// 找到一個就直接跳出for循環
break;
}
}
}
```
4. 將一級共視關鍵幀的子關鍵幀作為局部關鍵幀（將鄰居的孩子們拉攏入夥）
```
const set<KeyFrame*> spChilds = pKF->GetChilds();
for(set<KeyFrame*>::const_iterator sit=spChilds.begin(), send=spChilds.end(); sit!=send; sit++)
{
KeyFrame* pChildKF = *sit;
if(!pChildKF->isBad())
{
if(pChildKF->mnTrackReferenceForFrame!=mCurrentFrame.mnId)
{
mvpLocalKeyFrames.push_back(pChildKF);
pChildKF->mnTrackReferenceForFrame=mCurrentFrame.mnId;
// 找到一個就直接跳出for循環
break;
}
}
}
```
5. 將一級共視關鍵幀的父關鍵幀作為局部關鍵幀（將鄰居的父母們拉攏入夥）
```
KeyFrame* pParent = pKF->GetParent();
if(pParent)
{
// mnTrackReferenceForFrame防止重複添加局部關鍵幀
if(pParent->mnTrackReferenceForFrame!=mCurrentFrame.mnId)
{
mvpLocalKeyFrames.push_back(pParent);
pParent->mnTrackReferenceForFrame=mCurrentFrame.mnId;
//! 感覺是個bug！如果找到父關鍵幀會直接跳出整個循環
break;
}
}
}
```
6. 更新當前幀的參考關鍵幀，與自己共視程度最高的關鍵幀作為參考關鍵幀
```
if(pKFmax)
{
mpReferenceKF = pKFmax;
mCurrentFrame.mpReferenceKF = mpReferenceKF;
}
```
### Tracking::UpdateLocalPoints()
更新局部地圖點
在[Tracking::UpdateLocalMap()](#Tracking::UpdateLocalMap())中引用
#### 函式流程
1. 清空局部地圖點
2. 將局部關鍵幀的地圖點添加到mvpLocalMapPoints
#### 流程詳解
1. 清空局部地圖點
```
mvpLocalMapPoints.clear();
```
2. 將局部關鍵幀的地圖點添加到mvpLocalMapPoints
```
// 遍歷局部關鍵幀 mvpLocalKeyFrames
for(vector<KeyFrame*>::const_iterator itKF=mvpLocalKeyFrames.begin(), itEndKF=mvpLocalKeyFrames.end(); itKF!=itEndKF; itKF++)
{
KeyFrame* pKF = *itKF;
//取出關鍵幀的對應匹配地圖點
const vector<MapPoint*> vpMPs = pKF->GetMapPointMatches();
//遍歷關鍵幀的對應匹配地圖點
for(vector<MapPoint*>::const_iterator itMP=vpMPs.begin(), itEndMP=vpMPs.end(); itMP!=itEndMP; itMP++)
{
MapPoint* pMP = *itMP;
if(!pMP)
continue;
// 用該地圖點的成員變數mnTrackReferenceForFrame 記錄當前幀的id
// 表示它已經是當前幀的局部地圖點了，可以防止重複添加局部地圖點
if(pMP->mnTrackReferenceForFrame==mCurrentFrame.mnId)
continue;
if(!pMP->isBad())
{
mvpLocalMapPoints.push_back(pMP);
pMP->mnTrackReferenceForFrame=mCurrentFrame.mnId;
}
}
}
```
### Tracking::SearchLocalPoints()
用局部地圖點進行投影匹配，得到更多的匹配關係
在[Tracking::TrackLocalMap()](#Tracking::TrackLocalMap())中引用
#### 函式流程
1. 遍歷當前幀的地圖點，標記這些地圖點不參與之後的投影搜索匹配
2. 判斷所有局部地圖點中除當前幀地圖點外的點，是否在當前幀視野範圍內
3. 如果需要進行投影匹配的點的數目大於0，就進行投影匹配，增加更多的匹配關係
#### 流程詳解
1. 遍歷當前幀的地圖點，標記這些地圖點不參與之後的投影搜索匹配
```
for(vector<MapPoint*>::iterator vit=mCurrentFrame.mvpMapPoints.begin(), vend=mCurrentFrame.mvpMapPoints.end(); vit!=vend; vit++)
{
MapPoint* pMP = *vit;
if(pMP)
{
if(pMP->isBad())
{
*vit = static_cast<MapPoint*>(NULL);
}
else
{
// 更新能觀測到該點的幀數加1(被當前幀觀測了)
pMP->IncreaseVisible();
// 標記該點被當前幀觀測到
pMP->mnLastFrameSeen = mCurrentFrame.mnId;
// 標記該點在後面搜索匹配時不被投影，因為已經有匹配了
pMP->mbTrackInView = false;
}
}
}
```
2. 判斷所有局部地圖點中除當前幀地圖點外的點，是否在當前幀視野範圍內
```
// 準備進行投影匹配的點的數目
int nToMatch=0;
for(vector<MapPoint*>::iterator vit=mvpLocalMapPoints.begin(), vend=mvpLocalMapPoints.end(); vit!=vend; vit++)
{
MapPoint* pMP = *vit;
// 已經被當前幀觀測到的地圖點肯定在視野範圍內，跳過
if(pMP->mnLastFrameSeen == mCurrentFrame.mnId)
continue;
// 跳過壞點
if(pMP->isBad())
continue;
// 判斷地圖點是否在在當前幀視野內
if(mCurrentFrame.isInFrustum(pMP,0.5))
{
// 觀測到該點的幀數加1
pMP->IncreaseVisible();
// 只有在視野範圍內的地圖點才參與之後的投影匹配
nToMatch++;
}
}
```
跳到[Frame::isInFrustum()](#Frame::isInFrustum)
3. 如果需要進行投影匹配的點的數目大於0，就進行投影匹配，增加更多的匹配關係
```
if(nToMatch>0)
{
ORBmatcher matcher(0.8); //中等的匹配閾值
int th = 1;
if(mSensor==System::RGBD) //RGBD相機輸入的時候,搜索的閾值會變得稍微大一些
th=3;
// 如果不久前進行過重定位，那麼進行一個更加寬泛的搜索，閾值需要增大
if(mCurrentFrame.mnId<mnLastRelocFrameId+2)
th=5;
// 投影匹配得到更多的匹配關係
matcher.SearchByProjection(mCurrentFrame,mvpLocalMapPoints,th);
}
```
Sign in with Wallet
Connect another wallet