(01)ORB-SLAM2源码无死角解析-(65) BA优化(g2o)→闭环线程:Optimizer::OptimizeEssentialGraph→本质图优化

讲解关于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→官方认证

一、前言

该篇博客主要的讲解的 src/Optimizer.cc 文件中的 Optimizer::OptimizeEssentialGraph→本质图优化。该函数在闭环线程中调用顺序为：LoopClosing::Run()→ CorrectLoop()→ Optimizer::OptimizeEssentialGraph()。在进行讲解之前，先来回顾一下什么叫做本质图，可以简单的阅读一下：(01)ORB-SLAM2源码无死角解析-(27) 共视图、本质图、拓展图。可以了解到，本质图具备如下几个特征：
        (1):\color{blue}{(1):}(1): 扩展树连接关系
        (2):\color{blue}{(2):}(2): 形成闭环的连接关系，闭环后地图点变动后新增加的连接关系
        (3):\color{blue}{(3):}(3): 共视关系非常好（至少100个共视地图点）的连接关系
其相比于扩展树更稠密，共视图更稀疏。另外还要注意的就是本质图在CorrectLoop()被调用的时候，前面先执行SearchAndFuse(CorrectedSim3)，也就是说进行了地图点的融合。需要注意的是，如下图所示

其地图点的融合是利用Sim变换，将闭环相连关键帧组mvpLoopMapPoints 投影到当前关键帧组中，进行匹配，新增或替换当前关键帧组中KF的地图点。也就是说其地图点融合与更新是针对与上图绿圈与蓝圈中的关键帧。经过 SearchAndFuse(CorrectedSim3) 之后呢，大概有了下图中 b 的效果：
但是明显，我们希望的建图效果是如上图 c 所示。下面要讲解的 Optimizer::OptimizeEssentialGraph() 与 Optimizer::GlobalBundleAdjustemnt() 就是类似于实现 b 到 c 的过程。先简单看下Optimizer::OptimizeEssentialGraph() 的参数介绍：

/*** @brief 闭环检测后，EssentialGraph优化，仅优化所有关键帧位姿，不优化地图点** 1. Vertex:*     - g2o::VertexSim3Expmap，Essential graph中关键帧的位姿* 2. Edge:*     - g2o::EdgeSim3()，BaseBinaryEdge*         + Vertex：关键帧的Tcw，MapPoint的Pw*         + measurement：经过CorrectLoop函数步骤2，Sim3传播校正后的位姿*         + InfoMatrix: 单位矩阵     ** @param pMap               全局地图* @param pLoopKF            闭环匹配上的关键帧* @param pCurKF             当前关键帧* @param NonCorrectedSim3   未经过Sim3传播调整过的关键帧位姿* @param CorrectedSim3      经过Sim3传播调整过的关键帧位姿* @param LoopConnections    因闭环时地图点调整而新生成的边*/

二、顶点与边

顶点

首先要明确优化的目标，那就是本质图中所有关键帧的 Sim3 位姿，虽然顶点类型都是 g2o::VertexSim3Expmap，但是需要分两种情况进行对待：
(01)：\color{blue} (01)：(01)：如果该关键帧在闭环时通过Sim3传播调整过，优先用调整后的Sim3位姿
(02)：\color{blue} (02)：(02)：如果该关键帧在闭环时没有通过Sim3传播调整过，用跟踪时的位姿，尺度为1

边

边类型为 g2o::EdgeSim3(二元边)，在 Thirdparty\g2o\g2o\types\types_seven_dof_expmap.cpp 中可以看到如下代码：

  EdgeSim3::EdgeSim3() :BaseBinaryEdge<7, Sim3, VertexSim3Expmap, VertexSim3Expmap>(){}

参数7→表示测量值的维度，如Optimizer::OptimizeEssentialGraph()函数中的 g2o::Sim3 Sji=Sjw∗SwiSji = Sjw * SwiSji=Sjw∗Swi、g2o::Sim3 Sli=Slw∗SwiSli = Slw * SwiSli=Slw∗Swi 其都是测量值。包含旋转四元数4维、平移向量3维、缩放尺度1维。
参数Sim3→测量值的数据类型
参数VertexSim3Expmap，VertexSim3Expmap→两个连接顶点类型

