概述
ORB-SLAM3 是第一个同时具备纯视觉（visual）数据处理、视觉+惯性（visual-inertial）数据处理、和构建多地图（multi-map）功能，支持单目、双目以及 RGB-D 相机，同时支持针孔相机、鱼眼相机模型的 SLAM 系统。
主要创新点：
1.在 IMU 初始化阶段引入 MAP。该初始化方法可以实时快速进行，鲁棒性上有很大的提升（在大的场景还是小的场景，不管室内还是室外环境均有较好表现），并且比当前的其他方法具有 2-5 倍的精确度的提升。
2.基于一种召回率大大提高的 place recognition（也就是做回环检测，与历史数据建立联系）方法实现了一个多子地图（multi-maps）系统。ORB-SLAM3 在视觉信息缺乏的情况下更具有 long term 鲁棒性，当跟丢的时候，这个时候就会重新建一个子地图，并且在回环的时候与之前的子地图进行合并。ORB-SLAM3 是第一个可以重用历史所有算法模块的所有信息的系统。
主要结论：
在所有 sensor 配置下，ORB-SLAM3 的鲁棒性与现在的发表的各大系统中相近，精度上有了显著的提高。尤其使用Stereo-Inertial SLAM，在 EuRoC 数据集的平均误差接近 3.6 cm，在一个偏向 AR/VR 场景的 TUM-VI 数据集的平均误差接近 9mm。
ORB_SLAM3系统框图
ORB_SLAM3启动入口在Examples文件夹中，包含单目、双目、RGB-D，及其惯性组合。本文以双目+惯性作为例子，介绍ORB_SLAM3 Track线程。
Track线程在System.cc完成创建，主要完成读取配置文件参数./Examples/Stereo-Inertial/Realsense_D435i.yaml。在主文件stereo_inertial_realsense_D435i.cc中SLAM.TrackStereo()–>mpTracker->GrabImageStereo–>Track()开启跟踪。
一、读取相机数据
根据相机类型，确定是否需要needToRectify和needToResize，否则直接读取数据放入imLeftToFeed/imRightToFeed。
cv::Mat imLeftToFeed, imRightToFeed;
if(settings_ && settings_->needToRectify()){
cv::Mat M1l = settings_->M1l();
cv::Mat M2l = settings_->M2l();
cv::Mat M1r = settings_->M1r();
cv::Mat M2r = settings_->M2r();
cv::remap(imLeft, imLeftToFeed, M1l, M2l, cv::INTER_LINEAR);
cv::remap(imRight, imRightToFeed, M1r, M2r, cv::INTER_LINEAR);
}
else if(settings_ && settings_->needToResize()){
cv::resize(imLeft,imLeftToFeed,settings_->newImSize());
cv::resize(imRight,imRightToFeed,settings_->newImSize());
}
else{
imLeftToFeed = imLeft.clone();
imRightToFeed = imRight.clone();
}
二、检查模式是否改变（关闭局部建图线程仅进行追踪线程）
模式是否改变，取决于viewer中是否进行了设置ActivateLocalizationMode。
若mbActivateLocalizationMode为true，则关闭局部建图线程，进跟踪模式；
若mbDeactivateLocalizationMode为true，则同时进行局部建图、跟踪。
// 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);
}
// 局部地图关闭以后，只进行追踪的线程，只计算相机的位姿，没有对局部地图进行更新
mpTracker->InformOnlyTracking(true);
// 关闭线程可以使得别的线程得到更多的资源
mbActivateLocalizationMode = false;
}
if(mbDeactivateLocalizationMode)
{
mpTracker->InformOnlyTracking(false);
mpLocalMapper->Release();
mbDeactivateLocalizationMode = false;
}
}
三、检查地图是否重置
是否需要重置地图条件主要有：viewer中设置，跟踪线程中如果认为imu有问题重置等等，主要是根据跟踪线程中局部地图和imu相关状态进行处理。
// Check reset
{
unique_lock<mutex> lock(mMutexReset);
if(mbReset)
{
mpTracker->Reset();
mbReset = false;
mbResetActiveMap = false;
}
else if(mbResetActiveMap)
{
mpTracker->ResetActiveMap();
mbResetActiveMap = false;
}
}
四、如果传感器类型为IMU_STEREO，读取imu数据
读取imu数据接口为mpTracker->GrabImuData(vImuMeas[i_imu])
if (mSensor == System::IMU_STEREO)
for(size_t i_imu = 0; i_imu < vImuMeas.size(); i_imu++)
mpTracker->GrabImuData(vImuMeas[i_imu]);
数据存放在mlQueueImuData链表中
void Tracking::GrabImuData(const IMU::Point &imuMeasurement)
{
unique_lock<mutex> lock(mMutexImuQueue);
mlQueueImuData.push_back(imuMeasurement);
}
五、根据双目计算相机位姿
// std::cout << "start GrabImageStereo" << std::endl;
Sophus::SE3f Tcw = mpTracker->GrabImageStereo(imLeftToFeed,imRightToFeed,timestamp,filename);
进入GrabImageStereo
1.将RGB、RGBA图像转化为灰度图像
// step 1 ：将RGB或RGBA图像转为灰度图像
if(mImGray.channels()==3)
{
//cout << "Image with 3 channels" << endl;
if(mbRGB)
{
cvtColor(mImGray,mImGray,cv::COLOR_RGB2GRAY);
cvtColor(imGrayRight,imGrayRight,cv::COLOR_RGB2GRAY);
}
else
{
cvtColor(mImGray,mImGray,cv::COLOR_BGR2GRAY);
cvtColor(imGrayRight,imGrayRight,cv::COLOR_BGR2GRAY);
}
}
2.双目模式特征提取、匹配，得到当前帧信息mCurrentFrame
主要完成：
（1）ORB特征提取ORBextractor
（2）ORB特征匹配ComputeStereoMatches()
// 双目模式，注意跟两个相机模式区分开
if (mSensor == System::STEREO && !mpCamera2)
mCurrentFrame = Frame(
mImGray, // 左目图像
imGrayRight, // 右目图像
timestamp, // 时间戳
mpORBextractorLeft, // 左目特征提取器
mpORBextractorRight, // 右目特征提取器
mpORBVocabulary, // 字典
mK, // 内参矩阵
mDistCoef, // 去畸变参数
mbf, // 基线长度
mThDepth, // 远点,近点的区分阈值
mpCamera); // 相机模型
else if(mSensor == System::STEREO && mpCamera2)
mCurrentFrame = Frame(mImGray,imGrayRight,timestamp,mpORBextractorLeft,mpORBextractorRight,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth,mpCamera,mpCamera2,mTlr);
else if(mSensor == System::IMU_STEREO && !mpCamera2)
mCurrentFrame = Frame(mImGray,imGrayRight,timestamp,mpORBextractorLeft,mpORBextractorRight,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth,mpCamera,&mLastFrame,*mpImuCalib);
else if(mSensor == System::IMU_STEREO && mpCamera2)
mCurrentFrame = Frame(mImGray,imGrayRight,timestamp,mpORBextractorLeft,mpORBextractorRight,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth,mpCamera,mpCamera2,mTlr,&mLastFrame,*mpImuCalib);
3.跟踪Track
跟踪主要完成估计运动，跟踪局部地图，跟踪过程包括恒速模型跟踪，参考关键帧跟踪，局部地图跟踪。
该部分内容较多，只用文字的方式很难表达清楚，也显得太长，这部分等想到好的表达方式再填坑。
终于水完了。。。。
参考：
https://github.com/UZ-SLAMLab/ORB_SLAM3