本人讲解关于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→官方认证
一、前言
上一篇博客对建图线程: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() 函数，并没有重载，似乎是用g2o的自动求导，即差分。会不会慢一些？
对于 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)： (01)： 初始化g2o优化器，使用L-M迭代；获得当前关键帧pKF1与闭环候选帧pKF2相机内参与位姿。设置待优化的Sim3位姿作为顶点。根据传感器类型决定是否固定尺度，如果为双目或者深度相机则固定尺度。
( 02 ) ： \color{blue} (02)： (02)： 对当前帧pKF1的所有关键点进行遍历，如果其未与闭环候选帧pKF2的关键点匹配，则跳出循环。否则获得各自关键点对应的地图点pMP1，pMP2。如果地图点pMP1，pMP2未同时存在，或有任意一个地图点未坏点，则continue。
( 03 ) ： \color{blue} (03)： (03)： 为地图点pMP1创建一个VertexSBAPointXYZ类型顶点vPoint1，并且设置vPoint1->setFixed(true)，也就是地图点不作优化。同理，为地图点vPoint2也创建一个VertexSBAPointXYZ类型顶点vPoint2，并且设置vPoint2->setFixed(true)，地图点不作优化。
( 04 ) ： \color{blue} (04)： (04)： ①添加边x1 = S12 ∗ * ∗X2，设置了两个顶点，分别为vPoint2与vSim3，该边计算的误差为→地图点pMP2通过Sim变换正向投影到pKF1与pKF1的匹配点计算误差。②添加边x2 = S21 ∗ * ∗X1，设置了两个顶点，分别为vPoint1与vSim3，该边计算的误差为→地图点pMP1通过Sim变换的逆进行反向投影到pKF2与pKF2的匹配点计算误差。
( 04 ) ： \color{blue} (04)： (04)：g2o开始优化，先迭代5次。根据卡方检验删除向或反向投影任意一个超过误差阈值就删掉该边。如果有误差较大的边被剔除那么说明回环质量并不是非常好,还要多迭代几次;反之就少迭代几次，如果经过上面的剔除后剩下的匹配关系已经非常少了,那么就放弃优化。内点数直接设置为0。
( 05 ) ： \color{blue} (05)： (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 poses
const 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顶点，所以设置为false
vSim3->_principle_point1[0] = K1.at<float>(0,2); // 光心横坐标cx
vSim3->_principle_point1[1] = K1.at<float>(1,2); // 光心纵坐标cy
vSim3->_focal_length1[0] = K1.at<float>(0,0); // 焦距 fx
vSim3->_focal_length1[1] = K1.at<float>(1,1); // 焦距 fy
vSim3->_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);
}
else
continue;
}
else
continue;
// 对匹配关系进行计数
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 为 0
e12->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 inliers
int 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;
else
nMoreIterations=5;
// 如果经过上面的剔除后剩下的匹配关系已经非常少了,那么就放弃优化。内点数直接设置为0
if(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);
}
else
nIn++;
}
// Recover optimized Sim3
// Step 8：用优化后的结果来更新Sim3位姿。
g2o::VertexSim3Expmap* vSim3_recov = static_cast<g2o::VertexSim3Expmap*>(optimizer.vertex(0));
g2oS12= vSim3_recov->estimate();
return nIn;
}
五、结语
通过该篇博客，对闭环线程中的 Optimizer::OptimizeSim3→Sim3变换优化 进行详细讲解，下面要讲解的依旧是闭环线程中的优化，其为 Optimizer::OptimizeEssentialGraph()→本质图优化。