注：本文从 mono_tum.cc 文件开始分析（单目） ORB-SLAM2 的完整流程，重点关注的步骤和执行顺序，函数的具体实现大部分只是略讲，后面系列的笔记会有详细的解读。

ORB-SLAM2 从 mono_tum.cc 开始分析

1. 加载图像

  • 输入数据集路径 vector<string> vstrImageFilenames = /dataset/rgbd_dataset_freiburg1_xyz;
  • 读取包含数据集中图像名的 txt 文件 string strFile = string(argv[3])+"/rgb.txt";
  • 加载图像 LoadImages(strFile, vstrImageFilenames, vTimestamps);
    • 加载图像函数 LoadImages() 
      1
2
3

      void LoadImages(const string &strFile, 
                vector<string> &vstrImageFilenames, 
                vector<double> &vTimestamps)
    • 获取每帧图像的文件名 vector<string> vstrImageFilenames; 和时间戳 vector<double> vTimestamps;

2. 初始化 SLAM 系统

  • 通过 System 类的构造函数初始化一个 SLAM 系统
1
2
3
4

ORB_SLAM2::System SLAM( argv[1], 
                        argv[2], 
                        ORB_SLAM2::System::MONOCULAR, 
                        true);
  • 构造函数位于 System.cc
1
2
3
4
5
6
7
8
9

System::System( const string &strVocFile,      /* ORB 词典路径 */
                const string &strSettingsFile, /* 配置文件路径 */
                const eSensor sensor,          /* 传感器类型 */
                const bool bUseViewer)  :   mSensor(sensor),                      /* 初始化传感器的类型 */
                                            mpViewer(static_cast<Viewer*>(NULL)), /* 视图类 Viewer 的对象 */
                                            mbReset(false),                       /* 复位标志 ，否 */
                                            mbActivateLocalizationMode(false),    /* 模式转换标志位 ，无 */
                                            mbDeactivateLocalizationMode(false)   /* 模式转换标志位 ，无 */
{

2.1 相机相关

  • 相机传感器类型：mSensor==MONOCULAR；
  • 读取相机配置文件路径：cv::FileStorage fsSettings(strSettingsFile.c_str(), cv::FileStorage::READ);。

2.2 加载 ORB 词典

  • 创建一个词典对象：mpVocabulary = new ORBVocabulary();
  • 加载 ORB 词典：mpVocabulary->loadFromBinaryFile(strVocFile);
    • loadFromBinaryFile() 函数位于 ~/ORB_SLAM2/Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h 中 
      1
2
3

      template<class TDescriptor, class F>
bool TemplatedVocabulary<TDescriptor,F>::loadFromBinaryFile(const std::string &filename) 
{}

2.3 创建关键帧数据集 mpKeyFrameDatabase

1

mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);

2.4 创建地图类的对象 mpMap

1

mpMap = new Map();

2.5 创建创建帧绘制器 mpFrameDrawer 和地图绘制器 mpMapDrawer

1
2

mpFrameDrawer = new FrameDrawer(mpMap);
mpMapDrawer = new MapDrawer(mpMap, strSettingsFile);

2.6 初始化跟踪线程 mpTracker

位于 Tracking.cc 文件中

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21

Tracking::Tracking( System *pSys,                 /* 指针 */
                    ORBVocabulary* pVoc,          /* 字典 */
                    FrameDrawer *pFrameDrawer,    /* 帧绘制器 */
                    MapDrawer *pMapDrawer,        /* 地图绘制器 */
                    Map *pMap,                    /* 地图 */
                    KeyFrameDatabase* pKFDB,      /* 关键帧 */
                    const string &strSettingPath, /* 配置文件 */
                    const int sensor):            /* 传感器类型 */
                        mState(NO_IMAGES_YET),                          // 当前系统还没有准备好.
                        mSensor(sensor), 
                        mbOnlyTracking(false), 
                        mbVO(false), 
                        mpORBVocabulary(pVoc),
                        mpKeyFrameDB(pKFDB), 
                        mpInitializer(static_cast<Initializer*>(NULL)), 
                        mpSystem(pSys), 
                        mpViewer(NULL),
                        mpFrameDrawer(pFrameDrawer), 
                        mpMapDrawer(pMapDrawer), 
                        mpMap(pMap), 
                        mnLastRelocFrameId(0)

