ORB-SLAM2源码逐行解析系列（二）：追踪线程

最新推荐文章于 2024-10-01 16:07:18 发布

LDST_CSDN

最新推荐文章于 2024-10-01 16:07:18 发布

阅读量385

点赞数

文章标签：计算机视觉人工智能自动驾驶

本文链接：https://blog.csdn.net/LDST_CSDN/article/details/130672340

版权

本文深入解析ORB-SLAM2的追踪线程，涵盖Tracking类的构造与流程，包括相机参数设置、ORB特征点提取器ORBextractor的细节，以及GrabImageMonocular的图像处理和帧位姿获取。重点讨论ORBextractor如何利用圆的对称性加速计算特征点的矩和质心。

摘要由CSDN通过智能技术生成

1. Tracking类

（1）Tracking类的定义

// /Examples/Monocular/include/Tracking.h
class Tracking
{
   
public:
    /**
     * @brief 构造函数
     * 
     * @param[in] pSys              系统实例 
     * @param[in] pVoc              字典指针
     * @param[in] pFrameDrawer      帧绘制器
     * @param[in] pMapDrawer        地图绘制器
     * @param[in] pMap              地图句柄
     * @param[in] pKFDB             关键帧数据库句柄
     * @param[in] strSettingPath    配置文件路径
     * @param[in] sensor            传感器类型
     */
    Tracking(System* pSys, ORBVocabulary* pVoc, FrameDrawer* pFrameDrawer, 
             MapDrawer* pMapDrawer, Map* pMap, KeyFrameDatabase* pKFDB,
             const string &strSettingPath, const int sensor); 
    
    // 下面的函数都是对不同的传感器输入的图像进行处理(转换成为灰度图像),并且在内部调用Tracking线程
    /**
     * @brief 处理双目输入
     * 
     * @param[in] imRectLeft    左目图像
     * @param[in] imRectRight   右目图像
     * @param[in] timestamp     时间戳
     * @return cv::Mat          世界坐标系到该帧相机坐标系的变换矩阵
     */
    cv::Mat GrabImageStereo(const cv::Mat &imRectLeft,
                            const cv::Mat &imRectRight, 
                            const double &timestamp);
    /**
     * @brief 处理RGBD输入的图像
     * 
     * @param[in] imRGB         彩色图像
     * @param[in] imD           深度图像
     * @param[in] timestamp     时间戳
     * @return cv::Mat          世界坐标系到该帧相机坐标系的变换矩阵
     */
    cv::Mat GrabImageRGBD(const cv::Mat &imRGB,
                          const cv::Mat &imD, 
                          const double &timestamp);
    /**
     * @brief 处理单目输入图像
     * 
     * @param[in] im            图像
     * @param[in] timestamp     时间戳
     * @return cv::Mat          世界坐标系到该帧相机坐标系的变换矩阵
     */
    cv::Mat GrabImageMonocular(const cv::Mat &im, const double &timestamp);
    
protected:
    /** @brief 主追踪进程 */
    void Track();

    /** @brief 在双目输入和RGBD输入时所做的初始化,主要是产生初始地图 */
    void StereoInitialization();

    // 疑问: 这两个不一样吗???
    /** @brief 单目输入的时候所进行的初始化操作 */
    void MonocularInitialization();
    /** @brief 单目输入的时候生成初始地图 */
    void CreateInitialMapMonocular();
};

（2）Tracking构造函数

大致流程

读取相机参数，包括相机内参、畸变系数、双目基线、帧率等
设置ORB特征点参数
- 提取特征点个数和阈值
- 图像金字塔的层数、尺度
设置单目/双目模式下的特征点提取器
设置双目/RGBD模式下的远近点判断阈值
设置RGBD的深度尺度因子

代码实现

// /Examples/Monocular/src/Tracking.cc
Tracking::Tracking(
                    System *pSys,                       // 系统实例
                    ORBVocabulary* pVoc,                // BOW字典
                    FrameDrawer *pFrameDrawer,          // 帧绘制器
                    MapDrawer *pMapDrawer,              // 地图点绘制器
                    Map *pMap,                          // 地图句柄
                    KeyFrameDatabase* pKFDB,            // 关键帧产生的词袋数据库
                    const string &strSettingPath,       // 配置文件路径
                    const int sensor):                  // 传感器类型
                        mState(NO_IMAGES_YET),                             
                        mSensor(sensor),                                
                        mbOnlyTracking(false),                            
                        mbVO(false),                                        
                        mpORBVocabulary(pVoc),          
                        mpKeyFrameDB(pKFDB), 
                        mpInitializer(static_cast<Initializer*>(NULL)),     
                        mpSystem(pSys), 
                        mpViewer(NULL),                                    
                        mpFrameDrawer(pFrameDrawer),
                        mpMapDrawer(pMapDrawer), 
                        mpMap(pMap), 
                        mnLastRelocFrameId(0)  
{
   
    // Step 1 从配置文件中加载相机参数  读取内参
    cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ);
    float fx = fSettings["Camera.fx"];
    float fy = fSettings["Camera.fy"];
    float cx = fSettings["Camera.cx"];
    float cy = fSettings["Camera.cy"];

    //     |fx  0   cx|
    // K = |0   fy  cy|
    //     |0   0   1 |
    // 构造相机内参矩阵
    cv::Mat K = cv