【ORB_SLAM2源码解读】分析ORB_SLAM2 RGBD 第1帧是怎么计算位置姿态的

1. 进入非初始化分支

if(mState==NOT_INITIALIZED){
else

2. 更新最后一帧特征点生成的3D点

// Local Mapping might have changed some MapPoints tracked in last frame
// MapPoint* pMP = mLastFrame.mvpMapPoints[i]; 遍历最后一帧的每个特征点的3D点
// MapPoint* pRep = pMP->GetReplaced(); 如果3D点的成员变量mpReplaced不为空，那么替换最后一帧的当前特征点的3D点
CheckReplacedInLastFrame();

3. 根据参考关键帧计算当前帧的位置姿态

首先将当前普通帧2D特征点的描述子转化为BoW向量，然后通过词袋BoW加速当前帧与参考帧之间的特征点匹配，如果匹配的特征点数大于15个，则将上一关键帧的位姿作为当前帧位姿的初始值，最后通过关键帧的3D点和当前帧的2D特征点计算重投影误差来优化位姿

Tracking::TrackReferenceKeyFrame()
// 通过词袋加速计算两个相似的特征点的
int nmatches = matcher.SearchByBoW(mpReferenceKF,mCurrentFrame,vpMapPointMatches);
mCurrentFrame.mvpMapPoints = vpMapPointMatches;
mCurrentFrame.SetPose(mLastFrame.mTcw);
Optimizer::PoseOptimization(&mCurrentFrame);

第三帧是如何计算位置姿态的？

1. 根据匀速模型计算当前帧的位置姿态

if(mVelocity.empty() || mCurrentFrame.mnId<mnLastRelocFrameId+2){}
else{bOK = TrackWithMotionModel();}

2. 更新最后一帧的数据

这个最后一帧的数据在初始化的时候赋值，恒速模型中更新
所谓的恒速模型的这个速度就是两帧之间的变换矩阵, 所以当前帧的位置姿态等于上一帧的位置姿态左乘两帧之间的变换矩阵

Tracking::UpdateLastFrame()

3. 优化当前帧的位置姿态