在mono_tum的主循环中,在导入并处理完图片后,出现了SLAM.TrackMonocular(im,tframe);
看TrackMonocular定义, 主要是图像处理,判断SLAM模式,获取摄像机位姿。重点学习位姿获取函数Sophus::SE3f Tcw = mpTracker->GrabImageMonocular(imToFeed,timestamp,filename);其他代码做了简单注释
Sophus::SE3f System::TrackMonocular(const cv::Mat &im, const double ×tamp, const vector<IMU::Point>& vImuMeas, string filename)
{
{
unique_lock<mutex> lock(mMutexReset);
if(mbShutDown)
return Sophus::SE3f();
}
if(mSensor!=MONOCULAR && mSensor!=IMU_MONOCULAR)
{
cerr << "ERROR: you called TrackMonocular but input sensor was not set to Monocular nor Monocular-Inertial." << endl;
exit(-1);
}
//克隆图像,检查并调整大小
cv::Mat imToFeed = im.clone();
if(settings_ && settings_->needToResize()){
cv::Mat resizedIm;
cv::resize(im,resizedIm,settings_->newImSize());
imToFeed = resizedIm;
}
// Check mode change
{
unique_lock<mutex> lock(mMutexMode);
//判断定位模式,仅定位的话跟踪器只进行跟踪,不进行本地建图
if(mbActivateLocalizationMode)
{
mpLocalMapper->RequestStop();
// Wait until Local Mapping has effectively stopped
while(!mpLocalMapper->isStopped())
{
usleep(1000);
}
mpTracker->InformOnlyTracking(true);
mbActivateLocalizationMode = false;
}
if(mbDeactivateLocalizationMode)
{
mpTracker->InformOnlyTracking(false);
mpLocalMapper->Release();
mbDeactivateLocalizationMode = false;
}
}
// Check reset
{
unique_lock<mutex> lock(mMutexReset);
if(mbReset) //全局重置
{
mpTracker->Reset();
mbReset = false;
mbResetActiveMap = false;
}
else if(mbResetActiveMap) //活动地图重置
{
cout << "SYSTEM-> Reseting active map in monocular case" << endl;
mpTracker->ResetActiveMap();
mbResetActiveMap = false;
}
}
if (mSensor == System::IMU_MONOCULAR)//遍历IMU测量数据并保存
for(size_t i_imu = 0; i_imu < vImuMeas.size(); i_imu++)
mpTracker->GrabImuData(vImuMeas[i_imu]);
//获取相机位姿
Sophus::SE3f Tcw = mpTracker->GrabImageMonocular(imToFeed,timestamp,filename);
unique_lock<mutex> lock2(mMutexState);
mTrackingState = mpTracker->mState;
mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;
mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;
return Tcw;
}
Tracking::GrabImageMonocular接收一幅图像并进行预处理,然后生成一个 Frame
对象,用于后续的视觉里程计跟踪。
Sophus::SE3f Tracking::GrabImageMonocular(const cv::Mat &im, const double ×tamp, string filename)
{
//将输入图像 im 赋值给 mImGray,并转换为灰度图
mImGray = im;
if(mImGray.channels()==3)
{
if(mbRGB)
cvtColor(mImGray,mImGray,cv::COLOR_RGB2GRAY);
else
cvtColor(mImGray,mImGray,cv::COLOR_BGR2GRAY);
}
else if(mImGray.channels()==4)
{
if(mbRGB)
cvtColor(mImGray,mImGray,cv::COLOR_RGBA2GRAY);
else
cvtColor(mImGray,mImGray,cv::COLOR_BGRA2GRAY);
}
//根据传感器类型和状态创建当前帧(特征提取)
if (mSensor == System::MONOCULAR)
{
if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET ||(lastID - initID) < mMaxFrames)
mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mpCamera,mDistCoef,mbf,mThDepth);
else
mCurrentFrame = Frame(mImGray,timestamp,mpORBextractorLeft,mpORBVocabulary,mpCamera,mDistCoef,mbf,mThDepth);
}
else if(mSensor == System::IMU_MONOCULAR)
{
if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET)
{
mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mpCamera,mDistCoef,mbf,mThDepth,&mLastFrame,*mpImuCalib);
}
else
mCurrentFrame = Frame(mImGray,timestamp,mpORBextractorLeft,mpORBVocabulary,mpCamera,mDistCoef,mbf,mThDepth,&mLastFrame,*mpImuCalib);
}
//设置初始时间戳为当前时间戳
if (mState==NO_IMAGES_YET)
t0=timestamp;
//将文件名和数据集编号赋值给当前帧的相应属性。
mCurrentFrame.mNameFile = filename;
mCurrentFrame.mnDataset = mnNumDataset;
#ifdef REGISTER_TIMES
vdORBExtract_ms.push_back(mCurrentFrame.mTimeORB_Ext);
#endif
//更新最后帧并进行跟踪
lastID = mCurrentFrame.mnId;
Track();
return mCurrentFrame.GetPose();
}
这个track()函数比较重要,主要作用是跟踪并更新显示线程中的图像、特征点、地图点等信息;插入关键帧,更新显示中的位姿,源码注释如下
void Tracking::Track()
{
//步进控制
if (bStepByStep)
{
std::cout << "Tracking: Waiting to the next step" << std::endl;
while(!mbStep && bStepByStep)
usleep(500);
mbStep = false;
}
//当IMU状态不良时,重置地图
if(mpLocalMapper->mbBadImu)
{
cout << "TRACK: Reset map because local mapper set the bad imu flag " << endl;
mpSystem->ResetActiveMap();
return;
}
Map* pCurrentMap = mpAtlas->GetCurrentMap();
if(!pCurrentMap)
{
cout << "ERROR: There is not an active map in the atlas" << endl;
}
//当图像复位或为第一次运行时,mState=NO_IMAGES_YET
if(mState!=NO_IMAGES_YET)
{
if(mLastFrame.mTimeStamp>mCurrentFrame.mTimeStamp)//当前时间戳比之前的早,出现异常
{
cerr << "ERROR: Frame with a timestamp older than previous frame detected!" << endl;
//锁住mMutexImuQueue队列并清空
unique_lock<mutex> lock(mMutexImuQueue);
mlQueueImuData.clear();
//创建新的地图
CreateMapInAtlas();
return;
}
else if(mCurrentFrame.mTimeStamp>mLastFrame.mTimeStamp+1.0)//当前时间戳是否跳跃
{
// cout << mCurrentFrame.mTimeStamp << ", " << mLastFrame.mTimeStamp << endl;
// cout << "id last: " << mLastFrame.mnId << " id curr: " << mCurrentFrame.mnId << endl;
//检查地图是否为惯性导航模式
if(mpAtlas->isInertial())
{
//如果IMU已经初始化
if(mpAtlas->isImuInitialized())
{
cout << "Timestamp jump detected. State set to LOST. Reseting IMU integration..." << endl;
// 如果当前地图没有进行惯性BA优化
if(!pCurrentMap->GetIniertialBA2())
{
// 重置当前活动地图
mpSystem->ResetActiveMap();
}
else
{
// 在Atlas中创建新地图
CreateMapInAtlas();
}
}
else
{
cout << "Timestamp jump detected, before IMU initialization. Reseting..." << endl;
// 重置当前活动地图
mpSystem->ResetActiveMap();
}
return;
}
}
}
if ((mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD) && mpLastKeyFrame)
//更新偏执
mCurrentFrame.SetNewBias(mpLastKeyFrame->GetImuBias());
if(mState==NO_IMAGES_YET)
{
mState = NOT_INITIALIZED;
}
mLastProcessedState=mState;
if ((mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD) && !mbCreatedMap)
{
#ifdef REGISTER_TIMES
std::chrono::steady_clock::time_point time_StartPreIMU = std::chrono::steady_clock::now();
#endif
PreintegrateIMU();
#ifdef REGISTER_TIMES
std::chrono::steady_clock::time_point time_EndPreIMU = std::chrono::steady_clock::now();
double timePreImu = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndPreIMU - time_StartPreIMU).count();
vdIMUInteg_ms.push_back(timePreImu);
#endif
}
mbCreatedMap = false;
// Get Map Mutex -> Map cannot be changed
unique_lock<mutex> lock(pCurrentMap->mMutexMapUpdate);
mbMapUpdated = false;
int nCurMapChangeIndex = pCurrentMap->GetMapChangeIndex();
int nMapChangeIndex = pCurrentMap->GetLastMapChange();
if(nCurMapChangeIndex>nMapChangeIndex)
{
pCurrentMap->SetLastMapChange(nCurMapChangeIndex);
mbMapUpdated = true;
}
if(mState==NOT_INITIALIZED)
{
if(mSensor==System::STEREO || mSensor==System::RGBD || mSensor==System::IMU_STEREO || mSensor==System::IMU_RGBD)
{
StereoInitialization();
}
else
{
MonocularInitialization();//单目初始化
}
//mpFrameDrawer->Update(this);
//当系统为初始化,仅更新mlastframe并返回
if(mState!=OK) // If rightly initialized, mState=OK
{
mLastFrame = Frame(mCurrentFrame);
return;
}
//记录地图中的第一帧
if(mpAtlas->GetAllMaps().size() == 1)
{
mnFirstFrameId = mCurrentFrame.mnId;
}
}
else
{
// System is initialized. Track Frame.
bool bOK;
#ifdef REGISTER_TIMES
std::chrono::steady_clock::time_point time_StartPosePred = std::chrono::steady_clock::now();
#endif
// Initial camera pose estimation using motion model or relocalization (if tracking is lost)
//mbonlutracking=true为仅定位模式,=false为SLAM模式
if(!mbOnlyTracking)
{
// State OK
// Local Mapping is activated. This is the normal behaviour, unless
// you explicitly activate the "only tracking" mode.
if(mState==OK)
{
// Local Mapping might have changed some MapPoints tracked in last frame
CheckReplacedInLastFrame();
//当前帧在重定位帧后面或没有速度信息且IMU没有初始化,参考关键帧的方法进行跟踪
if((!mbVelocity && !pCurrentMap->isImuInitialized()) || mCurrentFrame.mnId<mnLastRelocFrameId+2)
{
Verbose::PrintMess("TRACK: Track with respect to the reference KF ", Verbose::VERBOSITY_DEBUG);
//bok=true 利用关键帧方法跟踪成功
bOK = TrackReferenceKeyFrame();
}
else
{//使用运动学模型进行跟踪
Verbose::PrintMess("TRACK: Track with motion model", Verbose::VERBOSITY_DEBUG);
bOK = TrackWithMotionModel();
if(!bOK)
bOK = TrackReferenceKeyFrame();
}
//跟踪失败,状态设置:LOST 状态;ECENTLY_LOST 状态 地图中有足够的关键帧时,系统刚刚丢失跟踪,记录时间戳用于后续处理。
if (!bOK)
{
if ( mCurrentFrame.mnId<=(mnLastRelocFrameId+mnFramesToResetIMU) &&
(mSensor==System::IMU_MONOCULAR || mSensor==System::IMU_STEREO || mSensor == System::IMU_RGBD))
{
mState = LOST;
}
else if(pCurrentMap->KeyFramesInMap()>10)
{
// cout << "KF in map: " << pCurrentMap->KeyFramesInMap() << endl;
mState = RECENTLY_LOST;
mTimeStampLost = mCurrentFrame.mTimeStamp;
}
else
{
mState = LOST;
}
}
}
else
{
if (mState == RECENTLY_LOST)
{
Verbose::PrintMess("Lost for a short time", Verbose::VERBOSITY_NORMAL);
bOK = true;
if((mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD))
{
if(pCurrentMap->isImuInitialized())
PredictStateIMU();
else
bOK = false;
if (mCurrentFrame.mTimeStamp-mTimeStampLost>time_recently_lost)
{
mState = LOST;
Verbose::PrintMess("Track Lost...", Verbose::VERBOSITY_NORMAL);
bOK=false;
}
}
else
{
// Relocalization 重定位操作
bOK = Relocalization();
//std::cout << "mCurrentFrame.mTimeStamp:" << to_string(mCurrentFrame.mTimeStamp) << std::endl;
//std::cout << "mTimeStampLost:" << to_string(mTimeStampLost) << std::endl;
if(mCurrentFrame.mTimeStamp-mTimeStampLost>3.0f && !bOK)
{
mState = LOST;
Verbose::PrintMess("Track Lost...", Verbose::VERBOSITY_NORMAL);
bOK=false;
}
}
}
else if (mState == LOST)//如果关键帧少于10,调用 ResetActiveMap() 方法重置当前地图。如果关键帧数量足够,调用 CreateMapInAtlas() 方法在地图中创建新地图。
{
Verbose::PrintMess("A new map is started...", Verbose::VERBOSITY_NORMAL);
if (pCurrentMap->KeyFramesInMap()<10)
{
mpSystem->ResetActiveMap();
Verbose::PrintMess("Reseting current map...", Verbose::VERBOSITY_NORMAL);
}else
CreateMapInAtlas();
//如果存在最后一个关键帧,释放对该关键帧的引用。
if(mpLastKeyFrame)
mpLastKeyFrame = static_cast<KeyFrame*>(NULL);
Verbose::PrintMess("done", Verbose::VERBOSITY_NORMAL);
return;
}
}
}
else
{
// Localization Mode: Local Mapping is deactivated (TODO Not available in inertial mode)
if(mState==LOST)
{
if(mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD)
Verbose::PrintMess("IMU. State LOST", Verbose::VERBOSITY_NORMAL);
bOK = Relocalization();
}
else
{
if(!mbVO)
{
// In last frame we tracked enough MapPoints in the map
if(mbVelocity)
{
bOK = TrackWithMotionModel();
}
else
{
bOK = TrackReferenceKeyFrame();
}
}
else
{
// In last frame we tracked mainly "visual odometry" points.
// We compute two camera poses, one from motion model and one doing relocalization.
// If relocalization is sucessfull we choose that solution, otherwise we retain
// the "visual odometry" solution.
bool bOKMM = false;
bool bOKReloc = false;
vector<MapPoint*> vpMPsMM;
vector<bool> vbOutMM;
Sophus::SE3f TcwMM;
if(mbVelocity)
{
bOKMM = TrackWithMotionModel();
vpMPsMM = mCurrentFrame.mvpMapPoints;
vbOutMM = mCurrentFrame.mvbOutlier;
TcwMM = mCurrentFrame.GetPose();
}
bOKReloc = Relocalization();
if(bOKMM && !bOKReloc)
{
mCurrentFrame.SetPose(TcwMM);
mCurrentFrame.mvpMapPoints = vpMPsMM;
mCurrentFrame.mvbOutlier = vbOutMM;
if(mbVO)
{
for(int i =0; i<mCurrentFrame.N; i++)
{
if(mCurrentFrame.mvpMapPoints[i] && !mCurrentFrame.mvbOutlier[i])
{
mCurrentFrame.mvpMapPoints[i]->IncreaseFound();
}
}
}
}
else if(bOKReloc)
{
mbVO = false;
}
bOK = bOKReloc || bOKMM;
}
}
}
//将最新的关键帧作为当前帧的参考关键帧
if(!mCurrentFrame.mpReferenceKF)
mCurrentFrame.mpReferenceKF = mpReferenceKF;
#ifdef REGISTER_TIMES
std::chrono::steady_clock::time_point time_EndPosePred = std::chrono::steady_clock::now();
double timePosePred = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndPosePred - time_StartPosePred).count();
vdPosePred_ms.push_back(timePosePred);
#endif
#ifdef REGISTER_TIMES
std::chrono::steady_clock::time_point time_StartLMTrack = std::chrono::steady_clock::now();
#endif
// If we have an initial estimation of the camera pose and matching. Track the local map.
if(!mbOnlyTracking)
{
if(bOK)
{ //对localmap进行跟踪,并优化位姿
bOK = TrackLocalMap();
}
if(!bOK)
cout << "Fail to track local map!" << endl;
}
else
{
// mbVO true means that there are few matches to MapPoints in the map. We cannot retrieve
// a local map and therefore we do not perform TrackLocalMap(). Once the system relocalizes
// the camera we will use the local map again.
if(bOK && !mbVO)
bOK = TrackLocalMap();
}
if(bOK)
mState = OK;
else if (mState == OK)
{
if (mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD)
{
Verbose::PrintMess("Track lost for less than one second...", Verbose::VERBOSITY_NORMAL);
if(!pCurrentMap->isImuInitialized() || !pCurrentMap->GetIniertialBA2())
{
cout << "IMU is not or recently initialized. Reseting active map..." << endl;
mpSystem->ResetActiveMap();
}
mState=RECENTLY_LOST;
}
else
mState=RECENTLY_LOST; // visual to lost
/*if(mCurrentFrame.mnId>mnLastRelocFrameId+mMaxFrames)
{*/
mTimeStampLost = mCurrentFrame.mTimeStamp;
//}
}
// Save frame if recent relocalization, since they are used for IMU reset (as we are making copy, it shluld be once mCurrFrame is completely modified)
if((mCurrentFrame.mnId<(mnLastRelocFrameId+mnFramesToResetIMU)) && (mCurrentFrame.mnId > mnFramesToResetIMU) &&
(mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD) && pCurrentMap->isImuInitialized())
{
// TODO check this situation
Verbose::PrintMess("Saving pointer to frame. imu needs reset...", Verbose::VERBOSITY_NORMAL);
Frame* pF = new Frame(mCurrentFrame);
pF->mpPrevFrame = new Frame(mLastFrame);
// Load preintegration
pF->mpImuPreintegratedFrame = new IMU::Preintegrated(mCurrentFrame.mpImuPreintegratedFrame);
}
if(pCurrentMap->isImuInitialized())
{
if(bOK)
{
if(mCurrentFrame.mnId==(mnLastRelocFrameId+mnFramesToResetIMU))
{
cout << "RESETING FRAME!!!" << endl;
ResetFrameIMU();
}
else if(mCurrentFrame.mnId>(mnLastRelocFrameId+30))
mLastBias = mCurrentFrame.mImuBias;
}
}
#ifdef REGISTER_TIMES
std::chrono::steady_clock::time_point time_EndLMTrack = std::chrono::steady_clock::now();
double timeLMTrack = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndLMTrack - time_StartLMTrack).count();
vdLMTrack_ms.push_back(timeLMTrack);
#endif
// Update drawer 更新显示线程中的图像、特征点、地图点等信息
mpFrameDrawer->Update(this);
if(mCurrentFrame.isSet())
mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.GetPose());
if(bOK || mState==RECENTLY_LOST)
{
// Update motion model 当前帧和上一帧都被设置时,计算位姿变化,存入mVelocity,mbVelocity = true
if(mLastFrame.isSet() && mCurrentFrame.isSet())
{
Sophus::SE3f LastTwc = mLastFrame.GetPose().inverse();
mVelocity = mCurrentFrame.GetPose() * LastTwc;
mbVelocity = true;
}
else {
mbVelocity = false;
}
if(mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD)
//显示更新位姿
mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.GetPose());
// Clean VO matches
for(int i=0; i<mCurrentFrame.N; i++)//N代表当前帧的关键点数
{
MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];//清除无效地图点
if(pMP)
if(pMP->Observations()<1)
{
mCurrentFrame.mvbOutlier[i] = false;
mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
}
}
// Delete temporal MapPoints
//删除临时地图点
for(list<MapPoint*>::iterator lit = mlpTemporalPoints.begin(), lend = mlpTemporalPoints.end(); lit!=lend; lit++)
{
MapPoint* pMP = *lit;
delete pMP;
}
mlpTemporalPoints.clear();
#ifdef REGISTER_TIMES
std::chrono::steady_clock::time_point time_StartNewKF = std::chrono::steady_clock::now();
#endif
bool bNeedKF = NeedNewKeyFrame();
// Check if we need to insert a new keyframe
// if(bNeedKF && bOK)检测并插入关键帧
if(bNeedKF && (bOK || (mInsertKFsLost && mState==RECENTLY_LOST &&
(mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD))))
CreateNewKeyFrame();
#ifdef REGISTER_TIMES
std::chrono::steady_clock::time_point time_EndNewKF = std::chrono::steady_clock::now();
double timeNewKF = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndNewKF - time_StartNewKF).count();
vdNewKF_ms.push_back(timeNewKF);
#endif
// We allow points with high innovation (considererd outliers by the Huber Function)
// pass to the new keyframe, so that bundle adjustment will finally decide
// if they are outliers or not. We don't want next frame to estimate its position
// with those points so we discard them in the frame. Only has effect if lastframe is tracked
for(int i=0; i<mCurrentFrame.N;i++)//删除被标记为外点的地图点
{
if(mCurrentFrame.mvpMapPoints[i] && mCurrentFrame.mvbOutlier[i])
mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
}
}
// Reset if the camera get lost soon after initialization
if(mState==LOST)
{
if(pCurrentMap->KeyFramesInMap()<=10)
{
mpSystem->ResetActiveMap();
return;
}
if (mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD)
if (!pCurrentMap->isImuInitialized())
{
Verbose::PrintMess("Track lost before IMU initialisation, reseting...", Verbose::VERBOSITY_QUIET);
mpSystem->ResetActiveMap();
return;
}
CreateMapInAtlas();
return;
}
if(!mCurrentFrame.mpReferenceKF)
mCurrentFrame.mpReferenceKF = mpReferenceKF;
mLastFrame = Frame(mCurrentFrame);
}
if(mState==OK || mState==RECENTLY_LOST)
{
// Store frame pose information to retrieve the complete camera trajectory afterwards.
if(mCurrentFrame.isSet())
{
Sophus::SE3f Tcr_ = mCurrentFrame.GetPose() * mCurrentFrame.mpReferenceKF->GetPoseInverse();
mlRelativeFramePoses.push_back(Tcr_);
mlpReferences.push_back(mCurrentFrame.mpReferenceKF);
mlFrameTimes.push_back(mCurrentFrame.mTimeStamp);
mlbLost.push_back(mState==LOST);
}
else
{
// This can happen if tracking is lost
mlRelativeFramePoses.push_back(mlRelativeFramePoses.back());
mlpReferences.push_back(mlpReferences.back());
mlFrameTimes.push_back(mlFrameTimes.back());
mlbLost.push_back(mState==LOST);
}
}
#ifdef REGISTER_LOOP
if (Stop()) {
// Safe area to stop
while(isStopped())
{
usleep(3000);
}
}
#endif
}
单目初始化函数代码注释如下
void Tracking::MonocularInitialization()
{
if(!mbReadyToInitializate)
{
// Set Reference Frame
if(mCurrentFrame.mvKeys.size()>100)
{
mInitialFrame = Frame(mCurrentFrame);
mLastFrame = Frame(mCurrentFrame);
//调整大小来记录未失真的关键点的坐标
mvbPrevMatched.resize(mCurrentFrame.mvKeysUn.size());
for(size_t i=0; i<mCurrentFrame.mvKeysUn.size(); i++)
mvbPrevMatched[i]=mCurrentFrame.mvKeysUn[i].pt;
//将 mvIniMatches 数组中的所有元素填充为 -1,表示没有匹配。
fill(mvIniMatches.begin(),mvIniMatches.end(),-1);
//如果是单目惯导模式,就创建新的积分对象,并赋值给当前帧
if (mSensor == System::IMU_MONOCULAR)
{
if(mpImuPreintegratedFromLastKF)
{
delete mpImuPreintegratedFromLastKF;
}
mpImuPreintegratedFromLastKF = new IMU::Preintegrated(IMU::Bias(),*mpImuCalib);
mCurrentFrame.mpImuPreintegrated = mpImuPreintegratedFromLastKF;
}
//已经准备好初始化,返回
mbReadyToInitializate = true;
return;
}
}
else
{ //关键点数量少于或等于100,或者在IMU单目的情况下,最后一帧的时间戳与初始帧的时间戳差大于1.0,则取消初始化准备。
if (((int)mCurrentFrame.mvKeys.size()<=100)||((mSensor == System::IMU_MONOCULAR)&&(mLastFrame.mTimeStamp-mInitialFrame.mTimeStamp>1.0)))
{
mbReadyToInitializate = false;
return;
}
// Find correspondences 创建orb特征匹配器进行特征匹配
ORBmatcher matcher(0.9,true);
int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);
// Check if there are enough correspondences
//如果特征匹配小于100对,则重新匹配
if(nmatches<100)
{
mbReadyToInitializate = false;
return;
}
Sophus::SE3f Tcw;
vector<bool> vbTriangulated; // Triangulated Correspondences (mvIniMatches)
//用初始帧和当前帧的未失真关键点,重建3D点
if(mpCamera->ReconstructWithTwoViews(mInitialFrame.mvKeysUn,mCurrentFrame.mvKeysUn,mvIniMatches,Tcw,mvIniP3D,vbTriangulated))
{
//循环遍历 mvIniMatches 数组,检查哪些匹配点成功进行了三角化。
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(Sophus::SE3f());
mCurrentFrame.SetPose(Tcw);
//创建初始地图
CreateInitialMapMonocular();
}
}
}