[学习slam]用ORB SLAM2跑自己的数据集 使用图片序列或摄像头或视频 实践篇

用ORB SLAM2跑自己的图片序列数据集

不跑代码，何谈理解，撸起袖子我们今天一起看看如何用ORB SLAM2跑通自己的数据集。往往我们的数据集是从现实生活的场景中来采集得到的，如果只是为了跑起来玩一下，很多博客都有介绍，大家可以搜一下就有很多参考资料，但是都是简单的玩一下。我们今天主要用的是：带时间戳信息的数据集，往往我们不是跑起来玩一下的，我们需要与现有的groundtruth作比较，我们介绍过TUM数据集以及相关的评测工具，都是需要将我们的运行结果与groundtruth进行时间上的对齐。那么我们就必不可少的会接触时间戳的数据集，那么如何使用呢？接下来我们一起来看看吧。

首先，我们需要知道图片的采集时间，这个可以从采集的仪器（各种各样的）上得到图片与时间一一对应。小树的问题描述：现有数据集视频，首先使用激光SLAM跑出来了一个轨迹，该轨迹作为groundtruth，将相同的数据集用纯视觉SLAM方法跑一下，然后与groundtruth做对比评判误差。

第一步：制作rgb/目录并把图片信息放进去。我们知道ORB SLAM2的输入是图片，首先将视频处理成图片，在windows平台下使用默认视频播放器都是可以处理的，不需要写代码便可以完成，我们可以设置按照1，2，3，4，5...这样的顺序存放。我们的采集频率是30帧/s，那么也按照相关的设置即可，最终得到5147张图片。

图像重新命名 对应时间戳的信息(按照时间戳为名字为图片依次重命)

 

由于我们需要对应时间戳的信息，接下来我们就来为图像重新命名吧。我们按照时间戳为名字为图片依次重命名。由于处理的量较大，我采用的处理思路是：将原来的名字存在一个文本中”yuanlaimm.txt“，将 将来的名字存在”jianglai.txt"文本文件中。接下来使用python实现重命名操作，并创建一个新的文件夹用于生成重命名的图片集合。python代码为：

import os
import shutil

file_old = open("yuanlaimm.txt")
filename_old = []
for line in file_old.readlines():
    line = line.strip()
    filename_old.append(line)
file_old.close()
file_new = open("jianglai.txt")
filename_new = []
for line in file_new.readlines():
    line = line.strip()
    filename_new.append(line)
file_new.close()

output_dir = "./rgb/"
if not os.path.exists(output_dir):
    os.mkdir(output_dir)

for i in range(len(filename_old)):
    print("%s => %s" % (filename_old[i], filename_new[i]))
    shutil.copy(filename_old[i], os.path.join(output_dir, filename_new[i]))

于是，我们将图片存放到ubuntu下某一文件夹中，我的路径是：/home/w/Pictures/leftCam0411/rgb

第二步：制作rgb.txt文件。拿到图片的时间信息，比如这里我的文件是videotime.txt

........
6347  324330090
6348  324363423
6349  324396756
6350  324430089
.....

第一列是帧数，第二列是图片一一对一的时间信息。我们按照TUM的数据集格式去制作一个rgb.txt文本文件，具体参考官方提供的比如”rgbd_dataset_freiburg1_xyz“数据集。先打开看看官方的rgb.txt格式长什么样子：

# color images
# file: 'rgbd_dataset_freiburg1_xyz.bag'
# timestamp filename
1305031102.175304 rgb/1305031102.175304.png
1305031102.211214 rgb/1305031102.211214.png
1305031102.243211 rgb/1305031102.243211.png
1305031102.275326 rgb/1305031102.275326.png
1305031102.311267 rgb/1305031102.311267.png
1305031102.343233 rgb/1305031102.343233.png
......

我们可以发现第一列就是我们在上一篇文章中对应的时间戳信息，第二列对应的是图片的路径（如果不清楚，先浏览一下上一篇文章哦），那么我们现在来写一个文件生成这个rgb.txt吧，小树提供C++代码：

#include 
#include 
#include 
#include 

using namespace std;

