ORB-SLAM2 프로젝트의 main()
기능 (실행 파일)은 Examples 폴더에 있으며 4 개의 하위 폴더가 있음을 알 수 있습니다. Monocular, Stereo 및 RGB-D 폴더는 데이터 세트에 저장된 장면 용 SLAM 용이며 ROS 폴더는 실제 장면 용 ROS 및 카메라를 사용하는 SLAM 용입니다.
TUM 데이터 세트에서 RGBD-SLAM을 예로 들어 보겠습니다. rgb_tum.cc
프로젝트의 실행 파일은 네 가지 주요 부분으로 구성됩니다.
- 이미지로드 : 명령 줄에서 입력 한 매개 변수에 따라 해당 폴더에 이미지 및 타임 스탬프를로드합니다.
LoadImages(strAssociationFilename, vstrImageFilenamesRGB, vstrImageFilenamesD, vTimestamps);
* 참고 :이 프로세스는 이미지의 이름을 해당 벡터 변수에 저장합니다. - SLAM 시스템 생성 : 명령 줄에 입력 된 매개 변수를 기반으로 SLAM 시스템을 생성하고 시스템을 초기화합니다.
ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::RGBD,true);
그중 argv [1]은 bag of words 파일의 경로를, argv [2]는 구성 파일의 경로를, ORB_SLAM2 :: System :: RGBD는 RGBD-SLAM 시스템으로 SLAM 시스템 생성을, true는 시각화 작업을 나타냅니다. - 추적을위한 수신 이미지 : 주기적 프로세스를 통해 추적하기 위해 이미지를 SLAM 시스템으로 전달합니다.
SLAM.TrackRGBD(imRGB,imD,tframe);
- SLAM 시스템 중지 및 궤적 저장 :
Shutdown()
함수를 통해 SLAM 시스템의 모든 스레드를 중지하고이SaveTrajectoryTUM()
기능을 사용하여 궤적을 저장합니다.
참조 링크 : https://blog.csdn.net/u014709760/article/details/87922386
코드와 주석은 다음과 같습니다.
#include<iostream>
#include<algorithm>
#include<fstream>
#include<chrono>
#include<opencv2/core/core.hpp>
#include<System.h>
using namespace std;
void LoadImages(const string &strAssociationFilename, vector<string> &vstrImageFilenamesRGB,
vector<string> &vstrImageFilenamesD, vector<double> &vTimestamps);
int main(int argc, char **argv)
{
// ./Examples/RGB-D/rgbd_tum -> arg[0] -> 运行rgbd_tum.cc文件,ORB_SLAM2的main函数
// Vocabulary/ORBvoc.txt -> arg[1] -> 词袋文件的路径
// Examples/RGB-D/TUM1.yaml -> arg[2] -> 配置文件的路径
// /media/Files/rgbdslam_DataSet/rgbd_dataset_freiburg1_desk -> arg[3] -> 下载的数据集文件夹
// Examples/RGB-D/associations/fr1_desk.txt -> arg[4] -> 关联文件的路径
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]);
//loadImage用于获取RGB和深度图像的路径,保存于vstrImageFilenamesRGB和vstrImageFilenamesD,
//时间戳保存于vTimestamps
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::System SLAM(argv[1],argv[2],ORB_SLAM2::System::RGBD,true); // -> System.cc, 创建了System类的对象
// 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,CV_LOAD_IMAGE_UNCHANGED->加载原图
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];
if(imRGB.empty())
{
cerr << endl << "Failed to load image at: "
<< string(argv[3]) << "/" << vstrImageFilenamesRGB[ni] << endl;
return 1;
}
#ifdef COMPILEDWITHC11 //时间点,steady_clock最短时间间隔0.1us
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.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
//duration还有一个成员函数count()返回Rep类型的Period数量(如0.01s,0.01->Rep,s->period)
//Rep表示一种数值类型,用来表示Period的数量,比如int float double
// Period是ratio类型,用来表示时间单位,比如second milisecond
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]; //0~ni-1,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");
SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
return 0;
}
void LoadImages(const string &strAssociationFilename, vector<string> &vstrImageFilenamesRGB,
vector<string> &vstrImageFilenamesD, vector<double> &vTimestamps)
{
ifstream fAssociation; //文件读操作
//在fstream类中,成员函数open()实现打开文件的操作,从而将数据流和文件进行关联
fAssociation.open(strAssociationFilename.c_str());
//C++ eof()函数可以帮助我们用来判断文件是否为空,或是判断其是否读到文件结尾。
while(!fAssociation.eof())
{
string s;
getline(fAssociation,s);
if(!s.empty())
{
stringstream ss;
ss << s;
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);
}
}
}