ORB-SLAM2源码学习：System.cc: System::TrackStereo、TrackRGBD、TrackMonocular追踪器接口
前言
在之前的单目实例中，我们在初始化一个SLAM的系统后，系统处理了接受的文件数据，初始化并运行了各个线程，之后就要给追踪器传入图像进行整个SLAM的流程了。
1.函数声明
追踪器的函数定义在System源文件下，针对不同的传感器模式，分别实现了三种追踪器的定义。
它们依次是单目、双目、RGBD追踪器。
cv::Mat System::TrackMonocular(const cv::Mat &im, const double ×tamp)
cv::Mat System::TrackStereo(const cv::Mat &imLeft, //左侧图像
const cv::Mat &imRight, //右侧图像
const double ×tamp)//时间戳
cv::Mat System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double ×tamp)
2.函数定义
以单目为例：
1.检查传感器
不是指定的传感器内容输出信息提示。
if(mSensor!=MONOCULAR)
{
cerr << "ERROR: you called TrackMonocular but input sensor was not set to Monocular." << endl;
exit(-1);
}2.改变追踪模式
追踪线程有两个模式，一个是定位建图另一个是纯定位模式，追踪器通过此部分代码来实现模式的改变。
// Check mode change
{
// 独占锁，主要是为了mbActivateLocalizationMode和mbDeactivateLocalizationMode不会发生混乱
unique_lock<mutex> lock(mMutexMode);
// mbActivateLocalizationMode为true会关闭局部地图线程
if(mbActivateLocalizationMode)
{
mpLocalMapper->RequestStop();
// Wait until Local Mapping has effectively stopped
while(!mpLocalMapper->isStopped())
{
usleep(1000);
}
// 局部地图关闭以后，只进行追踪的线程，只计算相机的位姿，没有对局部地图进行更新
// 设置mbOnlyTracking为真
mpTracker->InformOnlyTracking(true);
// 关闭线程可以使得别的线程得到更多的资源
mbActivateLocalizationMode = false;
}
// 如果mbDeactivateLocalizationMode是true，局部地图线程就被释放, 关键帧从局部地图中删除.
if(mbDeactivateLocalizationMode)
{
mpTracker->InformOnlyTracking(false);
mpLocalMapper->Release();
mbDeactivateLocalizationMode = false;
}
}3.检查是否有复位操作。
// Check reset
{
unique_lock<mutex> lock(mMutexReset);
if(mbReset)
{
mpTracker->Reset();
mbReset = false;
}4.获取相机位姿估计的结果并返回。
//获取相机位姿的估计结果
cv::Mat Tcw = mpTracker->GrabImageMonocular(im,timestamp);
unique_lock<mutex> lock2(mMutexState);
mTrackingState = mpTracker->mState;
mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;
mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;
return Tcw;其他追踪器的代码也差不多，直接列在下面了。
//双目输入时的追踪器接口
cv::Mat System::TrackStereo(const cv::Mat &imLeft, //左侧图像
const cv::Mat &imRight, //右侧图像
const double ×tamp)//时间戳
{
//检查输入数据类型是否合法
if(mSensor!=STEREO)
{
//不合法那就退出
cerr << "ERROR: you called TrackStereo but input sensor was not set to STEREO." << endl;
exit(-1);
}
//检查是否有运行模式的改变
// Check mode change
{
// TODO 锁住这个变量？防止其他的线程对它的更改？
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;
}//是否有复位请求
}//检查是否有复位的操作
//用矩阵Tcw来保存估计的相机 位姿，运动追踪器的GrabImageStereo函数才是真正进行运动估计的函数
cv::Mat Tcw = mpTracker->GrabImageStereo(imLeft,imRight,timestamp);
//给运动追踪状态上锁
unique_lock<mutex> lock2(mMutexState);
//获取运动追踪状态
mTrackingState = mpTracker->mState;
//获取当前帧追踪到的地图点向量指针
mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;
//获取当前帧追踪到的关键帧特征点向量的指针
mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;
//返回获得的相机运动估计
return Tcw;
}
//当输入图像 为RGBD时进行的追踪，参数就不在一一说明了
cv::Mat System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double ×tamp)
{
//判断输入数据类型是否合法
if(mSensor!=RGBD)
{
cerr << "ERROR: you called TrackRGBD but input sensor was not set to RGBD." << endl;
exit(-1);
}
// 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;
}
}
//获得相机位姿的估计
cv::Mat Tcw = mpTracker->GrabImageRGBD(im,depthmap,timestamp);
unique_lock<mutex> lock2(mMutexState);
mTrackingState = mpTracker->mState;
mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;
mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;
return Tcw;
}结束语
以上就是我学习到的内容，如果对您有帮助请多多支持我，如果哪里有问题欢迎大家在评论区积极讨论，我看到会及时回复。
原文地址：https://blog.csdn.net/2301_76831056/article/details/144634039
免责声明：本站文章内容转载自网络资源，如本站内容侵犯了原著者的合法权益，可联系本站删除。更多内容请关注自学内容网（zxcms.com）！