文章目录
- 一、函数作用
- 二、函数讲解
- 三、函数代码
- 四、调用的函数
- 1. Tracking::UpdateLastFrame()
- 1）. 函数讲解
- 2）. 函数代码
- 2. ORBmatcher::SearchByProjection()
- 1）. 函数讲解
- 2）. 函数代码
- 3. Optimizer::PoseOptimization(Frame *pFrame)
- 1）. 函数讲解
- 2）. 函数代码
- 五、总结
一、函数作用
本函数是第一级跟踪的三大跟踪函数之一的恒速跟踪模型，因为恒速跟踪最为简单，所以如果没有意外的话，希望一直使用恒速跟踪，但是恒速跟踪的跟踪强度不高，在跟踪失败时，就需要使用跟踪强度更高的参考关键帧跟踪。
二、函数讲解
本函数的实现分为下面的四个个步骤。本函数获取本帧初始位姿的方式是，获取上一帧的位姿，然后根据之前估计的速度来计算，因为可能不匀速或者出现其他特殊情况，就会出现特征点匹配不上或者匹配点过少的情况，所以说他是一种跟踪强度不高的跟踪方式。本函数的特征匹配方式是用的半径搜索方法，即相邻帧之间的运动变化不会太快，特征点的坐标变化范围有限，所有我们只需以上一帧特征点为中心，以一个合适的长度为半径（单目为7，双目和RGBD为15)，来搜索就可以，这样能更大限度的减小匹配时间。本函数的难点同样在特征匹配和位姿优化中（在调用的函数中讲到）。
- Step 1：更新上一帧的位姿；对于双目或RGB-D相机，还会根据深度值生成临时地图点
- Step 2：根据上一帧特征点对应地图点进行投影匹配
- Step 3：优化当前帧位姿
- Step 4：剔除地图点中外点
三、函数代码
/**
* @brief 根据恒定速度模型用上一帧地图点来对当前帧进行跟踪
* @return 如果匹配数大于10，认为跟踪成功，返回true
*/
bool Tracking::TrackWithMotionModel()
{
ORBmatcher matcher(0.9,true);
// Update last frame pose according to its reference keyframe
// Create "visual odometry" points if in Localization Mode
// 更新上一帧的位姿；对于双目或RGB-D相机，还会根据深度值生成临时地图点
UpdateLastFrame();
// 根据之前估计的速度，用恒速模型得到当前帧的初始位姿。
mCurrentFrame.SetPose(mVelocity*mLastFrame.mTcw);
// 清空当前帧的地图点
fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast<MapPoint*>(NULL));
// Project points seen in previous frame
// 设置特征匹配过程中的搜索半径
int th;
if(mSensor!=System::STEREO)
th=15;
else
th=7;
// 特征匹配
int nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,th,mSensor==System::MONOCULAR);
// If few matches, uses a wider window search
// 如果匹配的特征点数小于20 ，则将半径扩大一倍再次搜索
if(nmatches<20)
{
// 清空容器
fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast<MapPoint*>(NULL));
// 将半径扩大一倍再次搜索
nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,2*th,mSensor==System::MONOCULAR);
}
// 如果匹配点数仍然小于20，则返回匹配失败
if(nmatches<20)
return false;
// Optimize frame pose with all matches
// 利用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;
nmatches--;
}
else if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)
nmatchesMap++;
}
}
if(mbOnlyTracking)
{
mbVO = nmatchesMap<10;
return nmatches>20;
}
return nmatchesMap>=10;
}
四、调用的函数
1. Tracking::UpdateLastFrame()
1）. 函数讲解
本函数的作用是：更新上一帧的位姿；对于双目或RGB-D相机，还会根据深度值生成临时地图点。要注意这里是针对上一帧的，不针对本帧！！！这个非常重要，这里是用参考关键帧位姿和上一帧到参考关键帧的变换矩阵来计算上一帧的坐标。对于双目，会判断上一帧的特征点是否有对应的地图点，如果有就需要添加地图点，还会判断地图点有没有被观测过，如果没有被观测，则需要替换地图点。这里有一种点是不用生成地图点的，就是深度超出阈值（35倍基线长度）的点，这里面有个非常有意思的事情，就是深度是按照大小排列的，如果发现一个深度超出阈值，那么后面的点都超出了阈值，所以发现一个超出阈值的点，就可以直接停止生成地图点了，然后就是临时地图点数量超过100也会停止生成地图点。这里比较重要的就是如何使用特征点生成地图点，以双目相机为例，双目相机模型如下图所示，我们用初中学过的相似三角形（三角化）来求解相机的深度。d为视差，不难看出视差越大，深度越小，这时就会产生一个问题，如果视差足够小，就说明这个点在很远的地方，这样的点误差很大，也是我们不希望提取到的特征点，所以在上述条件中我们将深度超出阈值（35倍基线长度）的点不生成地图点。
2）. 函数代码
void Tracking::UpdateLastFrame()
{
// Update pose according to reference keyframe
// 获取上一帧的参考关键帧
KeyFrame* pRef = mLastFrame.mpReferenceKF;
// 获取最后一个相对位姿变换矩阵的引用，是当前帧位姿到参考关键帧位姿的变换矩阵
cv::Mat Tlr = mlRelativeFramePoses.back();
// 将前一帧的位姿设置为i参考帧位姿和Tlr的乘积
mLastFrame.SetPose(Tlr*pRef->GetPose());
// // 如果上一帧为关键帧，或者单目，或者仅跟踪的情况，则退出
if(mnLastKeyFrameId==mLastFrame.mnId || mSensor==System::MONOCULAR || !mbOnlyTracking)
return;
// Create "visual odometry" MapPoints
// We sort points according to their measured depth by the stereo/RGB-D sensor
// 创建特征点深度信息的容器
vector<pair<float,int> > vDepthIdx;
vDepthIdx.reserve(mLastFrame.N);
// 得到上一帧中具有有效深度值的特征点（不一定是地图点）
// 并设置vDepthIdx第一个元素是某个点的深度,第二个元素是对应的特征点id
for(int i=0; i<mLastFrame.N;i++)
{
float z = mLastFrame.mvDepth[i];
if(z>0)
{
vDepthIdx.push_back(make_pair(z,i));
}
}
// 如果上一帧中没有有效深度，则直接退出
if(vDepthIdx.empty())
return;
// 将深度从小到大排序
sort(vDepthIdx.begin(),vDepthIdx.end());
// We insert all close points (depth<mThDepth)
// If less than 100 close points, we insert the 100 closest ones.
// 从中找出不是地图点的部分，并生成新的临时地图点
int nPoints = 0;
// 遍历特征点
for(size_t j=0; j<vDepthIdx.size();j++)
{
// i = 特征点的索引
int i = vDepthIdx[j].second;
// 初始化创建新地图点变量为false
bool bCreateNew = false;
// 获取上一帧的地图点
MapPoint* pMP = mLastFrame.mvpMapPoints[i];
// 如果地图点不存在，需要生成新的地图点
if(!pMP)
bCreateNew = true;
// 检测该地图带年是否被至少观测一次，未被观测，也需要生成新的地图点
else if(pMP->Observations()<1)
{
bCreateNew = true;
}
// 如果上述情况bCreateNew = true则生成新的地图点
if(bCreateNew)
{
// 将该特征点投影到三维
cv::Mat x3D = mLastFrame.UnprojectStereo(i);
// 创建新的地图法点
MapPoint* pNewMP = new MapPoint(x3D,mpMap,&mLastFrame,i);
// 将其记录到上一帧对应的地方
mLastFrame.mvpMapPoints[i]=pNewMP;
// 将其追加到mlpTemporalPoints中
mlpTemporalPoints.push_back(pNewMP);
// 临时生成的地图点数+1
nPoints++;
}
// 如果bCreateNew = false，则
else
{
// ？？？
nPoints++;
}
// 如果该特征点深度超出阈值（35倍基线长度）或者临时生成的地图点数大于100 ，则停止生成地图点
// 因为按照深度排序的，如过发现一个深度超标，那后面的都超标了
if(vDepthIdx[j].first>mThDepth && nPoints>100)
break;
}
}
2. ORBmatcher::SearchByProjection()
1）. 函数讲解
本函数分为以下七个步骤，恒速追踪模型相对参考关键帧跟踪模型来说跟踪强度更小，造成这个的原因之一就是特征匹配的方式不同，在恒速追踪模型中（本函数）的特征匹配函数，认为上一帧相对于本帧的相对位移变化不打，所以只在一个有限的区域内搜索相匹配的点，这个范围就是，以特征点为圆心，以一个设定值为半径（单目，双目为15），来搜索和他匹配的点，这个搜索方法在之前的Frame::GetFeaturesInArea函数中讲到过，他的缺点就是，在平移时特征点们的相对运动时一致的，但是旋转不能保证每个点的幅度一致，这样就会使得一些点跑出匹配范围而没有被匹配到，如果旋转量相对较大的时候，可能造成匹配点数太少，使得恒速追踪模型失效。这个方法匹配后，同样使用了旋转直方图的方法来排除一些明显匹配错误点的（这个在前面的参考帧追踪中详细讲过，这里就不再赘述了）
- Step 1 建立旋转直方图，用于检测旋转一致性
- Step 2 计算当前帧和前一帧的平移向量
- Step 3 对于前一帧的每一个地图点，通过相机投影模型，得到投影到当前帧的像素坐标
- Step 4 根据相机的前后前进方向来判断搜索尺度范围
- Step 5 遍历候选匹配点，寻找距离最小的最佳匹配点
- Step 6 计算匹配点旋转角度差所在的直方图
- Step 7 进行旋转一致检测，剔除不一致的匹配
2）. 函数代码
/**
* @brief 将上一帧跟踪的地图点投影到当前帧，并且搜索匹配点。用于跟踪前一帧
* @param[in] CurrentFrame 当前帧
* @param[in] LastFrame 上一帧
* @param[in] th 搜索范围阈值，默认单目为7，双目15
* @param[in] bMono 是否为单目
* @return int 成功匹配的数量
*/
int ORBmatcher::SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame, const float th, const bool bMono)
{
// 初始化匹配成功点的数量
int nmatches = 0;
// Rotation Histogram (to check rotation consistency)
// 建立旋转直方图，用于检测旋转一致性
vector<int> rotHist[HISTO_LENGTH];
// 给每个“桶”，预先分配500个特征点空间
for(int i=0;i<HISTO_LENGTH;i++)
rotHist[i].reserve(500);
// 正确代码为const float factor = HISTO_LENGTH/360.0f;
const float factor = 1.0f/HISTO_LENGTH;
// 获取当前的相旋转矩阵和平移矩阵
const cv::Mat Rcw = CurrentFrame.mTcw.rowRange(0,3).colRange(0,3);
const cv::Mat tcw = CurrentFrame.mTcw.rowRange(0,3).col(3);
// 获取当前帧的平移量
const cv::Mat twc = -Rcw.t()*tcw;
// 获取上一的相旋转矩阵和平移矩阵
const cv::Mat Rlw = LastFrame.mTcw.rowRange(0,3).colRange(0,3);
const cv::Mat tlw = LastFrame.mTcw.rowRange(0,3).col(3);
// 获取上一帧的平移量
const cv::Mat tlc = Rlw*twc+tlw;
// 判读是前进还是后退
const bool bForward = tlc.at<float>(2)>CurrentFrame.mb && !bMono;
const bool bBackward = -tlc.at<float>(2)>CurrentFrame.mb && !bMono;
// 对于前一帧的每一个地图点，通过相机投影模型，得到投影到当前帧的像素坐标
for(int i=0; i<LastFrame.N; i++)
{
// 获取上一帧第i个特征点对应的地图点
MapPoint* pMP = LastFrame.mvpMapPoints[i];
// 如果存在地图点，且不是外点，则对上一帧有效的MapPoints投影到当前帧坐标系
if(pMP)
{
if(!LastFrame.mvbOutlier[i])
{
// Project
// 获取上一帧的地图点
cv::Mat x3Dw = pMP->GetWorldPos();
cv::Mat x3Dc = Rcw*x3Dw+tcw;
const float xc = x3Dc.at<float>(0);
const float yc = x3Dc.at<float>(1);
const float invzc = 1.0/x3Dc.at<float>(2);
if(invzc<0)
continue;
// 投影到当前帧中
float u = CurrentFrame.fx*xc*invzc+CurrentFrame.cx;
float v = CurrentFrame.fy*yc*invzc+CurrentFrame.cy;
if(u<CurrentFrame.mnMinX || u>CurrentFrame.mnMaxX)
continue;
if(v<CurrentFrame.mnMinY || v>CurrentFrame.mnMaxY)
continue;
// 获取上一帧地图点对应特征点的金字塔层级
int nLastOctave = LastFrame.mvKeys[i].octave;
// Search in a window. Size depends on scale
// 计算搜索半径
float radius = th*CurrentFrame.mvScaleFactors[nLastOctave];
// 记录候选匹配点的id
vector<size_t> vIndices2;
// Step 4 根据相机的前后前进方向来判断搜索尺度范围。
// 以下可以这么理解，例如一个有一定面积的圆点，在某个尺度n下它是一个特征点
// 当相机前进时，圆点的面积增大，在某个尺度m下它是一个特征点，由于面积增大，则需要在更高的尺度下才能检测出来
// 当相机后退时，圆点的面积减小，在某个尺度m下它是一个特征点，由于面积减小，则需要在更低的尺度下才能检测出来
if(bForward) // 前进,则搜索的金字塔层级为大于等于上一帧那个特征点在的层级
vIndices2 = CurrentFrame.GetFeaturesInArea(u,v, radius, nLastOctave);
else if(bBackward) // 后退,则搜索的金字塔层级为0到上一帧那个特征点在的层级
vIndices2 = CurrentFrame.GetFeaturesInArea(u,v, radius, 0, nLastOctave);
else // 在[nLastOctave-1, nLastOctave+1]中搜索
vIndices2 = CurrentFrame.GetFeaturesInArea(u,v, radius, nLastOctave-1, nLastOctave+1);
// 如果没有候选的匹配点，则返回，进入下一次循环
if(vIndices2.empty())
continue;
// 获取特征点的描述子
const cv::Mat dMP = pMP->GetDescriptor();
// 初始化最好的匹配距离和其索引
int bestDist = 256;
int bestIdx2 = -1;
// 遍历所有候选的特征点
for(vector<size_t>::const_iterator vit=vIndices2.begin(), vend=vIndices2.end(); vit!=vend; vit++)
{
// 创建特征点的指针
const size_t i2 = *vit;
// 如果地图点存在且观测数大于1，则返回，进入下一次循环
if(CurrentFrame.mvpMapPoints[i2])
if(CurrentFrame.mvpMapPoints[i2]->Observations()>0)
continue;
// 如果右目中有这个点（双目情况）
if(CurrentFrame.mvuRight[i2]>0)
{
// 获取右目特征点坐标
const float ur = u - CurrentFrame.mbf*invzc;
const float er = fabs(ur - CurrentFrame.mvuRight[i2]);
// 保证右目也在搜索范围内
if(er>radius)
continue;
}
// 获取当前帧的描述子
const cv::Mat &d = CurrentFrame.mDescriptors.row(i2);
// 计算描述子之间的距离
const int dist = DescriptorDistance(dMP,d);
// 寻找最佳距离，记录最佳距离和他的索引
if(dist<bestDist)
{
bestDist=dist;
bestIdx2=i2;
}
}
// 如果最佳距离小于等于阈值
if(bestDist<=TH_HIGH)
{
// 将这个地图点加入到当前帧地图点容器中
CurrentFrame.mvpMapPoints[bestIdx2]=pMP;
// 匹配成功的点数加1
nmatches++;
// 如果特征点的方向没有检查，则检查
if(mbCheckOrientation)
{
// 计算此帧去畸变后的特征点相对上一帧该点的旋转角
float rot = LastFrame.mvKeysUn[i].angle-CurrentFrame.mvKeysUn[bestIdx2].angle;
// 将其控制在360度以内
if(rot<0.0)
rot+=360.0f;
// 确定他所在的“桶”
int bin = round(rot*factor);
// 最后一个“桶”和第一个是同一个（因为是圆型）
if(bin==HISTO_LENGTH)
bin=0;
// 将特征点放入对应的“桶”内
assert(bin>=0 && bin<HISTO_LENGTH);
rotHist[bin].push_back(bestIdx2);
}
}
}
}
}
//Apply rotation consistency
// 找出装特征点数量前三的“桶”，剔除不在这三个“桶”中的特征点（这些点属于方向角错误的点，大概率是匹配错误）
if(mbCheckOrientation)
{
int ind1=-1;
int ind2=-1;
int ind3=-1;
ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3);
for(int i=0; i<HISTO_LENGTH; i++)
{
if(i!=ind1 && i!=ind2 && i!=ind3)
{
for(size_t j=0, jend=rotHist[i].size(); j<jend; j++)
{
CurrentFrame.mvpMapPoints[rotHist[i][j]]=static_cast<MapPoint*>(NULL);
nmatches--;
}
}
}
}
// 返回匹配成功点的个数
return nmatches;
}
3. Optimizer::PoseOptimization(Frame *pFrame)
1）. 函数讲解
这个函数分为以下几个步骤，这是我们学习到的第一种优化的第二个优化，使用的非线性优化方法为LM法，设置的顶点为本帧的SE3位姿，设置的边为同样是一元边，约束条件为地图点的坐标，使用的观测值时特征点去畸变后的坐标（x，y），这里有个细节是用信息矩阵来设置权重，原因是不同金字塔层级是经过缩放的，编号更大的金字塔层级的整体图像更小，那他的误差像也会小，但是并不代表他的误差真的很小，我们应该将其放在同一个尺度下进行比较，所以会给他分配一个更大的权重（将误差恢复到第0层金字塔的尺度下）。然后就开始优化了，进行4次优化，每次优化迭代10次，这和参考关键帧的优化是一致的，需要注意的是，优化中optimizer.initializeOptimization(0);代表的是只优化处于0层的边，这也是剔除边的依据，这次的优化，讲误差不符合卡方分布要求的边剔除了，后续还会学到很多的优化，其初始话的优化器表示方式是optimizer.initializeOptimization();代表他会优化所有的边，直到结束。
- Step 1: 创建优化器和求解器
- Step 2: 设置当前帧的顶点
- Step 3: 设置MapPoint的边
- Step 4: 匹配的点数量不足3就返回
- Step 5: 进行4次优化，,每次优化有10次迭代，逐次剔除外边（不符合要求的边）
- Step 6: 恢复优化后的位姿，并返回内点数量
2）. 函数代码
/*
* @brief Pose Only Optimization
*
* 3D-2D 最小化重投影误差 e = (u,v) - project(Tcw*Pw) \n
* 只优化Frame的Tcw，不优化MapPoints的坐标
*
* 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));
// 初始化优化器，0表示只优化第0层的边
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;
}
五、总结
本函数为第一层追踪的三大追踪方法之一的恒速追踪模型，也是用到最多的追踪方式，他的特征匹配非常的简单，匹配次数非常的少，之和自己附件的点进行匹配，所以会出现漏匹配的情况，在漏匹配比较多的时候，会造成匹配点数不符合要求这样的尴尬情况，这时就需要用更强大的追踪方式来进行追踪（当然，这样做的代价更大，比如说匹配次数会增加，这也是为什么优先恒速追踪模型），而且恒速追踪模型因为实在附近进行匹配点，所以他一旦匹配成功，可靠性就会很强，这也是为什么初始话匹配器时（ORBmatcher matcher(0.9,true);）第一个数值是0.9的原因。总的来说这个函数难度最大的地方是特征匹配和优化的部分，优化的学习需要积累，每看一次优化，就回头和之前的优化比较，会感觉到越学越轻松，如果有不懂的地方，欢迎大家交流。