2.6.1 读取配置文件

  • ① 构造相机内参矩阵
 1
 2
 3
 4
 5
 6
 7
 8
 9
10

//     |fx  0   cx|
// K = |0   fy  cy|
//     |0   0   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);
  • ② 图像矫正系数
1
2
3
4
5
6
7
8

// [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"];
DistCoef.copyTo(mDistCoef);
  • ③ 提取 ORB 特征的相关参数
1
2
3
4
5

int nFeatures = fSettings["ORBextractor.nFeatures"];        // 每一帧提取的特征点数 1000.
float fScaleFactor = fSettings["ORBextractor.scaleFactor"]; // 图像建立金字塔时的变化尺度 1.2.
int nLevels = fSettings["ORBextractor.nLevels"];            // 尺度金字塔的层数 8.
int fIniThFAST = fSettings["ORBextractor.iniThFAST"];       // 提取fast特征点的默认阈值 20.
int fMinThFAST = fSettings["ORBextractor.minThFAST"];       // 如果默认阈值提取不出足够fast特征点，则使用最小阈值 8.

2.6.2 创建特征点提取器

  • 创建 ORB 特征点提取器
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12

mpORBextractorLeft = new ORBextractor(  nFeatures,
                                        fScaleFactor,
                                        nLevels,
                                        fIniThFAST,
                                        fMinThFAST);
// 在单目初始化的时候，会用 mpIniORBextractor 来作为特征点提取器.
if(sensor==System::MONOCULAR)
   mpIniORBextractor = new ORBextractor(   2*nFeatures,
                                           fScaleFactor,
                                           nLevels,
                                           fIniThFAST,
                                           fMinThFAST);
  • ORBextractor 构造函数：位于 ORBextractor.cc 中

2.7 初始化局部建图线程并运行

2.7.1 初始化局部建图线程

1

mpLocalMapper = new LocalMapping(mpMap, mSensor == MONOCULAR);
  • LocalMapping 构造函数位于 LocalMapping.cc 中

2.7.2 运行局部建图线程

1

mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run,mpLocalMapper);
  • ORB_SLAM2::LocalMapping::Run 位于 LocalMapping.cc 中
  • bool LocalMapping::CheckNewKeyFrames() 函数检查关键帧列表是否为空，然后执行以下步骤
2.7.2.1 插入关键帧到地图中
1

void LocalMapping::ProcessNewKeyFrame(){}
  • TODO
2.7.2.2 剔除上一步引入的不合格的地图点
1

void LocalMapping::MapPointCulling(){}
  • TODO
2.7.2.3 与相邻帧三角化恢复一些地图点
1

void LocalMapping::CreateNewMapPoints(){}
  • TODO
2.7.2.4 检查并融合相邻帧的公共特征点
1

void LocalMapping::SearchInNeighbors(){}
  • TODO
2.7.2.5 执行局部 BA
  • 局部 BA 位于 src/Optimizer.cc 文件中
1
2

if(mpMap->KeyFramesInMap() > 2)
     Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame, &mbAbortBA, mpMap);
  • TODO
2.7.2.6 剔除冗余关键帧
1

void LocalMapping::KeyFrameCulling(){}
2.7.2.7 将当前帧加入到闭环检测队列
1

mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);

1

void LocalMapping::InsertKeyFrame(KeyFrame *pKF){}

2.8 初始化闭环线程并运行.

2.8.1 初始化闭环检测线程

  • LoopClosing 构造函数位于 LoopClosing.cc 文件中
1
2
3
4

