(01)ORB-SLAM2源码无死角解析-(35) 跟踪线程→恒速模型跟踪当前普通帧TrackWithMotionModel()

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解的(01)ORB-SLAM2源码无死角解析链接如下(本文内容来自计算机视觉life ORB-SLAM2 课程课件): (01)ORB-SLAM2源码无死角解析-(00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/123092196   文末正下方中心提供了本人 联系方式， 点击本人照片即可显示 W X → 官方认证 {\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证} 文末正下方中心提供了本人联系方式，点击本人照片即可显示WX→官方认证  

一、前言

在上一篇博客中，讲解了如何通过关键帧来估算当前帧的位姿，但是其最重要的核心函数 Optimizer::PoseOptimization(&mCurrentFrame) 没有进行讲解，这个知识点暂且搁置一下，先来看看恒速模型跟踪普通帧是如何实现的，恒速模型跟踪也叫做视觉里程计(不是很明白的朋友可以百度一下)。使用这种方式追踪呢，其假设相机移动的速度是均匀的，进一步利用微分的实现，就是上一帧变换的位姿与当前帧变换位姿是相同(或者说接近相同)。

如果两帧之间的速度(位姿变化)大致相同，另外两帧之间的时间间隔是比较小，一般为几十毫秒， 这里假设连续的三帧分别为 f 1 ， f 2 ， f 3 f1，f2，f3 f1，f2，f3。基于前面的推论， f 1 f1 f1 到 f 2 f2 f2，与 f 2 f2 f2 到 f 3 f3 f3 的位姿变换大致相同即 R 21 ≈ R 32 R_{21}\approx R_{32} R21​≈R32​, t 21 ≈ t 32 t_{21}\approx t_{32} t21​≈t32​(注意,这里使用的是约等于)。如果已知 R 21 R_{21} R21​，先令 R 21 = R 32 R_{21}= R_{32} R21​=R32​， t 21 = t 32 t_{21}=t_{32} t21​=t32​。那么 f 2 f_2 f2​ 的一个特征点 p 2 p_2 p2​ 根据 R 32 , t 32 R_{32},t_{32} R32​,t32​ 就能映射到 f 3 f3 f3 中为 p 3 ′ p'_3 p3′​。这里就需要注意了，通常情况 p 2 p_2 p2​ 与 p 3 ′ p'_3 p3′​ 不是匹配的特征点对，但是与 p 2 p_2 p2​ 匹配的特征代 p 3 p_3 p3​ 肯定就在 p 3 ′ p'_3 p3′​ 的附近。根据这里原理，对 p 3 ′ p'_3 p3′​ 周边的像素进行搜索，找到与 p 2 p_2 p2​ 匹配的特征点 p 3 p_3 p3​。

或者这样说大家还不是很明确,那么请看图示如下： 首先根据上上帧到上一帧的位姿，把上一帧的特征点像素坐标映射到当前帧，然后在映射后结果坐标周围进行搜索，找到与之匹配的特征点。

 

二、代码流程

通过 一、前言 对 恒速模型跟踪当前普通帧 进行了简单的原理介绍，代码为src/Tracking.cc中的Tracking::UpdateLastFrame() 函数，下面是其实现流程:

( 01 ) : \color{blue}{(01)}: (01): 调用 UpdateLastFrame() 函数更新上一帧的位姿，源码运行的过程中保存关键帧于mpReferenceKF变量中，但是并没有把所有视频帧都保存于缓存中。通过追踪函数 TrackReferenceKeyFrame(), TrackWithMotionModel() 或者 Relocalization() 计算出位姿之后，其并没有马上赋值，而运行到下一帧才对当前帧的位姿进行赋值。另外 TrackReferenceKeyFrame() 函数并没有调用 UpdateLastFrame(), 因为只有在刚完成初始化，或者恒速跟踪失败，才进行参考帧跟踪。 ①如果刚初始化完成，则上一帧为关键帧，关键帧已经存储位姿，所以不需要更新。 ②如果参考关键帧跟踪，则说明恒速跟踪失败，也就是上一帧已经没有了意义，又源码中不存储非关键帧，故不必对上一帧位姿进行更新。

( 02 ) : \color{blue}{(02)}: (02): 根据之前估算的速度(或者说位姿变换)，用恒速模型得到当前帧的初始位姿。代表速度的变量为 mVelocity。该变量在 Tracking::Track() 函数中被赋值。如果当前帧追踪成功，那么速度被赋值为 mVelocity = mCurrentFrame.mTcw*LastTwc。其中mCurrentFrame.mTcw相机坐标系变换到当前帧坐标系(弈可以理解为当前帧相机在世界坐标系下的位姿)，LastTwc表示上一帧世界相机坐标系到世界坐标的变换矩阵。所以mCurrentFrame.mTcw*LastTwc 表示上一帧相机位姿到当前帧位姿的变换矩阵。

( 03 ) : \color{blue}{(03)}: (03): 设置特征匹配的搜索半径(以像素为单位)，因为单目相机存在尺度漂移，所以搜索范围会大一些。然后利用重投影进行匹配，如果没有匹配点不够则扩大搜索路径重来一次。通过重投影进行特征点匹配的函数为SearchByProjection(),稍等会进行详细讲解。

( 04 ) : \color{blue}{(04)}: (04): 利用3D-2D投影关系，优化当前帧位姿。其核心为代码为 Optimizer::PoseOptimization(&mCurrentFrame);该函数比较重要，在上一章节已经提及到，该篇博客不进行讲解，后面再为大家进行分析。

( 05 ) : \color{blue}{(05)}: (05): 该步骤十分简单，主要就是根据判断地图点是否为外点，如果为外点，则清除他的所有关系。普配数目也同时更新。如果追踪匹配地图点超过10个则认为匹配成功。另外，纯定位模式下，如果追踪地图点非常少，那么 mbVO 标志就会定位。

其上 UpdateLastFrame() 与 SearchByProjection() 两个函数都是比较复杂。下面首先针对这两个函数进行讲解，然后再对TrackWithMotionModel() 进行整体的讲解(后面有代码注解)。

 

三、UpdateLastFrame()

该函数的实现位于 src/Tracking.cc 文件中，实现逻辑如下： ( 01 ) : \color{blue}{(01)}: (01): Tracking::Track() 函数中:①如果当前帧的位姿不为空(也就是说明当前帧被跟踪到)，那么就计算当前帧的参考帧到当前帧位姿变换，对应的代码为:

cv::Mat Tcr = mCurrentFrame.mTcw*mCurrentFrame.mpReferenceKF->GetPoseInverse();

计算相对姿态 T c r = T c w ∗ T w r , T w r = T r w − 1 Tcr = Tcw * Twr, Twr = Trw^{-1} Tcr=Tcw∗Twr,Twr=Trw−1。 T c r Tcr Tcr 表示参考帧到当前帧的变换矩阵，然后赋值给成员变量 mlRelativeFramePoses。②如果当前帧的位姿为空(也就是说明当前帧没有跟踪到)，那么 mlRelativeFramePoses 使用上一帧的值。 这样也就是说，知道了每一帧的关键帧到该帧的变换矩阵，那么在 UpdateLastFrame() 函数中的 cv::Mat Tlr 变量表示参考帧到上一阵的变换矩阵。结合关键帧在世界坐标系下的位姿，就能求解出上一帧在世界坐标系下的位姿。对应代码为 mLastFrame.SetPose(Tlr*pRef->GetPose())。这样就对上一帧的位姿进行了更新。

( 02 ) : \color{blue}{(02)}: (02): 如果上一阵为关键帧，或者是单目的情况，则退出该函数。因为针对双目或rgbd相机，还会为上一帧生成临时地图点(普通帧生成的地图点使用过后会被删除掉)。生成过程如下：获得到上一帧的所有特征点(不一定是地图点)，循环判断该点的深度是否有效，对有效特征点依照深度进行排序。然后再次进入循环，如果这个点对应在上一帧中的地图点没有,或者创建后就没有被观测到,那么就生成一个临时的地图点，地图点被创建后就没有被观测，认为不靠谱，也需要重新创建。创建新的地图点，主要使用反射影函数UnprojectStereo()。完成之后，加入到上一帧的地图点之中。

其具体代码实现如下:

/**
 * @brief 更新上一帧位姿，在上一帧中生成临时地图点
 * 单目情况：只计算了上一帧的世界坐标系位姿
 * 双目和rgbd情况：选取有有深度值的并且没有被选为地图点的点生成新的临时地图点，提高跟踪鲁棒性
 */
void Tracking::UpdateLastFrame()
{
    // Update pose according to reference keyframe
    // Step 1：利用参考关键帧更新上一帧在世界坐标系下的位姿
    // 上一普通帧的参考关键帧，注意这里用的是参考关键帧（位姿准）而不是上上一帧的普通帧
    KeyFrame* pRef = mLastFrame.mpReferenceKF;  
    // ref_keyframe 到 lastframe的位姿变换
    cv::Mat Tlr = mlRelativeFramePoses.back();

    // 将上一帧的世界坐标系下的位姿计算出来
    // l:last, r:reference, w:world
    // Tlw = Tlr*Trw 
    mLastFrame.SetPose(Tlr*pRef->GetPose()); 

    // 如果上一帧为关键帧，或者单目的情况，则退出
    if(mnLastKeyFrameId==mLastFrame.mnId || mSensor==System::MONOCULAR)
        return;

    // Step 2：对于双目或rgbd相机，为上一帧生成新的临时地图点
    // 注意这些地图点只是用来跟踪，不加入到地图中，跟踪完后会删除

    // Create "visual odometry" MapPoints
    // We sort points according to their measured depth by the stereo/RGB-D sensor
    // Step 2.1：得到上一帧中具有有效深度值的特征点（不一定是地图点）
    vector vDepthIdx;
    vDepthIdx.reserve(mLastFrame.N);

    for(int i=0; i0)
        {
            // vDepthIdx第一个元素是某个点的深度,第二个元素是对应的特征点id
            vDepthIdx.push_back(make_pair(z,i));
        }
    }

    // 如果上一帧中没有有效深度的点,那么就直接退出
    if(vDepthIdx.empty())
        return;

    // 按照深度从小到大排序
    sort(vDepthIdx.begin(),vDepthIdx.end());

    // We insert all close points (depth100)
            break;
    }
}

 

