文章目录
- 01 整体代码运行流程
- 1.1 运行官方 Demo
- 1.2 变量命名规则
- 1.3 多线程
- 1.4 线程锁
- 1.5 SLAM 主类 System
01 整体代码运行流程
1.1 运行官方 Demo
以 stereo_kitti 为例，执行
./stereo_kitti path_to_vocabulary path_to_settings path_to_sequence
-
./stereo_kitti：可执行文件 -
path_to_vocabulary：字典路径 -
path_to_settings：配置文件路径，包含相机参数和 ORB 特征提取参数 -
path_to_sequence：数据集路径
主函数
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;
}
// 载入左右目图片、时间戳
vector<string> vstrImageLeft;
vector<string> vstrImageRight;
vector<double> vTimestamps;
LoadImages(string(argv[3]), vstrImageLeft, vstrImageRight, vTimestamps);
const int nImages = vstrImageLeft.size();
// 创建 SLAM 对象
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;
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);
double tframe = vTimestamps[ni]; // 记录对应的时间
if(imLeft.empty())
{
cerr << endl << "Failed to load image at: "
<< string(vstrImageLeft[ni]) << endl;
return 1;
}
std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
// Pass the images to the SLAM system
SLAM.TrackStereo(imLeft,imRight,tframe);
std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
double ttrack= std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();
vTimesTrack[ni]=ttrack; // 记录下每次 track 耗时
// 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)
this_thread::sleep_for(std::chrono::microseconds((int)((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.SaveTrajectoryKITTI("CameraTrajectory.txt");
return 0;
}
1.2 变量命名规则
-
m开头的变量表示为某类的成员变量； -
变量名的第一、二个字母表示其数据类型：
s表示std::set类型v表示std::vector类型l表示std::list类型p表示指针类型n表示int类型b表示bool类型KF表示KeyFrame类型
1.3 多线程
包括 Tracking、LocalMapping、LoopClosing 三个线程。
当 Tracking 不产生关键帧时，LocalMapping、LoopClosing 线程基本处于空转状态；并且，Tracking 线程产生关键帧的频率和时机是不固定的，因此需要三个线程同时进行，LocalMapping 和 LoopClosing 不断循环查询 Tracking 是否产生了关键帧。
1.4 线程锁
unique_lock<mutex> lock(mMutexConnections); 即为加锁，锁的有效性仅限于大括号内，也就是说，到大括号外，锁就会自动释放。所以程序中的一些大括号并非可有可无，需要注意。
1.5 SLAM 主类 System
| 成员变量/函数 | 访问控制 | 备注 |
|---|---|---|
eSensor mSensor | private | 传感器类型，可选 MONOCULAR、STEREO、RGBD |
ORBVocabulary* mpVocabulary | private | 字典，用来保存 ORB 描述子聚类结果 |
KeyFrameDatabase* mpKeyFrameDatabase | private | 关键帧数据库 |
Map* mpMap | private | 地图 |
Tracking* mpTracker | private | 追踪器 |
LocalMapping* mpLocalMapper | private | 局部建图、BA |
LoopClosing* mpLoopCloser | private | 回环检测 |
Viewer* mpViewer | private | 查看器 |
FrameDrawer* mpFrameDrawer | private | 帧绘制器 |
MapDrawer* mpMapDrawer | private | 地图绘制器 |
std::thread* mptLocalMapping | private | 局部建图线程 |
std::thread* mptLoopClosing | private | 回环检测线程 |
std::thread* mptViewer | private | 查看器线程 |
System(...) | public | 构造函数：初始化 SLAM 系统，启动 建图、回环、查看器线程 |
cv::Mat TrackStereo(...) cv::Mat TrackRGBD(...) cv::Mat TrackMonocular(...) | public public public | 追踪双目相机，返回相机位姿 追踪 RGBD 相机，返回相机位姿 追踪单目相机，返回相机位姿 |
void ActivateLocalizationMode() void DeactivateLocalizationMode() std::mutex mMutexMode bool mbActivateLocalizationMode bool mbDeactivateLocalizationMode | public public private private private | 开启纯定位模式（关闭建图线程） 关闭纯定位模式 |
void Reset() std::mutex mMutexReset bool mbReset | public private private | 系统复位 |
void Shutdown() | public | 系统关闭 |
void SaveTrajectoryTUM void SaveKeyFrameTrajectoryTUM() void SaveTrajectoryKITTI() | public public public | 以 TUM/KITTI 格式保存相机运动轨迹和关键帧位姿 |
构造函数
// 依次传入数据集、配置文件、传感器类型、bUseViewer
System::System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor,
const bool bUseViewer):mSensor(sensor),mbReset(false),mbActivateLocalizationMode(false),
mbDeactivateLocalizationMode(false)
{
// Step1 初始化成员变量、
// Step1.1 读取配置文件
cv::FileStorage fsSettings(strSettingsFile.c_str(), cv::FileStorage::READ);
// Step1.2 载入 ORB 字典
mpVocabulary = new ORBVocabulary();
// Step1.3 创建关键帧数据库
mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);
// Step1.4 创建地图
mpMap = new Map();
// Step1.5 绘图
mpFrameDrawer = new FrameDrawer(mpMap);
mpMapDrawer = new MapDrawer(mpMap, strSettingsFile);
// Step2 三大线程
// Step2.1 Tracking 线程，只需创建 Tracking 对象即可
mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer,
mpMap, mpKeyFrameDatabase, strSettingsFile, mSensor);
// Step2.2 创建 Local Mapping 线程和 mpLocalMapper
mpLocalMapper = new LocalMapping(mpMap, mSensor==MONOCULAR);
mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run,mpLocalMapper);
// Step2.3 创建 LoopClosing 线程和 mpLoopCloser
mpLoopCloser = new LoopClosing(mpMap, mpKeyFrameDatabase, mpVocabulary, mSensor!=MONOCULAR);
mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run, mpLoopCloser);
// 线程间通信
mpTracker->SetLocalMapper(mpLocalMapper);
mpTracker->SetLoopClosing(mpLoopCloser);
mpLocalMapper->SetTracker(mpTracker);
mpLocalMapper->SetLoopCloser(mpLoopCloser);
mpLoopCloser->SetTracker(mpTracker);
mpLoopCloser->SetLocalMapper(mpLocalMapper);
}
注意到，创建 Tracking 线程时，并没有 std::thread 成员变量，仅仅初始化了 Tracking 对象。这是因为，在逻辑上三个线程是并发的，互不包含；但实际编程中，我们将 Tracking 线程视为主线程，LocalMapping 和 LoopClosing 为子线程，Tracking 通过持有两个子线程的指针实现对其控制。