mpLoopCloser = new LoopClosing( mpMap,              /* 地图 */
                                mpKeyFrameDatabase, /* 关键帧 */
                                mpVocabulary,       /* 字典 */
                                mSensor!=MONOCULAR);/* 传感器类型 */

2.8.2 运行闭环检测线程

1

mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run, mpLoopCloser);
  • ORB_SLAM2::LoopClosing::Run() 位于 LoopClosing.cc 文件中
2.8.2.1 闭环
  • ① 闭环检测 
    1

    bool LoopClosing::DetectLoop(){}
    • TODO
  • ② sim3 计算 
    1

    bool LoopClosing::ComputeSim3(){}
    • TODO
  • ③ 执行闭环优化 
    1

    void LoopClosing::CorrectLoop(){}

2.9 初始化可视化线程并运行.

2.9.1 初始化可视化线程

  • Viewer 构造函数位于 Viewer.cc 文件中
1
2
3
4
5

mpViewer = new Viewer(  this, 
                        mpFrameDrawer,              /* 帧绘制器 */
                        mpMapDrawer,                /* 地图绘制器 */
                        mpTracker,                  /* 跟踪线程跟踪器 */
                        strSettingsFile);           /* 配置文件 */

2.9.2 运行可视化线程

1

mptViewer = new thread(&Viewer::Run, mpViewer);
  • Viewer::Run() 位于 Viewer.cc 文件中
2.9.2.1 Pangolin 参数设置
  • TODO
2.9.2.2 获得相机位姿
1

mpMapDrawer->GetCurrentOpenGLCameraMatrix(Twc);
2.9.2.3 根据相机位姿调整视角
1

s_cam.Follow(Twc);
2.9.2.4 在线 SLAM 模式或定位模式
1
2

mpSystem->ActivateLocalizationMode();
mpSystem->DeactivateLocalizationMode();
  • TODO
2.9.2.5 绘制
  • 绘制当前相机
1

void MapDrawer::DrawCurrentCamera(pangolin::OpenGlMatrix &Twc){}
  • 绘制关键帧
1

void MapDrawer::DrawCurrentCamera(pangolin::OpenGlMatrix &Twc){}
  • 绘制地图点
1

void MapDrawer::DrawMapPoints(){}
2.9.2.6 OpenCV 显示特征提取视图
1

cv::Mat im = mpFrameDrawer->DrawFrame();
  • 函数 cv::Mat FrameDrawer::DrawFrame(){} 位于 FrameDrawer.cc 文件中
    • TODO
2.9.2.7 复位响应

3. 主循环逐帧处理

3.1 读取每一帧图像及其时间戳

1
2

cv::Mat im = cv::imread(string(argv[3]) + "/" + vstrImageFilenames[ni], CV_LOAD_IMAGE_UNCHANGED);
double tframe = vTimestamps[ni];

3.2 跟踪单目图像

  • cv::Mat System::TrackMonocular(const cv::Mat &im, const double &timestamp)
    • 输入：图像和时间戳
    • 返回：相机位姿（关键帧的相机位姿，KeyFrame.h）

3.2.1 检查模式变换检测

  • 可视化窗口上的勾选框不变化的时候不会进行检测，若有变化，则会执行一次（执行完又恢复原标志位）
    • 激活定位模式：mbActivateLocalizationMode
    • 取消激活定位模式：mbDeactivateLocalizationMode
  • 检查是否重置 mbReset
    • void Tracking::Reset()

3.2.3 开始运动估计

  • Tracking::GrabImageMonocular 函数，返回当前帧位姿
1
2

cv::Mat Tracking::GrabImageMonocular(   const cv::Mat &im, 
                                        const double &timestamp)
3.2.3.1 将输入图像转化为灰度图
1

cvtColor(mImGray,mImGray,CV_RGB2GRAY);
3.2.3.2 构造帧
  • 在没有图像时或者没有初始化时采用 mpIniORBextractor 特征提取器（特征点数量是初始化之后的两倍）
  • 初始化之后采用 mpORBextractorLeft 特征提取器（数量为 1000 ）
