本人讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解的(01)ORB-SLAM2源码无死角解析-接如下:
(01)ORB-SLAM2源码无死角解析-(00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/123092196
文末正下方中心提供了本人 联系方式， 点击本人照片即可显示 W X → 官方认证 {\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 S j i = S j w ∗ S w i Sji = Sjw * Swi Sji=Sjw∗Swi、g2o::Sim3 S l i = S l w ∗ S w i Sli = Slw * Swi Sli=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 E E(单位矩阵)，经过 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 E E(单位矩阵)，上面介绍边的时候，已经进行具体介绍。
( 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，用来设置为顶点的id
const 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传播调整过，用跟踪时的位姿，尺度为1
Eigen::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、任意两对关键帧，共视程度大于100
const 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传播调整的位姿
else
Swi = vScw[nIDi].inverse(); //没找到才考虑已经经过Sim3传播调整的位姿
KeyFrame* pParentKF = pKF->GetParent();
// Spanning tree edge
// Step 4.1：添加第2种边：生成树的边（有父关键帧）
// 父关键帧就是和当前帧共视程度最高的关键帧
if(pParentKF)
{
// 父关键帧id
int nIDj = pParentKF->mnId;
g2o::Sim3 Sjw;
LoopClosing::KeyFrameAndPose::const_iterator itj = NonCorrectedSim3.find(pParentKF);
//优先使用未经过Sim3传播调整的位姿
if(itj!=NonCorrectedSim3.end())
Sjw = itj->second;
else
Sjw = 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;
else
Slw = 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;
else
Snw = 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 id
if(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() 函数的前面。