初看单目和RGBD例程

上文中，我们源码编译安装ORB-SLAM2及其依赖， 并且在一个TUM数据集上，测试运行了基于单目相机和RGB-D相机的ORB-SLAM2。 本文，我们先介绍一下这个 TUM 数据集，再解析这两个demo的源文件mono_tum.cc和rgbd_tum.cc。读完本文后我们将对ORB_SLAM2的API有一定的了解，可以对接自己的相机进行稀疏三维重建。

1. 实验数据

我们所使用的TUM数据集是德国慕尼黑工业大学机器视觉组录制的一组RGB-D数据集和真值数据(ground-truth)， 用于评价视觉里程计(Visual Odometry, VO)和视觉SLAM系统。针对不同的场景和应用目的，该数据集被分成了若干个种类，下面的表格中罗列了其中的测试(Testing and Debugging)、 手持SLAM(Handheld SLAM)、机器人SLAM(Robot SLAM)、结构和纹理(Structure vs. Texture)、动态物体(Dynamic Objects)、三维物体重建(3D Object Reconstruction)。 与这些数据相对应的官方还提供了一个验证分类和标定分类，因为没有提供真值数据(ground-truth)，所以没有收录在下表中。数据量比较大，而且下载比较麻烦， 所以我准备了一个百度网盘(提取码:btku)保存这些数据。

展开 数据集基本情况 >>>

这个数据集的录制过程中，一共用到了三个相机，分别被标记为 fr1/freiburg1、fr2/freiburg2、fr3/freiburg3。其中，fr1和fr2是两个不同的 kinect 相机， fr3则是 Asus Xtion。TUM 以30帧每秒的频率保存了分辨率为 640×480 的深度图和彩色图。另外，TUM还通过一个高频的(100Hz)运动捕捉系统测量的相机真实运动轨迹， 它们把这个轨迹称为 ground-truth，用来与实验结果对比，评价SLAM算法或者系统的准确度。

视觉SLAM系统为了将2D的图像信息转换为3D的空间坐标，需要用到相机的内参。由于每台相机的制作过程多少有些差异，都需要专门进行标定，根据特定的相机模型计算内参。 TUM 以比较常用的针孔相机模型，对三个相机的光心、焦距和畸变系数进行了标定。 下面左侧的表格是从官方搬运过来的数据。 ORB-SLAM2把这三个相机的内参写到了配置文件TUM1.yaml, TUM2.yaml, TUM3.yaml中。我们在跑不同的数据集的时候，需要注意根据选用的相机调整相应的配置文件。

相机 fx fy cx cy d0 d1 d2 d3 d4 fr1/freiburg1 517.3 516.5 318.6 255.3 0.2624 -0.9531 -0.0054 0.0026 1.1633 fr2/freiburg2 520.9 521.0 325.1 249.7 0.2312 -0.7849 -0.0033 -0.0001 0.9172 fr3/freiburg3 535.4 539.2 320.1 247.6 0 0 0 0 0

上面右侧是我们在上一节中使用的fr1/xyz数据集的目录结构。 它一共有四个文本文件和两个子目录。其中，associations.txt 是我们为了运行 RGB-D 版本的 ORB-SLAM 通过官方脚本生成的深度图与彩色图的对应关系。 depth 和 rgb 是两个子目录，分别保存了分辨率为 640×480 的深度图和彩色图。文本文件 depth.txt 和 rgb.txt 中记录了采集图像的时间戳和文件名。 文件 accelerometer.txt 记录了一个加表的测量值和时间戳，根据它我们可以大体估计相机的俯仰角和横滚角。groundtruth.txt 记录的则是用来对比实验结果的真值数据。

2. ORB-SLAM2 的编译系统

        cmake_minimum_required(VERSION 2.8)
        project(ORB_SLAM2)
        IF(NOT CMAKE_BUILD_TYPE)
          SET(CMAKE_BUILD_TYPE Release)
        ENDIF()
        set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS}  -Wall  -O3 -march=native ")
        set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall   -O3 -march=native")
        set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")

