终于写到Tracking.cc了,想想还有点小激动呢。
本人邮箱jinbo666888@qq.com,欢迎交流。
本系列代码注释,是在吴博注释的基础上进一步注释。
1.Tracking 框架
Tracking线程流程框图:
各流程对应的主要函数(来自吴博@泡泡机器人):
Tracking整体流程图( 来自https://blog.csdn.net/chishuideyu/article/details/75314896)
上面这张图把Tracking.cc讲的特别明白。
tracking线程在获取图像数据后,会传给函数GrabImageStereo、GrabImageRGBD或GrabImageMonocular进行预处理,这里我以GrabImageMonocular为例。
- GrabImageMonocular(const cv::Mat &im, const double ×tamp)
函数功能 | 1、将图像转为mImGray并初始化mCurrentFrame,2、进行tracking过程,输出世界坐标系到该帧相机坐标系的变换矩阵 |
im | 输入图像 |
timestamp | 时间戳 |
cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double ×tamp)
{
mImGray = im;//读取图像
// 步骤1:将RGB或RGBA图像转为灰度图像
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);
}
// 步骤2:构造Frame
if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET)// 没有成功初始化的前一个状态就是NO_IMAGES_YET
mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
else
mCurrentFrame = Frame(mImGray,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
// 步骤3:跟踪
Track();
return mCurrentFrame.mTcw.clone();
}
数据,流到Track(),由于代码超长,我会分几段粘贴注释。
void Tracking::Track()
步骤 | 1. 判断tracking状态:如果是未初始化(NOT_INITIALIZED),则对单目和非单目分别执行MonocularInitialization()、StereoInitialization()进行初始化,并更新地图视图。 2.对于初始化成功的,接下来进行跟踪ORB-SLAM中关于跟踪状态有两种选择(由mbOnlyTracking判断) (1)只进行跟踪不建图 (2)同时跟踪和建图: 初始化之后ORB-SLAM有三种跟踪模型可供选择 a.TrackWithMotionModel(); 运动模型:根据运动模型估计当前帧位姿——根据匀速运动模型对上一帧的地图点进行跟踪——优化位姿。 b.TrackReferenceKeyFrame(); 关键帧模型:BoW搜索当前帧与参考帧的匹配点——将 |