單目+IMU模式下，前面的一些配置完成后，處理第一幀圖像時：

1、每幀圖像都會調(diào)用該函數(shù)：

clahe->apply(im,im);

目的：使灰度直方圖分布較為均勻，從而使整體對比度更強(qiáng)，便于后面特征點(diǎn)的提取等工作；

2、第一幀圖像（ni=0）時無IMU數(shù)據(jù)（vImuMeas容器為空），進(jìn)入下面的這個函數(shù)：

SLAM.TrackMonocular(im, tframe, vImuMeas);

該函數(shù)先是對一些狀態(tài)進(jìn)行判斷，做出相應(yīng)的處理。當(dāng)有IMU數(shù)據(jù)時，將IMU數(shù)據(jù)存儲到隊(duì)列mlQueueImuData中，其主要任務(wù)是計(jì)算當(dāng)前幀相機(jī)的位姿，通過調(diào)用下面這個函數(shù)來完成；

Sophus::SE3f Tcw = mpTracker->GrabImageMonocular(imToFeed, timestamp, filename);

3、先是將彩色圖像轉(zhuǎn)為單通道灰度圖像，然后按照是否有IMU和跟蹤狀態(tài)（mState）來構(gòu)造Frame類，此時跟蹤狀態(tài)mState==NO_IMAGES_YET。當(dāng)前幀構(gòu)造Frame類時需要提取5*1500個特征點(diǎn)，初始化成功后只需要提取1500個特征點(diǎn)；

4、Frame構(gòu)造函數(shù)中先是完成幀的ID賦值：

mnId = nNextId++;? ? // 先賦值再自增1

然后將金字塔的相關(guān)參數(shù)賦值給成員變量，接下來是對圖像進(jìn)行特征點(diǎn)提取，這時應(yīng)用的是仿函數(shù)，運(yùn)行的函數(shù)為：

int ORBextractor::operator()( InputArray _image, InputArray _mask, vector<KeyPoint>& _keypoints,
                                OutputArray _descriptors, std::vector<int> &vLappingArea)

上面仿函數(shù)一些注意的點(diǎn)是：

1）使用四叉樹的方式計(jì)算每層圖像的特征點(diǎn)并進(jìn)行均勻分配，均勻的特征點(diǎn)可以提高位姿計(jì)算精度；2）在計(jì)算特征點(diǎn)的描述子之前先對圖像進(jìn)行高斯模糊，主要是為了避免圖像噪聲的影響；

3）最后需要將金字塔中非0層的特征點(diǎn)坐標(biāo)恢復(fù)到原圖像（第0層）的坐標(biāo)系下，至于界限的意思我沒有看懂，后面也不清楚有什么用。

5、提取完特征點(diǎn)后先用OpenCV的矯正函數(shù)對特征點(diǎn)去畸變，然后計(jì)算去畸變后圖像邊界（此過程一般是在第一幀或者是相機(jī)標(biāo)定參數(shù)發(fā)生變化之后進(jìn)行），最后將特征點(diǎn)分配到圖像網(wǎng)格中（每個固定大小的網(wǎng)格記錄其里面特征點(diǎn)的索引），目的是加快后期的特征點(diǎn)匹配。

當(dāng)?shù)谝粠瑘D像進(jìn)Tracking::Track()時：

1、此時mpLastKeyFrame=NULL、mbCreatedMap=false，將跟蹤狀態(tài)mState更新為NOT_INITIALIZED；

2、進(jìn)行單目初始化，此時mbReadyToInitializate=false，創(chuàng)建初始化關(guān)鍵幀要求當(dāng)前幀的特征點(diǎn)數(shù)多于100個；

3、當(dāng)前幀滿足創(chuàng)建初始化關(guān)鍵幀條件后，更新mInitialFrame、mLastFrame和用于記錄"當(dāng)前幀"所有特征點(diǎn)的mvbPrevMatched，將容器mvIniMatches的所有值都設(shè)置為-1，該容器的長度為當(dāng)前幀特征點(diǎn)的個數(shù)；

4、mpImuPreintegratedFromLastKF的Bias設(shè)置為0，*mpImuCalib是在讀取IMU的yaml文件時設(shè)置的。初始化當(dāng)前幀預(yù)積分mCurrentFrame.mpImuPreintegrated = mpImuPreintegratedFromLastKF，最后將mbReadyToInitializate設(shè)為true，意為已經(jīng)創(chuàng)建初始化參考幀，當(dāng)前幀的變換矩陣Tcw為單位矩陣。

