(01)ORB-SLAM2源码无死角解析-(62) BA优化(g2o)→追踪线程:Optimizer::PoseOptimization→仅位姿优化

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解的(01)ORB-SLAM2源码无死角解析链接如下(本文内容来自计算机视觉life ORB-SLAM2 课程课件):
(01)ORB-SLAM2源码无死角解析-(00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/123092196

文末正下方中心提供了本人联系方式，点击本人照片即可显示WX→官方认证{\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证}文末正下方中心提供了本人联系方式，点击本人照片即可显示WX→官方认证

一、前言

在前面的博客中，有好些重要的函数到目前为止都还没有进行讲解如：

Optimizer::PoseOptimization();   //追踪线程仅位姿优化
Optimizer::LocalBundleAdjustment() //局部建图位姿与地图点优化
Optimizer::OptimizeSim3() //闭环线程Sim3变换优化
Optimizer::OptimizeEssentialGraph() //闭环线程本质图优化
Optimizer::GlobalBundleAdjustemnt() //闭环线程全局优化

针对于这些函数，接下来都会进行具体的分析，因为这里涉及到很多图优化的知识，如果不是很了解图优化朋友请先观看一下几篇博客：

史上最简SLAM零基础解读(10.1) - g2o(图优化)→简介环境搭建(slam十四讲第二版为例)
史上最简SLAM零基础解读(10.2) - g2o(图优化)→顶点 (Vertex)编程细节
史上最简SLAM零基础解读(10.3) - g2o(图优化)→边(Edge)编程细节
史上最简SLAM零基础解读(10.4) - g2o(图优化)→示例代码讲解(slam十四讲第二版为例)

在对图优化有了基本的了解之后的，下面就开始吧，首先要讲解的是 Optimizer::PoseOptimization() 函数，其主要在追踪线程中被调用，如 src/tracking.cc 文件中的 Tracking::TrackWithMotionModel()、Tracking::TrackReferenceKeyFrame()、Tracking::Relocalization()、Tracking::TrackLocalMap() 中都使用了该函数。通过上面推荐的博客我们知道 g2o 的主体流程如下：

1、顶点和边的类型定义；
2、构建图优化实例，配置求解器；
3、添加点和边，构建求解图；
4、执行优化。

那么下面就是对 src/Optimizer.cc 中的 int Optimizer::PoseOptimization(Frame *pFrame) 函数，主要核心要点如下：

/** @brief Pose Only Optimization* * 3D-2D 最小化重投影误差 e = (u,v) - project(Tcw*Pw)* 只优化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数量*/

一、构建图优化实例

这里的流程还是比较固定的。主要确定二个东西：①顶点与路标点(地图点)维度→6x3；②优化方式→莱文伯格-马夸特方法；就可以构建图优化实例了，如下：

    // Step 1：构造g2o优化器, BlockSolver_6_3表示：位姿 _PoseDim 为6维，路标点 _LandmarkDim 是3维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);

二、顶点与边定义

1、顶点

顶点还是比较好理解的，需要优化的位姿，即代码当前帧的 Tcw，就是顶点，有且仅有该位姿需要优化。

    // Set Frame vertex// Step 2：添加顶点：待优化当前帧的Tcwg2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();vSE3->setEstimate(Converter::toSE3Quat(pFrame->mTcw));// 设置idvSE3->setId(0);    // 要优化的变量，所以不能固定vSE3->setFixed(false);optimizer.addVertex(vSE3);

因为顶点是这里需要优化的量，所以执行vSE3->setFixed(false)，不能固定。宁外注意一下，这里使用 g2o::VertexSE3Expmap 类型的顶点李代数的相机位姿。，继承自 BaseVertex：

class  VertexSE3Expmap : public BaseVertex<6, SE3Quat> {...}

参数6 ：SE3Quat类型为六维，三维旋转，三维平移。
参数SE3Quat ：该类型旋转在前，平移在后，注意：类型内部使用的其实是四元数，不是李代数。

二、边

先对来对，边要复杂一些，在源码中使用了两种边，分别为EdgeSE3ProjectXYZOnlyPose(单目)，EdgeStereoSE3ProjectXYZOnlyPose(双目)。这里只对 EdgeSE3ProjectXYZOnlyPose 进行讲解，EdgeStereoSE3ProjectXYZOnlyPose进行类推即可。位于Thirdparty/g2o/g2o/types/types_six_dof_expmap.中定义，首先其继承关系如下：

class  EdgeSE3ProjectXYZOnlyPose: public  BaseUnaryEdge<2, Vector2d, VertexSE3Expmap>{......}

可以很明显看出继承于 BaseUnaryEdge，BaseUnaryEdge有三个模参数：
参数2：因为出来的误差是2维的，误差 = 关键点像素坐标(观测)- 地图点投影到关键点附近像素坐标(投影)。
Vector2d：表示误差值的类型。
VertexSE3Expmap：顶点类型为VertexSE3Expmap，也就是前面需要优化的顶点类型。

同时，通过前面关于 g2o(图优化) 的文章可以了解到，关于边的定义，只需要实现两个两个重要的函数即可，分别为:

virtual void computeError();
virtual void linearizeOplus();

先来看看 computeError()，其实呢，再史上最简SLAM零基础解读(10.3) - g2o(图优化)→边(Edge)编程细节中已经有比较详细的讲解。计算方式为：重投影误差=图像2d观测值 - 3d点经估计的相机位姿和相机内参K投影到图像平面的2d点值。也就是对应源码如下：

  void computeError()  {const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[0]);Vector2d obs(_measurement);_error = obs-cam_project(v1->estimate().map(Xw));}

