ORB-SLAM2位姿估计

ORB-SLAM2位姿估计

1.初始化位姿

当跟踪bOK=true时，会依次优先选择“运动模式跟踪”，“关键帧模式跟踪”，若前两者失败，则说明跟踪失败bOK=false，bLOST=true，进入“重定位模式”。

if(mState==OK)
    {
      // Local Mapping might have changed some MapPoints tracked in last frame
      CheckReplacedInLastFrame();

      if(mVelocity.empty() || mCurrentFrame.mnId2)
        {
          bOK = TrackReferenceKeyFrame();
        }
      else
        {
          bOK = TrackWithMotionModel();
          if(!bOK)
            bOK = TrackReferenceKeyFrame();
        }
    }
  else
    {
      bOK = Relocalization();
    }

（1）运动模式跟踪TrackWithMotionModel()

1.1当前帧位姿初始化为前一帧的位姿，并乘以速度矩阵。

mCurrentFrame.SetPose(mVelocity*mLastFrame.mTcw);

1.2与前一帧进行特征匹配

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

1.2.1前一帧的地图点投影到像素平面

MapPoint* pMP = LastFrame.mvpMapPoints[i];

1.2.2计算特征描述子，和描述子距离

vIndices2 = CurrentFrame.GetFeaturesInArea(u,v, radius, nLastOctave);
const int dist = DescriptorDistance(dMP,d);

1.2.3更新当前帧的地图点

CurrentFrame.mvpMapPoints[bestIdx2]=pMP;

（2）关键帧模式跟踪TrackReferenceKeyFrame()

2.1计算词典

mCurrentFrame.ComputeBoW();

2.2和参考关键帧进行特征匹配

int nmatches = matcher.SearchByBoW(mpReferenceKF,mCurrentFrame,vpMapPointMatches);

2.2.1当前帧的地图点设置为匹配的地图点
2.2.2当前帧位姿设置为上一帧位姿

mCurrentFrame.mvpMapPoints = vpMapPointMatches;
mCurrentFrame.SetPose(mLastFrame.mTcw);

（3）重定位模式跟踪Relocalization()

3.1计算当前帧词典

mCurrentFrame.ComputeBoW();

3.2在关键帧数据库中查找候选关键帧

vector vpCandidateKFs = mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame);

3.3对每一个候选关键帧进行特征匹配

int nmatches = matcher.SearchByBoW(pKF,mCurrentFrame,vvpMapPointMatches[i]);

3.3.1匹配成功的候选关键帧的地图点定义PnP对当前帧进行位姿计算

PnPsolver* pSolver = new PnPsolver(mCurrentFrame,vvpMapPointMatches[i]);

3.3.2对每个候选关键帧，若计算出平移矩阵，当前帧地图点为候选参考帧地图点

cv::Mat Tcw = pSolver->iterate(5,bNoMore,vbInliers,nInliers);

Tcw.copyTo(mCurrentFrame.mTcw);

mCurrentFrame.mvpMapPoints[j]=vvpMapPointMatches[i][j];

3.3.3继续和参考关键帧进行特征匹配，直到匹配结果由于阈值

int nadditional =matcher2.SearchByProjection(mCurrentFrame,vpCandidateKFs[i],sFound,10,100);

位姿优化

Optimizer::PoseOptimization(&mCurrentFrame);

g2o图优化

总结

运动模式跟踪 Tcw = velocity*Tcw_last，地图点通过重投影更新
关键帧模式跟踪Tcw = Tcw_last，地图点通过视觉词典更新
重定位模式跟踪Tcw = PnP（候选关键帧），地图点通过候选参考帧更新

最终都进行了位姿图优化