ORB-SLAM 代码笔记(四)tracking代码结构
首先要清楚ORB-SLAM视觉跟踪的原理,然后对tracking.cc中的函数逐个讲解
代码的前面部分是从配置文件中读取校准好的相机参数(内参和畸变参数,以及双目的深度测量设定),并且加载ORB特征点提取的参数(特征点数,金字塔层数,变化尺度,以及提取Fast关键点的阈值);以及四个线程之间锁的代码。
接下来是将从摄像头或者数据集读入的图像封装成Frame类型对象:
这里以单目为例,无论图片是RGB,BGR, 还是RGBA,BGRA,均转化为灰度图,放弃彩色信息。然后将当前读入帧封装为Frame类型的mCurrentFrame对象;为了让单目成功初始化(单目的初始化需要通过平移运动归一化尺度因子),初始化时mpIniORBextractor提取的特征点数量设定为普通帧的2倍。
mpIniORBextractor = new ORBextractor(2*nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);
有了mCurrentFrame对象,就可以进入跟踪模块(tracking()函数), 经过一系列跟踪处理(重点),可以返回当前帧的位姿。
cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double ×tamp) { mImGray = im; if(mImGray.channels()==3) { if(mbRGB) cvtColor(mImGray,mImGray,CV_RGB2GRAY); else cvtColor(mImGray,mImGray,CV_BGR2GRAY); } else if(mImGray.channels()==4) { if(mbRGB) cvtColor(mImGray,mImGray,CV_RGBA2GRAY); else cvtColor(mImGray,mImGray,CV_BGRA2GRAY); } if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET) mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth); else mCurrentFrame = Frame(mImGray,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth); Track(); return mCurrentFrame.mTcw.clone(); }
具体的tracking步骤如下:
(1)初始化
首先是初始化,分为双目、RGBD,以及单目两种不同的初始化过程,单目的初始化需要注意的较多,可以先看void Tracking::StereoInitialization()函数:
如果检测到的特征点数 N>500,则将当前帧构建为第一个关键帧,关键帧通过当前帧,地图点,以及BoW构建;同时,mpMap地图中也需要加入该关键帧(mpMap包含图像帧以及相应的地图点), 对每一个特征点,通过反投影得出3D地图点,地图点中通过3D位置信息,对应的关键帧,以及mpMap构建。
这其中的关系有点让人混乱,其实直观地去理解很容易:
mpMap就是我们整个位姿与地图(可以想象成ORB-SLAM运行时的那个界面世界),MapPoint和KeyFrame都被包含在这个mpMap中。因此创建这三者对象(地图,地图点,关键帧)时,三者之间的关系在构造函数中不能缺少。另外,由于一个关键帧提取出的特征点对应一个地图点集,因此需要记下每个地图点的在该帧中的编号;同理,一个地图点会被多帧关键帧观测到,也需要几下每个关键帧在该点中的编号。
地图点,还需要完成两个运算,第一个是在观测到该地图点的多个特征点中(对应多个关键帧),挑选出区分度最高的描述子,作为这个地图点的描述子;
pNewMP->ComputeDistinctiveDescriptors();
第二个是更新该地图点平均观测方向与观测距离的范围,这些都是为了后面做描述子融合做准备。
pNewMP->UpdateNormalAndDepth();
最后,还需要在局部地图中(为了实现局部Bundle Adjustment)插入当前关键帧,还需要初始化局部关键帧和局部地图点。将上一帧,上一关键帧,以及上一关键帧Id号初始化为当前帧。参考关键帧,当前帧的参考关键帧,当前地图中的参考地图点(集),并初始化当前相机姿态。
void Tracking::StereoInitialization() { if(mCurrentFrame.N>500) { // Set Frame pose to the origin mCurrentFrame.SetPose(cv::Mat::eye(4,4,CV_32F)); // Create KeyFrame KeyFrame* pKFini = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB); // Insert KeyFrame in the map mpMap->AddKeyFrame(pKFini); // Create MapPoints and asscoiate to KeyFrame 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; } } cout << "New map created with " << mpMap->MapPointsInMap() << " points" << endl; 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); mpMap->mvpKeyFrameOrigins.push_back(pKFini); mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw); mState=OK; } }
单目的初始化则相对负责,放在下一篇笔记中去讲。