其上的 Xw 表示地图点对应的世界坐标，v1是前面设置的顶点(当前帧位姿), 通过v1->estimate().map(Xw)→等价于_r*Xw+ _t。_r 和 _t 分别表示当前帧的旋转与平移。也就是说把地图点 Xw 变换到当前帧的相机坐标系下。然后再使用 cam_project() 函数投影到当前帧的像素坐标。最终与obs(地图点再当前帧对应特征点的像素坐标)作差，即获得误差。

那么再来看看 linearizeOplus() 函数，其实现于 Thirdparty/g2o/g2o/types/types_six_dof_expmap.cpp 文件中，代码如下：

  void EdgeSE3ProjectXYZOnlyPose:: linearizeOplus() {VertexSE3Expmap * vi = static_cast<VertexSE3Expmap *>(_vertices[0]);Vector3d xyz_trans = vi->estimate().map(Xw);double x = xyz_trans[0];double y = xyz_trans[1];double invz = 1.0/xyz_trans[2];double invz_2 = invz*invz;_jacobianOplusXi(0,0) =  x*y*invz_2 *fx;_jacobianOplusXi(0,1) = -(1+(x*x*invz_2)) *fx;_jacobianOplusXi(0,2) = y*invz *fx;_jacobianOplusXi(0,3) = -invz *fx;_jacobianOplusXi(0,4) = 0;_jacobianOplusXi(0,5) = x*invz_2 *fx;_jacobianOplusXi(1,0) = (1+y*y*invz_2) *fy;_jacobianOplusXi(1,1) = -x*y*invz_2 *fy;_jacobianOplusXi(1,2) = -x*invz *fy;_jacobianOplusXi(1,3) = 0;_jacobianOplusXi(1,4) = -invz *fy;_jacobianOplusXi(1,5) = y*invz_2 *fy;
}

这里就不进行推导了，有兴趣的朋友请，推荐大家阅读一下该博客：史上最简SLAM零基础解读(7) - Jacobian matrix(雅可比矩阵) → 理论分析与应用详解(Bundle Adjustment)。其有具体的推导过程，另外这里需要注意一个点，根据博客得到雅克比矩阵(45式)与源码是不对应的∂e∂Δξ=[fxZc0−fxXcZc−fxXcYcZc2fx+fxXc2Zc2−fxYcZc0fyZc−fyYcZc2−fy−fyYc2Zc2fyXcYcZc2fyXcZc](01)\color{green} \tag{01} \frac{\partial e}{\partial \Delta \xi} = \left[\begin{array}{cccccc} \frac{f_{x}}{Z_{c}} & 0 & -\frac{f_{x} X_{c}}{Z_{c}} & -\frac{f_{x} X_{c} Y_{c}}{Z_{c}^{2}} & f_{x}+\frac{f_{x} X_{c}^{2}}{Z_{c}^{2}} & -\frac{f_{x} Y_{c}}{Z_{c}} \\ \\ 0 & \frac{f_{y}}{Z_{c}} & -\frac{f_{y} Y_{c}}{Z_{c}^{2}} & -f_{y}-\frac{f_{y} Y_{c}^{2}}{Z_{c}^{2}} & \frac{f_{y} X_{c} Y_{c}}{Z_{c}^{2}} & \frac{f_{y} X_{c}}{Z_{c}} \end{array}\right]∂Δξ∂e=⎣⎡Zcfx00Zcfy−ZcfxXc−Zc2fyYc−Zc2fxXcYc−fy−Zc2fyYc2fx+Zc2fxXc2Zc2fyXcYc−ZcfxYcZcfyXc⎦⎤(01) 这是因为 se(3) 的定义方式是旋转在前，平移在后时，只要把这个矩阵的前三列与后三列对调即可。

