ORB-SLAM2 代码解读：从 rgbd_tum.cc 走一遍系统
- （一）ORB-SLAM2 从 Rgbd_tum.cc 的main函数开始
- 1. 读取图片信息
- 2. 彩色图像和深度图像数据的一致性检查
- 3. 初始化ORB-SLAM2系统（进入到System.cc）
- `至此System.cc结束。返回rgbd_tum.cc`
- 4 rgbd_tum.cc中main函数循环逐帧处理（Main loop）
- 5. 关闭线程，统计时间，保存轨迹
本文参考： 小吴同学的吴言吴语和 小六老师的代码解释
十分感谢！！！
（一）ORB-SLAM2 从 Rgbd_tum.cc 的main函数开始
1. 读取图片信息
LoadImages(strAssociationFilename, vstrImageFilenamesRGB, vstrImageFilenamesD, vTimestamps);
2. 彩色图像和深度图像数据的一致性检查
loadimages函数之后就是，不做过多解释。
3. 初始化ORB-SLAM2系统（进入到System.cc）
- 通过 System 类的构造函数初始化一个 SLAM 系统
ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::RGBD,true);
- 进入System.cc进行初始化
//系统的构造函数，将会启动其他的线程
System::System(const string &strVocFile, //词典文件路径
const string &strSettingsFile, //配置文件路径
const eSensor sensor, //传感器类型
const bool bUseViewer): //是否使用可视化界面
mSensor(sensor), //初始化传感器类型
mpViewer(static_cast<Viewer*>(NULL)), //空。。。对象指针？ TODO
mbReset(false), //无复位标志
mbActivateLocalizationMode(false), //没有这个模式转换标志
mbDeactivateLocalizationMode(false) //没有这个模式转换标志
{
3.1 相机相关
- 相机传感器类型：
mSensor==RGBD； - 读取相机配置文件路径：
cv::FileStorage fsSettings(strSettingsFile.c_str(), cv::FileStorage::READ);。
3.2 加载 ORB 词典
- 创建一个词典对象：
mpVocabulary = new ORBVocabulary(); - 加载 ORB 词典：`mpVocabulary->loadFromBinaryFile(strVocFile);
- loadFromBinaryFile()
函数位于~/ORB_SLAM2/Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h` 中
template<class TDescriptor, class F>
bool TemplatedVocabulary<TDescriptor,F>::loadFromBinaryFile(const std::string &filename)
{}
3.3 创建关键帧数据集
mpKeyFrameDatabase
mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);
3.4 创建地图类的对象
mpMap
mpMap = new Map();
3.5创建帧绘制器和地图绘制器
mpFrameDrawer和mpMapDrawer
//这里的帧绘制器和地图绘制器将会被可视化的Viewer所使用
mpFrameDrawer = new FrameDrawer(mpMap);
mpMapDrawer = new MapDrawer(mpMap, strSettingsFile);
3.6 初始化追踪线程（进入Tracking.cc）
mptracker
//在本主进程中初始化追踪线程
mpTracker = new Tracking(this, //现在还不是很明白为什么这里还需要一个this指针 TODO
mpVocabulary, //字典
mpFrameDrawer, //帧绘制器
mpMapDrawer, //地图绘制器
mpMap, //地图
mpKeyFrameDatabase, //关键帧地图
strSettingsFile, //设置文件路径
mSensor); //传感器类型iomanip
3.6.1 Tracking::Tracking``
- 位于 Tracking.cc 文件中
///构造函数
Tracking::Tracking(
System *pSys, //系统实例
ORBVocabulary* pVoc, //BOW字典
FrameDrawer *pFrameDrawer, //帧绘制器
MapDrawer *pMapDrawer, //地图点绘制器
Map *pMap, //地图句柄
KeyFrameDatabase* pKFDB, //关键帧产生的词袋数据库
const string &strSettingPath, //配置文件路径
const int sensor): //传感器类型
mState(NO_IMAGES_YET), //当前系统还没有准备好
mSensor(sensor),
mbOnlyTracking(false), //处于SLAM模式
mbVO(false), //当处于纯跟踪模式的时候，这个变量表示了当前跟踪状态的好坏
mpORBVocabulary(pVoc),
mpKeyFrameDB(pKFDB),
mpInitializer(static_cast<Initializer*>(NULL)), //暂时给地图初始化器设置为空指针
mpSystem(pSys),
mpViewer(NULL), //注意可视化的查看器是可选的，因为ORB-SLAM2最后是被编译成为一个库，所以对方人拿过来用的时候也应该有权力说我不要可视化界面（何况可视化界面也要占用不少的CPU资源）
mpFrameDrawer(pFrameDrawer),
mpMapDrawer(pMapDrawer),
mpMap(pMap),
mnLastRelocFrameId(0) //恢复为0,没有进行这个过程的时候的默认值
3.6.2 读取配置文件
- ① 构造相机内参矩阵
- ② 图像矫正系数
- ③ 提取 ORB 特征的相关参数
// Step 1 从配置文件中加载相机参数
//构造相机内参矩阵
cv::Mat K = cv::Mat::eye(3,3,CV_32F);
K.at<float>(0,0) = fx;
K.at<float>(1,1) = fy;
K.at<float>(0,2) = cx;
K.at<float>(1,2) = cy;
K.copyTo(mK);
// 图像矫正系数
// [k1 k2 p1 p2 k3]
cv::Mat DistCoef(4,1,CV_32F);
DistCoef.at<float>(0) = fSettings["Camera.k1"];
DistCoef.at<float>(1) = fSettings["Camera.k2"];
DistCoef.at<float>(2) = fSettings["Camera.p1"];
DistCoef.at<float>(3) = fSettings["Camera.p2"];
const float k3 = fSettings["Camera.k3"];
// Step 2 加载ORB特征点有关的参数,并新建特征点提取器
// 每一帧提取的特征点数 1000
int nFeatures = fSettings["ORBextractor.nFeatures"];
// 图像建立金字塔时的变化尺度 1.2
float fScaleFactor = fSettings["ORBextractor.scaleFactor"];
// 尺度金字塔的层数 8
int nLevels = fSettings["ORBextractor.nLevels"];
// 提取fast特征点的默认阈值 20
int fIniThFAST = fSettings["ORBextractor.iniThFAST"];
// 如果默认阈值提取不出足够fast特征点，则使用最小阈值 8
int fMinThFAST = fSettings["ORBextractor.minThFAST"];
3.6.3创建特征点提取器
- 创建 ORB 特征点提取器
// tracking过程都会用到mpORBextractorLeft作为特征点提取器
mpORBextractorLeft = new ORBextractor(
nFeatures, //参数的含义还是看上面的注释吧
fScaleFactor,
nLevels,
fIniThFAST,
fMinThFAST);
3.7 初始化局部建图线程并运行（返回System.cc进入LocalMapping.cc函数）
3.7.1 初始化局部建图
LocalMapping构造函数位于LocalMapping.cc中
//初始化局部建图线程并运行
//Initialize the Local Mapping thread and launch
mpLocalMapper = new LocalMapping(mpMap, //指定使iomanip
mSensor==MONOCULAR); // TODO 为什么这个要设置成为MONOCULAR？？？
//运行这个局部建图线程
mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run, //这个线程会调用的函数
mpLocalMapper); //这个调用函数的参数
3.7.2 运行化局部建图
ORB_SLAM2::LocalMapping::Run位于LocalMapping.cc中bool LocalMapping::CheckNewKeyFrames()函数检查关键帧列表是否为空，然后执行以下步骤
3.7.2.1 线程主函数
void LocalMapping::Run()
{
中间省略，只保留重要的函数
// 标记状态，表示当前run函数正在运行，尚未结束
mbFinished = false;
// 主循环
while(1)
{
// Step 1 告诉Tracking，LocalMapping正处于繁忙状态，请不要给我发送关键帧打扰我
// LocalMapping线程处理的关键帧都是Tracking线程发过的
SetAcceptKeyFrames(false);
// 等待处理的关键帧列表不为空
if(CheckNewKeyFrames())
{
// Step 2 处理列表中的关键帧，包括计算BoW、更新观测、描述子、共视图，插入到地图等
ProcessNewKeyFrame();
// Step 3 根据地图点的观测情况剔除质量不好的地图点
MapPointCulling();
// Step 4 当前关键帧与相邻关键帧通过三角化产生新的地图点，使得跟踪更稳
CreateNewMapPoints();
// 已经处理完队列中的最后的一个关键帧
if(!CheckNewKeyFrames())
{
// Step 5 检查并融合当前关键帧与相邻关键帧帧（两级相邻）中重复的地图点
SearchInNeighbors();
}
// 终止BA的标志
mbAbortBA = false;
// 已经处理完队列中的最后的一个关键帧，并且闭环检测没有请求停止LocalMapping
if(!CheckNewKeyFrames() && !stopRequested())
{
// Step 6 当局部地图中的关键帧大于2个的时候进行局部地图的BA
if(mpMap->KeyFramesInMap()>2)
// 注意这里的第二个参数是按地址传递的,当这里的 mbAbortBA 状态发生变化时，能够及时执行/停止BA
Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpMap);
// Step 7 检测并剔除当前帧相邻的关键帧中冗余的关键帧
// 冗余的判定：该关键帧的90%的地图点可以被其它关键帧观测到
KeyFrameCulling();
}
// Step 8 将当前帧加入到闭环检测队列中
// 注意这里的关键帧被设置成为了bad的情况,这个需要注意
mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);
}
else if(Stop()) // 当要终止当前线程的时候
{
// Safe area to stop
while(isStopped() && !CheckFinish())
{
// 如果还没有结束利索,那么等
// usleep(3000);
std::this_thread::sleep_for(std::chrono::milliseconds(3));
}
// 然后确定终止了就跳出这个线程的主循环
if(CheckFinish())
break;
}
// 查看是否有复位线程的请求
ResetIfRequested();
// Tracking will see that Local Mapping is not busy
SetAcceptKeyFrames(true);
// 如果当前线程已经结束了就跳出主循环
if(CheckFinish())
break;
//usleep(3000);
std::this_thread::sleep_for(std::chrono::milliseconds(3));
}
SetFinish();
}
3.7.2.2 插入关键帧到地图中
void LocalMapping::ProcessNewKeyFrame(){}
3.7.2.3 剔除上一步引入的不合格的地图点
void LocalMapping::MapPointCulling(){}
3.7.2.4 与相邻帧三角化恢复一些地图点
void LocalMapping::CreateNewMapPoints(){}
3.7.2.5 检查并融合相邻帧的公共特征点
void LocalMapping::SearchInNeighbors(){}
3.7.2.6 执行局部 BA
- 局部 BA 位于
src/Optimizer.cc文件中
if(mpMap->KeyFramesInMap() > 2)
Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame, &mbAbortBA, mpMap);
3.7.2.7 剔除冗余关键帧
void LocalMapping::KeyFrameCulling(){}
3.7.2.8 将当前帧加入到闭环检测队列 （进入loopclosing.cc）
1. mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);
2. void LocalMapping::InsertKeyFrame(KeyFrame *pKF){}
3.8 初始化局部建图线程并运行（返回System.cc进入Viewer.cc函数）
//如果指定了，程序的运行过程中需要运行可视化部分
//新建viewer
mpViewer = new Viewer(this, //又是这个
mpFrameDrawer, //帧绘制器
mpMapDrawer, //地图绘制器
mpTracker, //追踪器
strSettingsFile); //配置文件的访问路径
//新建viewer线程
mptViewer = new thread(&Viewer::Run, mpViewer);
//给运动追踪器设置其查看器
mpTracker->SetViewer(mpViewer);
3.8.1 初始化可视化线程
Viewer构造函数位于Viewer.cc文件中
mpViewer = new Viewer(this, //又是这个
mpFrameDrawer, //帧绘制器
mpMapDrawer, //地图绘制器
mpTracker, //追踪器
strSettingsFile); //配置文件的访问路径
3.8.2 运行可视化线程
Viewer::Run()位于Viewer.cc文件中
mptViewer = new thread(&Viewer::Run, mpViewer);
3.8.2.1 Pangolin绘制Viewer窗口
- 看不懂，省略
3.8.2.2 获得相机位姿
mpMapDrawer->GetCurrentOpenGLCameraMatrix(Twc);
3.8.2.3 根据相机位姿调整视角
s_cam.Follow(Twc);
3.8.2.4 更新定位模式或者是SLAM模式
1.mpSystem->ActivateLocalizationMode();
or
2.mpSystem->DeactivateLocalizationMode();
3.8.2.5 绘制地图和图像(3D部分)
- 绘制当前相机
- 转到
MapDrawer.cc中的void MapDrawer::DrawCurrentCamera(pangolin::OpenGlMatrix &Twc)
mpMapDrawer->DrawCurrentCamera(Twc);
- 绘制关键帧
-
- 转到
MapDrawer.cc中的void MapDrawer::DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph)
- 转到
mpMapDrawer->DrawKeyFrames(menuShowKeyFrames,menuShowGraph);
- 绘制地图点
- 转到
MapDrawer.cc中的void MapDrawer::DrawMapPoints()
mpMapDrawer->DrawMapPoints();
3.8.2.6 绘制当前帧图像和特征点提取匹配结果（OpenCV 显示特征提取视图）
- 转到
FrameDrawer.cc中的cv::Mat FrameDrawer::DrawFrame()
cv::Mat im = mpFrameDrawer->DrawFrame();
cv::imshow("ORB-SLAM2: Current Frame",im);
3.8.2.7 复位按钮
- 将所有的GUI控件恢复初始状态
至此System.cc结束。返回rgbd_tum.cc
4 rgbd_tum.cc中main函数循环逐帧处理（Main loop）
- 对图像序列中的每张图像展开遍历
for(int ni=0; ni<nImages; ni++)
{包括4.1-里面的所有}
4.1 读取图像
imRGB = cv::imread(string(argv[3])+"/"+vstrImageFilenamesRGB[ni],CV_LOAD_IMAGE_UNCHANGED);
imD = cv::imread(string(argv[3])+"/"+vstrImageFilenamesD[ni],CV_LOAD_IMAGE_UNCHANGED);
double tframe = vTimestamps[ni];
4.2 确定图像合法性
if(imRGB.empty())
{省略}
4.3 追踪RGBD
- 进入
System.cc中的cv::Mat System::TrackRGBD（）中 - 输入：图像和时间戳
（不确定） - 返回：相机位姿（关键帧的相机位姿，KeyFrame.h）
（不确定）
SLAM.TrackRGBD(imRGB,imD,tframe,detect_result);
detect_result.clear();
cv::Mat System::TrackRGBD(const cv::Mat &im, //输入：由 RGBD 相机捕获的输入 RGB 图像。
const cv::Mat &depthmap, //输入：由 RGBD 相机捕获的输入深度图，与 RGB 图像对应。
const double ×tamp, //输入：图像和深度图捕获的时间戳。
const vector<std::pair<vector<double>, int>>& detect_result) //输入（Chatgpt）：这是一个检测结果的向量，每个元素是一个包含特征向量（vector<double>）和标签（int）的对。
4.3.1 判断输入数据类型是否合法
if(mSensor!=RGBD){省略}
4.3.2 检查模式改变
- 可视化窗口上的勾选框不变化的时候不会进行检测，若有变化，则会执行一次（执行完又恢复原标志位）
- 激活定位模式：mbActivateLocalizationMode
- 取消激活定位模式：mbDeactivateLocalizationMode
- 检查是否重置 mbReset
- void Tracking::Reset()
4.3.3 获得相机位姿的估计
- 进入到
Tracking.cc Tracking::GrabImageRGBD函数，返回当前帧位姿
cv::Mat Tcw = mpTracker->GrabImageRGBD(im,depthmap,timestamp,detect_result);
4.3.3.1 将RGB或RGBA图像转为灰度图像
cvtColor(mImGray,mImGray,CV_RGB2GRAY);
4.3.3.2 将深度相机的disparity转为Depth , 也就是转换成为真正尺度下的深度
4.3.3.3 构造Frame
- 构造当前帧（在 Frame.cc 中）
mCurrentFrame = Frame(
mImGray, //灰度图像
imDepth, //深度图像
timestamp, //时间戳
mpORBextractorLeft, //ORB特征提取器
mpORBVocabulary, //词典
mK, //相机内参矩阵
mDistCoef, //相机的去畸变参数
mbf, //相机基线*相机焦距
mThDepth, //内外点区分深度阈值
detect_result);
Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const long double &timeStamp, ORBextractor* extractorLeft, ORBextractor* extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth)
:mpORBvocabulary(voc),mpORBextractorLeft(extractorLeft),mpORBextractorRight(extractorRight), mTimeStamp(timeStamp), mK(K.clone()),mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth),
mpReferenceKF(static_cast<KeyFrame*>(NULL))
①帧的ID 自增
mnId=nNextId++;
② 计算图像金字塔的参数
mnScaleLevels = mpORBextractorLeft->GetLevels();
//获取每层的缩放因子
mfScaleFactor = mpORBextractorLeft->GetScaleFactor();
//计算每层缩放因子的自然对数
mfLogScaleFactor = log(mfScaleFactor);
//获取各层图像的缩放因子
mvScaleFactors = mpORBextractorLeft->GetScaleFactors();
//获取各层图像的缩放因子的倒数
mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors();
//TODO 也是获取这个不知道有什么实际含义的sigma^2
mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares();
//计算上面获取的sigma^2的倒数
mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares();
③提取彩色图像的特征点(其实现在已经灰度化成为灰度图像)
- 输入灰度图；
- 输出：提取的特征点
std::vector<cv::KeyPoint> mvKeys和特征点的描述子cv::Mat mDescriptors（每一行与一个特征点关联）
void Frame::ExtractORB(int flag, const cv::Mat &im)
{
//std::cout << "wu 提取特征点啦 " << std::endl;
// 判断是左图还是右图.
if(flag==0)
{
// 左图的话就套使用左图指定的特征点提取器，并将提取结果保存到对应的变量中
// 其实这里的提取器句柄就是一个函数指针...或者说,是运算符更加合适
(*mpORBextractorLeft)( im, /* 目标图像 */
cv::Mat(),
mvKeys, /* 输出变量，用于保存提取后的特征点 */
mDescriptors); /* 输出变量，用于保存特征点的描述子 */
}
else
(*mpORBextractorRight)(im,cv::Mat(),mvKeysRight,mDescriptorsRight);
}
④ 用OpenCV的矫正函数、内参对提取到的特征点进行矫正
- 函数
void Frame::UndistortKeyPoints()， - 输入原始特征点
std::vector<cv::KeyPoint> mvKeys - 输出经过畸变处理后的特征点
std::vector<cv::KeyPoint> mvKeysUn- 判断是否经过了矫正，已经矫正了的话直接将 mvKeys 赋给 mvKeysUn
- 若没有经过矫正，首先将特征点的坐标保存到一个矩阵中
cv::Mat mat(N, 2,CV_32F); - 将矩阵调整为 2 通道，以便 opencv 畸变函数处理
mat = mat.reshape(2); - 调用 OpenCVC 函数去畸变矫正
cv::undistortPoints(mat, /* 输入的特征点坐标 */
mat, /* 输出的特征点坐标，也就是校正后的特征点坐标， NOTICE 并且看起来会自动写入到通道二里面啊 */
mK, /* 相机的内参数矩阵 */
mDistCoef, /* 保存相机畸变参数的变量 */
cv::Mat(), /* 一个空的cv::Mat()类型，对应为函数原型中的R */
mK); /* 相机的内参数矩阵，对应为函数原型中的P */
-
- 将存储坐标的矩阵调整会一个通道
调整回只有一个通道
- 将存储坐标的矩阵调整会一个通道
-
- 将得到的去畸变的点的坐标存储在mvKeysUn 中。
⑤获取图像的深度，并且根据这个深度推算其右图中匹配的特征点的视差
ComputeStereoFromRGBD(imDepth);
⑥初始化本帧的地图点
MapPoint是一个地图点，mvpMapPoints是与的关键点关联的地图点容器
mvpMapPoints = vector<MapPoint*>(N,static_cast<MapPoint*>(NULL));
- 先默认所有的点都是内点
- or
- 记录地图点是否为外点，初始化均为外点false
mvbOutlier = vector<bool>(N,false);
⑦计算去畸变后图像边界，将特征点分配到网格中
- 需要将检测到的特征尽可能的均匀化，后面将特征划分到图像对应的网格中，便于下一步的匹配。但是由于图像失真，存在畸变，转换之后的图像边界信息发生了变化；
// 畸变矫正之后的图像边界
ComputeImageBounds(imGray);
// 这个变量表示一个图像像素列相当于多少个图像网格列.
// mnMax(Min)X(Y) 是畸变矫正以后的边界
mfGridElementWidthInv=static_cast<float>(FRAME_GRID_COLS)/static_cast<float>(mnMaxX-mnMinX);
// 这个也是一样，不过代表是图像网格行.
mfGridElementHeightInv=static_cast<float>(FRAME_GRID_ROWS)/static_cast<float>(mnMaxY-mnMinY);
⑧ 计算立体匹配的 baseline
mb = mbf/fx;
⑨ 将特征点分配到图像网格中
（这个过程一般是在第一帧或者是相机标定参数发生变化之后进行）
- 实现函数：
void Frame::AssignFeaturesToGrid()，将关键点分布到 64 * 48 分割而成的网格中,为了加速匹配和均匀化关键点分布- 为图像中每个网格预分配空间
for(unsigned int i=0; i<FRAME_GRID_COLS;i++)
for (unsigned int j=0; j<FRAME_GRID_ROWS;j++)
mGrid[i][j].reserve(nReserve);
bool Frame::PosInGrid()函数返回特征点所在的网格
bool Frame::PosInGrid( const cv::KeyPoint &kp, /* 输入，指定的特征点 */
int &posX, /* 输出：指定的图像特征点所在的图像网格的横纵id（其实就是图像网格的坐标） */
int &posY)
- 将特征点分配到对应的网格中
mGrid[nGridPosX][nGridPosY].push_back(i);
4.3.3.4 开始跟踪 Track（）
Track();
4.3.3.4.1 初始化
- 双目RGBD相机的初始化共用一个函数
if(mSensor==System::STEREO || mSensor==System::RGBD)
//双目RGBD相机的初始化共用一个函数
StereoInitialization();
void Tracking::StereoInitialization()；都干了些什么？
答：
1.检查当前帧的特征点数量
2.设定初始位姿
3.创建初始关键帧
4.将关键帧插入地图
5.创建地图点并与关键帧关联
6.在局部地图中添加初始关键帧
7.更新上一帧和关键帧信息
8.更新局部地图点
9.更新地图绘制器中的相机位姿
10.设置追踪状态
总的来说，这个函数的主要操作是完成系统的初始化，通过双目视觉生成初始地图和关键帧，并建立特征点与地图点之间的关联，确保SLAM系统能够在后续的帧中进行有效的追踪和定位。
4.3.3.4.2 跟踪进入正常SLAM模式，有地图更新
① 在线 SLAM 模式
- 如果初始化正常
- 检查并更新上一帧被替换的地图点
void Tracking::CheckReplacedInLastFrame()； - 如果跟丢了或者重定位之后，通过 BoW 的方式在参考帧中找当前帧特征点的匹配点
bool Tracking::TrackReferenceKeyFrame()； - 如果正常跟踪，通过投影的方式在参考帧中找当前帧特征点的匹配点 bool
Tracking::TrackWithMotionModel() - 如果初始化不正常
- 只能重定位了 bool Tracking::Relocalization()
TrackReferenceKeyFrame()作用：按照关键帧来进行Track，从关键帧中查找Bow相近的帧，进行匹配优化位姿
1）按照关键帧进行Track的方法和运动模式恢复相机运动位姿的方法接近。首先求解当前帧的BOW向量。
2）再搜索当前帧和关键帧之间的关键点匹配关系，如果这个匹配关系小于15对的话，就Track失败了。
3）接着将当前帧的位置假定到上一帧的位置那里
4）并通过最小二乘法优化相机的位姿。
5）最后依然是抛弃无用的杂点，当match数大于等于10的时候，返回true成功。
TrackWithMotionModel()的作用：
1）先通过上一帧的位姿和速度预测当前帧相机的位姿
2）通过PnP方法估计相机位姿，在将上一帧的地图点投影到当前固定大小范围的帧平面上，如果匹配点少，那么扩大两倍的采点范围。
3）然后进行一次BA算法，通过最小二乘法优化相机的位姿
4）优化位姿之后，对当前帧的关键点和地图点，抛弃无用的杂点，剩下的点供下一次操作使用。若多点被观察数大于10返回true,否则false
② 定位模式（只进行跟踪，不进行建图）
- 如果跟丢了，则进行重定位；
- 如果当前帧有足够多的地图点
- 如果有足够的运动，则通过运动模型来跟踪 bool Tracking::TrackWithMotionModel()；
- 如果不满足恒速运动模型，则通过关键帧来定位 bool Tracking::TrackReferenceKeyFrame()；
- 如果当前帧没有足够的地图点
- 当运动模型非空的时候,根据运动模型计算位姿；(当运动模型有效的时候,根据运动模型计算位姿)
- 使用重定位的方法来得到当前帧的位姿；
- 如果重定位没有成功，但是跟踪成功了：增加地图点的观测次数
- 如果重定位成功了，则正常运行（定位与跟踪，更相信重定位）
③ 帧间匹配成功后对 local map 进行跟踪
- 帧间匹配得到初始的姿态后，现在对 local map 进行跟踪得到更多的匹配 bool Tracking::TrackLocalMap()
- 更新局部地图，包括局部关键帧和关键点
- 对局部 MapPoints 进行投影匹配
- 根据匹配对估计当前帧的姿态
- 根据姿态剔除误匹配
- 更新显示线程中的图像、特征点、地图点等信息
mpFrameDrawer->Update(this);
④ 跟踪成功，更新恒速运动模型
- 这里的速度矩阵存储的具体内容是当前帧的位姿乘以上一帧的位姿；
cv::Mat LastTwc = cv::Mat::eye(4,4,CV_32F);
// 这个是转换成为了相机相对世界坐标系的旋转?
mLastFrame.GetRotationInverse().copyTo(LastTwc.rowRange(0,3).colRange(0,3));
mLastFrame.GetCameraCenter().copyTo(LastTwc.rowRange(0,3).col(3));
// 这里的速度矩阵存储的具体内容是当前帧的位姿乘以上一帧的位姿.
mVelocity = mCurrentFrame.mTcw*LastTwc; // 其实就是 Tcl
⑤ 清除观测不到的地图点 Clean VO matches
- 清除 UpdateLastFrame 中为当前帧临时添加的 MapPoints
mCurrentFrame.mvbOutlier[i] = false;
mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
⑥ 清除恒速模型跟踪中 UpdateLastFrame中为当前帧临时添加的MapPoints（仅双目和rgbd）
Delete temporal MapPoints
- 上一步只是在当前帧中将这些 MapPoints 剔除，这里从 MapPoints 数据库中删除
⑦ 检测并插入关键帧，对于双目或RGB-D会产生新的地图点
- bool Tracking::NeedNewKeyFrame() 判断当前帧是否为关键帧；
- void Tracking::CreateNewKeyFrame() 创建新的关键帧；
⑧ 剔除外点
- 删除那些在 bundle adjustment 中检测为 outlier 的 3D map 点
for(int i=0; i<mCurrentFrame.N;i++)
{
// 这里第一个条件还要执行判断是因为, 前面的操作中可能删除了其中的地图点.
if(mCurrentFrame.mvpMapPoints[i] && mCurrentFrame.mvbOutlier[i])
mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
}
⑨ 如果初始化后不久就跟踪失败，并且relocation也没有搞定，只能重新Reset
mpSystem->Reset();
4.3.3.4.3 记录位姿信息，用于最后保存所有的轨迹（Tracking.cc线程结束）
- 这里的关键帧存储的位姿,表示的也是从参考关键帧的相机坐标系到世界坐标系的变换.
// 计算相对姿态Tcr = Tcw * Twr, Twr = Trw^-1
cv::Mat Tcr = mCurrentFrame.mTcw*mCurrentFrame.mpReferenceKF->GetPoseInverse();
//保存各种状态
mlRelativeFramePoses.push_back(Tcr);
mlpReferences.push_back(mpReferenceKF);
mlFrameTimes.push_back(mCurrentFrame.mTimeStamp);
mlbLost.push_back(mState==LOST);
4.4 计算耗时（）返回rgbd_tum.cc
double ttrack= std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();
5. 关闭线程，统计时间，保存轨迹
- List item关闭所有线程
SLAM.Shutdown();
- 统计跟踪时间
//统计分析追踪耗时
sort(vTimesTrack.begin(),vTimesTrack.end());
float totaltime = 0;
for(int ni=0; ni<nImages; ni++)
{
totaltime+=vTimesTrack[ni];
}
cout << "-------" << endl << endl;
cout << "median tracking time: " << vTimesTrack[nImages/2] << endl;
cout << "mean tracking time: " << totaltime/nImages << endl;
- 保存相机轨迹
- 实现函数 void System::SaveKeyFrameTrajectoryTUM(const string &filename)
- 获取每个关键帧的旋转（并转化为四元数表示）和平移
cv::Mat R = pKF->GetRotation().t();
vector<float> q = Converter::toQuaternion(R);
cv::Mat t = pKF->GetCameraCenter();
扫一扫
728
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
