ORB SLAM2学习笔记之mono_kitti(四)
探索Track函数之MonocularInitialization(单目初始化)
Tracking.cpp里面的Track()
函数可以说是 Tracking 线程最主体部分了,来看看调用Track()
函数的过程:
Tracking线程中初始化函数用的是MonocularInitialization()
,在第一次进入这个函数之前,初始化了mpInitializer
(单目初始器)赋值为null,所以一开始会进入下面的if语句:
void Tracking::MonocularInitialization()
{
//(第一次进入函数时)初始化tracking时赋了null
//(第二次进入函数时)第一次新建了mpInitializer,所以执行else
if(!mpInitializer)
{
// Set Reference Frame
// 单目初始帧的特征点数必须大于100
if(mCurrentFrame.mvKeys.size()>100)
{
//初始化mono的时候,初始帧和上一帧都是当前帧赋值
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;
// 这里实例化Initialize类的一个对象指针,初始化的值为选取的当前帧和测量误差1.0,RANSAC迭代次数200
mpInitializer = new Initializer(mCurrentFrame,1.0,200);
fill(mvIniMatches.begin(),mvIniMatches.end(),-1); //先都填上-1 现在还没匹配呢
return;
}
}
第二次进入这个函数的时候已经创建了一个mpInitializer
,所以会进入else语句:
else
{
// Try to initialize
if((int)mCurrentFrame.mvKeys.size()<=100)
{
delete mpInitializer;
mpInitializer = static_cast<Initializer*>(NULL);
fill(mvIniMatches.begin(),mvIniMatches.end(),-1);
return;
}
// Find correspondences
ORBmatcher matcher(0.9,true);
// mvbPrevMatched为前一帧的特征点,存储了mInitialFrame中哪些点将进行接下来的匹配
// mvIniMatches存储mInitialFrame,mCurrentFrame之间匹配的特征点,键值对是两帧匹配特征点的索引
// mvbPrevMatched,mvIniMatches获得更新
int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);
// Check if there are enough correspondences
if(nmatches<100)
{
delete mpInitializer;
mpInitializer = static_cast<Initializer*>(NULL);
return;
}
cv::Mat Rcw; // Current Camera Rotation
cv::Mat tcw; // Current Camera Translation
vector<bool> vbTriangulated; // Triangulated Correspondences (mvIniMatches)
//如果初始化成功,intializer得到Rcw, tcw
if(mpInitializer->Initialize(mCurrentFrame, mvIniMatches, Rcw, tcw, mvIniP3D, vbTriangulated))
{
for(size_t i=0, iend=mvIniMatches.size(); i<iend;i++)
{
if(mvIniMatches[i]>=0 && !vbTriangulated[i])
{
mvIniMatches[i]=-1;
nmatches--;
}
}
// Set Frame Poses 初始帧也就是第一帧为世界坐标系,所以无需旋转平移
mInitialFrame.SetPose(cv::Mat::eye(4,4,CV_32F));
cv::Mat Tcw = cv::Mat::eye(4,4,CV_32F);
//Tcw赋值给mCurrentFrame
Rcw.copyTo(Tcw.rowRange(0,3).colRange(0,3));
tcw.copyTo(Tcw.rowRange(0,3).col(3));
mCurrentFrame.SetPose