單目、雙目和RGB-D視覺SLAM初始化比較

無論單目、雙目還是RGB-D，首先是將從攝像頭或者數據集中讀入的圖像封裝成Frame類型對象：

首先都需要將彩色圖像處理成灰度圖像，繼而將圖片封裝成幀。

(1) 單目

mCurrentFrame = Frame(mImGray, timestamp, mpIniORBextractor, mpORBextractor, mpORBVocabulary, mK, mDistCoef, mbf, mThDepth);

 下面詳細介紹一下單目創建幀的過程，首先來看Frame的數據結構，它有三個構造函數，分別對應單目、雙目和RGB-D相機，重要的成員變量有：

public:　　
　　//字典，用於重定位檢測和閉環檢測 相關成員變量還有BowVec FeatVec
　　ORBVocabulary* mpORBVocabulary;
　　
　　//特征提取器，只有雙目的時候才會使用Right
   //這里注意Tracking結構體中同樣有這兩個ORBextractor類，在主程序初始化Tracking的時候即創建個該成員類，並通過fSettings()函數將ORBextractor.nFeatures、ORBextractor.scaleFactor等參數傳入其中
   //所以在創建Frame類時，直接將Tracking類中的mpORBextractorLeft、mpORBextractorRight傳入frame即可
   ORBextractor* mpORBextractorLeft, *mpORBextracotrRight;   
　　
　　//時間戳
　　double mTimeStamp;
　　//原始關鍵點圖像坐標
　　std::vector<cv::KeyPoint> mvKeys和mvKeysRight
　　//經過矯正模型矯正的關鍵點坐標
　　std::vector<cv::KeyPoint> mvKeysUn;
　　//ORB描述子，每行對應一個描述子
　　cv::Mat mDescriptors, mDescriptorsRight
　　//地圖點，關聯到每個關鍵點
　　std::vector<MapPoint*> mvpMapPoints;
　　//相機變換矩陣T
　　cv::Mat mTcw;
　　//當前幀id
　　long unsigned int mnId;
　　//參考關鍵幀
　　KeyFrame* mpReferenceKF;
private:
　　//旋轉矩陣和平移向量
　　cv::Mat mRcw;
　　cv::Mat mtcw;
　　cv::Mat mRwc;
　　cv::Mat mOw;

 創建幀的關鍵一步是ORB特征提取，ExtractORB()調用了ORBextractor類中的重載操作符void operator()。

ExtractORB(0,imGray);

void Frame::ExtracORB(int flag, const cv::Mat &im)
{
    if(flag==0)
         (*mpORBextractorLeft)(im, cv::Mat(), mvKeys, mDescriptors);
    else
         (*mpORBextractorLeft)(im, cv::Mat(), mvKeysRight, mDescriptorRight);             
}    

ExtractORB()調用了ORBextractor類中的重載操作符void operator()，完成特征提取，提取結果被保存在Frame類的成員變量std::vector<cv:KeyPoint> mvKeys和cv:Mat mDescriptors中。

void ORBextractor::operator()(InputArray_image, InputArray_mask, vector<KeyPoint>& _keypoints, OutputArray _descriptors);

得到關鍵幀之后便進行track()，首先是初始化。單目初始化是連續取兩幀特征點數量超過100的圖像幀，並且匹配點大於100，才可以開始初始化，否則重新接收數據幀。

//調用Initializer類中的初始化函數,只有單目才會調用該類
mpInitailizer->Initialize(mCurrent, mvIniMatches, Rcw, tcw, mvIniP3D, vbTriangulated);

 1)初始化第一步，開啟兩個線程，分別對應兩個運行模型，即通過求取H單應矩陣或F本質矩陣來得到R t，

thread threadH(&Initializer::FindHomography, this, ref(vbMatchesInliersH), ref(SH), ref(H));
thread threadF(&Initializer::FindFundamental, this, ref(vbMatchesInliersF), ref(SF), ref(F));

線程threadH調用Initializer::FindHomography函數，計算單應矩陣H，采用歸一化的直接線性變換(normalized DLT)。線程threadF調用Initializer::FindFundamental函數計算基礎矩陣F，采用方法為8點法。然后對結果進行評估，選擇合適的模型來計算。

// Compute ratio of scores
float RH = SH/(SH+SF);

// Try to reconstruct from homography or fundamental depending on the ratio (0.40-0.45)
if(RH>0.40)
    return ReconstructH(vbMatchesInliersH,H,mK,R21,t21,vP3D,vbTriangulated,1.0,50);