根据对 g2o(图优化) 的了解，边中有两个重要的函数，分别为 virtual void computeError()、virtual void linearizeOplus()。先来看看 computeError() 函数，其实现于 Thirdparty\g2o\g2o\types\types_seven_dof_expmap.h 中，代码如下：

    void computeError(){const VertexSim3Expmap* v1 = static_cast<const VertexSim3Expmap*>(_vertices[0]);const VertexSim3Expmap* v2 = static_cast<const VertexSim3Expmap*>(_vertices[1]);Sim3 C(_measurement);Sim3 error_=C*v1->estimate()*v2->estimate().inverse();_error = error_.log();}

理想情况下，因为C是观测值(认为其是正确得)，假如 v1->estimate()=_Siw，v2->estimate()=_Sjw， v2->estimate().inverse()=_Swj，近一步可得 v1->estimate()∗*∗v2->estimate().inverse()=_Siw∗*∗_Swj=_Sij，如果观测(真实)值() C=Sji，那么得 _error=Sij∗*∗_Sji=E\mathbf EE(单位矩阵)，经过 log 函数之后为 _error=0，当然这是理想情况。

其对于virtual void linearizeOplus() 函数，并没有重载，似乎是用g2o的自动求导，即差分。会不会慢一些？有知道的朋友可以评论一下。

三、源码逻辑

在对顶点与边进行讲解之后，剩下的就没有那么复杂了，来看看 src/optimizer.cc 中的 Optimizer::OptimizeEssentialGraph() 函数逻辑是如何的。

(01)：\color{blue} (01)：(01)：构造优化器、使用LM算法进行非线性迭代。获得当前地图中的所有关键帧vpKFs和地图点vpMPs。创建变量 vScw 用于记录所有优化前关键帧的位姿，循环遍历局地图中的所有的关键帧vpKFs。

(02)：\color{blue} (02)：(02)：把所有关键帧的Sim3位姿分两种情况作为顶点进行添加：①如果该关键帧在闭环时通过Sim3传播调整过，优先用调整后的Sim3位姿。②如果该关键帧在闭环时没有通过Sim3传播调整过，用跟踪时的位姿，尺度为1。另外闭环匹配上的帧不进行位姿优化（认为是准确的，作为基准），
同时要注意的是并没有锁住第0个关键帧，所以初始关键帧位姿也做了优化。

(03)：\color{blue} (03)：(03)：添加第1种边→闭环时因为地图点调整而出现的关键帧间的新连接关系。对调整过关系的关键帧 LoopConnections 进行遍历，记为 pKF，获得和 pKF 形成新连接关系所有关键帧 spConnections 且进行遍历。如果 pKF 与其形成新连接关系的关键帧，满足下面的任一要求：
①恰好是当前帧及其闭环帧 nIDi=pCurKF 并且nIDj=pLoopKF（此时忽略共视程度）
②任意两对关键帧，共视程度大于100
则会创建一条边，先获得 pKF 矫正过后的Sim3位姿Sjw，然后获得 Sji=Sjw∗*∗ Swi，其中Swi为Siw的逆。Sji作为观测值通过e->setMeasurement(Sji)进行设置。在图优化的时候，根据前面边的计算，理论上 Sij∗*∗_Sji=E\mathbf EE(单位矩阵)，上面介绍边的时候，已经进行具体介绍。

(04)：\color{blue} (04)：(04)：对于上诉的第一种边，其是因为闭环地图点调整而出现的关键帧间的新连接关系，而构建的边。也就是说其经过Sim3的传播，具备矫正过后的Sim3位姿。下面还会没有经过Sim3传播矫正关键帧添加边。

(05)：\color{blue} (05)：(05)：添加第2种边→生成树的边(有父关键帧)，父关键帧就是和当前帧共视程度最高的关键帧。先获得父关键帧未矫正的 Sim3 位姿(如果没有，则使用矫正后的位姿)。父子关键帧之间的相对位姿 Sji(假设该值是正确)，在闭环线程中，提到过父子关键帧之间距离特别近，所以忽略掉他们之间的尺度漂移，认为欧式变换等价于相似变换，所以认为 Sji 是正确的。第二种边，实际上是为整个优化过程添加了约束条件。

