这里是Tracking部分的第二部分,详细讲述各分支的代码及其实现原理。
单目初始化
MonocularInitialization()
目标:从初始的两帧单目图像中,对SLAM系统进行初始化(得到初始两帧的匹配,相机初始位姿,初始MapPoints),以便之后进行跟踪。
方式:并行得计算基础矩阵E和单应矩阵H,恢复出最开始两帧相机位姿;三角化得到MapPoints的深度,获得点云地图。
寻找匹配特征点
单目的初始化有专门的初始化器,只有连续的两帧特征点均>100个才能够成功构建初始化器。
完成前两帧特征点的匹配
ORBmatcher matcher(0.9,true);
int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);
mvIniMatches存储mInitialFrame,mCurrentFrame之间匹配的特征点。
这里的ORBmatcher类后面有机会再详细介绍。功能就是ORB特征点的匹配。
从匹配点中恢复初始相机位姿
在得到超过100个匹配点对后,运用RANSAC 8点法同时计算单应矩阵H和基础矩阵E,并选择最合适的结果作为相机的初始位姿。
mpInitializer = new Initializer(mCurrentFrame,1.0,200);
mpInitializer->Initialize(mCurrentFrame, mvIniMatches, Rcw, tcw, mvIniP3D, vbTriangulated)
Initializer类是初始化类。下面要详细讲一下类成员函数 intilize(),能够根据当前帧和两帧匹配点计算出相机位姿R|t ,以及三角化得到 3D特征点。
Initializer-> intilize()
(1)调用多线程分别用于计算fundamental matrix和homography
// 计算homograpy并打分
thread threadH(&Initializer::FindHomography,this,ref(vbMatchesInliersH), ref(SH), ref(H));
// 计算fundamental matrix并打分
thread threadF(&Initializer::FindFundamental,this,ref(vbMatchesInliersF), ref(SF), ref(F));
// Wait until both threads have finished
threadH.join();
threadF.join();
(2)计算得分比例,选取某个模型进行恢复R|t
float RH = SH/(SH+SF);
// 步骤5:从H矩阵或F矩阵中恢复R,t
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);
具体的:
FindHomography或FindFundamental
(1) 计算单应矩阵cv::Mat Hn = ComputeH21(vPn1i,vPn2i);,并进行评分currentScore = CheckHomography(H21i, H12i, vbCurrentInliers, mSigma);,分别通过单应矩阵将第一帧的特征点投影到第二帧,以及将第二帧的特征点投影到第一帧,计算两次重投影误差,叠加作为评分SH。
(2) 计算基础矩阵cv::Mat Fn = ComputeF21(vPn1i,vPn2i);,并进行评分currentScore = CheckFundamental(F21i, vbCurrentInliers, mSigma);,两次重投影误差叠加作为评分SF。
创建初始地图
如果恢复相机初始位姿成功,那么我们能得到前两帧图像的位姿,以及三角测量后的3维地图点。之后进行如下操作:
// 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);
// 步骤6:将三角化得到的3D点包装成MapPoints
// Initialize函数会得到mvIniP3D,
// mvIniP3D是cv::Point3f类型的一个容器,是个存放3D点的临时变量,
// CreateInitialMapMonocular将3D点包装成MapPoint类型存入KeyFrame和Map中
CreateInitialMapMonocular();
先将当前帧位姿Tcw设置好,之后CreateInitialMapMonocular () 创建初始地图,并进行相应的3D点数据关联操作:
CreateInitialMapMonocular ()
(1)创建关键帧
因为只有前两帧,所以将这两帧都设置为关键帧,并计算对应的BoW,并在地图中插入这两个关键帧。
(2)创建地图点+数据关联
将3D点包装成地图点,将地图点与关键帧,地图进行数据关联。
- 关键帧与地图点关联
一个地图点可被多个关键帧观测到,将观测到这个地图点的关键帧与这个地图点进行关联;关键帧能够观测到很多地图点,将这些地图点与该关键帧关联。
//关键帧上加地图点
pKFini->AddMapPoint(pMP,i);
pKFcur->AddMapPoint(pMP,mvIniMatches[i]);
//地图点的属性:能够观测到的关键帧
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;
- 关键帧与关键帧关联
关键帧之间会共视一个地图点,如果共视的地图点个数越多,说明这两个关键帧之间的联系越紧密。对于某个关键帧,统计其与其他关键帧共视的特征点个数,如果大于某个阈值,那么将这两个关键帧进行关联。
pKFini->UpdateConnections();
pKFcur->UpdateConnections();
(3)Global BA
对上述只有两个关键帧和地图点的地图进行全局BA。
// 步骤5:BA优化
Optimizer::GlobalBundleAdjustemnt(mpMap,20);
(4) 地图尺寸初始化(中值深度为1)
由于单目SLAM的的地图点坐标尺寸不是真实的,比如x=1可能是1m也可能是1cm。选择地图点深度的中位数作为单位尺寸1当作基准来进行地图的尺寸初始化