else //if(pF_HF>0.6)
    return ReconstructF(vbMatchesInliersF,F,mK,R21,t21,vP3D,vbTriangulated,1.0,50);

return false;

2）第二步，創建並添加關鍵幀和初始化地圖點

void Tracking::CreateInitialMapMonocular()

//將關鍵幀中加入該地圖點
pKFini->AddMapPoint(pMP,i);
pKFcur->AddMapPoint(pMP,mvIniMatches[i]);

//將兩個關鍵幀加入地圖點中
pMP->AddObservation(pKFini,i);
pMP->AddObservation(pKFcur,mvIniMatches[i]);  

//選擇地圖點描述子
pMP->ComputeDistinctiveDescriptors();  
//計算地圖點深度
pMP->UpdateNormalAndDepth();  
//將地圖點加入mpMap
mpMap->AddMapPoint(pMP);

3) 第三步，優化位姿

Optimizer::GlobalBundleAdjustement(mpMap, 20);

4) 第四步，對相關數據賦值

//更新局部地圖
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);

至此，單目初始化成功！！！

(2) 雙目

mCurrentFrame = Frame(mImGray, imGrayRight, timestamp, mpORBextractorLeft, mpORBextractorRight, mpORBVocabulary,mK, mDistCoef, mbf, mThDepth);

 (3) RGB-D

mCurrentFrame = Frame(mImGray, imDepth, timestamp, mpORBextractorLeft, mpORBVocabulary, mK, mDistCoef, mbf, mThDepth);

 由於雙目和RGB-D相機不需要通過兩個相鄰幀來恢復地圖點深度，所以初始化過程極其相似，只要當前到來幀滿足條件即可開始初始化。初始化步驟如下：

1）第一步，如果滿足條件，創建並添加關鍵幀和初始化地圖點

if(mCurrentFrame.N>500)
   //后來幀都以該幀為參考
   mCurrentFrame.SetPose(cv::Mat::eye(4,4,CV_32F));
   //創建關鍵幀
   KeyFrame* pKFini = new KeyFrame(mCurrentFrame, mpMap, mpKeyFrameDB);
   //將關鍵幀插入地圖
   mpMap->AddKeyFrame(pKFini);
   

//初始化地圖點並關聯關鍵幀
for(int i=0; i<mCurrentFrame.N;i++)
{
    float z = mCurrentFrame.mvDepth[i];
    if(z>0)
    {
        cv::Mat x3D = mCurrentFrame.UnprojectStereo(i);
        MapPoint* pNewMP = new MapPoint(x3D,pKFini,mpMap);
        pNewMP->AddObservation(pKFini,i);
        pKFini->AddMapPoint(pNewMP,i);
        pNewMP->ComputeDistinctiveDescriptors();
        pNewMP->UpdateNormalAndDepth();
        mpMap->AddMapPoint(pNewMP);
        mCurrentFrame.mvpMapPoints[i]=pNewMP;
    }
}

2）第二步，對相關數據賦值

//更新局部地圖
mpLocalMapper->InsertKeyFrame(pKFini);
//更新最后幀
mLastFrame = Frame(mCurrentFrame);
//更新最后關鍵幀
mnLastKeyFrameId=mCurrentFrame.mnId;
mpLastKeyFrame = pKFini;
//更新局部關鍵幀
mvpLocalKeyFrames.push_back(pKFini);
mvpLocalMapPoints=mpMap->GetAllMapPoints();
//更新參考幀
mpReferenceKF = pKFini;
mCurrentFrame.mpReferenceKF = pKFini;

mpMap->SetReferenceMapPoints(mvpLocalMapPoints);

雙目和RGB-D相機初始化即完成！！！

總結單目和雙目、RGB-D相機初始化的不同：

1. 通過不斷學習本人發現，初始化的目的是創建3d地圖點,為后續跟蹤提供初值。而單目通過一幀無法估計深度，所以初始化時需要使用兩幀。

2. 單目算出相機變換矩陣之后，又進行了位姿優化BundleAdjustement。

初始化完成之后，即進入跟蹤狀態TrackWithMotionModel()、TrackReferenceKeyFrame()、Relocalization()等。。。

 

參考文獻：

　　1. http://zhehangt.win/2018/01/11/SLAM/ORBSLAM/ORBSLAM2ORBExtractor/

　　2. https://blog.csdn.net/u010821666/article/details/52915238?locationNum=1&fps=1

　　3. https://www.cnblogs.com/shang-slam/p/6389129.html

　　4. https://blog.csdn.net/u010128736/article/details/53218140