和泡泡的小伙伴讨论“为什么初始化对Monocular SLAM很重要"。
选择合适的场景、两帧影像之间应该有一个合理的视差,不然无法计算pose、纯旋转时
------欢迎补充
在tracking中,单目相机进入GrabImageMonocular()函数,并在这个函数中,进行Frame的初始化和track()环节。track()函数中,每次都会判断是否需要初始化,这是在第一次初始化和后面进行relocalization,reset之类的,都会用到的。
if(mState==NOT_INITIALIZED)
{
if(mSensor==System::STEREO || mSensor==System::RGBD)
StereoInitialization();
else
{
MonocularInitialization();
}
mpFrameDrawer->Update(this);
if(mState!=OK)
return;
}
现在就是来看,系统如何实现这个MonocularInitialization()函数。
首先,mpInitializer是:mpInitializer(static_cast<Initializer*>(NULL))
void Tracking::MonocularInitialization()
{
int num = 100;
// 如果单目初始器还没有没创建,则创建单目初始器
// 第一帧 有效进来,
if(!mpInitializer)
{
// Set Reference Frame
if(mCurrentFrame.mvKeys.size()>num)
{
// step 1:得到用于初始化的第一帧,初始化需要两帧
mInitialFrame = Frame(mCurrentFrame);
// 记录最近的一帧
mLastFrame = Frame(mCurrentFrame);
// mvbPreMatched最大的情况就是当前帧所有的特征点都被匹配上
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;
// 由当前帧构造初始化器, sigma:1.0 iterations:200
mpInitializer = new Initializer(mCurrentFrame,1.0,200);
fill(mvIniMatches.begin(),mvIniMatches.end(),-1);
return;
}
}
// else表示已经有有 mpInitializer,并且已经有一帧了
else
{
// Try to initialize
// step2:如果当前帧特征点数大于100,则得到用于单目初始化的第二帧
// 如果当前帧特征点太少,重新构造初始器
// 因此只有连续两帧的特征点个数都大于100时,才能继续进行初始化过程
if((int)mCurrentFrame.mvKeys.size()<=num)
{
delete mpInitializer;
mpInitializer = static_cast<Initializer*>(NULL);
fill(mvIniMatches.begin(),mvIniMatches.end(),-1);
return;
}
// Find correspondences
// step3:在mInitialFrame与mCurrentFrame中找匹配的特征点对
// mvbPrevMatched为前一帧的特征点,存储了mInitialFrame中哪些点将进行接下来的匹配,类型 std::vector<cv::Point2f> mvbPrevMatched;
// mvIniMatches存储mInitialFrame, mCurrentFrame之间匹配的特征点,类型为std::vector<int> mvIniMatches; ????
ORBmatcher matcher(0.9,true);
int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);
LSDmatcher lmatcher; //建立线特征之间的匹配
int lineMatches = lmatcher.SerachForInitialize(mInitialFrame, mCurrentFrame, mvLineMatches);
// Check if there are enough correspondences
// step4:如果初始化的两帧之间的匹配点太少,重新初始化
if(nmatches<100)
{
delete mpInitializer;
mpInitializer = static_