四、SearchByProjection()

下面再来分析 SearchByProjection() 函数，该函数位于 ORBmatcher.cc 文件中，该函数实现的就是 一、前言 中的图示过程。具体如下：

( 01 ) : \color{blue}{(01)}: (01): 建立旋转差直方图，用于检测旋转一致性，最后会剔除不一致的匹配。

( 02 ) : \color{blue}{(02)}: (02): 首先获当前帧相机位姿 R c w , t c w R_{cw},t_{cw} Rcw​,tcw​，以及上一帧的位姿 R l w , t l w R_{lw},t_{lw} Rlw​,tlw​，然后令变换矩阵如下： T c w = [ R c w t c w 0 T 1 ]          T l w = [ R l w t l w 0 T 1 ] \begin{aligned} \mathbf T_{c w} =\left[\begin{array}{cc} R_{c w} & t_{c w} \\ 0^{T} & 1 \end{array}\right] ~~~~~~~~ \mathbf T_{l w}=\left[\begin{array}{cc} R_{l w} & t_{l w} \\ 0^{T} & 1 \end{array}\right] \end{aligned} Tcw​=[Rcw​0T​tcw​1​]        Tlw​=[Rlw​0T​tlw​1​]​ T w c = [ R c w t c w 0 T 1 ] = T c w − 1 = [ R c w T − R c w T t c w 0 T 1 ] \mathbf T_{wc}=\left[\begin{array}{cc} R_{c w} & t_{c w} \\ 0^{T} & 1 \end{array}\right]=\mathbf T_{c w}^{-1}=\left[\begin{array}{cc} R_{c w}^{T} & -R_{c w}^{T} t_{c w} \\ 0^{T} & 1 \end{array}\right] Twc​=[Rcw​0T​tcw​1​]=Tcw−1​=[RcwT​0T​−RcwT​tcw​1​] 其上 T w c \mathbf T_{wc} Twc​ 是根据 T w c T c w = E \mathbf T_{wc}\mathbf T_{cw}=\mathbf E Twc​Tcw​=E 求解而来。很明显可知 t w c = − R c w T t c w t_{wc}=-R_{c w}^{T} t_{c w} twc​=−RcwT​tcw​, 继续推导: T l c = T l w T c w − 1 = [ R l w t k w 0 T 1 ] [ R c w T − R c w T t c w 0 T 1 ] = [ R l w R c w T − R k w R c w T t c w + t l w 0 T 1 ] \begin{aligned} \mathbf T_{l c} &=\mathbf T_{l w} \mathbf T_{c w}^{-1} \\ &=\left[\begin{array}{cc} R_{l w} & t_{k w} \\ 0^{T} & 1 \end{array}\right]\left[\begin{array}{cc} R_{c w}^{T} & -R_{c w}^{T} t_{c w} \\ 0^{T} & 1 \end{array}\right] \\ &=\left[\begin{array}{cc} R_{l w} R_{c w}^{T} & -R_{k w} R_{c w}^{T} t_{c w}+t_{l w} \\ 0^{T} & 1 \end{array}\right] \end{aligned} Tlc​​=Tlw​Tcw−1​=[Rlw​0T​tkw​1​][RcwT​0T​−RcwT​tcw​1​]=[Rlw​RcwT​0T​−Rkw​RcwT​tcw​+tlw​1​]​所以最终可以推导出 t l c = − R l w R c w T t c w + t l w = R l w t w c + t l w t_{l c}=-R_{l w} R_{c w}^{T} t_{c w}+t_{l w}=R_{lw}t_{wc}+t_{lw} tlc​=−Rlw​RcwT​tcw​+tlw​=Rlw​twc​+tlw​, 这样就能计算出当前帧相对于上一帧相机的平移向量，根据 t l c t_{l c} tlc​ 最后一个深度变换值，可以知道相对于上一帧，相机是前进还是后退了。另外，非单目情况，如果Z大于基线，则表示相机明显前进；非单目情况，如果-Z小于基线，则表示相机明显后退。

