使用Kitti360数据集中的双目图像运行ORB-SLAM3

1. 数据下载

登录到kitti360数据集官网数据集下载页面，选择图中的Perspective IMages for Train &Val(128G),会下载一个用于下载数据集的脚本文件download_2d_perspective.sh，在ubuntu系统终端中运行该脚本，为了提高下载速度，在脚本中修改了下载工具

https://www.cvlibs.net/datasets/kitti-360/download.php

download_2d_perspective.sh：

#!/bin/bash#train_list=("2013_05_28_drive_0000_sync"
#            "2013_05_28_drive_0002_sync"
#        "2013_05_28_drive_0003_sync"
#            "2013_05_28_drive_0004_sync"
#            "2013_05_28_drive_0005_sync"
#            "2013_05_28_drive_0006_sync"
#            "2013_05_28_drive_0007_sync"
#            "2013_05_28_drive_0009_sync"
#        "2013_05_28_drive_0010_sync")train_list=("2013_05_28_drive_0000_sync")cam_list=("00" "01")root_dir=KITTI-360
data_2d_dir=data_2d_rawmkdir -p $root_dir
mkdir -p $root_dir/$data_2d_dircd $root_dir # perspective images
for sequence in ${train_list[@]}; dofor camera in ${cam_list[@]}; do zip_file=${sequence}_image_${camera}.zip#这里使用了mwget命令，从而多线程下载数据，原文件为wgetmwget https://s3.eu-central-1.amazonaws.com/avg-projects/KITTI-360/data_2d_raw/${zip_file}unzip -d ${data_2d_dir} ${zip_file} rm ${zip_file}done
done# timestamps
zip_file=data_timestamps_perspective.zip
wget https://s3.eu-central-1.amazonaws.com/avg-projects/KITTI-360/data_2d_raw/${zip_file}
unzip -d ${data_2d_dir} ${zip_file}
rm $zip_file

1. 修改配置文件

这里将ORB-SLAM3/examples/KITTI00-02.yaml文件复制了一份，命名为KITTI360.yaml, 然后修改了三部分，包括相机内参，图像宽高，双目基线

相机内参从https://s3.eu-central-1.amazonaws.com/avg-projects/KITTI-360/384509ed5413ccc81328cf8c55cc6af078b8c444/calibration.zip下载的标定文件中获取，具体是里面的perspective.txt文件中的P_rect_00这一行，获取fx,fy,cx,cy，宽高参考S_rect_00行，双目基线为：0.6，从官网上的设备分布图获取

%YAML:1.0#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------
File.version: "1.0"Camera.type: "Rectified"# Camera calibration and distortion parameters (OpenCV)
Camera1.fx: 552.554261
Camera1.fy: 552.554261
Camera1.cx: 682.049453
Camera1.cy: 238.769549Camera.width: 1408
Camera.height: 376# Camera frames per second
Camera.fps: 10Stereo.b: 0.60# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
Camera.RGB: 1# Close/Far threshold. Baseline times.
Stereo.ThDepth: 35.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.6
Viewer.KeyFrameLineWidth: 2.0
Viewer.GraphLineWidth: 1.0
Viewer.PointSize: 2.0
Viewer.CameraSize: 0.7
Viewer.CameraLineWidth: 3.0
Viewer.ViewpointX: 0.0
Viewer.ViewpointY: -1.8
Viewer.ViewpointZ: -0.1
Viewer.ViewpointF: 2000.0

1. 修改stereo_kitti.cc文件

因为kitti数据集文件存放格式与kitti360不同，因此需要修改部分文件读取代码，集中在LoadImages函数中

