526互联

ORB_SLAM3源码阅读笔记（二）

发布时间 2023-03-31 15:07:57作者: Shang-Zhi

Tracking线程

1 线程的创建与输入

    在对Tracking线程进行了解之前首先需要对其输入的参数进行一定的了解 ,在Sysyem.cc中追踪线程是这样出现在眼前的：

mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer,
                             mpAtlas, mpKeyFrameDatabase, strSettingsFile, mSensor, settings_, strSequence);

Tracking类构造函数原型是：

Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer,
		   MapDrawer *pMapDrawer, Atlas *pAtlas, KeyFrameDatabase* pKFDB, 
		   const string &strSettingPath,const int sensor, Settings* settings, 
		   const string &_nameSeq)

每个输入参数所表示的是含义是显而易见的。

  • 第一个参数System *pSys: 传入System指针。
  • 第二个参数ORBVocabulary* pVoc: 传入ORBVocabulary指针（对应着的就是词袋模型）。
  • 第三、四个参数FrameDrawer *pFrameDrawer, MapDrawer *pMapDrawer：传入对应的画帧和画图指针。
  • 第五、六个参数 Atlas *pAtlas, KeyFrameDatabase* pKFDB：传入对应的地图和基础关键帧数据指针。
  • 第七个参数const string &strSettingPath：传入配置文件目录，在ORB_SLAM3的单目中这个参数输入的是相机配置yaml文件。
  • 第八个参数const int sensor：表示的是传感器的类型，此处传感器的类型一共分为6种，进入System.h进行查看：
enum eSensor
   {
        MONOCULAR=0,          // 单目
        STEREO=1,             // 双目
        RGBD=2,               // RGB-D
        IMU_MONOCULAR=3,      // IMU+单目
        IMU_STEREO=4,         // IMU+双目
        IMU_RGBD=5,           // IMU+RGB-D
    };
  • 第九个参数Settings* settings: 传入设置指针，实际的就是读取配置文件中的各项参数（因为setting_是由Setting类创建的对象，而Setting类的构造函数中执行的功能就是首先读取相机或者图像数据的信息）。
  • 第十个参数const string &_nameSeq：表示的是传入的对应时间戳文件。

2 Tracking到底做了什么？

    为了避免模糊各种函数的输入参数，这里以单目为例对函数中的参数与实际输入进行对应，便于后续中对各项参数的快速了解。

ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::MONOCULAR, false);
// argv[2] 对应输入 ./Example/Monocular/EuRoc.yaml，其实就是相机以及一些必须的配置文件
System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor, const bool bUseViewer = true, const int initFr = 0, const string &strSequence = std::string())
// 此时，构造函数中的第一和第二个参数分别对应的就是输入的词袋模型文件和相机配置文件

所以总结起来就是：
ORBVoc.txt --> const string &strVocFile
EuRoc.yaml --> const string &strSettingFile
./EuRoc_Timestamps/MH01.txt --> const string &strSequence
简单知道函数参数的具体对应的文件即可。
    进入Tracking的构造函数看看究竟发生了什么。进入Tracking构造函数映入眼帘的第一部分便是从输入的配置文件中读取数据，这里将这一部分的代码完整贴出逐步分析：

if(settings){
        newParameterLoader(settings);
    }
    else{
        cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ);

        bool b_parse_cam = ParseCamParamFile(fSettings);
        if(!b_parse_cam)
        {
            std::cout << "*Error with the camera parameters in the config file*" << std::endl;
        }

        // Load ORB parameters
        bool b_parse_orb = ParseORBParamFile(fSettings);
        if(!b_parse_orb)
        {
            std::cout << "*Error with the ORB parameters in the config file*" << std::endl;
        }

        bool b_parse_imu = true;
        if(sensor==System::IMU_MONOCULAR || sensor==System::IMU_STEREO || sensor==System::IMU_RGBD)
        {
            b_parse_imu = ParseIMUParamFile(fSettings);
            if(!b_parse_imu)
            {
                std::cout << "*Error with the IMU parameters in the config file*" << std::endl;
            }

            mnFramesToResetIMU = mMaxFrames;
        }

        if(!b_parse_cam || !b_parse_orb || !b_parse_imu)
        {
            std::cerr << "**ERROR in the config file, the format is not correct**" << std::endl;
            try
            {
                throw -1;
            }
            catch(exception &e)
            {

            }
        }
    }

    首先假设if(setting)成立,进入newParameterLoader函数查看。这里根据读取的相机配置文件的参数开始构建，这里出现了一个指针mpCamera这是一个GeomtricCamera类的指针关于这个类这里暂时不做过多的说明。简而言之这个函数中首先配置了相机参数矩阵，然后判断了硬件设备if((mSensor==System::STEREO || mSensor==System::IMU_STEREO || mSensor==System::IMU_RGBD) &&settings->cameraType() == Settings::KannalaBrandt),当然这里是以单目为例进行分析的，如果是双目或者其他的类型则同样的进行类似的操作（比如是双目的，就需要对另一相机进行配置）。后面的紧接着的一个判断语句也是一样的，只不过是相机畸变模型不一样而已。在这个函数的后面可以清晰的看出该函数将读取了配置文件中ORB算法关键点提取等一系列算法所需的参数进行了赋值，当然也包括IMU的参数(如果配置文件中有IMU参数的话)，简单来说这个函数的功能还是属于初始化。当然，后面如果if(setting)不成立其对应的操作也是属于初始化，不再过多说明。
    Tracking构造函数中最后的内容便是对相机信息的输出，Tracking是主线程，随着图像的不断输入开始不断的进行位姿估计并不断的改变系统状态与变量。在单目的mono_euroc.cc中Tracking的类的TrackMonocular()成员函数不断的被调用：

SLAM.TrackMonocular(im,tframe); // im表示图像，tframe表示时间戳

进入Tracking类中查看其成员函数TrackMonocular()中具体发生了什么。首先，该成员函数的完整函数形式如下：

Sophus::SE3f TrackMonocular(const cv::Mat &im, const double &timestamp, const vector<IMU::Point>& vImuMeas = vector<IMU::Point>(), string filename="");

第一个和第二个参数已经说过了，第三个参数是IMU参数，第四个参数是文件名称。这里的IMU参数IMU::Point是构建的一个Point类，这个类就是包含了IMU的一些基础参数，例如白噪声、角速度……等。
    进入该函数 首先 该函数对停止标志位mbShutDown进行了检查，接着对传感器再次进行检查，如果停止标志位不为真而且传感器类型判断正确则开始对输入的图像进行处理，当然这里在对图像进行处理前同样也会判断读取的配置文件中有没有关于对图像作处理（实际上就是做缩放）的相关参数。然后就是对模式改变的检查了，接着又对重置状态进行检查这些都没有什么特别值得关注的，关键点在于最后的Sophus::SE3f Tcw = mpTracker->GrabImageMonocular(imToFeed,timestamp,filename);,该函数就是求解位姿的关键所在，直接进入函数进行查看。
GrabImageMonocular函数中首先对输入的图像进行了灰度转换（如果输入的是三通道图像），接着根据传感器的不同开始调用Frame.h中Frame类的成员函数Frame()开始进行ORB特征提取等等一系列的操作（反正感觉这就是一环套着一环的）具体的细节就不多说了，反正通过一系列操作返回了求解得到的位姿矩阵。
    整个跟踪线程的大致工作流程就是这样的，更多的细节部分还是需要自己花更多的时间去阅读。