( 03 ) : \color{blue}{(03)}: (03): 对上一帧的每一个地图点，通过相机投影模型，得到得到投影到当前帧的像素坐标。获取上一帧中地图点对应二维特征点所在的金字塔层级，根据相机的前后前进方向来判断搜索尺度范围。

①当相机前进时，圆点的面积增大，在某个尺度m下它是一个特征点，由于面积增大，则需要在更高的尺度下才能检测出来。 ②当相机后退时，圆点的面积减小，在某个尺度m下它是一个特征点，由于面积减小，则需要在更低的尺度下才能检测出来。 该步骤主要获得当前帧半径范围内的所有特征点，核心函数为GetFeaturesInArea()，前面已经进行讲解过。

( 04 ) : \color{blue}{(04)}: (04): 达到缩小搜索特征匹配范围目的之后，则进行两帧之间的特征点配对， 如果最佳匹配距离要小于设定阈值。则认为匹配成功，最后再计算匹配点旋转角度差所在的直方图，进行旋转一致检测，剔除不一致的匹配。

其具体代码实现如下：

/**
 * @brief 将上一帧跟踪的地图点投影到当前帧，并且搜索匹配点。用于跟踪前一帧
 * 步骤
 * Step 1 建立旋转直方图，用于检测旋转一致性
 * Step 2 计算当前帧和前一帧的平移向量
 * Step 3 对于前一帧的每一个地图点，通过相机投影模型，得到投影到当前帧的像素坐标
 * Step 4 根据相机的前后前进方向来判断搜索尺度范围
 * Step 5 遍历候选匹配点，寻找距离最小的最佳匹配点 
 * Step 6 计算匹配点旋转角度差所在的直方图
 * Step 7 进行旋转一致检测，剔除不一致的匹配
 * @param[in] CurrentFrame          当前帧
 * @param[in] LastFrame             上一帧
 * @param[in] th                    搜索范围阈值，默认单目为7，双目15
 * @param[in] bMono                 是否为单目
 * @return int                      成功匹配的数量
 */