當(dāng)?shù)诙瑘D像進(jìn)Tracking::Track()時：

1、此時有IMU數(shù)據(jù)，mState=NOT_INITIALIZED、mpLastKeyFrame=NULL，當(dāng)有異常的時間戳?xí)r，需要進(jìn)行處理；

2、對IMU數(shù)據(jù)進(jìn)行預(yù)積分，對每幀IMU數(shù)據(jù)有兩種預(yù)積分，一種是相對于上一幀，另一種是相對于上一個關(guān)鍵幀；

注：在Frame構(gòu)造函數(shù)中將當(dāng)前幀的mpPrevFrame=*mLastFrame，陀螺儀輸出角速度（rad/s）,加速度計(jì)輸出加速度（m/s2）。

void Tracking::PreintegrateIMU()  // 要求當(dāng)前幀的上一幀（mpPrevFrame）不為NULL且mlQueueImuData里有數(shù)據(jù)

該函數(shù)主要進(jìn)行的工作是：

1）進(jìn)入while(true)中，將符合下面這2個條件的IMU數(shù)據(jù)存入mvImuFromLastFrame容器中；

條件：a.當(dāng)前IMU時間戳 ≥ 上一幀時間戳 - mImuPer（0.001s）

b.當(dāng)前IMU時間戳 ＜ 當(dāng)前幀時間戳 - mImuPer（0.001s）

結(jié)束while(true)的條件：當(dāng)前IMU時間戳不滿足上面的2個條件或mlQueueImuData容器為空；

2）構(gòu)造IMU預(yù)積分處理器（pImuPreintegratedFromLastFrame）,偏置使用上一幀的偏置，每幀的mImuCalib都相同（其是在讀取IMU的yaml文件時設(shè)置的）；

3）第一幀IMU數(shù)據(jù)和最后一幀IMU數(shù)據(jù)需要單獨(dú)處理，中間的IMU數(shù)據(jù)直接處理，主要就是計(jì)算前后2幀IMU的平均加速度（acc）、平均角速度（angVel）和兩幀之間的時間差（tstep0）；

4）mpImuPreintegratedFromLastKF是在Tracking::ParseIMUParamFile(cv::FileStorage &fSettings)中設(shè)置的，其中偏置都設(shè)置為0；

5）最開始時（即當(dāng)前幀的上一幀）的dR為單位矩陣，dP、dV均為0。主要計(jì)算當(dāng)前幀相對于上一幀和上一關(guān)鍵幀之間的dP、dV和dR，計(jì)算JPa、JPg、JVa、JVg和JRg，最后計(jì)算出該時間段的協(xié)方差矩陣C；

mpImuPreintegratedFromLastKF->IntegrateNewMeasurement(acc, angVel, tstep);
pImuPreintegratedFromLastFrame->IntegrateNewMeasurement(acc, angVel, tstep);

6）更新當(dāng)前幀的mpImuPreintegratedFrame（基于上一幀的IMU預(yù)積分處理器指針）、mpImuPreintegrated（基于上一關(guān)鍵幀的IMU預(yù)積分處理器指針）和mpLastKeyFrame（上一個關(guān)鍵幀指針），最后將當(dāng)前幀的mbImuPreintegrated設(shè)為true，意為當(dāng)前幀完成了預(yù)積分。

當(dāng)?shù)诙瑘D像進(jìn)Tracking::MonocularInitialization()時：

1、此時單目初始化器（mInitialFrame）已創(chuàng)建，mbReadyToInitializate=true，要求當(dāng)前幀特征點(diǎn)超過100個或mSensor == System::IMU_MONOCULAR且當(dāng)前幀上一幀的時間戳比單目初始化參考幀的時間戳大于1秒；

2、滿足上面的條件后對當(dāng)前幀與初始化參考幀進(jìn)行特征點(diǎn)匹配，mvIniMatches容器的索引為初始化參考幀的特征點(diǎn)序號，若初始化參考幀中的該特征點(diǎn)與當(dāng)前幀有匹配，則該容器對應(yīng)的值為當(dāng)前幀特征點(diǎn)序號，否則為-1；