三、整体逻辑讲解

了解了顶点与边的定义之后，我们就来看看 Optimizer::PoseOptimization(Frame *pFrame) 的整体流程是如何的：

准备阶段

(01)：\color{blue} (01)：(01)：构造g2o优化器, BlockSolver_6_3表示：位姿 _PoseDim 为6维，路标点 _LandmarkDim 是3维

(02)：\color{blue} (02)：(02)：添加顶点：待优化当前帧的Tcw

(03)：\color{blue} (03)：(03)：循环添加一元边，因为所有边都只和一个顶点连接。

(04)：\color{blue} (04)：(04)：循环遍历当前帧的所有有效地图点，然后分单目情况与双目情况进行处理，同时把地图点对应的特征点标记为内点, 对应代码 pFrame->mvbOutlier[i] = false;

(05)：\color{blue} (05)：(05)：如果为单目(双目省略讲解)，创建一元边EdgeSE3ProjectXYZOnlyPose实例。设置顶点(当前帧位姿)、设置观测点(当前帧与地图点对应的特征点)。

(06)：\color{blue} (06)：(06)：设置置性度(信息矩阵→与特征点所处金字塔层级相关)：现在来考虑另一种情况，比方说在一次优化中，对于某一次测量，我们有十足的把握，它非常的准确，所以优化时我们希望对于这次测量给予更高的权重。对应带代码 e->setInformation()

(07)：\color{blue} (07)：(07)：设置鲁棒核函数，g2o中提供了鲁棒核函数来抑制某些误差特别大的点，避免拉偏整个优化结果。(鲁棒核函数不是g2o独有的，这是非线性优化方法中的一种常用手段)

(08)：\color{blue} (08)：(08)：设置卡法阈值(用来判断内外点)、设置相机内参、地图点的空间位置。

(09)：\color{blue} (09)：(09)：设置好的所有一元边之后(一个特征点对应一个一元边)，则添加到 vpEdgesStereo.reserve(N) 与 vnIndexEdgeStereo.reserve(N) 之中。

优化阶段

(10)：\color{blue} (10)：(10)：共进行四次优化，其中每次优化迭代10次(optimizer.optimize(its[it]))，优化等级为0，也就是说不对外点进行优化，外点等级为1。

(11)：\color{blue} (11)：(11)：每次优化结束之后，开始遍历每一条误差边，如果这条误差边是来自于outlier，则计算误差。如果一元边误差较大，设置为外点，误差较小设置为内点。

(12)：\color{blue} (12)：(12)：获取优化后当前帧的位姿，返回内点数目。

代码注释

代码位于 src/Optimizer.cc 文件中：

