(01)ORB-SLAM2源码无死角解析-(64) BA优化(g2o)→闭环线程:Optimizer::OptimizeSim3→Sim3变换优化

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

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

一、前言

上一篇博客对建图线程:Optimizer::LocalBundleAdjustment→局部建图位姿与地图点优化进行了详细的讲解。那么接下来要讲解的，就是闭环线程中的 Optimizer::OptimizeSim3→Sim3变换优化。该函数实现于 src/Optimizer.cc 文件中，被 src/LoopClosing.cc 文件中的 LoopClosing::ComputeSim3() 调用。如果当前关键帧，与某一候选关键帧匹配时,则会计算两帧之间的 Sim3 变换 gScm(候选关键帧到当前帧的Sim3变换)，如果感觉不太熟悉的朋友，可以回顾一下这篇博客：
(01)ORB-SLAM2源码无死角解析-(55) 闭环线程→计算Sim3:总体流程讲解ComputeSim3()

获得候选关键帧到当前帧的Sim3变换gScm之后，然后再利用两帧之间的匹配关系，对gScm进行优化，首先看下图：
在这里插入图片描述
KF1代表当前关键帧，KF2代表匹配成功的候选关键帧。g2oS12 表示 KF2→ KF1 的Sim变换(g2oS12 KF1→ KF2 的Sim变换)。大家需要注意，闭环线程中调用 LoopClosing::ComputeSim3() 时，还未进行地图点融合。也就是说KF1的关键点KP1与KF1的关键点KP2匹配，但是他们对应的地图点未必相同。

简单看一下 Optimizer::OptimizeSim3() 中相关参数的介绍：

/*** @brief 形成闭环时固定（不优化）地图点进行Sim3位姿优化* 1. Vertex:*     - g2o::VertexSim3Expmap()，两个关键帧的位姿*     - g2o::VertexSBAPointXYZ()，两个关键帧共有的MapPoints* 2. Edge:*     - g2o::EdgeSim3ProjectXYZ()，BaseBinaryEdge*         + Vertex：关键帧的Sim3，MapPoint的Pw*         + measurement：MapPoint在关键帧中的二维位置(u,v)*         + InfoMatrix: invSigma2(与特征点所在的尺度有关)*     - g2o::EdgeInverseSim3ProjectXYZ()，BaseBinaryEdge*         + Vertex：关键帧的Sim3，MapPoint的Pw*         + measurement：MapPoint在关键帧中的二维位置(u,v)*         + InfoMatrix: invSigma2(与特征点所在的尺度有关)* * @param[in] pKF1              当前帧* @param[in] pKF2              闭环候选帧* @param[in] vpMatches1        两个关键帧之间的匹配关系* @param[in] g2oS12            两个关键帧间的Sim3变换，方向是从2到1       * @param[in] th2               卡方检验是否为误差边用到的阈值* @param[in] bFixScale         是否优化尺度，单目进行尺度优化，双目/RGB-D不进行尺度优化* @return int                  优化之后匹配点中内点的个数*/

$核心：\color{red} 核心：$ 若 KP1 与 KP2 匹配，那么显然最理想的Sm3变换为: ①KP2→g2oS12=KP1；②KP1→g2oS21=KP2；

二、顶点与边

顶点

Sim优化中，涉及到顶点类类型为 VertexSim3Expmap，VertexSBAPointXYZ。其在前面博客中已经进行了详细介绍，这里就不再重复讲解了。

边

Sim优化中，主要的边为 EdgeSim3ProjectXYZ，与 EdgeInverseSim3ProjectXYZ。先来看看 EdgeInverseSim3ProjectXYZ，其与前面的 EdgeSE3ProjectXYZ 十分相像，仅仅是把欧式变换SE3更替成了Sim变换。其主要变化为其 virtual void computeError() 函数，实现于Thirdparty\g2o\g2o\types\types_seven_dof_expmap.h 文件中，如下：

    void computeError(){const VertexSim3Expmap* v1 = static_cast<const VertexSim3Expmap*>(_vertices[1]);const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);Vector2d obs(_measurement);_error = obs-v1->cam_map1(project(v1->estimate().map(v2->estimate())));}

