ORB_SLAM2代码逻辑分析2.1 -- Track线程（Tracking类）-- 位姿估计
（ORB-SLAM2 System类解读链接）
https://blog.csdn.net/weixin_43569276/article/details/101027717
上篇文章（博文链接如上）我们了解到，程序真正的入口是在Tracking.cc中的GrabImageRGBD函数中实现的。这篇博文，我就主要扒一下Tracking文件中对Tracker的实现，其中可能涉及到一些比较细节的函数（比如特征点检测），我以后有时间再做细致的博文。
1、图像输入
1.1、图像处理流程
输入到Tracking线程的图像都要进行一定的预处理，进行图像转换，转换完成后在按照Frame的要求构造一个Frame，输入到Tracking线程中对这一帧图像进行追踪。
1.2、帧的构造
这里需要提一下Frame类的构造函数，看代码的话会很长，我们这里说一下具体的输入参数：
Frame(mImGray, //灰度图像
imDepth, //深度图像
timestamp, //时间戳
mpORBextractorLeft, //特征点检测器ORBextractor
mpORBVocabulary, //视觉词典
mK, //相机内参矩阵
mDistCoef, //相机的畸变系数
mbf, //# IR projector baseline times fx (aprox.)Camera.bf: 40.0
mThDepth); //远近点的阈值
Frame构造 帧 的流程：
1. Frame ID //每一帧frame的ID
2. 初始化ORBextractor参数
3. 检测ORB特征点并均匀化（返回到frame类的mvKeys和mDescriptors），并记录特征点数量
//ExtractORB(0,imGray);
4.1 对于双目和RGBD相机，根据相机参数对特征点去畸变，得到其在相机坐标系中的真实位姿
//UndistortKeyPoints();
4.2 对于双目相机，计算一帧两幅图像的双目匹配，匹配成功的点将会计算深度，
//ComputeStereoMatches();
对于RGBD相机，会计算每一个KeyPoint对应的深度
//ComputeStereoFromRGBD(imDepth);
深度存放在nvuRight和mvDepth中
5. 对于第一帧，计算图像的边界，网格单元的大小等参数
//if(mbInitialComputations)
//{ ...; ...; mbInitialComputations=false;}
6. 将每个特征点的 *ID* 对应到相应的图像网格
//AssignFeaturesToGrid();
ORB-SLAM2对于每一帧图像，都进行了网格化，并将特征点对应于相应帧的相应网格，方便以后检索。默认的网格数量实64*48，网格大小根据分辨率自行计算进行调整。
2、Tracking线程
Tracking线程的大致逻辑流程如下图所示：
2.1、Tracking初始化
一、单目初始化：（留个坑，以后补上）
二、双目初始化：
不多说了，直接上代码和相关注释：
void Tracking::StereoInitialization()
{
if(mCurrentFrame.N>500)
{
// Set Frame pose to the origin -- 第一帧位姿 Tcw 设置为单位阵
mCurrentFrame.SetPose(cv::Mat::eye(4,4,CV_32F));//.mTcw
// Create KeyFrame -- 通过Frame构造KeyFrame，输入参数为当前的Frame，KeyFrame关联的Map，关联的KeyFrame数据库
KeyFrame* pKFini = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);
// Insert KeyFrame in the map -- 将当前keyframe插入到地图中
mpMap->AddKeyFrame(pKFini);
// Create MapPoints and asscoiate to KeyFrame
//对每一个特征点都进行如下处理 -- 创建Mappoint并且将其关联到相应的KeyFrame
for(int i=0; i<mCurrentFrame.N;i++)
{
float z = mCurrentFrame.mvDepth[i]; //取得特征点深度
if(z>0) //特征点深度大于0
{
//2D点反投影变换到3D，返回值为cv::Mat_<float>(3,1) << x, y, z
//为世界坐标系下3D点的坐标
cv::Mat x3D = mCurrentFrame.UnprojectStereo(i); //将第i个特征投射到3D坐标系中，以构建MapPoint
MapPoint* pNewMP = new MapPoint(x3D,pKFini,mpMap); //通过KeyFrame构造MapPoint
pNewMP->AddObservation(pKFini,i);//该MapPoint被观测次数+1
pKFini->AddMapPoint(pNewMP,i); //在当前关键帧中加入这一MapPoint，和该Mappoit在当前KeyFrame中的ID
pNewMP->ComputeDistinctiveDescriptors(); //计算用于快速匹配描述子（计算该MapPoint唯一的描述子）
pNewMP->UpdateNormalAndDepth(); //TODO：计算特征点法向量和深度 **有待深入理解
mpMap->AddMapPoint(pNewMP);//将该MapPoint加入到map中
mCurrentFrame.mvpMapPoints[i]=pNewMP;//mappoint与相关的keypoints建立联系
}
}
cout << "New map created with " << mpMap->MapPointsInMap() << " points" << endl;
//在mplocalmaper的 mlNewKeyFrames（list<keyframe*>类型）中加入 当前KeyFrame
mpLocalMapper->InsertKeyFrame(pKFini);
//关键数值赋值
mLastFrame = Frame(mCurrentFrame);
mnLastKeyFrameId=mCurrentFrame.mnId;//TODO：keyframe的ID就是frame的ID？还是只有第一帧如此？
mpLastKeyFrame = pKFini;
mvpLocalKeyFrames.push_back(pKFini);//在tracking的localKeyframes中加入初始帧
mvpLocalMapPoints=mpMap->GetAllMapPoints();//在tracking的localmappoints中加入初始帧的所有关键点
mpReferenceKF = pKFini;//参考关键帧为初始帧
mCurrentFrame.mpReferenceKF = pKFini;//当前帧相对的参考帧为初始帧
mpMap->SetReferenceMapPoints(mvpLocalMapPoints);//设置Map的 参考MapPoint 为局部地图点，即当前帧的Mappoint
mpMap->mvpKeyFrameOrigins.push_back(pKFini);//将第一帧插入到初始的关键帧中
mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw);//setpose为单位阵
mState=OK;//初始化完成
}
}
2.2、相机位姿估计
在正常跟踪模式下，即OnlyTracking == false，mState == OK的情况下，回首先执行一个 CheckReplacedInLastFrame() 函数：
// Local Mapping might have changed some MapPoints tracked in last frame
CheckReplacedInLastFrame();
这句话的含义是，在LoopClosing线程中，会调用函数 void MapPoint::Replace(MapPoint* pMP)，来更改某些需要更改的MapPoint，所以要先检查一下MapPoint有无更改，以免追踪到坏点影响位姿估计的精度。
2.2.1、跟踪关键帧 – TrackReferenceKeyFrame()
接下来的重点是 TrackReferenceKeyFrame()，将上一帧的位姿作为当前帧的初始位姿，通过BoW的方式在参考帧中找当前帧特征点的匹配点，优化每个特征点都对应3D点重投影误差即可得到当前帧位姿。（帧与帧之间的位姿估计），在以下两种模式中会触发该函数：
正常建图模式（mbOnlyTracking == false; mState == OK）
- 在没有运动模型且当前帧与最后定位追踪到的帧ID相差不超过2
- 有运动模型但是通过运动追踪（TrackWithMotionModel()）失败
定位追踪模式（mbOnlyTracking == true; mbVO == false）
- 没有运动模型，实现位姿估计
具体代码流程如下：
1.计算当前帧的词袋向量
mCurrentFrame.ComputeBoW();
2.通过词典的方式检索匹配的地图点
ORBmatcher matcher(0.7,true);//设置ORB点匹配Matcher
int nmatches = matcher.SearchByBoW(mpReferenceKF,mCurrentFrame,vpMapPointMatches);
3.通过上一帧的位姿估计当前帧位姿
mCurrentFrame.mvpMapPoints = vpMapPointMatches; //当前帧的MapPoints只保留匹配上的MapPoints
mCurrentFrame.SetPose(mLastFrame.mTcw); //将上一帧的位姿作为当前帧位姿，然后用于估计当前帧位姿
//3D-2D 最小化重投影误差 e = (u,v) - project(Tcw*Pw) ，只优化Frame的Tcw，不优化MapPoints的坐标。
Optimizer::PoseOptimization(&mCurrentFrame); //从上一帧的位姿利用BA迭代优化得到当前位姿
4.剔除外点
5.返回 -- 如果inlier匹配的点超过10个则为追踪成功
2.2.2、基于运动模型跟踪 – TrackWithMotionModel()
这个函数通过恒速模型（mVelocity）来设定当前帧的初始位姿，通过重投影的方式在参考帧中找当前帧特征点的匹配点，优化每个特征点所对应3D点的投影误差即可得到当前位姿。
所谓恒速模型mVelocity，就是一个表示位姿的矩阵，是上一帧到当前帧的坐标变换。即在两帧之间，相机位姿以这个模型所变换的量，并将变换后的量作为当前帧的位姿，再利用这个位姿进行BA位姿估计。
1.设定初始值
//设定初始位姿 = 速度模型 * 上一帧位姿
mCurrentFrame.SetPose(mVelocity*mLastFrame.mTcw);
//初始化当前帧对应的mvpMapPoints为NULL
fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast<MapPoint*>(NULL));
2.通过投影，对上一帧的特征点进行追踪
int nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,th,mSensor==System::MONOCULAR);
//如果跟踪的点少，则扩大搜索半径再搜索一次
if(nmatches<20)
{
fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast<MapPoint*>(NULL));
nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,2*th,mSensor==System::MONOCULAR);
}
3.位姿估计
// Optimize frame pose with all matches
Optimizer::PoseOptimization(&mCurrentFrame);
4.外点剔除 // Discard outliers
5.返回 -- 如果inlier匹配的点超过10个则为追踪成功
如果恒速模型追踪不成功，那么就利用跟踪参考帧的模式。
bOK = TrackWithMotionModel();
if(!bOK)
bOK = TrackReferenceKeyFrame();
2.2.3、重定位 – Relocalization()
如果初始化成功的话，但是mState的状态不是OK的话，那么就需要重定位。重定位主要用在正常建图中VO跟踪失败或重定位模式。
下面部分代码有些长，请耐心阅读
1.计算当前帧的词袋向量
mCurrentFrame.ComputeBoW();
2.检索候选帧，返回为候选帧容器，检索到的帧数量不定
vector<KeyFrame*> vpCandidateKFs = mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame);
3.通过词典查找匹配的MapPoint，如果匹配点数超过15,为这一帧建立PnPsolver，并记录符合条件的候选帧的帧数
for(int i=0; i<nKFs; i++)
{
int nmatches = matcher.SearchByBoW(pKF,mCurrentFrame,vvpMapPointMatches[i]);
if(nmatches<15)
{
vbDiscarded[i] = true;
continue;
}
else//匹配点数>15,设置每个PnPsolver的RANSAC迭代的参数，并记录符合条件的候选帧的帧数
{
PnPsolver* pSolver = new PnPsolver(mCurrentFrame,vvpMapPointMatches[i]);
pSolver->SetRansacParameters(0.99,10,300,4,0.5,5.991);
vpPnPsolvers[i] = pSolver;
nCandidates++;
}
}
4.PnP（EPnP）求解位姿，直到找到足够的内点(inliers>50)，才会将计算位姿作为当前位姿
（经过三次位姿优化）
//对于每一个候选KF
for(int i=0; i<nKFs; i++)
{
//PnP（EPnP）求解位姿
PnPsolver* pSolver = vpPnPsolvers[i];
cv::Mat Tcw = pSolver->iterate(5,bNoMore,vbInliers,nInliers);
// If a Camera Pose is computed, optimize
if(!Tcw.empty())
{
Tcw.copyTo(mCurrentFrame.mTcw);
//根据内点提取对应MapPoints
for(int j=0; j<np; j++)
{
if(vbInliers[j])
{
mCurrentFrame.mvpMapPoints[j]=vvpMapPointMatches[i][j];
sFound.insert(vvpMapPointMatches[i][j]);
}
else
mCurrentFrame.mvpMapPoints[j]=NULL;
}
//优化位姿 -- 一次优化
int nGood = Optimizer::PoseOptimization(&mCurrentFrame);
if(nGood<10) //匹配内点少于10，放弃这一帧
continue;
// If few inliers, search by projection in a coarse window and optimize again
//如果内点(10<nGood<50)，则通过投影的方式对之前当前帧和候选帧未匹配的点进行匹配(搜索边长20，ORB距离100)找到一些新的匹配点
if(nGood<50)
{
int nadditional =matcher2.SearchByProjection(mCurrentFrame,vpCandidateKFs[i],sFound,10,100);
//若此时好的匹配的数量和新找到的匹配点的数量大于50，则进行优化求解
if(nadditional+nGood>=50)
{
//二次位姿优化
nGood = Optimizer::PoseOptimization(&mCurrentFrame);
// If many inliers but still not enough, search by projection again in a narrower window
// the camera has been already optimized with many points
//如果位姿优化后的内点的匹配数量还没有超过50，那么就再次通过投影的方式对之前当前帧和候选帧未匹配的点
//进行匹配(搜索边长6，ORB距离64)找一些新的匹配关系，
if(nGood>30 && nGood<50)
{
sFound.clear();
for(int ip =0; ip<mCurrentFrame.N; ip++)
if(mCurrentFrame.mvpMapPoints[ip])
sFound.insert(mCurrentFrame.mvpMapPoints[ip]);
nadditional =matcher2.SearchByProjection(mCurrentFrame,vpCandidateKFs[i],sFound,3,64);
// Final optimization
if(nGood+nadditional>=50)
{
//三次位姿优化
nGood = Optimizer::PoseOptimization(&mCurrentFrame);
for(int io =0; io<mCurrentFrame.N; io++)
if(mCurrentFrame.mvbOutlier[io])
mCurrentFrame.mvpMapPoints[io]=NULL;
}
}
}
}
//再次优化后的内点数大于50,则最后一次优化的位姿就是当前帧的位姿
if(nGood>=50)
{
bMatch = true;
break;
}
}
}//结束循环
5.重定位成功，将最后重定位的帧设为当前帧
mnLastRelocFrameId = mCurrentFrame.mnId;
2.3、局部地图跟踪 – TrackLocalMap()
利用局部地图（LocalMap）追踪的目的是：在帧间匹配得到初始的姿态后，现在对LocalMap进行跟踪得到更多的匹配，并优化当前位姿。
TrackLocalMap()分为以下5个步骤：
1.更新局部地图，包括局部关键帧和关键点
UpdateLocalMap()
{
//根据当前帧剔除外点后的 MapPoints的共视关系来提取局部关键帧序列
UpdateLocalKeyFrames();
//将局部关键帧序列中的 MapPoints加入到局部地图
UpdateLocalPoints();
}
2.对局部局部MapPoints进行投影匹配，在局部地图中查找与当前帧匹配的MapPoints
SearchLocalPoints()
{
2.1. 当前帧中的mvpMP将不再参与检索，因为当前帧中的MP已经匹配在之前track中匹配过了
pMP->mbTrackInView = false;//该点之后不会被投影
2.2.将所有局部MapPoints投影到当前帧，判断是否在视野范围内，然后进行投影匹配，
已经被当前帧观测到MapPoint不再判断是否能被当前帧观测到
// Project (this fills MapPoint variables for matching)
//判断该MP是否在currF 且相对于currF中心60度的范围内
if(mCurrentFrame.isInFrustum(pMP,0.5))
{
pMP->IncreaseVisible();
nToMatch++;
}
2.3.对视野范围内的MapPoints通过投影进行特征点匹配
ORBmatcher matcher(0.8);
matcher.SearchByProjection(mCurrentFrame,mvpLocalMapPoints,th);
}
3.更新局部MapPoints后对位姿再次优化
Optimizer::PoseOptimization(&mCurrentFrame);
4.更新当前帧的MapPoints被观测程度，并统计跟踪局部地图的效果
如果该MP被观测到，且不属于当前帧的外点，则mnMatchesInliers++
5. 判断成功条件a.距离上次重定位超过mMaxFrames=fps（30）帧，b.匹配的内点数不小于50，c.匹配的内点数不小于30
满足以上条件，TrackLocalMap即为成功，否则为失败
与VINS的滑动窗口不同，ORB-SLAM2的LocalMap是由与当前帧相关联的关键帧序列以及序列中所包含的MapPoints所构成。找出与当前帧提取的MapPoints关联度对高的关键帧序列（UpdateLocalMap()），提取关联的MapPoints并将不是当前帧检测到的MapPoints投影到当前帧（SearchLocalPoints()），来进行位姿的优化。
2.4、小结
Tracking线程的四个主要功能函数就是由三个track函数和一个relocalization函数组成。建图模式下，在实际的代码逻辑中的调用也比较简单：即有运动模型就用运动模型跟踪，没有运动模型或者使用运动模型跟踪失败或者距离上一次重定位不超过1帧，就利用关键帧跟踪。如果正常跟踪中失败了，导致Tracking的状态LOST，则进行重定位。这三个函数都是利用当前帧检测的KeyPoints对应的MapPoints与参考帧的MapPoints进行匹配进行位姿估计，并不涉及到LocalMap。
TrackLocalMap则在与当前相关联的LocalMap中通过投影的方式找到更多的匹配点，进一步优化位姿。LocalMap的作用是管理一个关键帧序列，并对加入的关键帧通过局部BA优化其在Map中的位姿。TrackLocalMap()可以是优化过后的当前帧位姿更符合其在最终Map中的位姿。
3、关键帧决策与数据更新
3.1、数据更新操作
上一节中，我们已经估计出当前针对位姿，那么接下来就需要根据决策插入关键帧，并更新重要数据，为下一次位姿估计做准备。具体执行流程如下：
1.设定Tracking的追踪状态
if(bOK)
mState = OK;
else
mState=LOST;
2.如果跟踪正常，则
if(bOK)
{
2.1.更新运动模型mVelocity -- 表示上一帧到当前帧的位姿
// Update motion model
2.2.清除VO匹配的点，清楚临时MP
// Clean VO matches
2.3.清除纯定位模式下建立的临时MapPoints
// Delete temporal MapPoints
2.4.检查是否需要KF
// Check if we need to insert a new keyframe
2.5.去除掉检测为外点的MP
// We allow points with high innovation (considererd outliers by the Huber Function)
// pass to the new keyframe, so that bundle adjustment will finally decide
// if they are outliers or not. We don't want next frame to estimate its position
// with those points so we discard them in the frame.
}
3.如果追踪失败且Map中不多于5个KF，则重置系统
// Reset if the camera get lost soon after initialization
4.更新当前参考帧并保存上一帧数据
//更新当前帧的参考帧
if(!mCurrentFrame.mpReferenceKF)
mCurrentFrame.mpReferenceKF = mpReferenceKF;
//保存上一帧的数据
mLastFrame = Frame(mCurrentFrame);
其中最关键的莫过于关键帧决策。
3.2、关键帧决策 – NeedNewKeyFrame()
在关键帧决策中，有以下三种情况不需要关键帧：
1.纯定位模式下不需要关键帧
if(mbOnlyTracking)
return false;
2.如果，如果局部地图（mpLocalMapper）线程被闭环检测线程使用，则不插入关键帧
// If Local Mapping is freezed by a Loop Closure do not insert keyframes
if(mpLocalMapper->isStopped() || mpLocalMapper->stopRequested())
return false;
const int nKFs = mpMap->KeyFramesInMap();
3.如果距离上一次重定位不超过1s且Map中的关键帧超过了mMaxFrames，则不插入关键帧
// （重定位只在VO跟丢时运行）
// Do not insert keyframes if not enough frames have passed from last relocalisation
if(mCurrentFrame.mnId<mnLastRelocFrameId+mMaxFrames && nKFs>mMaxFrames)
return false;
关键帧决策有以下两个条件：
条件1a：1s没有插入关键帧
// Condition 1a: More than "MaxFrames" have passed from last keyframe insertion
const bool c1a = mCurrentFrame.mnId>=mnLastKeyFrameId+mMaxFrames;
条件1b：LocalMapping线程空闲
// Condition 1b: More than "MinFrames" have passed and Local Mapping is idle
const bool c1b = (mCurrentFrame.mnId>=mnLastKeyFrameId+mMinFrames && bLocalMappingIdle);
条件1c：传感器为3D相机的前提下，匹配的内点数小于0.25倍KF中的MP数或需要插入近点
//Condition 1c: tracking is weak
const bool c1c = mSensor!=System::MONOCULAR && (mnMatchesInliers<nRefMatches*0.25 || bNeedToInsertClose) ;
条件2：在本次为此估计的内点数大于15的情况下，匹配的内点数小于阈值设定倍数（0.75倍）的KF中的MP数，或需要插入近点
// Condition 2: Few tracked points compared to reference keyframe. Lots of visual odometry compared to map matches.
const bool c2 = ((mnMatchesInliers<nRefMatches*thRefRatio|| bNeedToInsertClose) && mnMatchesInliers>15);
满足c2条件下，且满足c1条件中的任一条件，则需要插入关键帧
if((c1a||c1b||c1c)&&c2)
4、总结
至此，我们就理清了ORB-SLAM2的正常位姿估计的逻辑，这一部分写的有点长，感谢各位耐心读到这里。纯定位模式我会下次博客按照代码逻辑从头理一遍，它和定位模式在细节上还是有不同的。
ORB-SLAM2的Tracking线程就是正常的VSLAM流程，基本流程都相同：特征检测，帧间位姿估计，与LocalMap匹配优化位姿，关键帧选择，数据更新后为下一次位姿估计做准备。这些都是基本的功能逻辑。
但是其中有很多细节需要注意，比如有些函数是只会在纯定位模式下使用，如UpdateLastFrame()等函数；再比如还有一些函数需要进一步理解其原理，如SearchByProjection(), isInFrustum()等，这些函数虽然不会影响你对代码的阅读，但是还是要知道其具体如何实现功能的比较好。
最后，如果有什么问题或者纰漏，欢迎评论留言，共勉！！！
参考链接
ORB-SLAM2 tracking线程
https://www.cnblogs.com/wall-e2/p/8057448.html
ORB_SLAM2代码阅读(2)——tracking线程
https://blog.csdn.net/u014709760/article/details/89465842#221_Tracking__16
3532
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
