文章目錄
ORB_SLAM2系統的rgbd_tum.cc
rgbd_tum運行命令
~/projects/ORB_SLAM2$ ./Examples/RGB-D/rgbd_tum Vocabulary/ORBvoc.txt Examples/RGB-D/TUM1.yaml /home/q/projects/ORB_SLAM2/TUM_Dataset/rgbd_dataset_freiburg1_xyz /home/q/projects/ORB_SLAM2/TUM_Dataset/rgbd_dataset_freiburg1_xyz/associations.txt
rgbd_tum軌跡可視化
可視化藉助的是視覺SLAM十四講第二版書中的可視化代碼
更換下面的路徑就可以了。
string trajectory_file = "/home/q/projects/ORB_SLAM2/CameraTrajectory.txt";
string trajectory_file = "/home/q/projects/ORB_SLAM2/KeyFrameTrajectory.txt";
rgbd_tum算法原理
gedit /home/q/projects/ORB_SLAM2/Examples/RGB-D/TUM1.yaml
# Camera calibration and distortion parameters (OpenCV)
# 相機內參數
Camera.fx: 517.306408
Camera.fy: 516.469215
Camera.cx: 318.643040
Camera.cy: 255.313989
# 相機畸變係數
Camera.k1: 0.262383
Camera.k2: -0.953104
Camera.p1: -0.005358
Camera.p2: 0.002628
Camera.k3: 1.163314
# 圖像尺度
Camera.width: 640
Camera.height: 480
# Camera frames per second 幀率
Camera.fps: 30.0
# IR projector baseline times fx (aprox.) 基線
Camera.bf: 40.0
# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
Camera.RGB: 1
# Close/Far threshold. Baseline times.
ThDepth: 40.0
# Deptmap values factor
DepthMapFactor: 5000.0
# 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: 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
通過參數文件我們可以發現分廠的簡單，相對於雙目和單目沒有外參數，以爲已經有了深度圖，不需要求深度，所以簡單很多，都是一些簡單算法，輸入深度圖，根據深度圖，和彩色圖就相當於建立了3D空間和2D像素平面之間的關係，實際上整個SLAM過程就是圍繞着這兩者的求解進行展開的，pose_estimation_2d2d.cpp是已知像素點和像素點之間的關係用對極幾何和三角化求解3D點，
pose_estimation_3d2d.cpp用三對點估計位姿P3P
輸入３對3D-2D匹配點，3D點世界座標系的座標，2D 相機成像平面的投影。
pose_estimation_3d3d.cpp一旦3D點在相機座標系下面的座標能夠算出來，我們就得到了3D-3D的對應點，把PnP問題轉換成了ICP問題。
初始化ORB-SLAM2的System系統類的實例化對象SLAM，並傳入參數，初始化參數列表,分別是：字典路徑，參數文件路徑，傳感器類型，是否可視化
// Create SLAM system. It initializes all system threads and gets ready to process frames.
ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::RGBD,true);
Track追蹤:給SLAM對象的TrackRGBD函數傳入初始值分別是：彩色圖 深度圖 時間戳
// Pass the image to the SLAM system
SLAM.TrackRGBD(imRGB,imD,tframe);
Stop all threads 終止SLAM對象調用的所有函數
SLAM.Shutdown();
slam對象調用SaveTrajectoryTUM函數和SaveKeyFrameTrajectoryTUM函數保存位姿
// Save camera trajectory
SLAM.SaveTrajectoryTUM("CameraTrajectory.txt");
SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
rgbd_tum源碼解讀
#include<iostream>
#include<algorithm>
#include<fstream>
#include<chrono>
#include<opencv2/core/core.hpp>
#include<System.h>
using namespace std;
/**
* @brief 加載圖像
* @param strAssociationFilename 關聯文件的訪問路徑
* @param vstrImageFilenamesRGB 彩色圖像路徑序列
* @param vstrImageFilenamesD 深度圖像路徑序列
* @param vTimestamps 時間戳
*/
void LoadImages(
const string &strAssociationFilename,
vector<string> &vstrImageFilenamesRGB,
vector<string> &vstrImageFilenamesD,
vector<double> &vTimestamps);
int main(int argc, char **argv){
if(argc != 5){
cerr << endl << "Usage: ./rgbd_tum path_to_vocabulary path_to_settings path_to_sequence path_to_association" << endl;
return 1;
}
// Retrieve paths to images
// 按順序存放需要讀取的彩色圖像、深度圖像的路徑，以及對應的時間戳的變量
vector<string> vstrImageFilenamesRGB;
vector<string> vstrImageFilenamesD;
vector<double> vTimestamps;
// 從命令行輸入參數中得到關聯文件的路徑
string strAssociationFilename = string(argv[4]);
// 從關聯文件中加載這些信息
LoadImages(strAssociationFilename, vstrImageFilenamesRGB, vstrImageFilenamesD, vTimestamps);
// Check consistency in the number of images and depthmaps
// 彩色圖像和深度圖像數據的一致性檢查
int nImages = vstrImageFilenamesRGB.size();
if(vstrImageFilenamesRGB.empty()){
cerr << endl << "No images found in provided path." << endl;
return 1;
}
else if(vstrImageFilenamesD.size()!=vstrImageFilenamesRGB.size()){
cerr << endl << "Different number of images for rgb and depth." << endl;
return 1;
}
// Create SLAM system. It initializes all system threads and gets ready to process frames.
// 初始化ORB-SLAM2系統
ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::RGBD,true);
// Vector for tracking time statistics
vector<float> vTimesTrack;
vTimesTrack.resize(nImages);
cout << endl << "-------" << endl;
cout << "Start processing sequence ..." << endl;
cout << "Images in the sequence: " << nImages << endl << endl;
// Main loop
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_LOAD_IMAGE_UNCHANGED);
imD = cv::imread(string(argv[3])+"/"+vstrImageFilenamesD[ni],CV_LOAD_IMAGE_UNCHANGED);
double tframe = vTimestamps[ni];
// 確定圖像是否讀取成功，如果不做檢測，即使沒有讀取到圖像，opencv也不會報錯
if(imRGB.empty()){
cerr << endl << "Failed to load image at: "
<< string(argv[3]) << "/" << vstrImageFilenamesRGB[ni] << endl;
return 1;
}
// 宏定義 通過設置特定變量 減少編譯負擔 如果不需要執行 就不編譯
#ifdef COMPILEDWITHC11
std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
#else
std::chrono::monotonic_clock::time_point t1 = std::chrono::monotonic_clock::now();
#endif
// Pass the image to the SLAM system
// Track 追蹤
SLAM.TrackRGBD(imRGB,imD,tframe);
#ifdef COMPILEDWITHC11
std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
#else
std::chrono::monotonic_clock::time_point t2 = std::chrono::monotonic_clock::now();
#endif
// 計算 Track 追蹤 耗時
double ttrack= std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();
// 將每一張圖片的 Track 追蹤 耗時 存儲到容器中
vTimesTrack[ni]=ttrack;
// Wait to load the next frame
// 根據前後兩幀圖像的時間戳好和單張圖像的Track耗時,計算需要休眠的時間，休眠後才加載下一張圖片
double T=0;
/**
ni: 0
cout << "\nnImages: " << nImages << endl;
nImages-1: 791
cout << "\nvTimestamps[ni+1]: " << setprecision(18) << vTimestamps[ni+1] << endl;
vTimestamps[ni+1]: 1305031102.21121407
cout << "\ntframe = vTimestamps[ni]: " << setprecision(18) << vTimestamps[ni] << endl;
tframe = vTimestamps[ni]: 1305031102.17530394
*/
if(ni<nImages-1){
T = vTimestamps[ni+1]-tframe;
// cout << "\nT: " << setprecision(18) << T << endl;
// T: 0.0359101295471191406
}
// 只有兩張圖片的情況？
else if(ni>0){
T = tframe-vTimestamps[ni-1];
// cout << "\nT: " << setprecision(18) << T << endl;
}
// 計算需要等待Track 追蹤 耗時 來設置 睡眠時間
if(ttrack<T){
usleep((T-ttrack)*1e6);
// cout << "\n(T-ttrack)*1e6: " << setprecision(18) << (T-ttrack)*1e6 << endl;
// (T-ttrack)*1e6: 15850.7185471191406
}
}
// Stop all threads 終止SLAM過程
SLAM.Shutdown();
// Tracking time statistics 統計分析追蹤耗時
// 對單張圖片track消耗時間進行排序
sort(vTimesTrack.begin(),vTimesTrack.end());
float totaltime = 0;
// 所欲圖片消耗時間求和
for(int ni=0; ni<nImages; ni++){
totaltime+=vTimesTrack[ni];
}
cout << "-------" << endl << endl;
cout << "median tracking time: " << vTimesTrack[nImages/2] << endl;
cout << "mean tracking time: " << totaltime/nImages << endl;
// Save camera trajectory
SLAM.SaveTrajectoryTUM("CameraTrajectory.txt");
SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
return 0;
}
// 從關聯文件中提取這些需要加載的圖像的路徑和時間戳
void LoadImages(
const string &strAssociationFilename,
vector<string> &vstrImageFilenamesRGB,
vector<string> &vstrImageFilenamesD,
vector<double> &vTimestamps)
{
// 輸入文件流
ifstream fAssociation;
// 打開關聯文件
fAssociation.open(strAssociationFilename.c_str());
// 一直讀取,直到文件結束
while(!fAssociation.eof()){
string s;
// 讀取一行的內容到字符串s中
getline(fAssociation,s);
// 如果不是空行就可以分析數據了
if(!s.empty()){
// 字符串流
stringstream ss;
ss << s;
// 字符串格式: 時間戳 rgb圖像路徑 時間戳 深度圖像路徑
double t;
string sRGB, sD;
ss >> t;
vTimestamps.push_back(t);
ss >> sRGB;
vstrImageFilenamesRGB.push_back(sRGB);
ss >> t;
ss >> sD;
vstrImageFilenamesD.push_back(sD);
}
}
}
ORB_SLAM2系統的stereo_euroc.cc
stereo_euroc運行命令
cd ORB_SLAM2
./Examples/Stereo/stereo_euroc Vocabulary/ORBvoc.txt Examples/Stereo/EuRoC.yaml /home/q/projects/ORB_SLAM2/EuRoC_Dataset/mav0/cam0/data /home/q/projects/ORB_SLAM2/EuRoC_Dataset/mav0/cam1/data Examples/Stereo/EuRoC_TimeStamps/MH01.txt
stereo_euroc運行視頻
ORB_SLAM2|stereo|euroc
https://www.bilibili.com/video/BV1RC4y1a7YV/
stereo_euroc算法原理
EuRoC.yaml文件參數解析
# Camera Parameters.Camera calibration and distortion parameters (OpenCV)
# 相機內參數
Camera.fx: 435.2046959714599
Camera.fy: 435.2046959714599
Camera.cx: 367.4517211914062
Camera.cy: 252.2008514404297
# 相機畸變係數
Camera.k1: 0.0
Camera.k2: 0.0
Camera.p1: 0.0
Camera.p2: 0.0
圖像寬高
Camera.width: 752
Camera.height: 480
# Camera frames per second 幀率
Camera.fps: 20.0
# stereo baseline times fx 基線
Camera.bf: 47.90639384423901
# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
Camera.RGB: 1
# Close/Far threshold. Baseline times.
ThDepth: 35
下面的參數用來對圖像在輸入跟蹤算法之前進行去畸變。
# Stereo Rectification. Only if you need to pre-rectify the images.
# Camera.fx, .fy, etc must be the same as in LEFT.P
LEFT.height: 480 高
LEFT.width: 752 寬
LEFT.D: !!opencv-matrix 畸變係數矩陣
rows: 1
cols: 5
dt: d
data:[-0.28340811(k1), 0.07395907(k2), 0.00019359(k3), 1.76187114e-05(p1), 0.0(p2)]
LEFT.K: !!opencv-matrix 內參矩陣
rows: 3
cols: 3
dt: d
data: [458.654(fx), 0.0, 367.215(cx), 0.0, 457.296(fy), 248.375(cy), 0.0, 0.0, 1.0]
LEFT.R: !!opencv-matrix 旋轉矩陣 對於雙目左目一般爲單位矩陣
rows: 3
cols: 3
dt: d
data: [0.999966347530033, -0.001422739138722922, 0.008079580483432283, 0.001365741834644127, 0.9999741760894847, 0.007055629199258132, -0.008089410156878961, -0.007044357138835809, 0.9999424675829176]
LEFT.P: !!opencv-matrix 投影矩陣 實現空間座標和像素座標的轉換
rows: 3
cols: 4
dt: d
data: [435.2046959714599(fx), 0, 367.4517211914062(cx), 0, 0, 435.2046959714599(fy), 252.2008514404297(cy), 0, 0, 0, 1, 0]
RIGHT.height: 480
RIGHT.width: 752
RIGHT.D: !!opencv-matrix
rows: 1
cols: 5
dt: d
data:[-0.28368365(k1), 0.07451284(k2), -0.00010473(k3), -3.555907e-05(p1), 0.0(p2)]
RIGHT.K: !!opencv-matrix
rows: 3
cols: 3
dt: d
data: [457.587(fx), 0.0, 379.999(cy), 0.0, 456.134(fy), 255.238(cy), 0.0, 0.0, 1]
RIGHT.R: !!opencv-matrix 旋轉矩陣
rows: 3
cols: 3
dt: d
data: [0.9999633526194376, -0.003625811871560086, 0.007755443660172947, 0.003680398547259526, 0.9999684752771629, -0.007035845251224894, -0.007729688520722713, 0.007064130529506649, 0.999945173484644]
RIGHT.P: !!opencv-matrix 投影矩陣
rows: 3
cols: 4
dt: d
data: [435.2046959714599(fx), 0, 367.4517211914062(cx), -47.90639384423901(baseline), 0, 435.2046959714599(fy), 252.2008514404297(cy), 0, 0, 0, 1, 0]
# ORB Parameters
# ORB Extractor: Number of features per image
ORBextractor.nFeatures: 1200
# 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
上面參數有一個生成投影矩陣和更新相機內參數的過程，這個過程需要對圖像進行去畸變，所以要用到畸變係數。
LEFT.D、LEFT.K、LEFT.R是Opencv標定出來的內外參數，LEFT.P是左相機和右相機生成的虛擬相機的參數，包含旋轉和平移(baseline)，更新的是fx、fy、cx、xy。後面使用的時候直接使用P矩陣相當於沒有畸變，所以Camera.k1、Camera.k2、Camera.p1、Camera.p2參數值是零。
相機幾何學——相機投影矩陣（ Camera Projection Matrix）
cv::initUndistortRectifyMap(K_l,D_l,R_l,P_l.rowRange(0,3).colRange(0,3),cv::Size(cols_l,rows_l),CV_32F,M1l,M2l);
cv::initUndistortRectifyMap(K_r,D_r,R_r,P_r.rowRange(0,3).colRange(0,3),cv::Size(cols_r,rows_r),CV_32F,M1r,M2r);
函數原型
CV_EXPORTS_W void initUndistortRectifyMap( InputArray cameraMatrix, InputArray distCoeffs, InputArray R, InputArray newCameraMatrix,Size size, int m1type, OutputArray map1, OutputArray map2 );
在雙目相機的例子中，這個函數調用兩次：一次是爲了每個相機的朝向，經過stereoRectify之後，依次調用cv::stereoCalibrate。但是如果這個雙目相機沒有標定，依然可以使用cv::stereoRectifyUncalibrated直接從單應性矩陣H中計算修正變換。對每個相機，函數計算像素域中的單應性矩陣H作爲修正變換，而不是3D空間中的旋轉矩陣R。R可以通過H矩陣計算得來。
視覺SLAM十四講第二版第五講的程序undistortImage.cpp實際上是沒調用這個函數自己實現了。
根據攝像頭畸變係數對採集到的圖像進行標定去畸變
雙線性插值算法的詳細總結
有助於理解插值算法，上面是通過原圖像和畸變圖像求出兩者的映射矩陣，下面就是用這個映射關係去處理每一個畸變的圖像，存在亞像素沒有值的情況所以需要用到插值算法。
cv::remap(imLeft,imLeftRect,M1l,M2l,cv::INTER_LINEAR);
cv::remap(imRight,imRightRect,M1r,M2r,cv::INTER_LINEAR);
slam對象的TrackStereo函數傳入去畸變後的左目圖像和右目圖像
// Pass the images to the SLAM system
SLAM.TrackStereo(imLeftRect,imRightRect,tframe);
stereo_euroc源碼解讀
#include<iostream>
#include<algorithm>
#include<fstream>
#include<iomanip>
#include<chrono>
#include<opencv2/core/core.hpp>
#include<System.h>
using namespace std;
/**
* @brief 獲得圖像位置
*
* @param const string &strPathLeft, 左目圖像文件名稱
* @param const string &strPathRight, 右目圖像文件名稱
* @param const string &strPathTimes, 圖像時間戳
*
* @param vector<string> &vstrImageLeft, 左目圖像文件名稱
* @param vector<string> &vstrImageRight, 右目圖像文件名稱
* @param vector<double> &vTimeStamps); 左目圖像和右目圖像對齊的時間戳
*/
void LoadImages(
const string &strPathLeft,
const string &strPathRight,
const string &strPathTimes,
vector<string> &vstrImageLeft,
vector<string> &vstrImageRight,
vector<double> &vTimeStamps)
{
ifstream fTimes;
fTimes.open(strPathTimes.c_str());
vTimeStamps.reserve(5000);
vstrImageLeft.reserve(5000);
vstrImageRight.reserve(5000);
while(!fTimes.eof()){
string s;
getline(fTimes,s);
if(!s.empty()){
stringstream ss;
ss << s;
vstrImageLeft.push_back(strPathLeft + "/" + ss.str() + ".png");
vstrImageRight.push_back(strPathRight + "/" + ss.str() + ".png");
double t;
ss >> t;
vTimeStamps.push_back(t/1e9);
}
}
}
int main(int argc, char **argv)
{
/**
* ./Examples/Stereo/stereo_euroc
* Vocabulary/ORBvoc.txt
* Examples/Stereo/EuRoC.yaml
* /home/q/projects/ORB_SLAM2/EuRoC_Dataset/mav0/cam0/data
* /home/q/projects/ORB_SLAM2/EuRoC_Dataset/mav0/cam1/data
* Examples/Stereo/EuRoC_TimeStamps/MH01.txt
*/
if(argc != 6){
cerr << endl << "Usage: "
"./stereo_euroc "
"path_to_vocabulary "
"path_to_settings "
"path_to_left_folder "
"path_to_right_folder "
"path_to_times_file" << endl;
return 1;
}
// Retrieve paths to images
vector<string> vstrImageLeft;
vector<string> vstrImageRight;
vector<double> vTimeStamp;
LoadImages(
string(argv[3]),
string(argv[4]),
string(argv[5]),
vstrImageLeft,
vstrImageRight,
vTimeStamp);
if(vstrImageLeft.empty() || vstrImageRight.empty()){
cerr << "ERROR: No images in provided path." << endl;
return 1;
}
if(vstrImageLeft.size()!=vstrImageRight.size()){
cerr << "ERROR: Different number of left and right images." << endl;
return 1;
}
// Read rectification(畸變) parameters
cv::FileStorage fsSettings(
argv[2],
cv::FileStorage::READ);
if(!fsSettings.isOpened()){
cerr << "ERROR: Wrong path to settings" << endl;
return -1;
}
cv::Mat K_l, K_r, P_l, P_r, R_l, R_r, D_l, D_r;
fsSettings["LEFT.K"] >> K_l;
fsSettings["RIGHT.K"] >> K_r;
fsSettings["LEFT.P"] >> P_l;
fsSettings["RIGHT.P"] >> P_r;
fsSettings["LEFT.R"] >> R_l;
fsSettings["RIGHT.R"] >> R_r;
fsSettings["LEFT.D"] >> D_l;
fsSettings["RIGHT.D"] >> D_r;
int rows_l = fsSettings["LEFT.height"];
int cols_l = fsSettings["LEFT.width"];
int rows_r = fsSettings["RIGHT.height"];
int cols_r = fsSettings["RIGHT.width"];
if(
K_l.empty() ||
K_r.empty() ||
P_l.empty() ||
P_r.empty() ||
R_l.empty() ||
R_r.empty() ||
D_l.empty() ||
D_r.empty() ||
rows_l==0 ||
rows_r==0 ||
cols_l==0 ||
cols_r==0)
{
cerr << "ERROR: Calibration parameters to rectify stereo are missing!" << endl;
return -1;
}
cv::Mat M1l,M2l,M1r,M2r;
/**
* K_l 相機內參矩陣
* D_l 畸變參數
* R_l 變換矩陣
* P_l.rowRange 新的相機內參矩陣
* cv::Size 去畸變之前的圖像尺寸
*/
cv::initUndistortRectifyMap(K_l,D_l,R_l,P_l.rowRange(0,3).colRange(0,3),cv::Size(cols_l,rows_l),CV_32F,M1l,M2l);
cv::initUndistortRectifyMap(K_r,D_r,R_r,P_r.rowRange(0,3).colRange(0,3),cv::Size(cols_r,rows_r),CV_32F,M1r,M2r);
const int nImages = vstrImageLeft.size();
// Create SLAM system. It initializes all system threads and gets ready to process frames.
ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::STEREO,true);
// Vector for tracking time statistics
vector<float> vTimesTrack;
vTimesTrack.resize(nImages);
cout << endl << "-------" << endl;
cout << "Start processing sequence ..." << endl;
cout << "Images in the sequence: " << nImages << endl << endl;
// Main loop
cv::Mat imLeft, imRight, imLeftRect, imRightRect;
for(int ni=0; ni<nImages; ni++)
{
// Read left and right images from file
imLeft = cv::imread(vstrImageLeft[ni],CV_LOAD_IMAGE_UNCHANGED);
imRight = cv::imread(vstrImageRight[ni],CV_LOAD_IMAGE_UNCHANGED);
if(imLeft.empty()){
cerr << endl << "Failed to load image at: "<< string(vstrImageLeft[ni]) << endl;
return 1;
}
if(imRight.empty()){
cerr << endl << "Failed to load image at: "<< string(vstrImageRight[ni]) << endl;
return 1;
}
cv::remap(imLeft,imLeftRect,M1l,M2l,cv::INTER_LINEAR);
cv::remap(imRight,imRightRect,M1r,M2r,cv::INTER_LINEAR);
double tframe = vTimeStamp[ni];
#ifdef COMPILEDWITHC11
std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
#else
std::chrono::monotonic_clock::time_point t1 = std::chrono::monotonic_clock::now();
#endif
// Pass the images to the SLAM system
SLAM.TrackStereo(imLeftRect,imRightRect,tframe);
#ifdef COMPILEDWITHC11
std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
#else
std::chrono::monotonic_clock::time_point t2 = std::chrono::monotonic_clock::now();
#endif
double ttrack= std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();
vTimesTrack[ni]=ttrack;
// Wait to load the next frame
double T=0;
if(ni<nImages-1)
T = vTimeStamp[ni+1]-tframe;
else if(ni>0)
T = tframe-vTimeStamp[ni-1];
if(ttrack<T)
usleep((T-ttrack)*1e6);
}
// Stop all threads
SLAM.Shutdown();
// Tracking time statistics
sort(vTimesTrack.begin(),vTimesTrack.end());
float totaltime = 0;
for(int ni=0; ni<nImages; ni++){
totaltime+=vTimesTrack[ni];
}
cout << "-------" << endl << endl;
cout << "median tracking time: " << vTimesTrack[nImages/2] << endl;
cout << "mean tracking time: " << totaltime/nImages << endl;
// Save camera trajectory
SLAM.SaveTrajectoryTUM("CameraTrajectory.txt");
return 0;
}
ORB_SLAM2系統的mono_tum.cc
mono_tum運行命令
cd ORB_SLAM2
./Examples/Monocular/mono_tum Vocabulary/ORBvoc.txt Examples/Monocular/TUM1.yaml /home/q/projects/ORB_SLAM2/TUM_Dataset/rgbd_dataset_freiburg1_xyz
mono_tum運行視頻
ORB_SLAM2|mono|tum
https://www.bilibili.com/video/BV1Ei4y1G7xa/
mono_tum算法原理
# Camera Parameters. Camera calibration and distortion parameters (OpenCV)
Camera.fx: 517.306408
Camera.fy: 516.469215
Camera.cx: 318.643040
Camera.cy: 255.313989
# 畸變係數
Camera.k1: 0.262383
Camera.k2: -0.953104
Camera.p1: -0.005358
Camera.p2: 0.002628
Camera.k3: 1.163314
# 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: 1
# 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: 20
ORBextractor.minThFAST: 7
#--------------------------------------------------------------------------------------------
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
單目的初始化很麻煩，所以去畸變是在ORB_SLAM2系統裏面做的
所以傳入系統的是原始的沒有處理的圖片。
SLAM.TrackMonocular(im,tframe);
mono_tum源碼解讀
#include<iostream>
#include<algorithm>
#include<fstream>
#include<chrono>
#include<opencv2/core/core.hpp>
#include<System.h>
using namespace std;
/**
* @brief 導入圖片
*
* @param[in] strFile 讀入的文件名稱
* @param[in&out] vstrImageFilenames 彩色圖片名稱
* @param[in&out] vTimestamps 記錄時間戳
*/
void LoadImages(const string &strFile, vector<string> &vstrImageFilenames, vector<double> &vTimestamps)
{
ifstream f;
f.open(strFile.c_str());
// skip first three lines
// 前三行是註釋，跳過
string s0;
getline(f,s0);
getline(f,s0);
getline(f,s0);
while(!f.eof())
{
string s;
getline(f,s);
if(!s.empty())
{
stringstream ss;
ss << s;
double t;
string sRGB;
ss >> t;
vTimestamps.push_back(t);
ss >> sRGB;
vstrImageFilenames.push_back(sRGB);
}
}
}
/**
* @brief
* @param argc
* @param argv
* @return
* ./Examples/Monocular/mono_tum 可執行程序
* Vocabulary/ORBvoc.txt 字典
* Examples/Monocular/TUM1.yaml 數據集參數
* /home/q/projects/ORB_SLAM2/TUM_Dataset/rgbd_dataset_freiburg1_xyz 數據集
*/
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;
}
// Retrieve paths to images
vector<string> vstrImageFilenames;
vector<double> vTimestamps;
string strFile = string(argv[3])+"/rgb.txt";
LoadImages(strFile, vstrImageFilenames, vTimestamps);
int nImages = vstrImageFilenames.size();
// Create SLAM system. It initializes all system threads and gets ready to process frames.
ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true);
// Vector for tracking time statistics
vector<float> vTimesTrack;
vTimesTrack.resize(nImages);
cout << endl << "-------" << endl;
cout << "Start processing sequence ..." << endl;
cout << "Images in the sequence: " << nImages << endl << endl;
// Main loop
cv::Mat im;
for(int ni=0; ni<nImages; ni++){
// Read image from file
im = cv::imread(string(argv[3])+"/"+vstrImageFilenames[ni],CV_LOAD_IMAGE_UNCHANGED);
double tframe = vTimestamps[ni];
if(im.empty()){
cerr << endl
<< "Failed to load image at: " << string(argv[3])
<< "/" << vstrImageFilenames[ni] << endl;
return 1;
}
#ifdef COMPILEDWITHC11
std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
#else
std::chrono::monotonic_clock::time_point t1 = std::chrono::monotonic_clock::now();
#endif
// Pass the image to the SLAM system
SLAM.TrackMonocular(im,tframe);
#ifdef COMPILEDWITHC11
std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
#else
std::chrono::monotonic_clock::time_point t2 = std::chrono::monotonic_clock::now();
#endif
double ttrack= std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();
vTimesTrack[ni]=ttrack;
// Wait to load the next frame
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);
}
// Stop all threads
SLAM.Shutdown();
// Tracking time statistics
sort(vTimesTrack.begin(),vTimesTrack.end());
float totaltime = 0;
for(int ni=0; ni<nImages; ni++){
totaltime+=vTimesTrack[ni];
}
cout << "-------" << endl << endl;
cout << "median tracking time: " << vTimesTrack[nImages/2] << endl;
cout << "mean tracking time: " << totaltime/nImages << endl;
// Save camera trajectory
SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
return 0;
}