其中map函数如下所示：

     Vector3d map (const Vector3d& xyz) const {return s*(r*xyz) + t;}

可以看到就是把欧式变换替换成了相似变换。其对于virtual void linearizeOplus() 函数，与 EdgeSE3ProjectXYZ 一致。

对于 EdgeInverseSim3ProjectXYZ ，就是 EdgeSE3ProjectXYZ 的逆操作。若 EdgeSE3ProjectXYZ 是把 KP2 对应的地图点投影到 KF1上，与 KP1计算误差，则 EdgeInverseSim3ProjectXYZ 是把 KP1 对应的地图点投影到 KF2 上，与 KP2 计算误差。具体在后面进行分析。

三、边缘化Marginalized

上一篇博客提到简单的说G2O 中对路标点设置边缘化(Point->setMarginalized(true))是为了在计算求解过程中，先消去路标点变量，实现先求解相机位姿，然后再利用求解出来的相机位姿，反过来计算路标点的过程，目的是为了加速求解，并非真的将路标点给边缘化掉。

那么在源码中是如何体现出来的呢？一般在初始化g2o优化器，添加顶点与边之后，都会执行类似 optimizer.optimize(5) 这样一句代码。这里使用的优化器为 g2o::SparseOptimizer optimizer，所以进入到 Thirdparty\g2o\g2o\core\sparse_optimizer.cpp 文件中，可以找到 SparseOptimizer::optimize() 函数。

其可以看到一句代码 ok = _algorithm->init(online)，这里的 _algorithm 实际就是创建求解器时设置的 g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr)。另外_algorithm->init(online)代码实现如下：

  bool OptimizationAlgorithmWithHessian::init(bool online){assert(_optimizer && "_optimizer not set");assert(_solver && "Solver not set");_solver->setWriteDebug(_writeDebug->value());bool useSchur=false;for (OptimizableGraph::VertexContainer::const_iterator it=_optimizer->activeVertices().begin(); it!=_optimizer->activeVertices().end(); ++it) {OptimizableGraph::Vertex* v= *it;if (v->marginalized()){useSchur=true;break;}}if (useSchur){if  (_solver->supportsSchur())_solver->setSchur(true);} else {if  (_solver->supportsSchur())_solver->setSchur(false);}bool initState = _solver->init(_optimizer, online);return initState;}

从上面可以看到，其调用了 v->marginalized() 函数，获得该顶点是否需要边缘化，如果进行边缘化，则会对 _solver 进行相关设置。这样对于边缘化的引用，算是有了一个初步的了解。至于后面的 _solver->setSchur(true) 与 _solver->setSchur(false) 有什么区别，有兴趣的朋友可以深入的了解一下代码。

四、源码逻辑

代码为 src/Optimizer.cc 中的 Optimizer::OptimizeSim3() 函数。

$(01)：\color{blue} (01)：$ 初始化g2o优化器，使用L-M迭代；获得当前关键帧pKF1与闭环候选帧pKF2相机内参与位姿。设置待优化的Sim3位姿作为顶点。根据传感器类型决定是否固定尺度，如果为双目或者深度相机则固定尺度。

$(02)：\color{blue} (02)：$ 对当前帧pKF1的所有关键点进行遍历，如果其未与闭环候选帧pKF2的关键点匹配，则跳出循环。否则获得各自关键点对应的地图点pMP1，pMP2。如果地图点pMP1，pMP2未同时存在，或有任意一个地图点未坏点，则continue。

$(03)：\color{blue} (03)：$ 为地图点pMP1创建一个VertexSBAPointXYZ类型顶点vPoint1，并且设置vPoint1->setFixed(true)，也就是地图点不作优化。同理，为地图点vPoint2也创建一个VertexSBAPointXYZ类型顶点vPoint2，并且设置vPoint2->setFixed(true)，地图点不作优化。