/** @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数量*/
int Optimizer::PoseOptimization(Frame *pFrame)
{// 该优化函数主要用于Tracking线程中：运动跟踪、参考帧跟踪、地图跟踪、重定位// Step 1：构造g2o优化器, BlockSolver_6_3表示：位姿 _PoseDim 为6维，路标点 _LandmarkDim 是3维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);// 输入的帧中,有效的,参与优化过程的2D-3D点对int nInitialCorrespondences=0;// Set Frame vertex// Step 2：添加顶点：待优化当前帧的Tcwg2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();vSE3->setEstimate(Converter::toSE3Quat(pFrame->mTcw));// 设置idvSE3->setId(0);    // 要优化的变量，所以不能固定vSE3->setFixed(false);optimizer.addVertex(vSE3);// Set MapPoint verticesconst int N = pFrame->N;// for Monocularvector<g2o::EdgeSE3ProjectXYZOnlyPose*> vpEdgesMono;vector<size_t> vnIndexEdgeMono;vpEdgesMono.reserve(N);vnIndexEdgeMono.reserve(N);// for Stereovector<g2o::EdgeStereoSE3ProjectXYZOnlyPose*> vpEdgesStereo;vector<size_t> vnIndexEdgeStereo;vpEdgesStereo.reserve(N);vnIndexEdgeStereo.reserve(N);// 自由度为2的卡方分布，显著性水平为0.05，对应的临界阈值5.991const float deltaMono = sqrt(5.991);  // 自由度为3的卡方分布，显著性水平为0.05，对应的临界阈值7.815   const float deltaStereo = sqrt(7.815);     // Step 3：添加一元边{// 锁定地图点。由于需要使用地图点来构造顶点和边,因此不希望在构造的过程中部分地图点被改写造成不一致甚至是段错误unique_lock<mutex> lock(MapPoint::mGlobalMutex);// 遍历当前地图中的所有地图点for(int i=0; i<N; i++){MapPoint* pMP = pFrame->mvpMapPoints[i];// 如果这个地图点还存在没有被剔除掉if(pMP){// Monocular observation// 单目情况if(pFrame->mvuRight[i]<0){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);}else  // Stereo observation 双目{nInitialCorrespondences++;pFrame->mvbOutlier[i] = false;//SET EDGE// 观测多了一项右目的坐标Eigen::Matrix<double,3,1> obs;// 这里和单目不同const cv::KeyPoint &kpUn = pFrame->mvKeysUn[i];const float &kp_ur = pFrame->mvuRight[i];obs << kpUn.pt.x, kpUn.pt.y, kp_ur;// 这里和单目不同// 新建边，一元边，误差为观测特征点坐标减去投影点的坐标g2o::EdgeStereoSE3ProjectXYZOnlyPose* e = new g2o::EdgeStereoSE3ProjectXYZOnlyPose();// 这里和单目不同e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));e->setMeasurement(obs);// 置信程度主要是看左目特征点所在的图层const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave];Eigen::Matrix3d Info = Eigen::Matrix3d::Identity()*invSigma2;e->setInformation(Info);g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;e->setRobustKernel(rk);rk->setDelta(deltaStereo);e->fx = pFrame->fx;e->fy = pFrame->fy;e->cx = pFrame->cx;e->cy = pFrame->cy;e->bf = pFrame->mbf;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);vpEdgesStereo.push_back(e);vnIndexEdgeStereo.push_back(i);} // 根据单目和双目不同的相机输入执行不同的操作过程}}} // 离开临界区// 如果没有足够的匹配点,那么就只好放弃了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 4：开始优化，总共优化四次，每次优化迭代10次,每次优化后，将观测分为outlier和inlier，outlier不参与下次优化// 由于每次优化后是对所有的观测进行outlier和inlier判别，因此之前被判别为outlier有可能变成inlier，反之亦然// 基于卡方检验计算出的阈值（假设测量有一个像素的偏差）const float chi2Mono[4]={5.991,5.991,5.991,5.991};          // 单目const float chi2Stereo[4]={7.815,7.815,7.815, 7.815};       // 双目const int its[4]={10,10,10,10};// 四次迭代，每次迭代的次数// bad 的地图点个数int nBad=0;// 一共进行四次优化for(size_t it=0; it<4; it++){vSE3->setEstimate(Converter::toSE3Quat(pFrame->mTcw));// 其实就是初始化优化器,这里的参数0就算是不填写,默认也是0,也就是只对level为0的边进行优化optimizer.initializeOptimization(0);// 开始优化，优化10次optimizer.optimize(its[it]);nBad=0;// 优化结束,开始遍历参与优化的每一条误差边(单目)for(size_t i=0, iend=vpEdgesMono.size(); i<iend; i++){g2o::EdgeSE3ProjectXYZOnlyPose* e = vpEdgesMono[i];const size_t idx = vnIndexEdgeMono[i];// 如果这条误差边是来自于outlierif(pFrame->mvbOutlier[idx]){e->computeError(); }// 就是error*\Omega*error,表征了这个点的误差大小(考虑置信度以后)const float chi2 = e->chi2();if(chi2>chi2Mono[it]){                pFrame->mvbOutlier[idx]=true;e->setLevel(1);                 // 设置为outlier , level 1 对应为外点,上面的过程中我们设置其为不优化nBad++;}else{pFrame->mvbOutlier[idx]=false;e->setLevel(0);                 // 设置为inlier, level 0 对应为内点,上面的过程中我们就是要优化这些关系}if(it==2)e->setRobustKernel(0); // 除了前两次优化需要RobustKernel以外, 其余的优化都不需要 -- 因为重投影的误差已经有明显的下降了} // 对单目误差边的处理// 同样的原理遍历双目的误差边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();}const float chi2 = e->chi2();if(chi2>chi2Stereo[it]){pFrame->mvbOutlier[idx]=true;e->setLevel(1);nBad++;}else{                e->setLevel(0);pFrame->mvbOutlier[idx]=false;}if(it==2)e->setRobustKernel(0);} // 对双目误差边的处理if(optimizer.edges().size()<10)break;} // 一共要进行四次优化// Recover optimized pose and return number of inliers// Step 5 得到优化后的当前帧的位姿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;
}

四、前言

通过该篇博客，对Optimizer::PoseOptimization→仅位姿优化进行了详细讲解，相信对于g2o的使用大家都更加熟悉了。下面针对于其他优化，理解起来会简单很多，那么下篇博客见。

本文内容来自计算机视觉life ORB-SLAM2 课程课件