SLAM系统最关键的部分是tracking和maping。除此之外还有让系统更加完整和完善的初始化、回环检测、可视化等模块。
初始化是SLAM系统在运行时是都要经历的一步,系统一定要有一个初始的传感器位姿和初始的地图。单目相机需要两帧具有一定视差的图像来完成这一任务。
为什么说ORB_SLAM2是相对完美的系统,因为它改进了初始化,在该系统中初始化是自动完成的。针对平面场景(planar scene)和非平面场景(non-planar scene), ORB-SLAM2提供了单应(homography)和基础矩阵(fundamental matrix)两种几何模型。
整个初始化过程可以总结为六个步骤:
1.计算两帧图像的ORB特征点,并进行匹配;
2.在两个线程中以RANSAC策略并行的计算单应矩阵和基础矩阵;
3.根据评判标准在单应矩阵和基础矩阵之间选择一个模型;
4.根据选定的模型分解相机的旋转矩阵和平移向量,并对匹配的特征点进行三角化;
5.建立关键帧和地图点,进行完全BA(full BA)优化;
6.以参考帧下深度的中位数为基准建立基础尺度;
初始化的最基本任务就是建立坐标系,估计机器人的初始位姿,创建初始的地图。在查看源码之后了解到,初始化的过程是在System中,在构建了System对象之后,由System对象中的成员mpTracker完成。
在mono_tum.cc中,TrackMonocular()是用来接收数据完成单目位姿估计的。输入为图像和时间戳。
SLAM.TrackMonocular(im,tframe);//用来接收输入,以便初始化
cv::Mat System::TrackMonocular(const cv::Mat &im, const double ×tamp)
{
if(mSensor!=MONOCULAR)//函数运行前先检查类别选择是否是单目
{
cerr << "ERROR: you called TrackMonocular but input sensor was not set to Monocular." << endl;//如果不是单目,报错提醒
exit(-1);//报错之后还会退出系统
}
//下面是对工作模式和复位模式的检查,常规操作,不是主要理解内容
// Check mode change
{
unique_lock<mutex> lock(mMutexMode);
if(mbActivateLocalizationMode)
{
mpLocalMapper->RequestStop();
// Wait until Local Mapping has effectively stopped
while(!mpLocalMapper->isStopped())
{
usleep(1000);