本文主要是介绍Result of orbslam2 orbslam3 on EuRoC dataset,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!
笔者刚跑了一下orb2和orb3的评估函数,如图所示
整体上结果差不多,可以看到orb2有个小角,推测是由于orb2没有多地图的设计导致它必须一直跟着跑图,而orb3有多地图的设计,可以“自断手臂”然后新图继续跟上。
评估代码来自orb2和orb3原生代码的./evaluation/evaluate_ate_scale.py,写了一个小脚本实现一起运行
#!/bin/bash
pathDatasetEuroc='/home/niumao/orbslam_ws/Datasets/EuRoC/MH_04_difficult'
pathtosave='/home/niumao/orbslam_ws/Datasets'# 这里根据自己的文件路径去调整
cd ../../# 这是orb3生成路径文件的脚本
# echo "Launching MH04 with Stereo sensor"
# ./Examples/Stereo/stereo_euroc ./Vocabulary/ORBvoc.txt ./Examples/Stereo/EuRoC.yaml "$pathDatasetEuroc" ./Examples/Stereo/EuRoC_TimeStamps/MH04.txt "$pathtosave"/temp/dataset-MH04_stereo# 这是orb2生成路径文件的脚本
pathDatasetEurocLeft='/home/niumao/orbslam_ws/Datasets/EuRoC/MH_04_difficult/mav0/cam0/data'
pathDatasetEurocRight='/home/niumao/orbslam_ws/Datasets/EuRoC/MH_04_difficult/mav0/cam1/data'
cd ../
# echo "Launching MH04 with Stereo sensor"
# ./Examples/Stereo/stereo_euroc ./Vocabulary/ORBvoc.txt ./Examples/Stereo/EuRoC.yaml "$pathDatasetEurocLeft" "$pathDatasetEurocRight" ./Examples/Stereo/EuRoC_TimeStamps/MH04.txt# echo "------------------------------------"
# 这里开始代码评估并生成结果pdfecho "Evaluation of MH04 trajectory with Stereo sensor --ORB3--"
python ./evaluation/evaluate_ate_scale.py ./evaluation/Ground_truth/EuRoC_left_cam/MH04_GT_1E9.txt /home/niumao/orbslam_ws/Datasets/temp/dataset-MH04_stereo_f.txt --plot /home/niumao/orbslam_ws/Datasets/temp/MH04_stereo-orb3.pdfecho "Evaluation of MH04 trajectory with Stereo sensor --ORB2--"
python ./evaluation/evaluate_ate_scale.py ./evaluation/Ground_truth/EuRoC_left_cam/MH04_GT_1E9.txt /home/niumao/orbslam_ws/Datasets/temp/CameraTrajectory.txt --plot /home/niumao/orbslam_ws/Datasets/temp/MH04_stereo-orb2.pdf
需要注意的点有:
# associate.py# 由于python版本的不同,这里可能报错,把注释的部分改成非注释的部分即可# first_keys = first_list.keys()
# second_keys = second_list.keys()
first_keys = list(first_list)
second_keys = list(second_list)# evaluate_ate_scale.py# first_stamps = first_list.keys() # 旧版本python
first_stamps = list(first_list)
# 若干代码...~...
# second_stamps = second_list.keys()
second_stamps = list(second_list)
以及数据集的groundtruth文件第一列时间戳的单位是秒的9次方,要处理一下把第一列的数除以1e9,如下脚本为处理的一种参考
# time1e9.py
# 用法:获取MH04_GT.txt,丢掉第一行,将第一列除以1e9,生成MH04_GT_1E9.txt,存在的问题有这样子除出来的浮点型丢了一两个单位的精度,不过实际测试似乎问题不大import sys
pathin = "./Ground_truth/EuRoC_left_cam/MH04_GT.txt" #数据来源
pathout = "./Ground_truth/EuRoC_left_cam/MH04_GT_1E9.txt"
import osf = open(pathin,'r',encoding = 'UTF-8')
open(pathout,"w+").close()list1 = [5000]line = f.readline() # MH04_GT.txt第一行为参数注释,丢掉就行
# line = f.readline()# -----单次调试----
'''
a = line.split(",")
b = a[0] #第一列数
c = a[1:] # c is a listlist1.append(b)
list1.append(c)if b is not None:b = float(b) / 1e9with open(pathout,"a") as f_out:# for i in list1:first = str(b)f_out.write(first + ' ')for j in range(len(c)-1):f_out.write(str(c[j]) + ' ')f_out.write(str(c[(len(c)-1)]))f_out.close()
'''
# ----while line is not None and line !='':line = f.readline()a = line.split(",")b = a[0] #第一列数c = a[1:] # c is a listlist1.append(b)list1.append(c)# with open(pathout,"w") as f_out:# f_out.write(str(list1))# f_out.close()if b != '':b = float(b) / 1e9with open(pathout,"a") as f_out:first = str(b)f_out.write(first + ' ')for j in range(len(c)-1):f_out.write(str(c[j]) + ' ')f_out.write(str(c[(len(c)-1)]))f_out.close()# print(b)f.close()print("write done!")
处理前后结果如图
最后附上生成轨迹的代码
// ORBSLAM3 - stereo_euroc.cc
/**
* This file is part of ORB-SLAM3
*
* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
*
* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public
* License as published by the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even
* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along with ORB-SLAM3.
* If not, see <http://www.gnu.org/licenses/>.
*/#include<iostream>
#include<algorithm>
#include<fstream>
#include<iomanip>
#include<chrono>#include<opencv2/core/core.hpp>#include<System.h>using namespace std;void LoadImages(const string &strPathLeft, const string &strPathRight, const string &strPathTimes,vector<string> &vstrImageLeft, vector<string> &vstrImageRight, vector<double> &vTimeStamps);int main(int argc, char **argv)
{ if(argc < 5){cerr << endl << "Usage: ./stereo_euroc path_to_vocabulary path_to_settings path_to_sequence_folder_1 path_to_times_file_1 (path_to_image_folder_2 path_to_times_file_2 ... path_to_image_folder_N path_to_times_file_N) (trajectory_file_name)" << endl;return 1;}const int num_seq = (argc-3)/2;cout << "num_seq = " << num_seq << endl;bool bFileName= (((argc-3) % 2) == 1);string file_name;if (bFileName){file_name = string(argv[argc-1]);cout << "file name: " << file_name << endl;}// Load all sequences:int seq;vector< vector<string> > vstrImageLeft;vector< vector<string> > vstrImageRight;vector< vector<double> > vTimestampsCam;vector<int> nImages;vstrImageLeft.resize(num_seq);vstrImageRight.resize(num_seq);vTimestampsCam.resize(num_seq);nImages.resize(num_seq);int tot_images = 0;for (seq = 0; seq<num_seq; seq++){cout << "Loading images for sequence " << seq << "...";string pathSeq(argv[(2*seq) + 3]);string pathTimeStamps(argv[(2*seq) + 4]);string pathCam0 = pathSeq + "/mav0/cam0/data";string pathCam1 = pathSeq + "/mav0/cam1/data";LoadImages(pathCam0, pathCam1, pathTimeStamps, vstrImageLeft[seq], vstrImageRight[seq], vTimestampsCam[seq]);cout << "LOADED!" << endl;nImages[seq] = vstrImageLeft[seq].size();tot_images += nImages[seq];}// Vector for tracking time statisticsvector<float> vTimesTrack;vTimesTrack.resize(tot_images);cout << endl << "-------" << endl;cout.precision(17);// Create SLAM system. It initializes all system threads and gets ready to process frames.ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::STEREO, true); // 最后一个标记位是控制是否显示跑的过错可视化界面cv::Mat imLeft, imRight;for (seq = 0; seq<num_seq; seq++){// Seq loopdouble t_resize = 0;double t_rect = 0;double t_track = 0;int num_rect = 0;int proccIm = 0;for(int ni=0; ni<nImages[seq]; ni++, proccIm++){// Read left and right images from fileimLeft = cv::imread(vstrImageLeft[seq][ni],cv::IMREAD_UNCHANGED); //,cv::IMREAD_UNCHANGED);imRight = cv::imread(vstrImageRight[seq][ni],cv::IMREAD_UNCHANGED); //,cv::IMREAD_UNCHANGED);if(imLeft.empty()){cerr << endl << "Failed to load image at: "<< string(vstrImageLeft[seq][ni]) << endl;return 1;}if(imRight.empty()){cerr << endl << "Failed to load image at: "<< string(vstrImageRight[seq][ni]) << endl;return 1;}double tframe = vTimestampsCam[seq][ni];#ifdef COMPILEDWITHC11std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();#elsestd::chrono::monotonic_clock::time_point t1 = std::chrono::monotonic_clock::now();#endif// Pass the images to the SLAM systemSLAM.TrackStereo(imLeft,imRight,tframe, vector<ORB_SLAM3::IMU::Point>(), vstrImageLeft[seq][ni]);#ifdef COMPILEDWITHC11std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();#elsestd::chrono::monotonic_clock::time_point t2 = std::chrono::monotonic_clock::now();#endif#ifdef REGISTER_TIMESt_track = t_resize + t_rect + std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t2 - t1).count();SLAM.InsertTrackTime(t_track);
#endifdouble ttrack= std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();vTimesTrack[ni]=ttrack;// Wait to load the next framedouble T=0;if(ni<nImages[seq]-1)T = vTimestampsCam[seq][ni+1]-tframe;else if(ni>0)T = tframe-vTimestampsCam[seq][ni-1];if(ttrack<T)usleep((T-ttrack)*1e6); // 1e6}if(seq < num_seq - 1){cout << "Changing the dataset" << endl;SLAM.ChangeDataset();}}// Stop all threadsSLAM.Shutdown();// Save camera trajectoryif (bFileName){// const string kf_file = "kf_" + string(argv[argc-1]) + ".txt";// const string f_file = "f_" + string(argv[argc-1]) + ".txt";const string kf_file = string(argv[argc-1]) + "_kf" + ".txt";const string f_file = string(argv[argc-1]) + "_f" + ".txt";SLAM.SaveTrajectoryEuRoC(f_file);SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file);}else{SLAM.SaveTrajectoryEuRoC("CameraTrajectory.txt");SLAM.SaveKeyFrameTrajectoryEuRoC("KeyFrameTrajectory.txt");}return 0;
}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/1e18); //1e9改成了1e18,跑出来第一列时间戳单位为秒 }}
}
//ORBSLAM2 - stereo_euroc.cc
/**
* This file is part of ORB-SLAM2.
*
* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
* For more information see <https://github.com/raulmur/ORB_SLAM2>
*
* ORB-SLAM2 is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* ORB-SLAM2 is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
*/#include<iostream>
#include<algorithm>
#include<fstream>
#include<iomanip>
#include<chrono>#include<opencv2/core/core.hpp>#include<System.h>
#include<unistd.h>
using namespace std;/*** @brief 加载图像* @param[in] strPathLeft 保存左目图像文件名称的文件的路径* @param[in] strPathRight 保存右目图像文件名称的文件的路径* @param[in] strPathTimes 保存图像时间戳的文件的路径* @param[out] vstrImageLeft 左目图像序列中每张图像的文件名* @param[out] vstrImageRight 右目图像序列中每张图像的文件名* @param[out] vTimeStamps 图像序列中每张图像的时间戳(认为左目图像和右目图像的时间戳已经对齐)*/
void LoadImages(const string &strPathLeft, const string &strPathRight, const string &strPathTimes,vector<string> &vstrImageLeft, vector<string> &vstrImageRight, vector<double> &vTimeStamps);int main(int argc, char **argv)
{// step 0 参数检查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;}// step 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;}// step 2 从给出的配置文件中读取去畸变参数// Read rectification parameterscv::FileStorage fsSettings(argv[2], // path_to_settingscv::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;//TODO 目测是经过双目立体矫正后的投影矩阵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;//根据相机内参,去畸变参数以及立体矫正后,得到的新的相机内参来计算从原始图像到处理后理想双目图像的映射矩阵//这个函数的定义参考:[https://blog.csdn.net/u013341645/article/details/78710740]// 左目cv::initUndistortRectifyMap( //计算无畸变和修正转换映射K_l, //输入的相机内参矩阵 (单目标定阶段得到的相机内参矩阵)D_l, //单目标定阶段得到的相机的去畸变参数R_l, //可选的修正变换矩阵,3*3, 从 cv::stereoRectify 得来.如果这个矩阵为空矩阵,那么就将会被设置成为单位矩阵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);// step 3 构造SLAM系统// 获取图像数目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); //true:开启显示界面// Vector for tracking time statisticsvector<float> vTimesTrack;vTimesTrack.resize(nImages);cout << endl << "-------" << endl;cout << "Start processing sequence ..." << endl;cout << "Images in the sequence: " << nImages << endl << endl;// Main loop// step 4 对每一张输入的图像执行追踪cv::Mat imLeft, imRight, imLeftRect, imRightRect;for(int ni=0; ni<nImages; ni++){// Read left and right images from file// step 4.1 读取原始图像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;}// step 4.2 对左右目图像进行双目矫正和去畸变处理//参考博客 [https://blog.csdn.net/sss_369/article/details/52983123]// 左目cv::remap( //重映射,就是把一幅图像中某位置的像素放置到另一个图片指定位置的过程。imLeft, //输入图像imLeftRect, //输出图像M1l, //第一个映射矩阵表M2l, //第二个映射矩阵cv::INTER_LINEAR);// 右目cv::remap(imRight,imRightRect,M1r,M2r,cv::INTER_LINEAR);double tframe = vTimeStamp[ni];// step 4.3 开始计时#ifdef COMPILEDWITHC11std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
#elsestd::chrono::monotonic_clock::time_point t1 = std::chrono::monotonic_clock::now();
#endif// Pass the images to the SLAM system// step 4.5 开始追踪SLAM.TrackStereo(imLeftRect,imRightRect,tframe);// step 4.6 追踪完成,停止计时,计算追踪时间#ifdef COMPILEDWITHC11std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
#elsestd::chrono::monotonic_clock::time_point t2 = std::chrono::monotonic_clock::now();
#endifdouble ttrack= std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();vTimesTrack[ni]=ttrack;// Wait to load the next frame// step 4.7 等待一段时间以符合下一帧图像的时间戳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);}// step 5 追踪过程执行完毕,退出系统// Stop all threadsSLAM.Shutdown();// Tracking time statistics// step 6 统计追踪时间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// step 7 以TUM格式保存轨迹文件(普通帧+ 关键帧)// const string kf_file = string(argv[argc]) + "_kf" + ".txt";// SLAM.SaveTrajectoryTUM(kf_file);SLAM.SaveTrajectoryTUM("CameraTrajectory.txt");return 0;
}//加载图像,和单目中的做法一样
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); //除时间戳让单位到秒}}
}
参考文献:
六哥的注释代码-ORBSLAM2
六哥的注释代码-ORBSLAM3
https://blog.csdn.net/william_hehe/article/details/80160508
slam算法指标
https://blog.csdn.net/quiet_girl/article/details/80113591
评估工具-EVO官方代码
这篇关于Result of orbslam2 orbslam3 on EuRoC dataset的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!