讲解关于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→官方认证
一、前言首先这里再重诉一下追踪的 核心思想 : \color{red}{核心思想:} 核心思想: 在追踪的过程中，随着当前帧在一帧一帧的变化，可想而知，当前帧可以观测到的地图点也在变化，利用已经存在的地图点可以计算出相机位姿，同时当前帧会剩下一些未匹配上的关键点，那么这些关键点结合位姿则可以产生新的地图点，这些新的地图点可以提供给下一帧用于位姿估算。
为了把更多关键帧的信息利用起来，先对局部地图进行更新，假设更新之后的局部地图如下(红色代表局部地图点)： 
可以看到，红色的点还是很多的，也就是说局部地图点很多，这个时候大家可以想象一下，之前当前帧未匹配上地图点的特征点，是否有可能在这么多的红色地图点中找到一个合适的地图点？答案当然是有可能的。
主体步骤: ( 1 ) : \color{blue}{(1)}: (1): 更新局部关键帧mvpLocalKeyFrames和局部地图点mvpLocalMapPoints ( 2 ) : \color{blue}{(2)}: (2): 在局部地图中查找与当前帧匹配的MapPoints, 其实也就是对局部地图点进行跟踪 ( 3 ) : \color{blue}{(3)}: (3): 更新局部所有MapPoints后对位姿再次优化 ( 4 ) : \color{blue}{(4)}: (4): 更新当前帧的MapPoints被观测程度，并统计跟踪局部地图的效果 ( 5 ) : \color{blue}{(5)}: (5): 决定是否跟踪成功
函数 TrackLocalMap() 位于 src/Tracking.cc 文件中，
/**
* @brief 用局部地图进行跟踪，进一步优化位姿
*
* 1. 更新局部地图，包括局部关键帧和关键点
* 2. 对局部MapPoints进行投影匹配
* 3. 根据匹配对估计当前帧的姿态
* 4. 根据姿态剔除误匹配
* @return true if success
*
* Step 1：更新局部关键帧mvpLocalKeyFrames和局部地图点mvpLocalMapPoints
* Step 2：在局部地图中查找与当前帧匹配的MapPoints, 其实也就是对局部地图点进行跟踪
* Step 3：更新局部所有MapPoints后对位姿再次优化
* Step 4：更新当前帧的MapPoints被观测程度，并统计跟踪局部地图的效果
* Step 5：决定是否跟踪成功
*/
bool Tracking::TrackLocalMap()
{
// We have an estimation of the camera pose and some map points tracked in the frame.
// We retrieve the local map and try to find matches to points in the local map.
// Update Local KeyFrames and Local Points
// Step 1：更新局部关键帧 mvpLocalKeyFrames 和局部地图点 mvpLocalMapPoints
UpdateLocalMap();
// Step 2：筛选局部地图中新增的在视野范围内的地图点，投影到当前帧搜索匹配，得到更多的匹配关系
SearchLocalPoints();
// Optimize Pose
// 在这个函数之前，在 Relocalization、TrackReferenceKeyFrame、TrackWithMotionModel 中都有位姿优化，
// Step 3：前面新增了更多的匹配关系，BA优化得到更准确的位姿
Optimizer::PoseOptimization(&mCurrentFrame);
mnMatchesInliers = 0;
// Update MapPoints Statistics
// Step 4：更新当前帧的地图点被观测程度，并统计跟踪局部地图后匹配数目
for(int i=0; iIncreaseFound();
//查看当前是否是在纯定位过程
if(!mbOnlyTracking)
{
// 如果该地图点被相机观测数目nObs大于0，匹配内点计数+1
// nObs： 被观测到的相机数目，单目+1，双目或RGB-D则+2
if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)
mnMatchesInliers++;
}
else
// 记录当前帧跟踪到的地图点数目，用于统计跟踪效果
mnMatchesInliers++;
}
// 如果这个地图点是外点,并且当前相机输入还是双目的时候,就删除这个点
// ?单目就不管吗
else if(mSensor==System::STEREO)
mCurrentFrame.mvpMapPoints[i] = static_cast(NULL);
}
}
// Decide if the tracking was succesful
// More restrictive if there was a relocalization recently
// Step 5：根据跟踪匹配数目及重定位情况决定是否跟踪成功
// 如果最近刚刚发生了重定位,那么至少成功匹配50个点才认为是成功跟踪
if(mCurrentFrame.mnIdmnLastFrameSeen = mCurrentFrame.mnId;
// 标记该点在后面搜索匹配时不被投影，因为已经有匹配了
pMP->mbTrackInView = false;
}
}
}
// 准备进行投影匹配的点的数目
int nToMatch=0;
// Project points in frame and check its visibility
// Step 2：判断所有局部地图点中除当前帧地图点外的点，是否在当前帧视野范围内
for(vector::iterator vit=mvpLocalMapPoints.begin(), vend=mvpLocalMapPoints.end(); vit!=vend; vit++)
{
MapPoint* pMP = *vit;
// 已经被当前帧观测到的地图点肯定在视野范围内，跳过
if(pMP->mnLastFrameSeen == mCurrentFrame.mnId)
continue;
// 跳过坏点
if(pMP->isBad())
continue;
// Project (this fills MapPoint variables for matching)
// 判断地图点是否在在当前帧视野内
if(mCurrentFrame.isInFrustum(pMP,0.5))
{
// 观测到该点的帧数加1
pMP->IncreaseVisible();
// 只有在视野范围内的地图点才参与之后的投影匹配
nToMatch++;
}
}
// Step 3：如果需要进行投影匹配的点的数目大于0，就进行投影匹配，增加更多的匹配关系
if(nToMatch>0)
{
ORBmatcher matcher(0.8);
int th = 1;
if(mSensor==System::RGBD) //RGBD相机输入的时候,搜索的阈值会变得稍微大一些
th=3;
// If the camera has been relocalised recently, perform a coarser search
// 如果不久前进行过重定位，那么进行一个更加宽泛的搜索，阈值需要增大
if(mCurrentFrame.mnIdmbTrackInView = false;
// 3D in absolute coordinates
// Step 1 获得这个地图点的世界坐标
cv::Mat P = pMP->GetWorldPos();
// 3D in camera coordinates
// 根据当前帧(粗糙)位姿转化到当前相机坐标系下的三维点Pc
const cv::Mat Pc = mRcw*P+mtcw;
const float &PcX = Pc.at(0);
const float &PcY = Pc.at(1);
const float &PcZ = Pc.at(2);
// Check positive depth
// Step 2 关卡一：将这个地图点变换到当前帧的相机坐标系下，如果深度值为正才能继续下一步。
if(PcZGetMaxDistanceInvariance();
const float minDistance = pMP->GetMinDistanceInvariance();
// 得到当前地图点距离当前帧相机光心的距离,注意P，mOw都是在同一坐标系下才可以
// mOw：当前相机光心在世界坐标系下坐标
const cv::Mat PO = P-mOw;
//取模就得到了距离
const float dist = cv::norm(PO);
//如果不在有效范围内，认为投影不可靠
if(distmaxDistance)
return false;
// Check viewing angle
// Step 5 关卡四：计算当前相机指向地图点向量和地图点的平均观测方向夹角，小于60°才能进入下一步。
cv::Mat Pn = pMP->GetNormal();
// 计算当前相机指向地图点向量和地图点的平均观测方向夹角的余弦值，注意平均观测方向为单位向量
const float viewCos = PO.dot(Pn)/dist;
//夹角要在60°范围内，否则认为观测方向太偏了，重投影不可靠，返回false
if(viewCosPredictScale(dist, //这个点到光心的距离
this); //给出这个帧
// Step 7 记录计算得到的一些参数
// Data used by the tracking
// 通过置位标记 MapPoint::mbTrackInView 来表示这个地图点要被投影
pMP->mbTrackInView = true;
// 该地图点投影在当前图像（一般是左图）的像素横坐标
pMP->mTrackProjX = u;
// bf/z其实是视差，相减得到右图（如有）中对应点的横坐标
pMP->mTrackProjXR = u - mbf*invz;
// 该地图点投影在当前图像（一般是左图）的像素纵坐标
pMP->mTrackProjY = v;
// 根据地图点到光心距离，预测的该地图点的尺度层级
pMP->mnTrackScaleLevel = nPredictedLevel;
// 保存当前相机指向地图点向量和地图点的平均观测方向夹角的余弦值
pMP->mTrackViewCos = viewCos;
//执行到这里说明这个地图点在相机的视野中并且进行重投影是可靠的，返回true
return true;
}
五、结语
在 SearchLocalPoints() 中还调用了一个比较重要的函数，那就是 SearchByProjection()，这个函数已经在前面讲解过了，这里就再进行重简介了。通过 SearchLocalPoints() 获得更多的匹配关系关系之后，再调用 Optimizer::PoseOptimization(&mCurrentFrame); 进行位姿优化。该函数放在后面进行详细的讲解。
本文内容来自计算机视觉life ORB-SLAM2 课程课件 
