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

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

首先，我们需要知道图片的采集时间，这个可以从采集的仪器（各种各样的）上得到图片与时间一一对应。小树的问题描述：现有数据集视频，首先使用激光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.cc文件开始写代码，这段代码定义了一个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.cc)
target_link_libraries(myslam ${PROJECT_NAME})

编译新的ORB_SLAM2:

./build.sh

终端进入ORB_SLAM2文件，开启摄像头运行ORB_SLAM2:

./Examples/Monocular/myslam Vocabulary/ORBvoc.txt myslam.yaml 

注：此处注意各文件的文件路径。

现在你应该可以运行起来了，感觉如何？是不是还算简单，当然这些代码和文件具体内容还是需要深入理解一下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.cc文件，打开事先录制的视频文件，然后将图片传递给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.cc)
target_link_libraries(myvideo ${PROJECT_NAME})

建立更小的工程