当前位置：首页 > article >正文

ORB-SLAM2源码学习：System.cc: System::TrackStereo、TrackRGBD、TrackMonocular追踪器接口

article 2025/3/1 16:02:03

前言

在之前的单目实例中，我们在初始化一个SLAM的系统后，系统处理了接受的文件数据，初始化并运行了各个线程，之后就要给追踪器传入图像进行整个SLAM的流程了。

1.函数声明

追踪器的函数定义在System源文件下，针对不同的传感器模式，分别实现了三种追踪器的定义。

它们依次是单目、双目、RGBD追踪器。

cv::Mat System::TrackMonocular(const cv::Mat &im, const double &timestamp)
cv::Mat System::TrackStereo(const cv::Mat &imLeft, 		//左侧图像
							const cv::Mat &imRight, 	//右侧图像
							const double &timestamp)	//时间戳
cv::Mat System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double &timestamp)

2.函数定义

以单目为例：

1.检查传感器

不是指定的传感器内容输出信息提示。

if(mSensor!=MONOCULAR)
    {
        cerr << "ERROR: you called TrackMonocular but input sensor was not set to Monocular." << endl;
        exit(-1);
    }

2.改变追踪模式

追踪线程有两个模式，一个是定位建图另一个是纯定位模式，追踪器通过此部分代码来实现模式的改变。

// Check mode change
    {
        // 独占锁，主要是为了mbActivateLocalizationMode和mbDeactivateLocalizationMode不会发生混乱
        unique_lock<mutex> lock(mMutexMode);
        // mbActivateLocalizationMode为true会关闭局部地图线程
        if(mbActivateLocalizationMode)
        {
            mpLocalMapper->RequestStop();

            // Wait until Local Mapping has effectively stopped
            while(!mpLocalMapper->isStopped())
            {
                usleep(1000);
            }

            // 局部地图关闭以后，只进行追踪的线程，只计算相机的位姿，没有对局部地图进行更新
            // 设置mbOnlyTracking为真
            mpTracker->InformOnlyTracking(true);
            // 关闭线程可以使得别的线程得到更多的资源
            mbActivateLocalizationMode = false;
        }
        // 如果mbDeactivateLocalizationMode是true，局部地图线程就被释放, 关键帧从局部地图中删除.
        if(mbDeactivateLocalizationMode)
        {
            mpTracker->InformOnlyTracking(false);
            mpLocalMapper->Release();
            mbDeactivateLocalizationMode = false;
        }
    }

3.检查是否有复位操作。

// Check reset
    {
    unique_lock<mutex> lock(mMutexReset);
    if(mbReset)
    {
        mpTracker->Reset();
        mbReset = false;
    }

4.获取相机位姿估计的结果并返回。

//获取相机位姿的估计结果
    cv::Mat Tcw = mpTracker->GrabImageMonocular(im,timestamp);

    unique_lock<mutex> lock2(mMutexState);
    mTrackingState = mpTracker->mState;
    mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;
    mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;

    return Tcw;

其他追踪器的代码也差不多，直接列在下面了。

//双目输入时的追踪器接口
cv::Mat System::TrackStereo(const cv::Mat &imLeft, 		//左侧图像
							const cv::Mat &imRight, 	//右侧图像
							const double &timestamp)	//时间戳
{
	//检查输入数据类型是否合法
    if(mSensor!=STEREO)
    {
    	//不合法那就退出
        cerr << "ERROR: you called TrackStereo but input sensor was not set to STEREO." << endl;
        exit(-1);
    }   

    //检查是否有运行模式的改变
    // Check mode change
    {
    	// TODO 锁住这个变量？防止其他的线程对它的更改？
        unique_lock<mutex> lock(mMutexMode);
        //如果激活定位模式
        if(mbActivateLocalizationMode)
        {
        	//调用局部建图器的请求停止函数
            mpLocalMapper->RequestStop();

            // Wait until Local Mapping has effectively stopped
            while(!mpLocalMapper->isStopped())
            {
                usleep(1000);
            }
            //运行到这里的时候，局部建图部分就真正地停止了
            //告知追踪器，现在 只有追踪工作
            mpTracker->InformOnlyTracking(true);// 定位时，只跟踪
            //同时清除定位标记
            mbActivateLocalizationMode = false;// 防止重复执行
        }//如果激活定位模式
        if(mbDeactivateLocalizationMode)
        {
        	//如果取消定位模式
        	//告知追踪器，现在地图构建部分也要开始工作了
            mpTracker->InformOnlyTracking(false);
            //局部建图器要开始工作呢
            mpLocalMapper->Release();
            //清楚标志
            mbDeactivateLocalizationMode = false;// 防止重复执行
        }//如果取消定位模式
    }//检查是否有模式的改变

    // Check reset，检查是否有复位的操作
    {
    	//上锁
	    unique_lock<mutex> lock(mMutexReset);
	    //是否有复位请求？
	    if(mbReset)
	    {
	    	//有，追踪器复位
	        mpTracker->Reset();
	        //清除标志
	        mbReset = false;
	    }//是否有复位请求
    }//检查是否有复位的操作

    //用矩阵Tcw来保存估计的相机 位姿，运动追踪器的GrabImageStereo函数才是真正进行运动估计的函数
    cv::Mat Tcw = mpTracker->GrabImageStereo(imLeft,imRight,timestamp);

    //给运动追踪状态上锁
    unique_lock<mutex> lock2(mMutexState);
    //获取运动追踪状态
    mTrackingState = mpTracker->mState;
    //获取当前帧追踪到的地图点向量指针
    mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;
    //获取当前帧追踪到的关键帧特征点向量的指针
    mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;
    //返回获得的相机运动估计
    return Tcw;
}

//当输入图像 为RGBD时进行的追踪，参数就不在一一说明了
cv::Mat System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double &timestamp)
{
	//判断输入数据类型是否合法
    if(mSensor!=RGBD)
    {
        cerr << "ERROR: you called TrackRGBD but input sensor was not set to RGBD." << 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);
            }

            mpTracker->InformOnlyTracking(true);
            mbActivateLocalizationMode = false;
        }
        if(mbDeactivateLocalizationMode)
        {
            mpTracker->InformOnlyTracking(false);
            mpLocalMapper->Release();
            mbDeactivateLocalizationMode = false;
        }
    }

    // Check reset
    //检查是否有复位请求
    {
    unique_lock<mutex> lock(mMutexReset);
    if(mbReset)
    {
        mpTracker->Reset();
        mbReset = false;
    }
    }

    //获得相机位姿的估计
    cv::Mat Tcw = mpTracker->GrabImageRGBD(im,depthmap,timestamp);

    unique_lock<mutex> lock2(mMutexState);
    mTrackingState = mpTracker->mState;
    mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;
    mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;
    return Tcw;
}