1
2
3
4
5

mpORBextractorLeft = new ORBextractor(  nFeatures,
                                        fScaleFactor,
                                        nLevels,
                                        fIniThFAST,
                                        fMinThFAST);
  • 构造当前帧（在 Frame.cc 中）
1
2
3
4
5
6
7
8

mCurrentFrame = Frame(  mImGray,            /* 灰度图 */
                        timestamp,          /* 时间戳 */
                        mpIniORBextractor,  /* 单目初始化 mpIniORBextractor 提取特征*/
                        mpORBVocabulary,    /* 词典 */
                        mK,                 /* 相机内参矩阵 */
                        mDistCoef,          /* 相机的去畸变参数 */
                        mbf,                /* 相机基线*相机焦距 */
                        mThDepth);          /* 内外点区分深度阈值 */
① 计算图像金字塔的参数
② 提取特征点
  • 输入灰度图；
  • 输出：提取的特征点 std::vector<cv::KeyPoint> mvKeys 和特征点的描述子 cv::Mat mDescriptors（每一行与一个特征点关联）
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16

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);
}
③ 对提取的特征点进行矫正
  • 函数 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 函数去畸变矫正 
      1
2
3
4
5
6

      cv::undistortPoints(mat,        /* 输入的特征点坐标 */
                    mat,        /* 输出的特征点坐标，也就是校正后的特征点坐标， NOTICE 并且看起来会自动写入到通道二里面啊 */
                    mK,         /* 相机的内参数矩阵 */
                    mDistCoef,  /* 保存相机畸变参数的变量 */
                    cv::Mat(),  /* 一个空的cv::Mat()类型，对应为函数原型中的R */
                    mK);        /* 相机的内参数矩阵，对应为函数原型中的P */
    • 将存储坐标的矩阵调整会一个通道 调整回只有一个通道
    • 将得到的去畸变的点的坐标存储在mvKeysUn 中。
④ 初始化本帧地图点
  • MapPoint 是一个地图点，mvpMapPoints 是与的关键点关联的地图点容器
1

mvpMapPoints = vector<MapPoint*>(N, static_cast<MapPoint*>(NULL));
  • 先默认所有的点都是内点
1

mvbOutlier = vector<bool>(N,false);
⑤ 计算畸变矫正的图像边界
  • 需要将检测到的特征尽可能的均匀化，后面将特征划分到图像对应的网格中，便于下一步的匹配。但是由于图像失真，存在畸变，转换之后的图像边界信息发生了变化；
1
2
3
4
5
6
7
8

// 畸变矫正之后的图像边界
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
1

mb = mbf/fx;
⑦ 将特征点分配到图像网格中
  • 实现函数：void Frame::AssignFeaturesToGrid()，将关键点分布到 64 * 48 分割而成的网格中,为了加速匹配和均匀化关键点分布
    • 为图像中每个网格预分配空间 
      1
2
3

      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() 函数返回特征点所在的网格 
      1
2
3

      bool Frame::PosInGrid(  const cv::KeyPoint &kp,     /* 输入，指定的特征点 */
                        int &posX,                  /* 输出：指定的图像特征点所在的图像网格的横纵id（其实就是图像网格的坐标） */
                        int &posY)
    • 将特征点分配到对应的网格中 
      1

      mGrid[nGridPosX][nGridPosY].push_back(i);

3.2.3.3 开始跟踪 Track()

3.2.3.3.1 初始化
  • 单目初始化：void Tracking::MonocularInitialization()，并行地计算基础矩阵和单应性矩阵，选取其中一个模型，恢复出最开始两帧之间的相对姿态以及点云得到初始两帧的匹配、相对运动、初始MapPoints；