/**
* 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 &strPathToSequence, vector<string> &vstrImageLeft,vector<string> &vstrImageRight, vector<double> &vTimestamps);int main(int argc, char **argv)
{if(argc != 4){cerr << endl << "Usage: ./stereo_kitti path_to_vocabulary path_to_settings path_to_sequence" << endl;return 1;}// Retrieve paths to imagesvector<string> vstrImageLeft;vector<string> vstrImageRight;vector<double> vTimestamps;LoadImages(string(argv[3]), vstrImageLeft, vstrImageRight, vTimestamps);const int nImages = vstrImageLeft.size();// 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);float imageScale = SLAM.GetImageScale();// 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;   double t_track = 0.f;double t_resize = 0.f;// Main loopcv::Mat imLeft, imRight;for(int ni=0; ni<nImages; ni++){// Read left and right images from fileimLeft = cv::imread(vstrImageLeft[ni],cv::IMREAD_UNCHANGED); //,cv::IMREAD_UNCHANGED);imRight = cv::imread(vstrImageRight[ni],cv::IMREAD_UNCHANGED); //,cv::IMREAD_UNCHANGED);double tframe = vTimestamps[ni];if(imLeft.empty()){cerr << endl << "Failed to load image at: "<< string(vstrImageLeft[ni]) << endl;return 1;}if(imageScale != 1.f){
#ifdef REGISTER_TIMES#ifdef COMPILEDWITHC11std::chrono::steady_clock::time_point t_Start_Resize = std::chrono::steady_clock::now();#elsestd::chrono::monotonic_clock::time_point t_Start_Resize = std::chrono::monotonic_clock::now();#endif
#endifint width = imLeft.cols * imageScale;int height = imLeft.rows * imageScale;cv::resize(imLeft, imLeft, cv::Size(width, height));cv::resize(imRight, imRight, cv::Size(width, height));
#ifdef REGISTER_TIMES#ifdef COMPILEDWITHC11std::chrono::steady_clock::time_point t_End_Resize = std::chrono::steady_clock::now();#elsestd::chrono::monotonic_clock::time_point t_End_Resize = std::chrono::monotonic_clock::now();#endift_resize = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t_End_Resize - t_Start_Resize).count();SLAM.InsertResizeTime(t_resize);
#endif}#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);#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 + 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-1)T = vTimestamps[ni+1]-tframe;else if(ni>0)T = tframe-vTimestamps[ni-1];if(ttrack<T)usleep((T-ttrack)*1e6);}// Stop all threadsSLAM.Shutdown();// Tracking time statisticssort(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 trajectorySLAM.SaveTrajectoryKITTI("CameraTrajectory.txt");return 0;
}void LoadImages(const string &strPathToSequence, vector<string> &vstrImageLeft,vector<string> &vstrImageRight, vector<double> &vTimestamps)
{ifstream fTimes;//修改时间戳文件读取位置string strPathTimeFile = strPathToSequence + "/image_00/timestamps.txt";std::cout<<strPathTimeFile<<std::endl;fTimes.open(strPathTimeFile.c_str());while(!fTimes.eof()){string s;getline(fTimes,s);std::cout<<s<<std::endl;if(!s.empty()){//这里使用sscanf读取时间戳文件中2013-05-28 08:46:02.904483072形式的时间double t;int year, month, day, hour,minute;//ss >> t;sscanf(s.c_str(),"%04d-%02d-%02d %02d:%02d:%02lf",&year,&month, &day,&hour,&minute,&t);t += minute*60 + hour*3600;vTimestamps.push_back(t);}}//图像位置修改string strPrefixLeft = strPathToSequence + "/image_00/data_rect/";string strPrefixRight = strPathToSequence + "/image_01/data_rect/";const int nTimes = vTimestamps.size();vstrImageLeft.resize(nTimes);vstrImageRight.resize(nTimes);for(int i=0; i<nTimes; i++){stringstream ss;//将setw(6)改为setw（10）ss << setfill('0') << setw(10) << i;vstrImageLeft[i] = strPrefixLeft + ss.str() + ".png";vstrImageRight[i] = strPrefixRight + ss.str() + ".png";}
}

运行：

./Examples/Stereo/stereo_kitti360 ./Vocabulary/ORBvoc.txt ./Examples/Stereo/KITTI360.yaml /media/YiJiang/EAGET忆捷/dataSet/kitti360/KITTI-360/data_2d_raw/2013_05_28_drive_0000_sync

1. 运行效果：

运行时比较卡顿，可能是电脑性能问题。

1. 轨迹处理：

orb-slam3生成的轨迹文件CameraTrajectory.txt每行有12个数据，表示3*4的变换矩阵，而kitti360提供的poses.txt文件每行有13个数据，第一个数据表示对应的索引，同时并不是每一帧图片都有对应的位姿，因此，需要根据对现有的文件做一定修改

1.根据poses.txt中的index索引，选取CameraTrajectory.txt文件中的部分行的位姿数据生成新的文件norbpose.txt文件

2.将poses.txt文件中第一行索引去掉，仅保留变换数据，生成新的nposes.txt文件

修改代码如下：

#include <iostream>
#include <fstream>
#include <vector>
#include <string>
using namespace std;struct K360Poses
{int index;double a00, a01, a02, a03, a10, a11, a12, a13, a20, a21, a22, a23;
};int main(){//原始的两个位姿文件string posesfile = "./poses.txt";string orbposefile = "./orbpose.txt";string outposesfileN = "./nposes.txt";string outorbposefileN = "./norbpose.txt";vector<struct K360Poses> kposes;vector<struct K360Poses> orbposes;ofstream outkposefile;outkposefile.open(outposesfileN,ios::out);ofstream outorbposefile;outorbposefile.open( outorbposefileN,ios::out);ifstream fint(orbposefile);while(!fint.eof()){struct K360Poses epose;epose.index = 0;fint>>epose.a00>>epose.a01>>epose.a02>>epose.a03>>epose.a10>>epose.a11>>epose.a12>>epose.a13>>epose.a20>>epose.a21>>epose.a22>>epose.a23;cout<<"reading orb pose index : "<<epose.index<<endl;orbposes.push_back(epose);}ifstream fin(posesfile);while(!fin.eof()){struct K360Poses epose;fin>>epose.index>>epose.a00>>epose.a01>>epose.a02>>epose.a03>>epose.a10>>epose.a11>>epose.a12>>epose.a13>>epose.a20>>epose.a21>>epose.a22>>epose.a23;cout<<"reading kpose index : "<<epose.index<<endl;cout<<epose.a00<<" "<<epose.a01<<" "<<epose.a02<<" "<<epose.a03<<" "<<epose.a10<<" "<<epose.a11<<" "<<epose.a12<<" "<<epose.a13<<" "<<epose.a20<<" "<<epose.a21<<" "<<epose.a22<<" "<<epose.a23<<"\n";cout<<orbposes[epose.index-1].a00<<" "<<orbposes[epose.index-1].a01<<" "<<orbposes[epose.index-1].a02<<" "<<orbposes[epose.index-1].a03<<" "<<orbposes[epose.index-1].a10<<" "<<orbposes[epose.index-1].a11<<" "<<orbposes[epose.index-1].a12<<" "<<orbposes[epose.index-1].a13<<" "<<orbposes[epose.index-1].a20<<" "<<orbposes[epose.index-1].a21<<" "<<orbposes[epose.index-1].a22<<" "<<orbposes[epose.index-1].a23<<"\n";outkposefile<<epose.a00<<" "<<epose.a01<<" "<<epose.a02<<" "<<epose.a03<<" "<<epose.a10<<" "<<epose.a11<<" "<<epose.a12<<" "<<epose.a13<<" "<<epose.a20<<" "<<epose.a21<<" "<<epose.a22<<" "<<epose.a23<<"\n";outorbposefile<<orbposes[epose.index-1].a00<<" "<<orbposes[epose.index-1].a01<<" "<<orbposes[epose.index-1].a02<<" "<<orbposes[epose.index-1].a03<<" "<<orbposes[epose.index-1].a10<<" "<<orbposes[epose.index-1].a11<<" "<<orbposes[epose.index-1].a12<<" "<<orbposes[epose.index-1].a13<<" "<<orbposes[epose.index-1].a20<<" "<<orbposes[epose.index-1].a21<<" "<<orbposes[epose.index-1].a22<<" "<<orbposes[epose.index-1].a23<<"\n";//kposes.push_back(epose);}outkposefile.close();outorbposefile.close();fin.close();fint.close();cout<<"success create : "<<outposesfileN<<" "<< outorbposefileN<<endl;}

根据新生成的轨迹文件使用evo工具可视化轨迹：

evo_traj kitti --ref=nposes.txt norbpose.txt  -p --plot_mode=xy -s -a