(06)：\color{blue} (06)：(06)：添加第3种边→当前帧与闭环匹配帧之间的连接关系(这里面也包括了当前遍历到的这个关键帧之前曾经存在过的回环边)。首先通过pKF->GetLoopEdges()找到和当前关键帧形成闭环关系的关键帧sLoopEdges，然后优先使用未经过Sim3传播调整的位姿建立边。

(07)：\color{blue} (07)：(07)：添加第4种边→共视程度超过100的关键帧(本质图)也作为边进行优化，取出和当前关键帧共视程度超过100的关键帧，然后构建边。但是需要注意得是避免以下情况：最小生成树中的父子关键帧关系,以及和当前遍历到的关键帧构成了回环关系。

(08)：\color{blue} (08)：(08)：开始g2o优化，迭代20次，将优化后的位姿更新到关键帧中。地图点根据参考帧优化前后的相对关系调整自己的位置。

四、源码注释

/*** @brief 闭环检测后，EssentialGraph优化，仅优化所有关键帧位姿，不优化地图点** 1. Vertex:*     - g2o::VertexSim3Expmap，Essential graph中关键帧的位姿* 2. Edge:*     - g2o::EdgeSim3()，BaseBinaryEdge*         + Vertex：关键帧的Tcw，MapPoint的Pw*         + measurement：经过CorrectLoop函数步骤2，Sim3传播校正后的位姿*         + InfoMatrix: 单位矩阵     ** @param pMap               全局地图* @param pLoopKF            闭环匹配上的关键帧* @param pCurKF             当前关键帧* @param NonCorrectedSim3   未经过Sim3传播调整过的关键帧位姿* @param CorrectedSim3      经过Sim3传播调整过的关键帧位姿* @param LoopConnections    因闭环时地图点调整而新生成的边*/
void Optimizer::OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* pCurKF,const LoopClosing::KeyFrameAndPose &NonCorrectedSim3,const LoopClosing::KeyFrameAndPose &CorrectedSim3,const map<KeyFrame *, set<KeyFrame *> > &LoopConnections, const bool &bFixScale)
{// Setup optimizer// Step 1：构造优化器g2o::SparseOptimizer optimizer;optimizer.setVerbose(false);g2o::BlockSolver_7_3::LinearSolverType * linearSolver =new g2o::LinearSolverEigen<g2o::BlockSolver_7_3::PoseMatrixType>();g2o::BlockSolver_7_3 * solver_ptr= new g2o::BlockSolver_7_3(linearSolver);// 使用LM算法进行非线性迭代g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);// 第一次迭代的初始lambda值，如未指定会自动计算一个合适的值solver->setUserLambdaInit(1e-16);optimizer.setAlgorithm(solver);// 获取当前地图中的所有关键帧 和地图点const vector<KeyFrame*> vpKFs = pMap->GetAllKeyFrames();const vector<MapPoint*> vpMPs = pMap->GetAllMapPoints();// 最大关键帧id，用于添加顶点时使用const unsigned int nMaxKFid = pMap->GetMaxKFid();// 记录所有优化前关键帧的位姿，优先使用在闭环时通过Sim3传播调整过的Sim3位姿vector<g2o::Sim3,Eigen::aligned_allocator<g2o::Sim3> > vScw(nMaxKFid+1);// 记录所有关键帧经过本次本质图优化过的位姿vector<g2o::Sim3,Eigen::aligned_allocator<g2o::Sim3> > vCorrectedSwc(nMaxKFid+1);// 这个变量没有用vector<g2o::VertexSim3Expmap*> vpVertices(nMaxKFid+1);// 两个关键帧之间共视关系的权重的最小值const int minFeat = 100;// Set KeyFrame vertices// Step 2：将地图中所有关键帧的位姿作为顶点添加到优化器// 尽可能使用经过Sim3调整的位姿// 遍历全局地图中的所有的关键帧for(size_t i=0, iend=vpKFs.size(); i<iend;i++){KeyFrame* pKF = vpKFs[i];if(pKF->isBad())continue;g2o::VertexSim3Expmap* VSim3 = new g2o::VertexSim3Expmap();// 关键帧在所有关键帧中的id，用来设置为顶点的idconst int nIDi = pKF->mnId;LoopClosing::KeyFrameAndPose::const_iterator it = CorrectedSim3.find(pKF);if(it!=CorrectedSim3.end()){// 如果该关键帧在闭环时通过Sim3传播调整过，优先用调整后的Sim3位姿vScw[nIDi] = it->second;VSim3->setEstimate(it->second);}else{// 如果该关键帧在闭环时没有通过Sim3传播调整过，用跟踪时的位姿，尺度为1Eigen::Matrix<double,3,3> Rcw = Converter::toMatrix3d(pKF->GetRotation());Eigen::Matrix<double,3,1> tcw = Converter::toVector3d(pKF->GetTranslation());g2o::Sim3 Siw(Rcw,tcw,1.0); vScw[nIDi] = Siw;VSim3->setEstimate(Siw);}// 闭环匹配上的帧不进行位姿优化（认为是准确的，作为基准）// 注意这里并没有锁住第0个关键帧，所以初始关键帧位姿也做了优化if(pKF==pLoopKF)VSim3->setFixed(true);VSim3->setId(nIDi);VSim3->setMarginalized(false);// 和当前系统的传感器有关，如果是RGBD或者是双目，那么就不需要优化sim3的缩放系数，保持为1即可VSim3->_fix_scale = bFixScale;// 添加顶点optimizer.addVertex(VSim3);// 优化前的位姿顶点，后面代码中没有使用vpVertices[nIDi]=VSim3;}// 保存由于闭环后优化sim3而出现的新的关键帧和关键帧之间的连接关系,其中id比较小的关键帧在前,id比较大的关键帧在后set<pair<long unsigned int,long unsigned int> > sInsertedEdges;// 单位矩阵const Eigen::Matrix<double,7,7> matLambda = Eigen::Matrix<double,7,7>::Identity();// Set Loop edges// Step 3：添加第1种边：闭环时因为地图点调整而出现的关键帧间的新连接关系for(map<KeyFrame *, set<KeyFrame *> >::const_iterator mit = LoopConnections.begin(), mend=LoopConnections.end(); mit!=mend; mit++){KeyFrame* pKF = mit->first;const long unsigned int nIDi = pKF->mnId;// 和pKF 形成新连接关系的关键帧const set<KeyFrame*> &spConnections = mit->second;const g2o::Sim3 Siw = vScw[nIDi];const g2o::Sim3 Swi = Siw.inverse();// 对于当前关键帧nIDi而言，遍历每一个新添加的关键帧nIDj链接关系for(set<KeyFrame*>::const_iterator sit=spConnections.begin(), send=spConnections.end(); sit!=send; sit++){const long unsigned int nIDj = (*sit)->mnId;// 同时满足下面2个条件的跳过// 条件1：至少有一个不是pCurKF或pLoopKF// 条件2：共视程度太少(<100),不足以构成约束的边if((nIDi!=pCurKF->mnId || nIDj!=pLoopKF->mnId)   && pKF->GetWeight(*sit)<minFeat)       continue;// 通过上面考验的帧有两种情况：// 1、恰好是当前帧及其闭环帧 nIDi=pCurKF 并且nIDj=pLoopKF（此时忽略共视程度）// 2、任意两对关键帧，共视程度大于100const g2o::Sim3 Sjw = vScw[nIDj];// 得到两个位姿间的Sim3变换const g2o::Sim3 Sji = Sjw * Swi;g2o::EdgeSim3* e = new g2o::EdgeSim3();  e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(nIDj)));e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(nIDi)));// Sji内部是经过了Sim调整的观测e->setMeasurement(Sji);// 信息矩阵是单位阵,说明这类新增加的边对总误差的贡献也都是一样大的e->information() = matLambda;optimizer.addEdge(e);// 保证id小的在前,大的在后sInsertedEdges.insert(make_pair(min(nIDi,nIDj),max(nIDi,nIDj)));} }// Set normal edges// Step 4：添加跟踪时形成的边、闭环匹配成功形成的边for(size_t i=0, iend=vpKFs.size(); i<iend; i++){KeyFrame* pKF = vpKFs[i];const int nIDi = pKF->mnId;g2o::Sim3 Swi;LoopClosing::KeyFrameAndPose::const_iterator iti = NonCorrectedSim3.find(pKF);if(iti!=NonCorrectedSim3.end())Swi = (iti->second).inverse();  //优先使用未经过Sim3传播调整的位姿elseSwi = vScw[nIDi].inverse();     //没找到才考虑已经经过Sim3传播调整的位姿KeyFrame* pParentKF = pKF->GetParent();// Spanning tree edge// Step 4.1：添加第2种边：生成树的边（有父关键帧）// 父关键帧就是和当前帧共视程度最高的关键帧if(pParentKF){// 父关键帧idint nIDj = pParentKF->mnId;g2o::Sim3 Sjw;LoopClosing::KeyFrameAndPose::const_iterator itj = NonCorrectedSim3.find(pParentKF);//优先使用未经过Sim3传播调整的位姿if(itj!=NonCorrectedSim3.end())Sjw = itj->second;elseSjw = vScw[nIDj];// 计算父子关键帧之间的相对位姿g2o::Sim3 Sji = Sjw * Swi;g2o::EdgeSim3* e = new g2o::EdgeSim3();e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(nIDj)));e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(nIDi)));// 希望父子关键帧之间的位姿差最小e->setMeasurement(Sji);// 所有元素的贡献都一样;每个误差边对总误差的贡献也都相同e->information() = matLambda;optimizer.addEdge(e);}// Loop edges// Step 4.2：添加第3种边：当前帧与闭环匹配帧之间的连接关系(这里面也包括了当前遍历到的这个关键帧之前曾经存在过的回环边)// 获取和当前关键帧形成闭环关系的关键帧const set<KeyFrame*> sLoopEdges = pKF->GetLoopEdges();for(set<KeyFrame*>::const_iterator sit=sLoopEdges.begin(), send=sLoopEdges.end(); sit!=send; sit++){KeyFrame* pLKF = *sit;// 注意要比当前遍历到的这个关键帧的id小,这个是为了避免重复添加if(pLKF->mnId<pKF->mnId){g2o::Sim3 Slw;LoopClosing::KeyFrameAndPose::const_iterator itl = NonCorrectedSim3.find(pLKF);//优先使用未经过Sim3传播调整的位姿if(itl!=NonCorrectedSim3.end())Slw = itl->second;elseSlw = vScw[pLKF->mnId];g2o::Sim3 Sli = Slw * Swi;g2o::EdgeSim3* el = new g2o::EdgeSim3();el->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pLKF->mnId)));el->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(nIDi)));// 根据两个位姿顶点的位姿算出相对位姿作为边el->setMeasurement(Sli);el->information() = matLambda;optimizer.addEdge(el);}}// Covisibility graph edges// Step 4.3：添加第4种边：共视程度超过100的关键帧也作为边进行优化// 取出和当前关键帧共视程度超过100的关键帧const vector<KeyFrame*> vpConnectedKFs = pKF->GetCovisiblesByWeight(minFeat);for(vector<KeyFrame*>::const_iterator vit=vpConnectedKFs.begin(); vit!=vpConnectedKFs.end(); vit++){KeyFrame* pKFn = *vit;// 避免重复添加// 避免以下情况：最小生成树中的父子关键帧关系,以及和当前遍历到的关键帧构成了回环关系if(pKFn && pKFn!=pParentKF && !pKF->hasChild(pKFn) && !sLoopEdges.count(pKFn)) {// 注意要比当前遍历到的这个关键帧的id要小,这个是为了避免重复添加if(!pKFn->isBad() && pKFn->mnId<pKF->mnId){// 如果这条边已经添加了，跳过if(sInsertedEdges.count(make_pair(min(pKF->mnId,pKFn->mnId),max(pKF->mnId,pKFn->mnId))))continue;g2o::Sim3 Snw;LoopClosing::KeyFrameAndPose::const_iterator itn = NonCorrectedSim3.find(pKFn);// 优先未经过Sim3传播调整的位姿if(itn!=NonCorrectedSim3.end())Snw = itn->second;elseSnw = vScw[pKFn->mnId];// 也是同样计算相对位姿g2o::Sim3 Sni = Snw * Swi;g2o::EdgeSim3* en = new g2o::EdgeSim3();en->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKFn->mnId)));en->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(nIDi)));en->setMeasurement(Sni);en->information() = matLambda;optimizer.addEdge(en);}} // 如果这个比较好的共视关系的约束之前没有被重复添加过} // 遍历所有于当前遍历到的关键帧具有较好的共视关系的关键帧} // 添加跟踪时形成的边、闭环匹配成功形成的边// Optimize!// Step 5：开始g2o优化，迭代20次optimizer.initializeOptimization();optimizer.optimize(20);// 更新地图前，先上锁，防止冲突unique_lock<mutex> lock(pMap->mMutexMapUpdate);// SE3 Pose Recovering. Sim3:[sR t;0 1] -> SE3:[R t/s;0 1]// Step 6：将优化后的位姿更新到关键帧中// 遍历地图中的所有关键帧for(size_t i=0;i<vpKFs.size();i++){KeyFrame* pKFi = vpKFs[i];const int nIDi = pKFi->mnId;g2o::VertexSim3Expmap* VSim3 = static_cast<g2o::VertexSim3Expmap*>(optimizer.vertex(nIDi));g2o::Sim3 CorrectedSiw =  VSim3->estimate();vCorrectedSwc[nIDi]=CorrectedSiw.inverse();Eigen::Matrix3d eigR = CorrectedSiw.rotation().toRotationMatrix();Eigen::Vector3d eigt = CorrectedSiw.translation();double s = CorrectedSiw.scale();// 转换成尺度为1的变换矩阵的形式eigt *=(1./s); //[R t/s;0 1]cv::Mat Tiw = Converter::toCvSE3(eigR,eigt);// 将更新的位姿写入到关键帧中pKFi->SetPose(Tiw);}// Correct points. Transform to "non-optimized" reference keyframe pose and transform back with optimized pose// Step 7：步骤5和步骤6优化得到关键帧的位姿后，地图点根据参考帧优化前后的相对关系调整自己的位置// 遍历所有地图点for(size_t i=0, iend=vpMPs.size(); i<iend; i++){MapPoint* pMP = vpMPs[i];if(pMP->isBad())continue;int nIDr;// 该地图点在闭环检测中被当前KF调整过，那么使用调整它的KF idif(pMP->mnCorrectedByKF==pCurKF->mnId){nIDr = pMP->mnCorrectedReference;}else{// 通常情况下地图点的参考关键帧就是创建该地图点的那个关键帧KeyFrame* pRefKF = pMP->GetReferenceKeyFrame();nIDr = pRefKF->mnId;}// 得到地图点参考关键帧优化前的位姿g2o::Sim3 Srw = vScw[nIDr];// 得到地图点参考关键帧优化后的位姿g2o::Sim3 correctedSwr = vCorrectedSwc[nIDr];cv::Mat P3Dw = pMP->GetWorldPos();Eigen::Matrix<double,3,1> eigP3Dw = Converter::toVector3d(P3Dw);Eigen::Matrix<double,3,1> eigCorrectedP3Dw = correctedSwr.map(Srw.map(eigP3Dw));cv::Mat cvCorrectedP3Dw = Converter::toCvMat(eigCorrectedP3Dw);// 这里优化后的位置也是直接写入到地图点之中的pMP->SetWorldPos(cvCorrectedP3Dw);// 记得更新一下pMP->UpdateNormalAndDepth();} // 使用相对位姿变换的方法来更新地图点的位姿
}

五、结语

通过该篇博客，对闭环线程中的Optimizer::OptimizeEssentialGraph→本质图优化进行讲解，下面要讲解的就是闭环线程中的Optimizer::GlobalBundleAdjustemnt()→全局优化了。另外，这里提及到一个点，src/LoopClosing.cc 中的 LoopClosing::CorrectLoop() 函数：

    // Optimize graph// Step 6：进行本质图优化，优化本质图中所有关键帧的位姿和地图点// LoopConnections是形成闭环后新生成的连接关系，不包括步骤7中当前帧与闭环匹配帧之间的连接关系Optimizer::OptimizeEssentialGraph(mpMap, mpMatchedKF, mpCurrentKF, NonCorrectedSim3, CorrectedSim3, LoopConnections, mbFixScale);// Add loop edge// Step 7：添加当前帧与闭环匹配帧之间的边（这个连接关系不优化）// !这两句话应该放在OptimizeEssentialGraph之前，因为OptimizeEssentialGraph的步骤4.2中有优化mpMatchedKF->AddLoopEdge(mpCurrentKF);mpCurrentKF->AddLoopEdge(mpMatchedKF);

对于下面的两句代码应该放在 OptimizeEssentialGraph() 函数的前面。

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