用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 <iostream>
#include <string>
#include <fstream>
#include <sstream>
 
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<<lie<<" "<<"rgb/"<<lie<<".png"<<"\n";
getline(f,temp);
}
return 0;
}

編譯運行我們就得到了這個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 <opencv2/opencv.hpp>
 
// ORB-SLAM的系統接口
#include "System.h"
#include <string>
#include <chrono>   // for time stamp
#include <iostream>
 
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<chrono::milliseconds>(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 <opencv2/opencv.hpp>
 
// ORB-SLAM的系統接口
#include "System.h"
 
#include <string>
#include <chrono>   // for time stamp
#include <iostream>
 
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<chrono::milliseconds>(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})

建立更小的工程

用ORB SLAM2跑自己的數據集使用圖片序列或攝像頭或視頻實踐篇

用ORB SLAM2跑自己的圖片序列數據集

圖像重新命名對應時間戳的信息(按照時間戳爲名字爲圖片依次重命)

c++中capacity()函數用法

C++ STL中vector的內存機制和性能分析

c++ vector的內存釋放

Linux內存監視

鏈表ListNode詳細解釋

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結

用ORB SLAM2跑自己的數據集 使用圖片序列或攝像頭或視頻 實踐篇

用ORB SLAM2跑自己的圖片序列數據集

圖像重新命名 對應時間戳的信息(按照時間戳爲名字爲圖片依次重命)

用ORB SLAM2跑自己的數據集使用圖片序列或攝像頭或視頻實踐篇

圖像重新命名對應時間戳的信息(按照時間戳爲名字爲圖片依次重命)