int ORBmatcher::SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame, const float th, const bool bMono)
{
    int nmatches = 0;

    // Rotation Histogram (to check rotation consistency)
    // Step 1 建立旋转直方图，用于检测旋转一致性
    vector rotHist[HISTO_LENGTH];
    for(int i=0;i CurrentFrame.mb && !bMono;     // 非单目情况，如果Z大于基线，则表示相机明显前进
    const bool bBackward = -tlc.at(2) > CurrentFrame.mb && !bMono;   // 非单目情况，如果-Z小于基线，则表示相机明显后退

    //  Step 3 对于前一帧的每一个地图点，通过相机投影模型，得到投影到当前帧的像素坐标
    for(int i=0; iGetWorldPos();
                cv::Mat x3Dc = Rcw*x3Dw+tcw;

                const float xc = x3Dc.at(0);
                const float yc = x3Dc.at(1);
                const float invzc = 1.0/x3Dc.at(2);

                if(invzc0)
                    {
                        // 双目和rgbd的情况，需要保证右图的点也在搜索半径以内
                        const float ur = u - CurrentFrame.mbf*invzc;
                        const float er = fabs(ur - CurrentFrame.mvuRight[i2]);
                        if(er>radius)
                            continue;
                    }

                    const cv::Mat &d = CurrentFrame.mDescriptors.row(i2);

                    const int dist = DescriptorDistance(dMP,d);

                    if(dist