在正式查看源码之前，我们先来简单看看它的CMakeLists.txt，可以从总体上了解工程的组织方式。 右侧的代码片段是在指定版本、Release 编译方式、设定了一些编译选项。需要注意的是，这里我们改用 c++14 进行编译了，作者原来使用的是 c++11 或 c++0x。

之所以这样，是因为在我们源码编译了 v0.8 版的 Pangolin。这个库后来改用 c++17 进行编译，本来我也想直接改成 17， 但是源码中有一个 bool 变量 mnFullBAIdx 阻止了我。这个变量要进行 '++' 这个在 17 标准中删除的操作。 我本想着，c语言里不是常用整型来表示布尔值嘛，0 对应 false，非 0 对应 true。那我直接将这个变量改成 int 类型好了。但是改完之后，运行程序时总是崩掉(coredump)。不得已，我们走了一个中间的路子， 用 c++14 编译，这样能够正常链接 Pangolin ，bool 类型还有 '++' 运算符。吐个槽，我还是第一次见到 bool 类型做加法运算的。 至于有啥特性，我们到时候再分析吧，那时再看看能改成整型不。

接下来将源码目录中的 cmake_module 添加到 cmake 的包搜索路径中。然后，查找依赖 OpenCV, Pangolin 和 Eigen，并将依赖的头文件目录添加到头文件搜索路径中。 因为我在源码安装 Eigen 的时候指定了安装目录为 ${HOME}/local，所以这里就硬编码了 Eigen 的路径。这样做的另一方面的原因是改起来不麻烦， Eigen 是一个只有头文件的库只需要指定头文件目录就可以了，没有依赖也不需要链接。Pangolin 的 CMake 也被我这样硬编码了 Eigen 的安装目录。

        LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)
        find_package(OpenCV 4.0 REQUIRED)
        find_package(Pangolin REQUIRED)
        SET(EIGEN3_INCLUDE_DIR $ENV{HOME}/local/include/eigen3/)
        include_directories(${PROJECT_SOURCE_DIR} ${PROJECT_SOURCE_DIR}/include ${EIGEN3_INCLUDE_DIR} ${Pangolin_INCLUDE_DIRS})

下面的代码片段在指引编译 ORB-SLAM2 的核心。先通过变量 CMAKE_LIBRARY_OUTPUT_DIRECTORY 指示了编译出来的库文件保存路径 lib。然后把 src 目录下的很多 *.cc 一起编译成动态库。 最后通过指令 target_link_libraries 完成链接。

        set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib)
        add_library(${PROJECT_NAME} SHARED src/System.cc ... 省略了很多 *.cc 文件)
        target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBS} ${Pangolin_LIBRARIES}
            ${PROJECT_SOURCE_DIR}/Thirdparty/DBoW2/lib/libDBoW2.so
            ${PROJECT_SOURCE_DIR}/Thirdparty/g2o/lib/libg2o.so)

剩下的 CMake 代码就是在编译各个 demo。下面我们只列举了 RGB-D 的例程，其它几个例程的编译大同小异。首先通过变量 CMAKE_RUNTIME_OUTPUT_DIRECTORY 指定了编译出的可执行程序的路径。 然后用指令 add_executable 编译带有 main 函数的源文件，最后跟刚才编译出的动态链接库 libORB_SLAM2.so 完成链接生成最后的可执行程序。

        set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/RGB-D)
        add_executable(rgbd_tum Examples/RGB-D/rgbd_tum.cc)
        target_link_libraries(rgbd_tum ${PROJECT_NAME})

        set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Monocular)
        add_executable(mono_tum Examples/Monocular/mono_tum.cc)
        target_link_libraries(mono_tum ${PROJECT_NAME})

总的来说，ORB-SLAM2 的依赖并不多，这个 CMakeLists.txt 也很干净。我们可以看到 "Examples/Monocular/mono_tum.cc" 和 "Examples/RGB-D/rgbd_tum.cc" 分别是单目和RGB-D相机的例程入口。

2. 基于单目相机的demo——mono_tum