$(04)：\color{blue} (04)：$ ①添加边x1 = S12 $*$ X2，设置了两个顶点，分别为vPoint2与vSim3，该边计算的误差为→地图点pMP2通过Sim变换正向投影到pKF1与pKF1的匹配点计算误差。②添加边x2 = S21 $*$ X1，设置了两个顶点，分别为vPoint1与vSim3，该边计算的误差为→地图点pMP1通过Sim变换的逆进行反向投影到pKF2与pKF2的匹配点计算误差。

$(04)：\color{blue} (04)：$ g2o开始优化，先迭代5次。根据卡方检验删除向或反向投影任意一个超过误差阈值就删掉该边。如果有误差较大的边被剔除那么说明回环质量并不是非常好,还要多迭代几次;反之就少迭代几次，如果经过上面的剔除后剩下的匹配关系已经非常少了,那么就放弃优化。内点数直接设置为0。

$(05)：\color{blue} (05)：$ 再次g2o优化剔除后剩下的边，统计第二次优化之后,这些匹配点中是内点的个数，
用优化后的结果来更新Sim3位姿。

五、源码注释

/*** @brief 形成闭环时固定（不优化）地图点进行Sim3位姿优化* 1. Vertex:*     - g2o::VertexSim3Expmap()，两个关键帧的位姿*     - g2o::VertexSBAPointXYZ()，两个关键帧共有的MapPoints* 2. Edge:*     - g2o::EdgeSim3ProjectXYZ()，BaseBinaryEdge*         + Vertex：关键帧的Sim3，MapPoint的Pw*         + measurement：MapPoint在关键帧中的二维位置(u,v)*         + InfoMatrix: invSigma2(与特征点所在的尺度有关)*     - g2o::EdgeInverseSim3ProjectXYZ()，BaseBinaryEdge*         + Vertex：关键帧的Sim3，MapPoint的Pw*         + measurement：MapPoint在关键帧中的二维位置(u,v)*         + InfoMatrix: invSigma2(与特征点所在的尺度有关)* * @param[in] pKF1              当前帧* @param[in] pKF2              闭环候选帧* @param[in] vpMatches1        两个关键帧之间的匹配关系* @param[in] g2oS12            两个关键帧间的Sim3变换，方向是从2到1       * @param[in] th2               卡方检验是否为误差边用到的阈值* @param[in] bFixScale         是否优化尺度，单目进行尺度优化，双目/RGB-D不进行尺度优化* @return int                  优化之后匹配点中内点的个数*/
int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &vpMatches1, g2o::Sim3 &g2oS12, const float th2, const bool bFixScale)
{// Step 1：初始化g2o优化器// 先构造求解器g2o::SparseOptimizer optimizer;// 构造线性方程求解器，Hx = -b的求解器g2o::BlockSolverX::LinearSolverType * linearSolver;// 使用dense的求解器，（常见非dense求解器有cholmod线性求解器和shur补线性求解器）linearSolver = new g2o::LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>();g2o::BlockSolverX * solver_ptr = new g2o::BlockSolverX(linearSolver);// 使用L-M迭代g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);optimizer.setAlgorithm(solver);// Calibration// 内参矩阵const cv::Mat &K1 = pKF1->mK;const cv::Mat &K2 = pKF2->mK;// Camera posesconst cv::Mat R1w = pKF1->GetRotation();const cv::Mat t1w = pKF1->GetTranslation();const cv::Mat R2w = pKF2->GetRotation();const cv::Mat t2w = pKF2->GetTranslation();// Set Sim3 vertex// Step 2: 设置待优化的Sim3位姿作为顶点g2o::VertexSim3Expmap * vSim3 = new g2o::VertexSim3Expmap();    // 根据传感器类型决定是否固定尺度vSim3->_fix_scale=bFixScale;vSim3->setEstimate(g2oS12);vSim3->setId(0);// Sim3 需要优化vSim3->setFixed(false);                             // 因为要优化Sim3顶点，所以设置为falsevSim3->_principle_point1[0] = K1.at<float>(0,2);    // 光心横坐标cxvSim3->_principle_point1[1] = K1.at<float>(1,2);    // 光心纵坐标cyvSim3->_focal_length1[0] = K1.at<float>(0,0);       // 焦距 fxvSim3->_focal_length1[1] = K1.at<float>(1,1);       // 焦距 fyvSim3->_principle_point2[0] = K2.at<float>(0,2);vSim3->_principle_point2[1] = K2.at<float>(1,2);vSim3->_focal_length2[0] = K2.at<float>(0,0);vSim3->_focal_length2[1] = K2.at<float>(1,1);optimizer.addVertex(vSim3);// Set MapPoint vertices// Step 3: 设置匹配的地图点作为顶点const int N = vpMatches1.size();// 获取pKF1的地图点const vector<MapPoint*> vpMapPoints1 = pKF1->GetMapPointMatches();vector<g2o::EdgeSim3ProjectXYZ*> vpEdges12;         //pKF2对应的地图点到pKF1的投影边vector<g2o::EdgeInverseSim3ProjectXYZ*> vpEdges21;  //pKF1对应的地图点到pKF2的投影边vector<size_t> vnIndexEdge;                         //边的索引vnIndexEdge.reserve(2*N);vpEdges12.reserve(2*N);vpEdges21.reserve(2*N);// 核函数的阈值const float deltaHuber = sqrt(th2);int nCorrespondences = 0;// 遍历每对匹配点for(int i=0; i<N; i++){if(!vpMatches1[i])continue;// pMP1和pMP2是匹配的地图点MapPoint* pMP1 = vpMapPoints1[i];MapPoint* pMP2 = vpMatches1[i];// 保证顶点的id能够错开const int id1 = 2*i+1;const int id2 = 2*(i+1);// i2 是 pMP2 在pKF2中对应的索引const int i2 = pMP2->GetIndexInKeyFrame(pKF2);if(pMP1 && pMP2){if(!pMP1->isBad() && !pMP2->isBad() && i2>=0){// 如果这对匹配点都靠谱，并且对应的2D特征点也都存在的话，添加PointXYZ顶点g2o::VertexSBAPointXYZ* vPoint1 = new g2o::VertexSBAPointXYZ();// 地图点转换到各自相机坐标系下的三维点cv::Mat P3D1w = pMP1->GetWorldPos();cv::Mat P3D1c = R1w*P3D1w + t1w;vPoint1->setEstimate(Converter::toVector3d(P3D1c));vPoint1->setId(id1);// 地图点不优化vPoint1->setFixed(true);optimizer.addVertex(vPoint1);g2o::VertexSBAPointXYZ* vPoint2 = new g2o::VertexSBAPointXYZ();cv::Mat P3D2w = pMP2->GetWorldPos();cv::Mat P3D2c = R2w*P3D2w + t2w;vPoint2->setEstimate(Converter::toVector3d(P3D2c));vPoint2->setId(id2);vPoint2->setFixed(true);optimizer.addVertex(vPoint2);}elsecontinue;}elsecontinue;// 对匹配关系进行计数nCorrespondences++;// Step 4: 添加边（地图点投影到特征点）// Set edge x1 = S12*X2// 地图点pMP1对应的观测特征点Eigen::Matrix<double,2,1> obs1;const cv::KeyPoint &kpUn1 = pKF1->mvKeysUn[i];obs1 << kpUn1.pt.x, kpUn1.pt.y;// Step 4.1 闭环候选帧地图点投影到当前关键帧的边 -- 正向投影g2o::EdgeSim3ProjectXYZ* e12 = new g2o::EdgeSim3ProjectXYZ();// vertex(id2)对应的是pKF2 VertexSBAPointXYZ 类型的三维点e12->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id2)));// ? 为什么这里添加的节点的id为0？// 回答：因为vertex(0)对应的是 VertexSim3Expmap 类型的待优化Sim3，其id 为 0e12->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));e12->setMeasurement(obs1);// 信息矩阵和这个特征点的可靠程度（在图像金字塔中的图层）有关const float &invSigmaSquare1 = pKF1->mvInvLevelSigma2[kpUn1.octave];e12->setInformation(Eigen::Matrix2d::Identity()*invSigmaSquare1);// 使用鲁棒核函数g2o::RobustKernelHuber* rk1 = new g2o::RobustKernelHuber;e12->setRobustKernel(rk1);rk1->setDelta(deltaHuber);optimizer.addEdge(e12);// Set edge x2 = S21*X1// Step 4.2 当前关键帧地图点投影到闭环候选帧的边 -- 反向投影// 地图点pMP2对应的观测特征点Eigen::Matrix<double,2,1> obs2;const cv::KeyPoint &kpUn2 = pKF2->mvKeysUn[i2];obs2 << kpUn2.pt.x, kpUn2.pt.y;g2o::EdgeInverseSim3ProjectXYZ* e21 = new g2o::EdgeInverseSim3ProjectXYZ();// vertex(id1)对应的是pKF1 VertexSBAPointXYZ 类型的三维点，内部误差公式也不同e21->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id1)));e21->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));e21->setMeasurement(obs2);float invSigmaSquare2 = pKF2->mvInvLevelSigma2[kpUn2.octave];e21->setInformation(Eigen::Matrix2d::Identity()*invSigmaSquare2);g2o::RobustKernelHuber* rk2 = new g2o::RobustKernelHuber;e21->setRobustKernel(rk2);rk2->setDelta(deltaHuber);optimizer.addEdge(e21);vpEdges12.push_back(e12);vpEdges21.push_back(e21);vnIndexEdge.push_back(i);}// Optimize!// Step 5：g2o开始优化，先迭代5次optimizer.initializeOptimization();optimizer.optimize(5);// Step 6：用卡方检验剔除误差大的边// Check inliersint nBad=0;for(size_t i=0; i<vpEdges12.size();i++){g2o::EdgeSim3ProjectXYZ* e12 = vpEdges12[i];g2o::EdgeInverseSim3ProjectXYZ* e21 = vpEdges21[i];if(!e12 || !e21)continue;if(e12->chi2()>th2 || e21->chi2()>th2){// 正向或反向投影任意一个超过误差阈值就删掉该边size_t idx = vnIndexEdge[i];vpMatches1[idx]=static_cast<MapPoint*>(NULL);optimizer.removeEdge(e12);optimizer.removeEdge(e21);vpEdges12[i]=static_cast<g2o::EdgeSim3ProjectXYZ*>(NULL);vpEdges21[i]=static_cast<g2o::EdgeInverseSim3ProjectXYZ*>(NULL);// 累计删掉的边 数目nBad++;}}// 如果有误差较大的边被剔除那么说明回环质量并不是非常好,还要多迭代几次;反之就少迭代几次int nMoreIterations;if(nBad>0)nMoreIterations=10;elsenMoreIterations=5;// 如果经过上面的剔除后剩下的匹配关系已经非常少了,那么就放弃优化。内点数直接设置为0if(nCorrespondences-nBad<10)return 0;     // Optimize again only with inliers// Step 7：再次g2o优化剔除后剩下的边optimizer.initializeOptimization();optimizer.optimize(nMoreIterations);// 统计第二次优化之后,这些匹配点中是内点的个数int nIn = 0;for(size_t i=0; i<vpEdges12.size();i++){g2o::EdgeSim3ProjectXYZ* e12 = vpEdges12[i];g2o::EdgeInverseSim3ProjectXYZ* e21 = vpEdges21[i];if(!e12 || !e21)continue;if(e12->chi2()>th2 || e21->chi2()>th2){size_t idx = vnIndexEdge[i];vpMatches1[idx]=static_cast<MapPoint*>(NULL);}elsenIn++;}// Recover optimized Sim3// Step 8：用优化后的结果来更新Sim3位姿。g2o::VertexSim3Expmap* vSim3_recov = static_cast<g2o::VertexSim3Expmap*>(optimizer.vertex(0));g2oS12= vSim3_recov->estimate();return nIn;
}