匀速运动模型估计相机位姿
我们在分析轨迹跟踪的总体过程时， 看到ORB-SLAM2完成了地图初始化建立地图坐标系之后，将会估计每一帧相机在该坐标系下的位姿。 有人把这一任务称为视觉里程计。 一般情况下ORB-SLAM都是先通过匀速运动模型粗略的估计相机位姿，再通过特征点匹配和优化方法给出一个较为精确的位姿估计。有时相机可能运动过快，导致里程计跟丢了，此时ORB-SLAM会通过词袋模型， 以及特征点匹配来重定位。
本文中，我们将详细分析根据匀速运动模型估计相机位姿的过程。查看ORB-SLAM2如何根据估计的相机位姿进行特征点匹配，并使用g2o完成位姿估计的优化。
1. 匀速运动模型定位总体过程
所谓的匀速运动模型，就是假设从上一帧到当前帧的这段时间里机器人的线速度和角速度都没有变化， 直接通过速度和时间间隔估计前后两帧的相对位姿变换，再将之直接左乘到上一帧的位姿上，从而得到当前帧的位姿估计。
虽然轨迹跟踪器中有一个成员变量 mVelocity 专门记录相机的速度， 但是 ORB-SLAM2 并没有真正计算过相机的速度。其系统对象的数据驱动接口虽然也输入了时间戳， 但是从来没有拿来估算过时间间隔。实际上，轨迹跟踪器的成员函数 Track() 在成功调用 TrackLocalMap() 之后，并通过 NeedNewKeyFrame() 和 CreateNewKeyFrame() 判定并生成关键帧之前， 更新过成员变量 mVelocity，如下面的代码片段所示。在分析总体过程的时候，我们把这一段当做是系统状态更新，给省略了。
void Tracking::Track() {
// 成功调用 TrackLocalMap() 之后
cv::Mat LastTwc = cv::Mat::eye(4,4,CV_32F);
mLastFrame.GetRotationInverse().copyTo(LastTwc.rowRange(0,3).colRange(0,3));
mLastFrame.GetCameraCenter().copyTo(LastTwc.rowRange(0,3).col(3));
mVelocity = mCurrentFrame.mTcw*LastTwc;
// 通过 NeedNewKeyFrame() 和 CreateNewKeyFrame() 判定并生成关键帧之前
}
在 ORB-SLAM2 中，Twc 表示从相机坐标系到世界坐标系的变换，Tcw 则是世界到相机坐标系的变换，对于同一帧图像而言，它们是互逆的关系，即，I = Twc * Tcw。
在右侧代码的第4行中通过Frame的接口 GetRotationInverse 获取了上一帧相机的旋转矩阵的逆，
第5行中的 GetCameraCenter() 则是相机光心在世界坐标系下的坐标。这两行构成了上一帧图像的变换矩阵的逆 LastTwc。第6行的意义是，当前帧的位姿乘以上一帧位姿的逆，
得到的就是当前帧相对于上一帧的位姿变化量。
假设相机的帧率是固定的，而且没有丢帧的情况，那么将这个 mVelocity 的成员变量左乘到当前帧位姿上，得到的就是根据匀速模型估计的下一帧图像的相机位姿。 所以这个 mVelocity 实际就是相机位姿的增量，叫做 mDeltaTcw 更合适。匀速运动模型的跟踪定位方法是在函数 TrackWithMotionModel() 中实现的。
在函数一开始，先创建了一个ORB匹配器，用于在当前帧上搜索，与地图点匹配的特征点。 我们给匹配器的构造函数传递了两个参数，第一个参数是一个接受最佳匹配的系数，只有当最佳匹配点的汉明距离小于次佳匹配点距离的0.9倍时，才认为最佳匹配点是合理的； 第二个参数表示匹配特征点时是否考虑方向。 接下来该函数还调用了UpdateLastFrame，这个函数主要是在纯定位模式下构建一个视觉里程计，对于建图模式作用不大，不再展开。
bool Tracking::TrackWithMotionModel() {
ORBmatcher matcher(0.9,true);
UpdateLastFrame();
然后就通过匀速运动模型估计当前帧的位姿。在下面的代码片段中，ORB-SLAM2 将成员变量 mVelocity 左乘到上一帧图像的位姿矩阵 mTcw 上，作为当前帧的位姿估计。
mCurrentFrame.SetPose(mVelocity * mLastFrame.mTcw);
估计了当前帧位姿之后，就可以将上一帧中看到的地图点投影到当前帧上，在当前帧的指定范围里搜索匹配的特征点。这点是通过ORB匹配器的SearchByProjection接口实现的， 这个接口有四个参数，前两个分别是当前帧和上一帧。第三个参数th是一个控制搜索半径的参数，最后一个参数用于判定是否为单目相机。由于RGB-D和双目相机都能够采集深度信息， 所以它们的投影计算方式略有不同。本文中将具体查看一下该函数的实现。
如下面代码片段的第7行所示，刚开始的时候 SearchByProjection 的搜索范围比较小。 如果找不到足够多的匹配特征点(超过 20 个匹配点)，将把搜索半径扩大两倍再进行搜索。如果匹配点的数量仍然不够，就认为匀速运动模型跟丢了，如第12，13行所示。
fill(mCurrentFrame.mvpMapPoints.begin(), mCurrentFrame.mvpMapPoints.end(), static_cast<MapPoint*>(NULL));
int th = (mSensor != System::STEREO) ? 15 : 7;
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);
}
if (nmatches < 20)
return false;
当我们找到了足够多的匹配点，就通过优化器根据匹配的特征点优化相机位姿。ORB-SLAM使用g2o进行优化， 我们将专门写篇文章详细介绍这个库。 本文中我们将具体分析函数 PoseOptimization 的实现过程。
Optimizer::PoseOptimization(&mCurrentFrame);
按照文章中的说法， 优化了位姿之后，基于匀速运动模型的跟踪定位就应该结束了。 但该函数并没有退出，它还进一步的检查了当前帧看到的各个地图点，抛弃了那些野点(outlier)。 它定义了一个局部整型nmatchesMap用于记录当前帧实际看到的地图点数量，并在一个for循环中遍历所有特征点。当前帧对象的容器mvpMapPoints和mvbOutlier与特征点是一一对应的， mvpMapPoints记录了各个特征点所对应的地图点指针，如果没有对应地图点则为NULL。mvbOutlier记录了各个特征点是否为野点(在优化的时候会更新这个状态)，若是野点则直接抛弃之。
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++;
} }
}
在最后退出的时候，再看一眼还剩下多少匹配的地图点，是否不少于10个，如果太少就认为跟丢了。
// 省略 mbOnlyTracking
return nmatchesMap >= 10;
}
2. 函数 ORBmatcher::SearchByProjection
ORB-SLAM2重载了好几个 SearchByProjection 的函数，基本上都是将各种特征点和地图点根据当前相机的位姿投影到当前帧上，再进行搜索完成特征点匹配。 这几个函数的套路都是一样的，写了挺多重复的代码，我们只分析刚才提到的对上一帧和当前帧进行特征点匹配的 SearchByProjection。下面是该函数的代码片段， 它有四个参数，其中 CurrentFrame 和 LastFrame 分别是当前帧和上一帧，th 指定了搜索半径，bMono 则用于标识是否单目相机采集的图像。
int ORBmatcher::SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame, const float th, const bool bMono) {
在函数的一开始建构建了角度直方图，用于检查图像特征点角度的一致性的。如果图像发生了旋转，那么所有的特征点应当大致旋转相同的角度。 该直方图就是用来统计匹配点对的角度偏差量的频率。一个合理的匹配点对，它们的角度偏差量出现的频率应当很高。在后面分析如何统计直方图， 并筛选匹配点的时候，会看到 SearchByProjection 只保留角度偏差量出现频率最高的三个小区间对应的特征点。 这句话真别扭，改了半天了。。。
std::vector<int> rotHist[HISTO_LENGTH];
for(int i = 0; i < HISTO_LENGTH; i++)
rotHist[i].reserve(500);
const float factor = 1.0f / HISTO_LENGTH;
Rcw 和 tcw 分别是当前帧从世界到相机的旋转矩阵和平移向量。第8行中，先对旋转矩阵 Rcw 转置得到的是它的逆，Rwc，然后将之乘到 tcw 上并取负号得到的 twc 是相机到世界的平移向量， 即，当前相机光心的世界坐标。类似的，Rlw 和 tlw 分别是上一帧相机的旋转矩阵和平移向量，第11行中的 tlc 则是当前帧到上一帧的平移向量。
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;
下面的这两行代码用于深度相机和双目相机，判定相机的运动方向。CurrentFrame.mb 记录的是相机的基线。
const bool bForward = tlc.at<float>(2) > CurrentFrame.mb && !bMono;
const bool bBackward = -tlc.at<float>(2) > CurrentFrame.mb && !bMono;
基础的数据准备完毕之后，就遍历上一帧中成功定位的地图点。类 Frame 的成员容器 mvpMapPoints 中的元素与特征点一一对应，是一个类 MapPoint 的指针。如果一个特征点没有成功定位， 则 mvpMapPoints 的对应元素为 nullptr，将忽略之。mvbOutlier 也是一个与特征点一一对应的容器，如果为 true 则说明特征点已经被当做野点抛弃掉了，也忽略之。 在下面的第16, 17行中，我把源码中的判定条件取反了，不喜欢因为一堆大括号导致代码缩进太多。
for(int i = 0; i < LastFrame.N; i++) {
MapPoint* pMP = LastFrame.mvpMapPoints[i];
if (!pMP) continue;
if(LastFrame.mvbOutlier[i]) continue;
然后在下面的代码片段中，先获取地图点的世界坐标，再根据当前帧的位姿估计和相机内参将之投影到当前帧中，得到 uv 坐标。
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;
ORB-SLAM2 以 1.2 的比例在8级的图像金字塔上提取 FAST 角点。这样可以保证提取的特征点可以描述不同尺度的场景。 同一个物体距离越远，在图像中就越小，对应的场景尺度就越大。对应的特征点搜索区域就需要适当的调整。如下面的代码片段所示， 类 Frame 中有一个数组 mvScaleFactors 记录了各层金字塔的尺度缩放系数，乘上输入的阈值 th，确定了实际的搜索半径。
int nLastOctave = LastFrame.mvKeys[i].octave;
float radius = th*CurrentFrame.mvScaleFactors[nLastOctave];
接着在当前帧中通过成员函数 GetFeaturesInArea ，获取指定像素中心和半径的特征点，将索引值记录在局部的容器 vIndices2 中。 对于单目相机，SearchByProjection 直接在下面的代码片段的 42 行中，在上下两级图像金字塔之间搜索。 下面的37到40行是在处理双目或者深度相机的图像，此时可以根据相机的位置估计大致推测出相机是前进了还是后退了。 如果相机前进了，物体就会变大，就向下层具有高分辨率的图像金字塔中搜索；若后退了，物体变小，向金字塔上层搜索。
vector<size_t> vIndices2;
if(bForward)
vIndices2 = CurrentFrame.GetFeaturesInArea(u,v, radius, nLastOctave);
else if(bBackward)
vIndices2 = CurrentFrame.GetFeaturesInArea(u,v, radius, 0, nLastOctave);
else
vIndices2 = CurrentFrame.GetFeaturesInArea(u,v, radius, nLastOctave-1, nLastOctave+1);
if(vIndices2.empty())
continue;
然后，遍历 vIndices2 中记录的当前帧特征点，通过计算描述子的距离，评价特征点的匹配度，选取最佳的匹配点，记录到 bestIdx2 中。bestDist 则记录最佳匹配距离。
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;
// 如果当前帧的特征点已经有对应的地图点，则不再参与匹配
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)
continue;
CurrentFrame.mvpMapPoints[bestIdx2]=pMP;
nmatches++;
类 ORBmatcher 的成员变量 mbCheckOrientation，是在构造函数时就已经通过参数初始化的变量。该变量控制是否需要检查图像特征点角度的一致性的。
如果需要则统计直方图。下面代码片段中的第77行，计算角度偏差量。然后，将之结果调整到区间\([0, 360°]\)内，并通过 round(rot * factor)
将偏差量缩放到 \({0, 1, \cdots, HISTO\_LENGTH - 1}\) 栅格中。第 84 行通过 push_back 接口将当前帧的特征点索引记录到对应的直方图栅格中。
if (mbCheckOrientation) {
float rot = LastFrame.mvKeysUn[i].angle-CurrentFrame.mvKeysUn[bestIdx2].angle;
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);
}
当遍历完上一帧中所有的特征点，如果有需要还会进一步的根据角度偏差量直方图对匹配特征点进行筛选。 下面的代码片段中，先在第89行中统计直方图中出现频率最高的三个栅格，然后将其它栅格中记录的特征点都通过将 mvpMapPoints 对应项都置为 null 的方式剔除掉。 最后一行中那么多花括号真的非常讨厌。。。
}
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(NULL);
nmatches--;
} } } } }
3. 函数 Optimizer::PoseOptimization
当函数 SearchByProjection 找到了足够多的匹配点，函数 TrackWithMotionModel 就会调用 Optimizer::PoseOptimization 根据匹配特征点构建优化器修正相机位姿。 下面是该函数的代码片段，它只有一个 Frame 类型的指针 pFrame。 该函数根据 pFrame 中已经成功定位的地图点 mvpMapPoints，通过相机的位姿估计，构建3D-2D的投影关系，最小化投影误差函数，修正相机位姿估计。
int Optimizer::PoseOptimization(Frame *pFrame) {
首先构造了 g2o 的优化器和求解器对象。下面代码中第3行的 BlockSolver_6_3 的 6 是指待优化的相机位姿有6个自由度，3 是指特征点的空间坐标是三维的。
g2o 是一个通用的图优化器，主要还是用于 SLAM 或者求解 BA(Bundle Adjustment) 问题。
g2o::SparseOptimizer optimizer;
g2o::BlockSolver_6_3::LinearSolverType * linearSolver;
linearSolver = new g2o::LinearSolverDense<g2o::BlockSolver_6_3::PoseMatrixType>();
g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
optimizer.setAlgorithm(solver);
然后，为估计相机位姿构建了一个 vSE3 的节点。在 g2o 中，把系统的状态量描述成一个个节点(Vertex)，状态量之间的约束抽象成节点之间的边(Edge)， 进而把优化问题描述成一个图。系统的状态量中，有变量和常量之分。其中变量就是优化问题需要求解的，比如这里的相机位姿估计。 在下面的第11行中，将节点 vSE3 设置成变量。这里的 SE3 是指描述相机位姿的那个\(4 \times 4\)的齐次矩阵，所有描述位姿的齐次矩阵构成一个群记作 SE(3)。
g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
vSE3->setEstimate(Converter::toSE3Quat(pFrame->mTcw));
vSE3->setId(0);
vSE3->setFixed(false);
optimizer.addVertex(vSE3);
在 PoseOptimization 中，优化问题构成的图只有一个节点，就是相机的位姿。边都是从该节点出发指向该节点的，描述的各个特征点从3D空间中投影到图像上的误差。 下面是针对单目相机的边定义和构造。我们暂时省去双目和深度相机的部分， 放到针对双目和深度相机的调整(TRACKING)中介绍。
在下面的代码片段中，容器 vpEdgesMono 用于保存各个根据地图点构建的边的对象。vnIndexEdgeMono 用于记录各个边的索引。 变量 nInitialCorrespondences 记录参与优化的有效边数量。这里的优化器采用 Huber 核函数来提供鲁棒的回归估计，降低异常点的负面作用。 最后定义的变量 deltaMono 是 Huber 核函数的一个参数。
const int N = pFrame->N;
vector<g2o::EdgeSE3ProjectXYZOnlyPose*> vpEdgesMono;
vector<size_t> vnIndexEdgeMono;
vpEdgesMono.reserve(N);
vnIndexEdgeMono.reserve(N);
int nInitialCorrespondences=0;
const float deltaMono = sqrt(5.991); // 看样子是自由度为2，显著性水平为0.05的卡方阈值。
然后，遍历帧 pFrame 中所有的地图点，构造边对象。需要注意第22行中对互斥量 MapPoint::mGlobalMutex 加锁的动作， 由于地图点坐标有可能会被 LOCAL MAPPING, LOOP CLOSING 两个线程修改，为了防止资源竞争，这里对类 MapPoint 的静态成员变量 mGlobalMutex 加锁， 以防止在构建边的过程中对任何地图点进行修改。
{
unique_lock<mutex> lock(MapPoint::mGlobalMutex);
for (int i = 0; i < N; i++) {
MapPoint* pMP = pFrame->mvpMapPoints[i];
if (!pMP)
continue;
在下面的35到39行中，构建了 g2o::EdgeSE3ProjectXYZOnlyPose 类型的边，该类型内置了3D到2D的投影误差函数。通过 setVertex 描述边连接的两个节点对象， 在36行中指定的两个节点都是刚刚建立的 vSE3。该边以特征点的uv像素坐标作为观测值调用接口 setMeasurement。 接口 setInformation 可以给边一个类似于权重的东西，称为信息矩阵，在高斯分布下可以认为是协方差矩阵的逆。 该值越大，说明不确定性越小，在优化时的作用就越大。
// 省略双目相机相关的代码
nInitialCorrespondences++;
pFrame->mvbOutlier[i] = false;
Eigen::Matrix<double,2,1> obs;
const cv::KeyPoint &kpUn = pFrame->mvKeysUn[i];
obs << kpUn.pt.x, kpUn.pt.y;
g2o::EdgeSE3ProjectXYZOnlyPose* e = new g2o::EdgeSE3ProjectXYZOnlyPose();
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));
e->setMeasurement(obs);
const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave];
e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);
g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
e->setRobustKernel(rk);
rk->setDelta(deltaMono);
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);
optimizer.addEdge(e);
vpEdgesMono.push_back(e);
vnIndexEdgeMono.push_back(i);
}
}
接下来，进行4轮迭代优化，每轮优化都进行10次迭代。下面的数组 chi2Mono 和 its 都只有4个元素，对应着每轮迭代优化。 chi2Mono 是一个根据卡方统计量设定的阈值，its 表示每轮优化的迭代次数。在每轮优化之后，都会对各边进行依次过滤，将那些误差大于阈值 chi2Mono 的边所对应的点标记为野点， 在下一轮优化中将不参与迭代。
const float chi2Mono[4]={5.991,5.991,5.991,5.991};
const int its[4]={10,10,10,10};
int nBad=0;
for (size_t it = 0; it < 4; it++) {
如下面的代码片段所示，每次通过接口 optimize 优化之前，都会先通过 setEstimate 设置迭代初值为当前帧的位姿估计。接口 initializeOptimization 有一个为 0 的参数， 表示只使用 level 为 0 的边进行优化，初次构造边的时候，level 的默认值就是 0。初始化完成之后，就通过 optimize 接口完成 10 次迭代。
vSE3->setEstimate(Converter::toSE3Quat(pFrame->mTcw));
optimizer.initializeOptimization(0);
optimizer.optimize(its[it]);
完成一轮优化之后，就需要根据投影误差对特征点进行筛选，把那些超出阈值的点称为外点抛弃掉。下面的代码片段中，遍历所有的边， 根据之前记录在 vnIndexEdgeMono 中索引遍历地图点。
nBad=0;
for(size_t i=0, iend=vpEdgesMono.size(); i < iend; i++) {
g2o::EdgeSE3ProjectXYZOnlyPose* e = vpEdgesMono[i];
const size_t idx = vnIndexEdgeMono[i];
如果对应的地图点被标记为外点，即，mvbOutlier[idx] 为 true。说明在上一轮优化过程中，该点因为误差过大没有参与优化。需要显式的计算一下误差。
if (pFrame->mvbOutlier[idx])
e->computeError();
然后，根据边的接口获取投影误差 chi2。如果超出了阈值，则将之抛弃掉。如下面的73,74行所示，在帧的成员变量 mvbOutlier 中将对应点记为外点， 将边的 level 设置为 1，在下一轮中将不参与优化。如果在阈值之内，就在 mvbOutlier 中将之记为内点，并将边的 level 设置为 0，参与下一轮的优化。
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);
}
}
下面这两句是说，只在前三轮优化的时候使用鲁棒核函数，这里用的是 Huber 核，最后一轮优化不再使用鲁棒核函数。
if (it == 2)
e->setRobustKernel(0);
}
// 省略双目相关的语句
}
最后，完成4轮优化之后，从 g2o 的节点中恢复出相机位姿，更新到 pFrame 中，并返回内点的数量。 上述的4轮优化，每次都用 pFrame->mTcw 设置一次初值，但是在优化之后并没有更新 pFrame 的位姿，只在最后更新了一次。 感觉这4轮优化，除了剔除一些野点之外，实际只优化了一轮。
g2o::VertexSE3Expmap* vSE3_recov = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(0));
g2o::SE3Quat SE3quat_recov = vSE3_recov->estimate();
cv::Mat pose = Converter::toCvMat(SE3quat_recov);
pFrame->SetPose(pose);
return nInitialCorrespondences-nBad;
}
4. 完
本文中，我们介绍了匀速运动模型，并详细分析了根据该模型估计相机位姿的过程。 其整体过程，先根据匀速运动模型，估计一个相机位姿，再通过投影关系搜索匹配特征点。 最后使用 g2o 构建一个以相机位姿为优化变量，地图点到像素投影误差为约束的图，优化相机位姿估计，进一步筛选误匹配的地图点。