我们先来看一下单目例程的入口源文件Examples/Monocular/mono_tum.cc。 这个文件中一共就两个函数，main 函数是C/C++程序的入口控制了整个程序的运行逻辑，LoadImages处理TUM数据集方便ORB-SLAM调用。

下面是main函数的代码片段，在程序运行之初先简单检查一下命令行的输入参数 argc，如果数量不对就在输出日志之后退出。输出日志的这行字符串说明了例程的打开方式， 需要输入3个参数，分别记录着字典文件(argv[1])、相机和算法相关的配置文件(argv[2])、数据包路径(argv[3])。

        int main(int argc, char **argv) {
            if(argc != 4) {
                cerr << endl << "Usage: ./mono_tum path_to_vocabulary path_to_settings path_to_sequence" << endl;
                return 1;
            }

然后创建了两个临时的容器 vstrImageFilenames 和 vTimestamps 分别用于保存图像名称以及采集这些图像时的时间戳。 通过终端输入的参数 argv[3]，找到数据集目录下记录有图像名称和时间戳对应关系的文本文件 rgb.txt，将完整的路径保存在字符串 strFile 中。 最后调用函数 LoadImages 解析该文件并填充容器 vstrImageFilenames 和 vTimestamps。

            vector<string> vstrImageFilenames;
            vector<double> vTimestamps;
            string strFile = string(argv[3])+"/rgb.txt";
            LoadImages(strFile, vstrImageFilenames, vTimestamps);

接着通过ORB-SLAM2的API创建一个单目的SLAM系统， 其中输入参数 argv[1] 指示了特征字典文件的路径，argv[2] 记录了相机的标定参数以及ORB-SLAM系统的运行配置。第三个参数说明建图时使用的传感器是单目相机(Monocular)， 最后一个参数控制了是否创建一个线程来可视化稀疏三维重建结果。

            ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true);

下面的片段获取了数据包中图像数量，同时创建容器 vTimesTrack 为每帧图像分配了一个元素，用于记录处理每帧数据的跟踪时间。

            int nImages = vstrImageFilenames.size();
            vector<float> vTimesTrack;
            vTimesTrack.resize(nImages);

接着就进入了正题，在一个 for 循环中先获取单目图像和时间戳，然后在第 20 行调用接口 TrackMonocular 驱动 TRACKING 线程跟踪相机位姿完成 SLAM 系统的更新。 在调用该接口前后，我们通过标准库 chrono 获取系统时间 t1, t2，两者作差就得到了调用接口的时间。

            cv::Mat im;
            for(int ni=0; ni<nImages; ni++) {
                im = cv::imread(string(argv[3])+"/"+vstrImageFilenames[ni],cv::IMREAD_UNCHANGE);
                double tframe = vTimestamps[ni];
                // 省略 im 为空的检查
                std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
                SLAM.TrackMonocular(im,tframe);
                std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
                double ttrack= std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();
                vTimesTrack[ni]=ttrack;

为了模拟相机的实际运行过程，作者在记录了调用时间之后，通过 usleep 延迟了一段时间才处理下一帧数据。首先计算下一帧到当前帧的时间差， 如果是最后一帧图像的话就用当前帧与上一帧的时间差来替代。在实际的系统当中，我们通常以相机的数据驱动系统运行。 这个 TrackMonocular 实际上只是 SLAM 系统的前端里程计，它的处理过程一般都比较快，可以在下一帧数据到来之前完成，这样才能达到实时跟踪相机和机器人位姿的目的。

                // 延时到下一帧
                double T=0;
                if (ni < nImages-1)
                    T = vTimestamps[ni+1]-tframe;
                else if (ni > 0)
                    T = tframe-vTimestamps[ni-1];
                if (ttrack < T)
                    usleep((T-ttrack)*1e6);
            }

遍历完所有图像后，调用系统接口 Shutdown 停止所有后台线程，通过 SaveKeyFrameTrajectoryTUM 将相机轨迹保存到指定文件中，方便分析算法能力。

            SLAM.Shutdown();
            // 省略时间统计相关代码
            SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
            return 0;
        }

