文章目录
一、函数作用
本函数是用参考关键帧的地图点来对当前普通帧进行跟踪，是第一级跟踪中三大跟踪之一，三种跟踪方式分别为：恒速模型跟踪、参考关键帧跟踪、重定位跟踪。因为恒速跟踪最简单，所有恒速跟踪是最希望的方式，但在运动模型是空的或刚完成重定位时，恒速跟踪会失效，这是就会用到关键参考帧跟踪。另外用很素模型跟踪失败时，也可以用参考关键帧跟踪，从这一点看来，参考关键帧跟踪是比恒速跟踪更强力的跟踪方式。
二、函数讲解
本函数的追踪思想是，用参考关键帧的地图点来对当前普通帧进行跟踪，分为以下五个步骤，函数存在返回值，返回值为布尔类型，因为整个算法计算量最大，耗时最长的地方就在特征点的提取匹配上，这里采用了词袋向量（BoW）的方法来加速匹配，这里使用的优化方法为3D-2D（优化是算法的暗线，很重要），本函数最重要的地方是Step 2和Step 4，这步分别调用可重要函数（在下面的调用的函数中讲解）。
- Step 1：将当前普通帧的描述子转化为BoW向量
- Step 2：通过词袋BoW加速当前帧与参考帧之间的特征点匹配
- Step 3: 将上一帧的位姿态作为当前帧位姿的初始值
- Step 4: 通过优化3D-2D的重投影误差来获得位姿
- Step 5：剔除优化后的匹配点中的外点
三、函数代码
/*
* @brief 用参考关键帧的地图点来对当前普通帧进行跟踪
* @return 如果匹配数超10，返回true
*/
bool Tracking::TrackReferenceKeyFrame()
{
// Compute Bag of Words vector
// 将当前帧的描述子转化为BoW向量
mCurrentFrame.ComputeBoW();
// We perform first an ORB matching with the reference keyframe
// If enough matches are found we setup a PnP solver
// 初始化一个ORB特征匹配器（用于目标帧与参考关键帧之间的特征匹配）
ORBmatcher matcher(0.7,true); // 其中匹配的阈值为0.7，true表示启用描述子距离的归一化（转化为单位向量）
// 创建一个地图点指针容器
vector<MapPoint*> vpMapPointMatches;
// 使用BoW向量进行ORB特征匹配。
// SearchByBoW函数会在当前帧（mCurrentFrame）和参考关键帧（mpReferenceKF）之间找到足够多的匹配点，
// 并将结果存储在vpMapPointMatches中。nmatches 记录了成功匹配到的特征点数量。
int nmatches = matcher.SearchByBoW(mpReferenceKF,mCurrentFrame,vpMapPointMatches);
// 上面一行代码返回了匹配成功的特征点对的数量
// 如果数量小于15，就返回false（跟踪失败）
if(nmatches<15)
return false;
// 获取匹配到的点，将其放入当前帧的地图点容器中
mCurrentFrame.mvpMapPoints = vpMapPointMatches;
// 然后将上一帧的位姿（mLastFrame.mTcw）作为当前帧的初始位姿，进行粗略初始化
mCurrentFrame.SetPose(mLastFrame.mTcw);
// 通过优化3D-2D的重投影误差来获得位姿
Optimizer::PoseOptimization(&mCurrentFrame);
// Discard outliers
// 剔除优化后的匹配点中的外点
// 匹配的地图点
int nmatchesMap = 0;
// 遍历当前帧中的每个地图点
for(int i =0; i<mCurrentFrame.N; i++)
{
// 如果这个地图点存在
if(mCurrentFrame.mvpMapPoints[i])
{
// 判断是否为外点，清除他在该镇中的痕迹
if(mCurrentFrame.mvbOutlier[i])
{
MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
mCurrentFrame.mvbOutlier[i]=false;
pMP->mbTrackInView = false;
pMP->mnLastFrameSeen = mCurrentFrame.mnId;
// 成功匹配的特征点数量-1
nmatches--;
}
// 如果该点被观测次数大于0，则匹配的地图点数量+1
else if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)
nmatchesMap++;
}
}
// nmatchesMap>=10则返回true（跟踪成功），否则返回false（跟踪失败）
return nmatchesMap>=10;
}
四、调用的函数
1. ORBmatcher::SearchByBoW()
1). 函数讲解
本函数的作用是通过词袋，对关键帧的特征点进行跟踪，分为以下5个步骤，这里的匹配方式和剔除误匹配点的方式非常有意思。参考关键帧和本帧之间的特征点匹配，只匹配在相同词袋中的特征点，这样就不用挨个匹配，大量的减少了匹配时间，匹配的方式是计算描述子之间的距离，找出最佳匹配距离和次佳匹配距离（用于后续的筛选）。然后对匹配点进行筛选，只有满足1.最佳匹配距离小于50；2.最佳匹配距离小于次佳匹配距离的0.7倍；的点才算匹配成功，但是这些匹配点中存在误匹配的点，用直方图的方式来筛选。本人习惯用“桶”来描述每个容器，将旋转角度（0-360°）分成12个容器，然后将每一对匹配的点分配到对应的容器中。因为参考关键帧到本帧的旋转是固定的，所以匹配的点的旋转角度不应该差别很大，所以我们认为匹配最多点的那几个容器的匹配是合格的，其他的为误匹配，如下图中，保留11、0 、1这三个容器内的匹配点，其他的匹配点认为是误匹配。这里我认为可以优化以下，如果匹配点数最多的容器内的点超过总匹配点数的百分之70，就认为这个角度才是正确的，其他容器内的点都认为是误匹配的点
- Step 1：分别取出属于同一node的ORB特征点(只有属于同一node，才有可能是匹配点)
- Step 2：遍历KF中属于该node的特征点
- Step 3：遍历F中属于该node的特征点，寻找最佳匹配点
- Step 4：根据阈值 和 角度投票剔除误匹配
- Step 5：根据方向剔除误匹配的点
2). 函数代码
/*
* @brief 通过词袋，对关键帧的特征点进行跟踪
* 步骤
* Step 1：分别取出属于同一node的ORB特征点(只有属于同一node，才有可能是匹配点)
* Step 2：遍历KF中属于该node的特征点
* Step 3：遍历F中属于该node的特征点，寻找最佳匹配点
* Step 4：根据阈值 和 角度投票剔除误匹配
* Step 5：根据方向剔除误匹配的点
* @param pKF 关键帧
* @param F 当前普通帧
* @param vpMapPointMatches F中地图点对应的匹配，NULL表示未匹配
* @return 成功匹配的数量
*/
int ORBmatcher::SearchByBoW(KeyFrame* pKF,Frame &F, vector<MapPoint*> &vpMapPointMatches)
{
// 获取关键帧（参考关键帧跟踪中是参考关键帧）的地图点
const vector<MapPoint*> vpMapPointsKF = pKF->GetMapPointMatches();
// 创建并初始化F（普通帧）的容器
vpMapPointMatches = vector<MapPoint*>(F.N,static_cast<MapPoint*>(NULL)); // 容器大小为F.N,成员初始化为NULL
// 取出关键帧的词袋特征向量
const DBoW2::FeatureVector &vFeatVecKF = pKF->mFeatVec;
// 将匹配的特征点的个数初始化为0
int nmatches=0;
// 初始化特征点角度旋转差统计用的直方图
// 可以将整个角度范围 [0°, 360°] 划分为 HISTO_LENGTH 个“桶”，每个“桶”对应一个角度范围。
// HISTO_LENGTH = 30，那么每个“桶”覆盖 360°/30 = 12° 的角度范围。
vector<int> rotHist[HISTO_LENGTH];
// 给每个“桶”预分配500的容量
for(int i=0;i<HISTO_LENGTH;i++)
rotHist[i].reserve(500);
// 将0~360的转换为0~HISTO_LENGTH
// 这里出现了bug，应该改为const float factor = HISTO_LENGTH/360.0f;
const float factor = 1.0f/HISTO_LENGTH;
// We perform the matching over ORB that belong to the same vocabulary node (at a certain level)
// 获取关键帧和普通帧词袋特征向量的第一个成员和最后一个成员
DBoW2::FeatureVector::const_iterator KFit = vFeatVecKF.begin();
DBoW2::FeatureVector::const_iterator Fit = F.mFeatVec.begin();
DBoW2::FeatureVector::const_iterator KFend = vFeatVecKF.end();
DBoW2::FeatureVector::const_iterator Fend = F.mFeatVec.end();
// 将属于同一节点(特定层)的ORB特征进行匹配
while(KFit != KFend && Fit != Fend)
{
// Step 1：分别取出属于同一node的ORB特征点(只有属于同一node，才有可能是匹配点)
// first 元素就是node id，遍历
if(KFit->first == Fit->first)
{
// 分别取出KF和F中的第二个元素（second 是该node内存储的特征点索引）
const vector<unsigned int> vIndicesKF = KFit->second;
const vector<unsigned int> vIndicesF = Fit->second;
// 遍历KF中属于该node的特征点
for(size_t iKF=0; iKF<vIndicesKF.size(); iKF++)
{
// 获取第iKF个特征点
const unsigned int realIdxKF = vIndicesKF[iKF];
// 获取该特征点对应的地图点
MapPoint* pMP = vpMapPointsKF[realIdxKF];
// 如果pMP中的指针数量为空（成员数量）则结束本次循环，进入下一次循环
if(!pMP)
continue;
/// 如果特征点无效则结束本次循环，进入下一次循环
if(pMP->isBad())
continue;
// 取出KF中该特征对应的描述子
const cv::Mat &dKF= pKF->mDescriptors.row(realIdxKF);
// 定义一个描述子距离阈值
int bestDist1=256;
int bestIdxF =-1 ;
int bestDist2=256;
// 遍历F中属于该node的特征点
for(size_t iF=0; iF<vIndicesF.size(); iF++)
{
// 这部分个上述一样，不再赘述
const unsigned int realIdxF = vIndicesF[iF];
if(vpMapPointMatches[realIdxF])
continue;
const cv::Mat &dF = F.mDescriptors.row(realIdxF);
// 计算描述子之间的距离
const int dist = DescriptorDistance(dKF,dF);
// 遍历，记录最佳距离、最佳距离对应的索引、次佳距离等
// 如果 dist < bestDist1 < bestDist2，更新bestDist1 bestDist2
if(dist<bestDist1)
{
bestDist2=bestDist1;
bestDist1=dist;
bestIdxF=realIdxF;
}
// 如果bestDist1 < dist < bestDist2，更新bestDist2
else if(dist<bestDist2)
{
bestDist2=dist;
}
}
// 根据阈值 和 角度投票剔除误匹配
// Step 4.1：第一关筛选：匹配距离必须小于设定阈值
if(bestDist1<=TH_LOW)
{
// Step 4.2：第二关筛选：最佳匹配比次佳匹配明显要好，那么最佳匹配才真正靠谱
if(static_cast<float>(bestDist1)<mfNNratio*static_cast<float>(bestDist2))
{
// Step 4.3：记录成功匹配特征点的对应的地图点(来自关键帧)
vpMapPointMatches[bestIdxF]=pMP;
// 获取当其特征点的去畸变后的坐标
const cv::KeyPoint &kp = pKF->mvKeysUn[realIdxKF];
// Step 4.4：计算匹配点旋转角度差所在的直方图
if(mbCheckOrientation)
{
// 计算匹配的两个点的旋转角度
float rot = kp.angle-F.mvKeys[bestIdxF].angle;
// 讲角度转换到0-360度
if(rot<0.0)
rot+=360.0f;
// 计算这个旋转量所在的“桶”
int bin = round(rot*factor);
// 最后一个“桶”和第一个是同一个
if(bin==HISTO_LENGTH)
bin=0;
// assert（）为调试函数，括号内的值为false则打印错误信息，为true则程序继续运行
assert(bin>=0 && bin<HISTO_LENGTH);
// 将匹配的特征点的索引放入对应的“桶”内
rotHist[bin].push_back(bestIdxF);
}
// 匹配的特征点数+1
nmatches++;
}
}
}
KFit++;
Fit++;
}
// 如果KFit->first < Fit->first，则将KFit跳到一个不小于Fit->first的值
else if(KFit->first < Fit->first)
{
KFit = vFeatVecKF.lower_bound(Fit->first);
}
// 如果KFit->first > Fit->first，则将Fit跳到一个不小于KFit->first的值
else
{
Fit = F.mFeatVec.lower_bound(KFit->first);
}
}
// 根据方向剔除误匹配的点
if(mbCheckOrientation)
{
int ind1=-1;
int ind2=-1;
int ind3=-1;
// 筛选出在旋转角度差落在在直方图区间内数量最多的前三个bin的索引
ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3);
// 遍历每个“桶”
for(int i=0; i<HISTO_LENGTH; i++)
{
// 如果特征点的旋转角度变化量属于这三个组，则保留
if(i==ind1 || i==ind2 || i==ind3)
continue;
// 剔除掉不在前三的匹配对，因为他们不符合“主流旋转方向”
for(size_t j=0, jend=rotHist[i].size(); j<jend; j++)
{
// 将不符合的点设置为空指针（剔除）
vpMapPointMatches[rotHist[i][j]]=static_cast<MapPoint*>(NULL);
// 将匹配的特征点对-1
nmatches--;
}
}
}
// 返回成功匹配的特征点对的数量
return nmatches;
}
2. Optimizer::PoseOptimization()
1). 函数讲解
此函数是一个优化函数，它只优化位姿，不优化地图点的坐标，分为以下6步。本次优化的顶点为本帧的相机位姿，边为一元边，意思是只有一个顶点。优化（Step 5）分为四次优化，每次优化迭代10次，迭代完成后判断误差是大于合卡方分布误差7.815，则算作外边，不进行下一轮的优化，优化结束的条件是1.参与优化的边数量小于10；2.四次优化完成。(吐槽一下：这段代码非常的糟糕，单目双目都进行了，没有判断条件，还增加了一些没有用的计数。）优化完成后提取优化结果作为本帧的新位姿。这里的一元边非常的抽象，但是看下图就会好理解一些，顶点都是相机位姿，但是没条边的约束不一样，这也是为什么要遍历地图点，每个地图点的投影来作为观测值，求出来的误差都不一样，所以遍历地图点的意义相当重要。
- Step 1: 创建优化器和求解器
- Step 2: 设置当前帧的顶点
- Step 3: 设置MapPoint的边
- Step 4: 匹配的点数量不足3就返回
- Step 5: 进行4次优化，逐次剔除外点
- Step 6: 恢复优化后的位姿，并返回内点数量
2). 函数代码
/*
* @brief Pose Only Optimization
* 3D-2D 最小化重投影误差 e = (u,v) - project(Tcw*Pw) \n
* 1. Vertex: g2o::VertexSE3Expmap()，即当前帧的Tcw
* 2. Edge:
* - g2o::EdgeSE3ProjectXYZOnlyPose()，BaseUnaryEdge
* + Vertex：待优化当前帧的Tcw
* + measurement：MapPoint在当前帧中的二维位置(u,v)
* + InfoMatrix: invSigma2(与特征点所在的尺度有关)
* - g2o::EdgeStereoSE3ProjectXYZOnlyPose()，BaseUnaryEdge
* + Vertex：待优化当前帧的Tcw
* + measurement：MapPoint在当前帧中的二维位置(ul,v,ur)
* + InfoMatrix: invSigma2(与特征点所在的尺度有关)
*
* @param pFrame Frame
* @return inliers数量
*/
// 通过优化3D-2D的重投影误差来获得位姿
int Optimizer::PoseOptimization(Frame *pFrame)
{
// 该优化函数主要用于Tracking线程中：运动跟踪、参考帧跟踪、地图跟踪、重定位
// Step 1: 创建优化器和求解器
// 创建优化器对象
g2o::SparseOptimizer optimizer;
// Step 1：构造g2o优化器, BlockSolver_6_3表示：位姿 _PoseDim 为6维，路标点 _LandmarkDim 是3维
// 创建一个linearSolver指针
g2o::BlockSolver_6_3::LinearSolverType * linearSolver;
// LinearSolverDense是稠密的线性求解器 g2o::BlockSolver_6_3::PoseMatrixType是线性求解器件的一个类型，6维位姿3维特征点
linearSolver = new g2o::LinearSolverDense<g2o::BlockSolver_6_3::PoseMatrixType>();
// 创建一个 块求解器（BlockSolver）对象
g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
// 创建一个 g2o::OptimizationAlgorithmLevenberg 对象
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
// 设置算法为solver
// solver 是我们之前创建的优化算法，它结合了 BlockSolver_6_3 和 LinearSolverDense 来处理优化问题中的线性方程系统。
optimizer.setAlgorithm(solver);
// 有效的配对点个数
int nInitialCorrespondences=0;
// Set Frame vertex
// Step 2: 设置当前帧的顶点
// 创建了一个 VertexSE3Expmap 的新顶点对象 vSE3 是一个指向该顶点的指针
g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
// 将 pFrame->mTcw，转换为SE3的四元数表示，用于初始化顶点估计值。
vSE3->setEstimate(Converter::toSE3Quat(pFrame->mTcw));
// setId(0) 为该顶点分配一个ID，顶点的ID被设为0，表明这是优化问题中的第一个顶点。
vSE3->setId(0);
// setFixed(false) 表示这个顶点在优化过程中不被固定，即它的位姿是可以在优化过程中被调整的。
vSE3->setFixed(false);
// 这行代码将顶点 vSE3 添加到优化器 optimizer 中，开始参与优化。
optimizer.addVertex(vSE3);
// Set MapPoint vertices
// Step 3: 设置MapPoint的边
// 获取地图点个数
const int N = pFrame->N;
// 单目先不看
vector<g2o::EdgeSE3ProjectXYZOnlyPose*> vpEdgesMono;
vector<size_t> vnIndexEdgeMono;
vpEdgesMono.reserve(N);
vnIndexEdgeMono.reserve(N);
// 双目
// 这行代码定义了一个 vector 容器，名为vpEdgesStereo，它储存的是双目边的信息
vector<g2o::EdgeStereoSE3ProjectXYZOnlyPose*> vpEdgesStereo;
// 这行代码定义了一个 vector 容器，名为 vnIndexEdgeStereo，它存储的是 size_t 类型的数据，通常用于存储索引值。
vector<size_t> vnIndexEdgeStereo;
vpEdgesStereo.reserve(N);
vnIndexEdgeStereo.reserve(N);
// 自由度为2的卡方分布，显著性水平为0.05，对应的临界阈值5.991
const float deltaMono = sqrt(5.991);
// 自由度为3的卡方分布，显著性水平为0.05，对应的临界阈值7.815
const float deltaStereo = sqrt(7.815);
{
unique_lock<mutex> lock(MapPoint::mGlobalMutex);
// 遍历所有的地图点
for(int i=0; i<N; i++)
{
// 获取地图点
MapPoint* pMP = pFrame->mvpMapPoints[i];
// 如果pMP里面的元素不空
if(pMP)
{
// Monocular observation
// 单目情况
// mvuRight[i]容器装的是右图像中与左图像对应的特征点的横坐标
// mvuRight[i]<0说明右目中没有这个特征点
if(pFrame->mvuRight[i]<0)
{
// 匹配的点+1
nInitialCorrespondences++;
// 将这个点标记为非离群点（true为离群点）
pFrame->mvbOutlier[i] = false;
// 定义一个2*1的列向量
// obs 用来表示图像中单个特征点的二维坐标观测值（x，y）
Eigen::Matrix<double,2,1> obs;
// 获取去畸变后的坐标点
const cv::KeyPoint &kpUn = pFrame->mvKeysUn[i];
// 将去畸变后的x，y坐标放入obs中
obs << kpUn.pt.x, kpUn.pt.y;
// 创建一个g2o::EdgeStereoSE3ProjectXYZOnlyPose类型的边
// Edge表示边，SE3表示位姿的群，OnlyPose表示只优化位姿
g2o::EdgeSE3ProjectXYZOnlyPose* e = new g2o::EdgeSE3ProjectXYZOnlyPose();
// 设置图优化的顶点，dynamic_cast<g2o::OptimizableGraph::Vertex*>表示
// 将结果安全的转换为g2o::OptimizableGraph::Vertex*类型
// 第一个0表示，设置第一个顶点（index为0），optimizer.vertex(0)表示将相机中的第一个顶点（实际上相机中就位姿这一个顶点）
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));
// 将obs设置为边e的测量值
e->setMeasurement(obs);
// 从帧 pFrame 中获取与当前特征点 kpUn 的尺度等级（octave）对应的逆尺度系数 invSigma2
const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave];
// 设置了边 e 的 信息矩阵，即优化过程中的权重矩阵。
// Identity()是2*2的单位矩阵，乘以invSigma2：金字塔层级缩放因子平方的倒数
e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);
// 用Huber鲁棒核函数 来处理图优化中的外点问题，进一步提升优化的稳健性
// Huber 核函数是一种常用的鲁棒核，能够在误差较小时使用二次损失函数（即标准误差函数），
// 在误差较大时使用线性损失函数，从而减少外点对优化的影响
g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
// 是将我们创建的 Huber 核函数 rk 设置为边 e 的 鲁棒核函数
e->setRobustKernel(rk);
// 设置阈值，误差小于deltaMono时用二次模型，误差大于deltaMono时用一次模型（减小大误差的影响）
rk->setDelta(deltaMono);
// 将相机内参传递给边e
e->fx = pFrame->fx;
e->fy = pFrame->fy;
e->cx = pFrame->cx;
e->cy = pFrame->cy;
// 获取该特征代年的世界坐标
cv::Mat Xw = pMP->GetWorldPos();
// 获取地图点的空间位置,作为迭代的初始值
e->Xw[0] = Xw.at<float>(0);
e->Xw[1] = Xw.at<float>(1);
e->Xw[2] = Xw.at<float>(2);
// 将边e放入优化器中
optimizer.addEdge(e);
// 将e放入储存边的容器中
vpEdgesMono.push_back(e);
// 将这个边的索引放入存储索引的容器中
vnIndexEdgeMono.push_back(i);
}
// 双目情况
// mvuRight[i]>0说明右目中有这个特征点，说明是双目
else
{
// 匹配的点+1
nInitialCorrespondences++;
// 将这个点标记为非离群点（true为离群点）
pFrame->mvbOutlier[i] = false;
//SET EDGE（设置边）
// 单目为2维度，因为单目没有右目图像，双目中多了一个右目的横坐标
// obs 用来表示图像中单个特征点的二维坐标观测值x，y 和右目特征点的横坐标
Eigen::Matrix<double,3,1> obs;
// 获取去畸变的特征点坐标
const cv::KeyPoint &kpUn = pFrame->mvKeysUn[i];
// 获取右目特征点的横左边
const float &kp_ur = pFrame->mvuRight[i];
// 将二维坐标x，y和右目横坐标放入obs中
obs << kpUn.pt.x, kpUn.pt.y, kp_ur;
// 创建一个g2o::EdgeStereoSE3ProjectXYZOnlyPose类型的边
// Edge表示边，SE3表示位姿的群，OnlyPose表示只优化位姿
g2o::EdgeStereoSE3ProjectXYZOnlyPose* e = new g2o::EdgeStereoSE3ProjectXYZOnlyPose();
// 将相机位姿设置为边e的第一个顶点（索引为0的顶点）
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));
// 将obs设置为边e的观测值
e->setMeasurement(obs);
// 获取缩放因子平方的倒数
const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave];
// // 设置了边 e 的 信息矩阵，即优化过程中的权重矩阵。
Eigen::Matrix3d Info = Eigen::Matrix3d::Identity()*invSigma2;
// 将Info设置为边e的信息矩阵
e->setInformation(Info);
// // 用Huber鲁棒核函数 来处理图优化中的外点问题，进一步提升优化的稳健性
g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
e->setRobustKernel(rk);
rk->setDelta(deltaStereo);
// 将相机内参传递给边e
e->fx = pFrame->fx;
e->fy = pFrame->fy;
e->cx = pFrame->cx;
e->cy = pFrame->cy;
e->bf = pFrame->mbf;
// 将世界坐标传递给边e
cv::Mat Xw = pMP->GetWorldPos();
e->Xw[0] = Xw.at<float>(0);
e->Xw[1] = Xw.at<float>(1);
e->Xw[2] = Xw.at<float>(2);
// 将边e加入优化器中
optimizer.addEdge(e);
// 将边e和他的索引放入相应的容器中
vpEdgesStereo.push_back(e);
vnIndexEdgeStereo.push_back(i);
}
}
}
}
// Step 4: 观测数量不足时直接返回
// 如果观测数量小于3就返回
if(nInitialCorrespondences<3)
return 0;
// We perform 4 optimizations, after each optimization we classify observation as inlier/outlier
// At the next optimization, outliers are not included, but at the end they can be classified as inliers again.
// Step 5: 进行4次优化，逐次剔除外点
// 定义一些常量数组和变量
// 存储单目（Mono）观测的卡方（Chi-squared，chi²）检验值
const float chi2Mono[4]={5.991,5.991,5.991,5.991};
// 存储双目（Stereo）观测的卡方检验值的数组
const float chi2Stereo[4]={7.815,7.815,7.815, 7.815};
// 这是一个长度为 4 的常量整数数组，每个元素值为10，表示优化的最大迭代次数
const int its[4]={10,10,10,10};
// 统计优化过程中被标记为“坏”点的数量
int nBad=0;
// 优化4次
for(size_t it=0; it<4; it++)
{
// 为优化器的顶点设置初值
vSE3->setEstimate(Converter::toSE3Quat(pFrame->mTcw));
// 初始化优化器
optimizer.initializeOptimization(0);
// 开始优化，优化10次
optimizer.optimize(its[it]);
// 将nBad重置为0（因为要优化4次，前面的优化会对后面产生影响）
nBad=0;
// 这里不够简洁，无论单目还是双目，都将运行两个函数（单目和双目的）
// 优化结束,开始遍历参与优化的每一条误差边(单目) ，先不看
for(size_t i=0, iend=vpEdgesMono.size(); i<iend; i++)
{
g2o::EdgeSE3ProjectXYZOnlyPose* e = vpEdgesMono[i];
const size_t idx = vnIndexEdgeMono[i];
if(pFrame->mvbOutlier[idx])
{
e->computeError();
}
const float chi2 = e->chi2();
if(chi2>chi2Mono[it])
{
pFrame->mvbOutlier[idx]=true;
e->setLevel(1);
nBad++;
}
else
{
pFrame->mvbOutlier[idx]=false;
e->setLevel(0);
}
if(it==2)
e->setRobustKernel(0);
}
// 优化结束,开始遍历参与优化的每一条误差边(单目)
for(size_t i=0, iend=vpEdgesStereo.size(); i<iend; i++)
{
// 获取这个边的信息
g2o::EdgeStereoSE3ProjectXYZOnlyPose* e = vpEdgesStereo[i];
// 获取这个边的索引
const size_t idx = vnIndexEdgeStereo[i];
// 如果这个点不是外点
if(pFrame->mvbOutlier[idx])
{
// 计算误差值
e->computeError();
}
// 计算边 e 的卡方误差值
// 就是χ2= eTΩe，Ω 是信息矩阵Info
const float chi2 = e->chi2();
// 层级 0：表示这条边在所有优化迭代中都会被使用；
// 层级 1：这些边通常会在优化的某些阶段中被跳过，而只在较高层次（如全局优化阶段）中参与
// 如果误差大于阈值(7.815）,将点设置为外点，将这个边设置为第1层，nBad++
if(chi2>chi2Stereo[it])
{
pFrame->mvbOutlier[idx]=true;
e->setLevel(1);
nBad++;
}
// 如果误差值小于阈值，则将这个边设置为第0层
else
{
e->setLevel(0);
pFrame->mvbOutlier[idx]=false;
}
// 当到达第二此优化时，移除边e的鲁棒核函数
// 第一次优化时减少大误差对结果的影响过大，经过一次优化后，误差不会太大移除可以充分的运用所有数据
if(it==2)
e->setRobustKernel(0);
}
// 如果优化的边数小于10，则结束
if(optimizer.edges().size()<10)
break;
}
// Recover optimized pose and return number of inliers
// Step 6: 恢复优化后的位姿，并返回内点数量
// 通过 static_cast 将优化器中的第 0 号顶点转换为 g2o::VertexSE3Expmap 类型的顶点指针
g2o::VertexSE3Expmap* vSE3_recov = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(0));
// 估计相机位姿
g2o::SE3Quat SE3quat_recov = vSE3_recov->estimate();
// 将为位姿转换为Mat类型
cv::Mat pose = Converter::toCvMat(SE3quat_recov);
// 重新设置相机的位姿（优化后）
pFrame->SetPose(pose);
// 返回内点数目
return nInitialCorrespondences-nBad;
}
五、总结
本函数是第一次追踪的三大追踪之一的-----参考关键帧追踪，他的追踪能里很强，如果参考关键帧追踪都失败了，那只能进行重定位了。本函数的难点和重点是特征匹配和位姿优化，这也是ORB-SLAM2中最重要的两个点，特征匹配的重要就不再赘述，位姿优化是整个ORB-SLAM2的一条暗线，这是学的第一个优化函数，在学习的过程中，个人认为优化函数是最难理解的，但是看整个文件后，回头将几个优化函数放一起对比学习，发现原来优化函数套路非常的一致且简单，如果觉得理解有困难的可以多看视觉SLAM十四讲，复习一下图优化的知识，也欢迎大家和我沟通。
标签：Tracking,匹配,函数,Step,----,SLAM2,pFrame,优化,const From： https://blog.csdn.net/uzi_ccc/article/details/143716423