单目、双目和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