3.2.3.3.2 跟踪
① 在线 SLAM 模式
  • 如果初始化正常
    • 检查并更新上一帧被替换的地图点 void Tracking::CheckReplacedInLastFrame()；
    • 如果跟丢了或者重定位之后，通过 BoW 的方式在参考帧中找当前帧特征点的匹配点 bool Tracking::TrackReferenceKeyFrame()；
    • 如果正常跟踪，通过投影的方式在参考帧中找当前帧特征点的匹配点 bool Tracking::TrackWithMotionModel()。
  • 如果初始化不正常
    • 只能重定位了 bool Tracking::Relocalization()
② 定位模式（只进行跟踪，不进行建图）
  • 如果跟丢了，则进行重定位；
  • 如果当前帧有足够多的地图点
    • 如果有足够的运动，则通过运动模型来跟踪 bool Tracking::TrackWithMotionModel()；
    • 如果不满足恒速运动模型，则通过关键帧来定位 bool Tracking::TrackReferenceKeyFrame()；
  • 如果当前帧没有足够的地图点
    • 当运动模型非空的时候,根据运动模型计算位姿；
    • 使用重定位的方法来得到当前帧的位姿；
      • 如果重定位没有成功，但是跟踪成功了：增加地图点的观测次数
      • 如果重定位成功了，则正常运行（定位与跟踪，更相信重定位）.
③ 帧间匹配成功后对 local map 进行跟踪
  • 帧间匹配得到初始的姿态后，现在对 local map 进行跟踪得到更多的匹配 bool Tracking::TrackLocalMap()
    • 更新局部地图，包括局部关键帧和关键点
    • 对局部 MapPoints 进行投影匹配
    • 根据匹配对估计当前帧的姿态
    • 根据姿态剔除误匹配
  • 更新地图
1

mpFrameDrawer->Update(this);
④ 更新恒速模型
  • 这里的速度矩阵存储的具体内容是当前帧的位姿乘以上一帧的位姿；
1
2
3
4
5
6

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
1
2

mCurrentFrame.mvbOutlier[i] = false;
mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
⑥ Delete temporal MapPoints
  • 上一步只是在当前帧中将这些 MapPoints 剔除，这里从 MapPoints 数据库中删除
⑦ 检查并插入关键帧
  • bool Tracking::NeedNewKeyFrame() 判断当前帧是否为关键帧；
  • void Tracking::CreateNewKeyFrame() 创建新的关键帧；
⑧ 剔除外点
  • 删除那些在 bundle adjustment 中检测为 outlier 的 3D map 点
1
2
3
4
5
6

for(int i=0; i<mCurrentFrame.N;i++)
{
     // 这里第一个条件还要执行判断是因为, 前面的操作中可能删除了其中的地图点.
     if(mCurrentFrame.mvpMapPoints[i] && mCurrentFrame.mvbOutlier[i])
           mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
}
3.2.3.3.3 保存位姿信息
  • 这里的关键帧存储的位姿,表示的也是从参考关键帧的相机坐标系到世界坐标系的变换.
1

cv::Mat Tcr = mCurrentFrame.mTcw*mCurrentFrame.mpReferenceKF->GetPoseInverse();

4. 关闭线程，统计时间，保存轨迹

  • 关闭所有线程
1

SLAM.Shutdown();
  • 统计跟踪时间
1
2
3
4
5
6

sort(vTimesTrack.begin(),vTimesTrack.end());
float totaltime = 0;
for(int ni=0; ni<nImages; ni++)
{
    totaltime+=vTimesTrack[ni];
}

4.1 保存相机轨迹

  • 实现函数 void System::SaveKeyFrameTrajectoryTUM(const string &filename)
    • 获取每个关键帧的旋转（并转化为四元数表示）和平移 
      1
2
3

      cv::Mat R = pKF->GetRotation().t();
vector<float> q = Converter::toQuaternion(R);
cv::Mat t = pKF->GetCameraCenter();

至此，整个流程执行结束了。

2019.03.20
wuyanminmaxgmail.com