int main(int argc, char const *argv[])
{
string a="/home/w/Pictures/videotime.txt";
ifstream f(a);
string temp;
getline(f,temp);
ofstream ff("rgb.txt");
while( !f.eof() && !temp.empty())
{
istringstream line(temp);
string lie;
line >> lie;
line >>lie;
ff<

 

编译运行我们就得到了这个rgb.txt文件，如下图：

第三步，按照ORB SLAM2的格式制作相机标定的文件。我们可以直接copy一份TUM1.yaml然后修改自己相机对应的相机内参和畸变参数就可以了。比如：

Camera.fx: 306.12388801
Camera.fy: 306.10328046
Camera.cx: 626.89620968
Camera.cy: 343.62712523

Camera.k1: 0.0890022226
Camera.k2: 0.0006550354
Camera.p1: 0.0025311656
Camera.p2: -0.001790449
Camera.k3: 0

第四步，执行程序：

./Examples/Monocular/mono_tum  Vocabulary/ORBvoc.txt /home/w/Documents/left.yaml  /home/w/Pictures/leftCam0411

上截图～

慢慢等待，直到程序运行结束，把对应的关键帧轨迹保存在

KeyFrameTrajectory.txt文件中。

 

使用摄像头或视频运行ORB SLAM2

大家可以使用自己的笔记本的摄像头来读取周围的环境以运行ORB SLAM2，看看是不是可以成功运行。如果你的笔记本恰好没有摄像头，你也可以外接一个USB摄像头，这样就不需要非礼的端着笔记本在环境中跑了。有的同学如果使用的是台式机或者摄像头故障也没有关系，你可以实现录制一段视频，然后放入ORB SLAM2中运行一下。

假设大家都是熟悉CMake工程的，并且具备按照ORB SLAM2的github介绍正确运行ORB SLAM2的能力。接下来我们就步入正题。

 

ORB SLAM2提供了若干种数据集中的运行示例，这可以作为我们学习运行自己摄像头程序的参考，因为代码理解后其实结构是十分相似的。对于数据集上的⽰例，ORB-SLAM2 会⾸先读取数据集中的图像，再放到SLAM 中处理。那么对于我们⾃⼰的摄像头，同样可以这样处理。所以最⽅便的⽅案是直接将我们的程序作为⼀个新的可执⾏程序，加⼊到ORB-SLAM2 ⼯程中。

 

首先我们制作一下相机的标定文件，如果不知道如何标定你的摄像头也没有关系，我们可以使用一个不那么好的标定参数进行运行，毕竟只要不使用鱼眼相机之类的相机，ORB SLAM2对于标定参数也不太敏感。那我们先假设一个普通的参数吧。大家可以运行一下看看效果。

%YAML:1.0

#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------

# Camera calibration and distortion parameters (OpenCV) 
Camera.fx: 500.0
Camera.fy: 500.0
Camera.cx: 320.0
Camera.cy: 240.0

Camera.k1: 0
Camera.k2: 0
Camera.p1: 0
Camera.p2: 0
Camera.k3: 0

# Camera frames per second 
Camera.fps: 30.0

# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
Camera.RGB: 0

#--------------------------------------------------------------------------------------------
# ORB Parameters
#--------------------------------------------------------------------------------------------

# ORB Extractor: Number of features per image
ORBextractor.nFeatures: 1000

# ORB Extractor: Scale factor between levels in the scale pyramid     
ORBextractor.scaleFactor: 1.2

# ORB Extractor: Number of levels in the scale pyramid    
ORBextractor.nLevels: 8

# ORB Extractor: Fast threshold
# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.
# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST
# You can lower these values if your images have low contrast            
ORBextractor.iniThFAST: 10
ORBextractor.minThFAST: 5

#--------------------------------------------------------------------------------------------
# Viewer Parameters
#--------------------------------------------------------------------------------------------
Viewer.KeyFrameSize: 0.05
Viewer.KeyFrameLineWidth: 1
Viewer.GraphLineWidth: 0.9
Viewer.PointSize: 2
Viewer.CameraSize: 0.08
Viewer.CameraLineWidth: 3
Viewer.ViewpointX: 0
Viewer.ViewpointY: -0.7
Viewer.ViewpointZ: -1.8
Viewer.ViewpointF: 500

接下来，我们新建一个myslam.cpp文件开始写代码，这段代码定义了一个SLAM对象，然后会打开你⾃带的摄像头（或视频），读取图像，并交给ORB-SLAM2 处理。所有的主要的代码我都在后面加了注释，这里就不再继续增加解释了。

// 该文件将打开你电脑的摄像头，并将图像传递给ORB-SLAM2进行定位

// opencv
#include 

// ORB-SLAM的系统接口
#include "System.h"
#include 
#include    // for time stamp
#include 

using namespace std;

// 参数文件与字典文件
// 如果你系统上的路径不同，请修改它
string parameterFile = "./myslam.yaml";
string vocFile = "./Vocabulary/ORBvoc.txt";

int main(int argc, char **argv) {

    // 声明 ORB-SLAM2 系统
    ORB_SLAM2::System SLAM(vocFile, parameterFile, ORB_SLAM2::System::MONOCULAR, true);

    // 获取相机图像代码
    cv::VideoCapture cap(0);    // change to 1 if you want to use USB camera.

    // 分辨率设为640x480
    cap.set(CV_CAP_PROP_FRAME_WIDTH, 640);
    cap.set(CV_CAP_PROP_FRAME_HEIGHT, 480);

    // 记录系统时间
    auto start = chrono::system_clock::now();

    while (1) {
        cv::Mat frame;
        cap >> frame;   // 读取相机数据
        auto now = chrono::system_clock::now();
        auto timestamp = chrono::duration_cast(now - start);
        SLAM.TrackMonocular(frame, double(timestamp.count())/1000.0);
    }

    return 0;
}

 

同样，这是一个Cmake工程，接下来我们需要讲这个文件与ORB SLAM2进行结合。我们把这些代码文件放在ORB SLAM2/目录下，在ORB SLAM2对应的CMakeLists.txt文件的末尾加入一下代码：

#生成调用摄像头可执行文件
add_executable(myslam myslam.cpp)
target_link_libraries(myslam ${PROJECT_NAME})

 

现在你应该可以运行起来了，感觉如何？是不是还算简单，当然这些代码和文件具体内容还是需要深入理解一下ORB SLAM2的整体的。注意：当ORB SLAM2初始化的时候，请不要直接开始旋转或者运动速度过猛烈，有经验的SLAMer会先左右平移运动让SLAM系统成功初始化然后进行的。

 

接下来，我们看看如何使用事先录制的视频文件运行ORB SLAM2。大家可以使用自己的手机等设备事先录制一段视频，假设保存为myvideo.mp4文件，请在光照不会发生明显剧变，环境中人等的走动不是十分剧烈，纹理特征丰富的场所录制。

然后第二部，我们开始制作一个相机标定的文件。同样我们假设参数。

%YAML:1.0

#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------

# Camera calibration and distortion parameters (OpenCV) 
Camera.fx: 500.0
Camera.fy: 500.0
Camera.cx: 320.0
Camera.cy: 180.0

Camera.k1: 0
Camera.k2: 0
Camera.p1: 0
Camera.p2: 0
Camera.k3: 0

# Camera frames per second 
Camera.fps: 30.0

# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
Camera.RGB: 0

#--------------------------------------------------------------------------------------------
# ORB Parameters
#--------------------------------------------------------------------------------------------

# ORB Extractor: Number of features per image
ORBextractor.nFeatures: 2000

# ORB Extractor: Scale factor between levels in the scale pyramid     
ORBextractor.scaleFactor: 1.2

# ORB Extractor: Number of levels in the scale pyramid    
ORBextractor.nLevels: 8

# ORB Extractor: Fast threshold
# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.
# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST
# You can lower these values if your images have low contrast            
ORBextractor.iniThFAST: 20
ORBextractor.minThFAST: 7

#--------------------------------------------------------------------------------------------
# Viewer Parameters
#--------------------------------------------------------------------------------------------
Viewer.KeyFrameSize: 0.05
Viewer.KeyFrameLineWidth: 1
Viewer.GraphLineWidth: 0.9
Viewer.PointSize: 2
Viewer.CameraSize: 0.08
Viewer.CameraLineWidth: 3
Viewer.ViewpointX: 0
Viewer.ViewpointY: -0.7
Viewer.ViewpointZ: -1.8
Viewer.ViewpointF: 500

 

第三步，我们编写程序myvideo.cpp文件，打开事先录制的视频文件，然后将图片传递给ORB SLAM2进行运行。关键的代码已经加了注释了，就不再一一介绍了。

// 该文件将打开给定的视频文件，并将图像传递给ORB-SLAM2进行定位

// 需要opencv
#include 

// ORB-SLAM的系统接口
#include "System.h"

#include 
#include    // for time stamp
#include 

using namespace std;

// 参数文件与字典文件
// 如果你系统上的路径不同，请修改它
string parameterFile = "./myvideo.yaml";
string vocFile = "./Vocabulary/ORBvoc.txt";

// 视频文件
string videoFile = "./myvideo.mp4";

int main(int argc, char **argv) {

    // 声明 ORB-SLAM2 系统
    ORB_SLAM2::System SLAM(vocFile, parameterFile, ORB_SLAM2::System::MONOCULAR, true);

    // 获取视频图像
    cv::VideoCapture cap(videoFile);    // change to 1 if you want to use USB camera.

    // 记录系统时间
    auto start = chrono::system_clock::now();

    while (1) {
        cv::Mat frame;
        cap >> frame;   // 读取相机数据
        if ( frame.data == nullptr )
            break;

        // rescale because image is too large
        cv::Mat frame_resized;
        cv::resize(frame, frame_resized, cv::Size(640,360));

        auto now = chrono::system_clock::now();
        auto timestamp = chrono::duration_cast(now - start);
        SLAM.TrackMonocular(frame_resized, double(timestamp.count())/1000.0);
        cv::waitKey(30);
    }

    SLAM.Shutdown();
    return 0;
}

 

第四步，我们需要讲这个文件与ORB SLAM2进行结合。我们把这些代码文件放在ORB SLAM2/目录下，在ORB SLAM2对应的CMakeLists.txt文件的末尾加入一下代码：

#生成调用myvideo.mp4 可执行文件
add_executable(myvideo myvideo.cpp)
target_link_libraries(myvideo ${PROJECT_NAME})

 

 

 

建立更小的工程