如此看来，ORB-SLAM2 的应用封装接口设计的挺简洁的，只需要构建一个 ORB_SLAM2::System 系统对象，然后将采集到的图像通过接口 TrackMonocular 逐个喂给系统完成定位建图。 最后 Shutdown 关掉系统的后台线程。下面可以看到 RGB-D 相机的 demo 运行过程也是类似的套路，只是从 TrackMonocular 换成了 TrackRGBD。

3. 基于RGBD相机的demo——rgbd_tum

再来看一下 RGB-D 相机例程的入口源文件Examples/Monocular/mono_tum.cc。 同样的，这个文件通过函数 LoadImages 加载 TUM 数据集，在 main 函数中完成了整个程序的运行逻辑。

下面是 main 函数的代码片段，在程序在检查输入参数合法之后，用字符串 strAssociationFilename 从终端输入参数中获取了记录了 RGB 图像与深度图的对应关系的文件路径。 同时创建了容器 vstrImageFilenamesRGB 和 vstrImageFilenamesD 保存一一对应的 RGB 图像和深度图的名称，容器 vTimestamps 记录图像的时间戳。 通过函数 LoadImages 解析相关文件填充这些容器。

        int main(int argc, char **argv) {
            // 省略检查系统输入参数
            vector<string> vstrImageFilenamesRGB;
            vector<string> vstrImageFilenamesD;
            vector<double> vTimestamps;
            string strAssociationFilename = string(argv[4]);
            LoadImages(strAssociationFilename, vstrImageFilenamesRGB, vstrImageFilenamesD, vTimestamps);

在确认RGB图像与深度图数量一致之后，创建一个基于RGBD相机的SLAM系统， 和单目例程类似，其输入参数 argv[1] 和 argv[2] 分别指示了特征字典文件和系统配置文件的路径。区别在于第三个参数，说明建图的时候使用的传感器是 RGBD 相机。

            // 省略检查RGB图像与深度图数量是否一致
            ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::RGBD,true);

下面代码获取图像数量，并创建容器vTimesTrack用于记录处理每帧数据的跟踪时间。

            int nImages = vstrImageFilenames.size();
            vector<float> vTimesTrack;
            vTimesTrack.resize(nImages);

接下来就正式开始SLAM建图，在一个for循环中先获取单目图像和时间戳，然后调用接口TrackMonocular即可完成SLAM系统的更新。

            cv::Mat imRGB, imD;
            for (int ni = 0; ni < nImages; ni++) {
                // Read image and depthmap from file
                imRGB = cv::imread(string(argv[3])+"/"+vstrImageFilenamesRGB[ni],cv::IMREAD_UNCHANGE);
                imD = cv::imread(string(argv[3])+"/"+vstrImageFilenamesD[ni],cv::IMREAD_UNCHANGE);
                double tframe = vTimestamps[ni];
                // 省略时间统计相关代码
                SLAM.TrackRGBD(imRGB,imD,tframe);
                // 省略时间统计和延时相关代码
            }

最后在遍历完所有的图像之后，就调用接口Shutdown停止所有线程，并通过SaveKeyFrameTrajectoryTUM将相机轨迹保存到指定文件中。

            SLAM.Shutdown();
            // 省略时间统计相关代码
            SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
            return 0;
        }

4. 完

通过对demo的入口函数的分析，我们可以看到ORB-SLAM的封装接口还是很简洁的。两个demo的套路如出一辙，都是先加载TUM数据集的图像和时间戳信息， 再创建一个ORB_SLAM2::System的对象，然后在一个for循环中依次把数据喂给该对象来更新SLAM系统，最后结束的时候记录相机轨迹方便分析算法的能力。

在构建 ORB_SLAM2::System 对象的时候，通过参数来指定建图模式，更新 SLAM 系统的时候需要根据不同的建图模式通过 TrackXXX 的接口实现。这个接口只是前端的 TRACKING 线程。 完成定位建图时还需要 Shutdown 结束后台运行的闭环检测全局优化的线程。