3、先是對兩幀中的特征點(diǎn)進(jìn)行校正，然后通過RANSAC（循環(huán)200次）雙線程計(jì)算基礎(chǔ)矩陣F和單應(yīng)矩陣H，分別記錄得分最高的SH和SF對應(yīng)的H和F矩陣，最后通過條件來判斷是通過基礎(chǔ)矩陣F還是單應(yīng)矩陣H進(jìn)行重建；

4、重建的意思是通過F或H恢復(fù)R、t以及滿足相應(yīng)條件恢復(fù)出的三維點(diǎn)的個數(shù)，最后輸出滿足相關(guān)條件最好的Tcw、重建的地圖點(diǎn)和初始化參考幀中哪些特征點(diǎn)成功重建了地圖點(diǎn)（存入容器vbTriangulated中）；

5、將初始化參考幀作為世界坐標(biāo)系，因此第一幀變換矩陣為單位矩陣，最后設(shè)置當(dāng)前幀位姿（Tcw）。

Tracking::CreateInitialMapMonocular()函數(shù)：

1、將初始化參考幀和當(dāng)前幀都設(shè)置為關(guān)鍵幀，將初始化參考幀的mpImuPreintegrated（基于上一關(guān)鍵幀的IMU預(yù)處理器指針）設(shè)置為NULL；

2、將初始化參考關(guān)幀和當(dāng)前關(guān)鍵幀的描述子轉(zhuǎn)為BoW，主要是完成mBowVec和mFeatVec容器的賦值，其中mBowVec容器的數(shù)據(jù)類型為map，第一個數(shù)據(jù)為單詞id，第二個值為權(quán)重（weight），如果圖像中多個特征點(diǎn)對應(yīng)同一個單詞，則weight會疊加。mFeatVec容器的數(shù)據(jù)類型為map，第一個數(shù)據(jù)為節(jié)點(diǎn)id（L=2時），第二個數(shù)據(jù)類型為vector容器，其里面的內(nèi)容為該節(jié)點(diǎn)id包含的描述子索引，該容器的目的是為后面特征點(diǎn)匹配進(jìn)行加速；注：只有關(guān)鍵幀才會把其的描述子轉(zhuǎn)為Bow

3、用初始化得到的3D點(diǎn)來生成地圖點(diǎn)MapPoints，其中mvIniP3D容器中的數(shù)據(jù)為世界坐標(biāo)系下點(diǎn)的坐標(biāo) ：

1）用3D點(diǎn)（世界坐標(biāo)系下）來構(gòu)造地圖點(diǎn)；

2）為該地圖點(diǎn)添加屬性：

a.更新觀測到該MapPoint的關(guān)鍵幀，容器mObservations中第一個元素為關(guān)鍵幀指針，第二個元素為該地圖點(diǎn)對應(yīng)此關(guān)鍵幀中的特征點(diǎn)索引，同時更新該地圖點(diǎn)的觀測數(shù)目（nObs），雙目和RGB相機(jī)：nObs=nObs+2，單目相機(jī)：nObs=nObs+1；

b.從眾多關(guān)鍵幀中能觀測到該MapPoint的特征點(diǎn)中挑選最有代表性的描述子，地圖點(diǎn)最好的描述子與其它能觀測到該地圖點(diǎn)的特征點(diǎn)的描述子應(yīng)該具有最小的距離中值；注：由于一個MapPoint會被許多相機(jī)觀測到，因此在插入關(guān)鍵幀后，需要判斷是否更新當(dāng)前MapPoint最有代表性的描述子。

c.更新該MapPoint平均觀測方向以及觀測距離的范圍。

3）更新關(guān)鍵幀間的連接關(guān)系，每個邊有一個權(quán)重，邊的權(quán)重是該關(guān)鍵幀與當(dāng)前關(guān)鍵幀公共3D點(diǎn)的個數(shù)；

4）全局BA優(yōu)化，優(yōu)化當(dāng)前關(guān)鍵幀的位姿和MapPoint；

單目初始化之后，得到的初始地圖中的所有點(diǎn)都是局部地圖點(diǎn)。

5）mState = OK，此時單目初始化全部完成。文章來源地址http://www.zghlxwxcb.cn/news/detail-688089.html