3.4ORBSLAM3之跟踪恒速运动模型TrackWithMotionModel()

0总述

跟踪运动模型核心思想：假设在极短时间内，相机的运动相同。设相邻时刻的三帧图像分别为k-2帧，k-1帧，k帧，则认为k-2帧到k-1帧相机的运动T_delta1和k-1帧到k帧相机的运动T_delta2相等，如下图所示。当相机运动到当前帧图像时，可以由T_delta1和k-1帧时相机的位姿计算当前帧的位姿，为投影匹配和优化提供初值。

跟踪恒速运动模型主要分为以下几步：

• 更新上一帧的位姿
• 根据预积分或者恒速模型得到当前帧初始位姿
• 使用上一帧的地图点和初始位姿进行投影匹配
• 利用3D-2D的重投影误差对当前图像帧的位姿进行优化
• 剔除当前帧地图点中的外点

1.更新上一帧的位姿

上一帧的位姿通过上一帧参考关键帧位姿以及和参考关键帧之间的变换关系给出

// 上一帧的位姿 = Tlr * 上一帧参考关键帧位姿
Sophus::SE3f Tlr = mlRelativeFramePoses.back();
mLastFrame.SetPose(Tlr * pRef->GetPose());

对于双目或rgbd相机，为上一帧生成新的临时地图点，对生成的地图点通过阈值进行了限制，选择距离相机较近的前100个点，距离较远时精度会较低。

2.根据预积分或者恒速模型得到当前帧初始位姿

如果是IMU模式，会直接通过预积分结果设置当前帧的位姿，并返回该位姿。

    if (mpAtlas->isImuInitialized() && (mCurrentFrame.mnId > mnLastRelocFrameId + mnFramesToResetIMU))
    {
        // Predict state with IMU if it is initialized and it doesnt need reset
        // 如果是IMU模式 && IMU已经初始化了 && 距离重定位挺久不需要重置IMU，用IMU来估计位姿
        // 基于IMU估计位姿后直接返回，不需要再进行计算投影和优化
        PredictStateIMU();
        return true;
    }

如果是纯视觉模式，或者不满足IMU的使用条件，则选择恒速模型估计相机运动，得到一个初始的相机位姿。

mCurrentFrame.SetPose(mVelocity * mLastFrame.GetPose());

3.使用上一帧的地图点和初始位姿进行投影匹配

这一步只查找匹配，判断一下当前帧是否能正常跟踪，如果和上一帧的匹配较多满足条件才有下一步优化的必要，并为位姿优化提供匹配关系(隐式地更新在图像帧成员变量mvpMapPoints中)，但不涉及位姿的计算。

int nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,th,mSensor==System::MONOCULAR || mSensor==System::IMU_MONOCULAR);

1.首先，将lastframe的3D地图点由世界坐标系转换到currentframe对应的相机坐标系

Eigen::Vector3f x3Dw = pMP->GetWorldPos();// 获取3D点转换到世界坐标系
Eigen::Vector3f x3Dc = Tcw * x3Dw;// 将3D点由世界坐标系转换到currentFrame的相机坐标系,Tcw是当前帧位姿

2.然后，根据相机内参将该3D点投影到当前帧图像坐标系上，得到投影坐标(u,v)，对投影后的2D像素坐标和图像边界进行比较，筛掉超出边界的点

// 反投影
Eigen::Vector2f uv = CurrentFrame.mpCamera->project(x3Dc);
// 边界判断
if(uv(0)<CurrentFrame.mnMinX || uv(0)>CurrentFrame.mnMaxX)
    continue;
if(uv(1)<CurrentFrame.mnMinY || uv(1)>CurrentFrame.mnMaxY)
    continue;

3.然后，根据特征点的尺度信息确定在当前帧图像上的搜索半径，尺度越大搜索半径就越大，根据搜索半径可以确定该半径范围内所有的网格(在图像帧创建时给每个特征点都分配了网格)，将对应尺度的对应网格中的特征点对应id取出，作为候选的匹配特征点

// 确定搜索半径，每个特征点重投影的搜索半径和其尺度有关
// 尺度越大，搜索范围越大
float radius = th*CurrentFrame.mvScaleFactors[nLastOctave];
// 记录候选匹配点的id
vector<size_t> vIndices2;
// 根据相机的前后前进方向来判断搜索尺度以及范围。
// 以下可以这么理解，例如一个有一定面积的圆点，在某个尺度n下它是一个特征点
// 当相机前进时，圆点的面积增大，在某个尺度m下它是一个特征点，由于面积增大，则需要在更高的尺度下才能检测出来
// 当相机后退时，圆点的面积减小，在某个尺度m下它是一个特征点，由于面积减小，则需要在更低的尺度下才能检测出来                    
if(bForward)// 前进,则上一帧兴趣点在所在的尺度nLastOctave<=nCurOctave
    vIndices2 = CurrentFrame.GetFeaturesInArea(uv(0),uv(1), radius, nLastOctave);
else if(bBackward)// 后退,则上一帧兴趣点在所在的尺度0<=nCurOctave<=nLastOctave
    vIndices2 = CurrentFrame.GetFeaturesInArea(uv(0),uv(1), radius, 0, nLastOctave);
else// 如果当前帧在z方向上的移动距离小于基线，即没有明显的运动，那么就在[nLastOctave-1, nLastOctave+1]中搜索
    vIndices2 = CurrentFrame.GetFeaturesInArea(uv(0),uv(1), radius, nLastOctave-1, nLastOctave+1);

4.然后，遍历所有候选特征点(根据id信息)，计算每个特征点描述子和当前3D地图点描述子的距离，将最小距离对应的特征点作为最佳匹配，同时匹配特征点的角度之差

5.然后，对所有匹配点对的旋转角度之差进行直方图统计，选择直方图最高的3个对应特征点作为本次匹配查找的最终结果，这一步称为旋转一致性检验

//  Step 7 进行旋转一致检测，剔除不一致的匹配
if(mbCheckOrientation)
{
    int ind1=-1;
    int ind2=-1;
    int ind3=-1;
    ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3);
    for(int i=0; i<HISTO_LENGTH; i++){
        // 对于数量不是前3个的点对，剔除
        if(i!=ind1 && i!=ind2 && i!=ind3){
            for(size_t j=0, jend=rotHist[i].size(); j<jend; j++){
                CurrentFrame.mvpMapPoints[rotHist[i][j]]=static_cast<MapPoint*>(NULL);
                nmatches--;}}}
}

6.最后，如果匹配结果较少就加大搜索半径再进行一次查找，若匹配数量依然不满足条件，跟踪恒速运动模型就此失败

4.利用3D-2D的重投影误差对当前图像帧的位姿进行优化

Optimizer::PoseOptimization(&mCurrentFrame);

使用g2o进行优化，使用基于恒速模型计算得到的当前帧位姿作为优化迭代的初始值，当前帧位姿在函数中隐式地更新。

在优化时会对未能成功匹配的外点进行判断，这个判断根据投影误差进行筛选，如果最终优化后得到的外点太多，好的匹配太少就会判定跟踪失败