1. 前言
LoopClosing.cc这个文件是闭环检测与矫正的代码，其逻辑比较清晰。由于用到了多地图集，所以闭环检测不仅在当前地图中进行，还会在以前的地图中检测。如果是在当前地图中检测到了回环，则进行回环矫正；如果是在以前的地图中检测到了回环，则在回环处进行地图的融合，并矫正融合地图中所有的关键帧位姿和地图点。
2. 代码分析
2.1. OptimizeEssentialGraph
LoopClosing::CorrectLoop() 回环矫正时使用，纯视觉，全局本质图优化
优化目标：
- 地图中所有MP
- 关键帧
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);
// 指定线性方程求解器使用Eigen的块求解器
// 7表示位姿是sim3 3表示三维点坐标维度
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); // 存放每一帧优化前的sim3
// 记录所有关键帧经过本次本质图优化过的位姿
vector<g2o::Sim3, Eigen::aligned_allocator<g2o::Sim3>> vCorrectedSwc(nMaxKFid + 1); // 存放每一帧优化后的sim3，修正mp位姿用
// 这个变量没有用
vector<g2o::VertexSim3Expmap *> vpVertices(nMaxKFid + 1); // 存放节点，没用，还占地方
// 调试用，暂时不去管
vector<Eigen::Vector3d> vZvectors(nMaxKFid + 1); // For debugging
Eigen::Vector3d z_vec;
z_vec << 0.0, 0.0, 1.0;
// 两个关键帧之间共视关系的权重（也就是共视点的数目，单目x1，双目/rgbd x2）的最小值
const int minFeat = 100; // MODIFICATION originally was set to 100
// Set KeyFrame vertices
// Step 2：将地图中所有关键帧的pose作为顶点添加到优化器
// 尽可能使用经过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传播调整过，用跟踪时的位姿
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); //尺度为1
vScw[nIDi] = Siw;
VSim3->setEstimate(Siw);
}
// 固定第一帧
if (pKF->mnId == pMap->GetInitKFid())
VSim3->setFixed(true);
VSim3->setId(nIDi);
VSim3->setMarginalized(false);
// 和当前系统的传感器有关，如果是RGBD或者是双目，那么就不需要优化sim3的缩放系数，保持为1即可
VSim3->_fix_scale = bFixScale;
optimizer.addVertex(VSim3);
vZvectors[nIDi] = vScw[nIDi].rotation().toRotationMatrix() * z_vec; // For debugging
// 优化前的pose顶点，后面代码中没有使用
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种边：LoopConnections是闭环时因为MapPoints调整而出现的新关键帧连接关系（包括当前帧与闭环匹配帧之间的连接关系）
int count_loop = 0;
for (map<KeyFrame *, set<KeyFrame *>>::const_iterator mit = LoopConnections.begin(), mend = LoopConnections.end(); mit != mend; mit++)
{
// 3.1 取出帧与帧们
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;
// 这里的约束有点意思，对于每一个连接，只要是存在pCurKF或者pLoopKF 那这个连接不管共视了多少MP都优化
// 反之没有的话共视度要大于100 构建本质图
if ((nIDi != pCurKF->mnId || nIDj != pLoopKF->mnId) && pKF->GetWeight(*sit) < minFeat)
continue;
// 通过上面考验的帧有两种情况：
// 得到两个pose间的Sim3变换，个人认为这里好像有些问题，假设说一个当前帧的共视帧，他在vScw中保存的位姿是更新后的
// 如果与之相连的关键帧没有更新，那么是不是两个相对位姿的边有问题，先留个记号，可以调试看看
const g2o::Sim3 Sjw = vScw[nIDj];
// 得到两个pose间的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);
count_loop++;
// 保证id小的在前,大的在后
sInsertedEdges.insert(make_pair(min(nIDi, nIDj), max(nIDi, nIDj)));
}
}
int count_spa_tree = 0;
int count_cov = 0;
int count_imu = 0;
int count_kf = 0;
// Set normal edges
// 4. 添加跟踪时形成的边、闭环匹配成功形成的边
for (size_t i = 0, iend = vpKFs.size(); i < iend; i++)
{
count_kf = 0;
KeyFrame *pKF = vpKFs[i];
const int nIDi = pKF->mnId;
g2o::Sim3 Swi; // 校正前的sim3
LoopClosing::KeyFrameAndPose::const_iterator iti = NonCorrectedSim3.find(pKF);
// 找到的话说明是关键帧的共视帧，没找到表示非共视帧，非共视帧vScw[nIDi]里面装的都是矫正前的
// 所以不管怎样说 Swi都是校正前的
if (iti != NonCorrectedSim3.end())
Swi = (iti->second).inverse();
else
Swi = vScw[nIDi].inverse();
KeyFrame *pParentKF = pKF->GetParent();
// Spanning tree edge
// 4.1 只添加扩展树的边（有父关键帧）
if (pParentKF)
{
int nIDj = pParentKF->mnId;
g2o::Sim3 Sjw;
LoopClosing::KeyFrameAndPose::const_iterator itj = NonCorrectedSim3.find(pParentKF);
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);
count_kf++;
count_spa_tree++;
e->information() = matLambda;
optimizer.addEdge(e);
}
// Loop edges
// 4.2 添加在CorrectLoop函数中AddLoopEdge函数添加的闭环连接边（当前帧与闭环匹配帧之间的连接关系）
// 使用经过Sim3调整前关键帧之间的相对关系作为边
const set<KeyFrame *> sLoopEdges = pKF->GetLoopEdges();
for (set<KeyFrame *>::const_iterator sit = sLoopEdges.begin(), send = sLoopEdges.end(); sit != send; sit++)
{
KeyFrame *pLKF = *sit;
if (pLKF->mnId < pKF->mnId)
{
g2o::Sim3 Slw;
LoopClosing::KeyFrameAndPose::const_iterator itl = NonCorrectedSim3.find(pLKF);
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);
count_kf++;
count_loop++;
}
}
// Covisibility graph edges
// 4.3 对有很好共视关系的关键帧也作为边进行优化
// 使用经过Sim3调整前关键帧之间的相对关系作为边
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))
{
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);
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);
count_kf++;
count_cov++;
}
}
}
// Inertial edges if inertial
// 如果是imu的话还会找前一帧做优化
if (pKF->bImu && pKF->mPrevKF)
{
g2o::Sim3 Spw;
LoopClosing::KeyFrameAndPose::const_iterator itp = NonCorrectedSim3.find(pKF->mPrevKF);
if (itp != NonCorrectedSim3.end())
Spw = itp->second;
else
Spw = vScw[pKF->mPrevKF->mnId];
g2o::Sim3 Spi = Spw * Swi;
g2o::EdgeSim3 *ep = new g2o::EdgeSim3();
ep->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(pKF->mPrevKF->mnId)));
ep->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(nIDi)));
ep->setMeasurement(Spi);
ep->information() = matLambda;
optimizer.addEdge(ep);
count_kf++;
count_imu++;
}
}
// Optimize!
// 5. 开始g2o优化
optimizer.initializeOptimization();
optimizer.computeActiveErrors();
float err0 = optimizer.activeRobustChi2();
optimizer.optimize(20);
optimizer.computeActiveErrors();
float errEnd = optimizer.activeRobustChi2();
unique_lock<mutex> lock(pMap->mMutexMapUpdate);
// SE3 Pose Recovering. Sim3:[sR t;0 1] -> SE3:[R t/s;0 1]
// 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();
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
// 7. 步骤5和步骤6优化得到关键帧的位姿后，MapPoints根据参考帧优化前后的相对关系调整自己的位置
for (size_t i = 0, iend = vpMPs.size(); i < iend; i++)
{
MapPoint *pMP = vpMPs[i];
if (pMP->isBad())
continue;
int nIDr;
// 该MapPoint经过Sim3调整过，(LoopClosing.cpp，CorrectLoop函数，步骤2.2_
if (pMP->mnCorrectedByKF == pCurKF->mnId)
{
nIDr = pMP->mnCorrectedReference;
}
else
{
// 通过情况下MapPoint的参考关键帧就是创建该MapPoint的那个关键帧
KeyFrame *pRefKF = pMP->GetReferenceKeyFrame();
nIDr = pRefKF->mnId;
}
// 得到MapPoint参考关键帧步骤5优化前的位姿
g2o::Sim3 Srw = vScw[nIDr];
// 得到MapPoint参考关键帧优化后的位姿
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();
}
pMap->IncreaseChangeIndex();
}
2.2. OptimizeEssentialGraph4DoF
LoopClosing::CorrectLoop() 回环矫正时使用，IMU加视觉，全局本质图优化，流程基本与上面 OptimizeEssentialGraph 一模一样，同样有严重BUG
优化目标：
- 地图中所有MP
- 关键帧
void Optimizer::OptimizeEssentialGraph4DoF(Map *pMap, KeyFrame *pLoopKF, KeyFrame *pCurKF,
const LoopClosing::KeyFrameAndPose &NonCorrectedSim3,
const LoopClosing::KeyFrameAndPose &CorrectedSim3,
const map<KeyFrame *, set<KeyFrame *>> &LoopConnections)
{
typedef g2o::BlockSolver<g2o::BlockSolverTraits<4, 4>> BlockSolver_4_4;
// Setup optimizer
// 1. 构建优化器
g2o::SparseOptimizer optimizer;
optimizer.setVerbose(false);
g2o::BlockSolverX::LinearSolverType *linearSolver =
new g2o::LinearSolverEigen<g2o::BlockSolverX::PoseMatrixType>();
g2o::BlockSolverX *solver_ptr = new g2o::BlockSolverX(linearSolver);
g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
optimizer.setAlgorithm(solver);
const vector<KeyFrame *> vpKFs = pMap->GetAllKeyFrames(); // 所有关键帧
const vector<MapPoint *> vpMPs = pMap->GetAllMapPoints(); // 所有mp
const unsigned int nMaxKFid = pMap->GetMaxKFid();
vector<g2o::Sim3, Eigen::aligned_allocator<g2o::Sim3>> vScw(nMaxKFid + 1); // 存放每一帧优化前的sim3
vector<g2o::Sim3, Eigen::aligned_allocator<g2o::Sim3>> vCorrectedSwc(nMaxKFid + 1); // 存放每一帧优化后的sim3，修正mp位姿用
vector<VertexPose4DoF *> vpVertices(nMaxKFid + 1);
const int minFeat = 100; // 100 本质图的权重
// Set KeyFrame vertices
// 2. 关键帧节点
for (size_t i = 0, iend = vpKFs.size(); i < iend; i++)
{
KeyFrame *pKF = vpKFs[i];
if (pKF->isBad())
continue;
// 自定义的一个优化4自由度的节点
VertexPose4DoF *V4DoF;
const int nIDi = pKF->mnId;
LoopClosing::KeyFrameAndPose::const_iterator it = CorrectedSim3.find(pKF);
// 找到了表示与当前关键帧共视，使用其他函数更新后的位姿
if (it != CorrectedSim3.end())
{
vScw[nIDi] = it->second;
const g2o::Sim3 Swc = it->second.inverse();
Eigen::Matrix3d Rwc = Swc.rotation().toRotationMatrix();
Eigen::Vector3d twc = Swc.translation();
V4DoF = new VertexPose4DoF(Rwc, twc, pKF);
}
else
{
// 没有在CorrectedSim3里面找到表示与pCurKF并不关联，也就是离得远，并没有经过计算得到一个初始值，这里直接使用原始的位置
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;
V4DoF = new VertexPose4DoF(pKF);
}
// 固定回环帧
if (pKF == pLoopKF)
V4DoF->setFixed(true);
V4DoF->setId(nIDi);
V4DoF->setMarginalized(false);
optimizer.addVertex(V4DoF);
vpVertices[nIDi] = V4DoF;
}
set<pair<long unsigned int, long unsigned int>> sInsertedEdges;
// Edge used in posegraph has still 6Dof, even if updates of camera poses are just in 4DoF
Eigen::Matrix<double, 6, 6> matLambda = Eigen::Matrix<double, 6, 6>::Identity();
matLambda(0, 0) = 1e3;
matLambda(1, 1) = 1e3;
matLambda(0, 0) = 1e3;
// Set Loop edges
// 3. 添加边：LoopConnections是闭环时因为MapPoints调整而出现的新关键帧连接关系（包括当前帧与闭环匹配帧之间的连接关系）
Edge4DoF *e_loop;
for (map<KeyFrame *, set<KeyFrame *>>::const_iterator mit = LoopConnections.begin(), mend = LoopConnections.end(); mit != mend; mit++)
{
// 3.1 取出帧与帧们
KeyFrame *pKF = mit->first;
const long unsigned int nIDi = pKF->mnId;
const set<KeyFrame *> &spConnections = mit->second;
const g2o::Sim3 Siw = vScw[nIDi]; // 优化前的位姿
const g2o::Sim3 Swi = Siw.inverse();
for (set<KeyFrame *>::const_iterator sit = spConnections.begin(), send = spConnections.end(); sit != send; sit++)
{
const long unsigned int nIDj = (*sit)->mnId;
// 这里的约束有点意思，对于每一个连接，只要是存在pCurKF或者pLoopKF 那这个连接不管共视了多少MP都优化
// 反之没有的话共视度要大于100 构建本质图
if ((nIDi != pCurKF->mnId || nIDj != pLoopKF->mnId) && pKF->GetWeight(*sit) < minFeat)
continue;
const g2o::Sim3 Sjw = vScw[nIDj];
// 得到两个pose间的Sim3变换，个人认为这里好像有些问题，假设说一个当前帧的共视帧，他在vScw中保存的位姿是更新后的
// 如果与之相连的关键帧没有更新，那么是不是两个相对位姿的边有问题，先留个记号，可以调试看看
const g2o::Sim3 Sij = Siw * Sjw.inverse();
Eigen::Matrix4d Tij;
Tij.block<3, 3>(0, 0) = Sij.rotation().toRotationMatrix();
Tij.block<3, 1>(0, 3) = Sij.translation();
Tij(3, 3) = 1.;
// 认为相对位姿会比较准，那这个当一个约束
Edge4DoF *e = new Edge4DoF(Tij);
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(nIDj)));
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(nIDi)));
e->information() = matLambda;
e_loop = e;
optimizer.addEdge(e);
sInsertedEdges.insert(make_pair(min(nIDi, nIDj), max(nIDi, nIDj)));
}
}
// 1. Set normal edges
// 4. 添加跟踪时形成的边、闭环匹配成功形成的边
for (size_t i = 0, iend = vpKFs.size(); i < iend; i++)
{
KeyFrame *pKF = vpKFs[i];
const int nIDi = pKF->mnId;
g2o::Sim3 Siw;
// Use noncorrected poses for posegraph edges
LoopClosing::KeyFrameAndPose::const_iterator iti = NonCorrectedSim3.find(pKF);
// 找到的话说明是关键帧的共视帧，没找到表示非共视帧，非共视帧vScw[nIDi]里面装的都是矫正前的
// 所以不管怎样说 Swi都是校正前的
if (iti != NonCorrectedSim3.end())
Siw = iti->second;
else
Siw = vScw[nIDi];
// 1.1.0 Spanning tree edge
// 4.1 只添加扩展树的边（有父关键帧） 这里并没有父帧，这段没执行到
KeyFrame *pParentKF = static_cast<KeyFrame *>(NULL);
if (pParentKF)
{
int nIDj = pParentKF->mnId;
g2o::Sim3 Swj;
LoopClosing::KeyFrameAndPose::const_iterator itj = NonCorrectedSim3.find(pParentKF);
if (itj != NonCorrectedSim3.end())
Swj = (itj->second).inverse();
else
Swj = vScw[nIDj].inverse();
g2o::Sim3 Sij = Siw * Swj;
Eigen::Matrix4d Tij;
Tij.block<3, 3>(0, 0) = Sij.rotation().toRotationMatrix();
Tij.block<3, 1>(0, 3) = Sij.translation();
Tij(3, 3) = 1.;
Edge4DoF *e = new Edge4DoF(Tij);
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(nIDi)));
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(nIDj)));
e->information() = matLambda;
optimizer.addEdge(e);
}
// 1.1.1 Inertial edges
// 代替父帧的是利用mPrevKF，流程与上面一样
KeyFrame *prevKF = pKF->mPrevKF;
if (prevKF)
{
int nIDj = prevKF->mnId;
g2o::Sim3 Swj;
LoopClosing::KeyFrameAndPose::const_iterator itj = NonCorrectedSim3.find(prevKF);
if (itj != NonCorrectedSim3.end())
Swj = (itj->second).inverse();
else
Swj = vScw[nIDj].inverse();
g2o::Sim3 Sij = Siw * Swj;
Eigen::Matrix4d Tij;
Tij.block<3, 3>(0, 0) = Sij.rotation().toRotationMatrix();
Tij.block<3, 1>(0, 3) = Sij.translation();
Tij(3, 3) = 1.;
Edge4DoF *e = new Edge4DoF(Tij);
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(nIDi)));
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(nIDj)));
e->information() = matLambda;
optimizer.addEdge(e);
}
// 1.2 Loop edges
// 4.2 添加在CorrectLoop函数中AddLoopEdge函数添加的闭环连接边（当前帧与闭环匹配帧之间的连接关系）
// 使用经过Sim3调整前关键帧之间的相对关系作为边
const set<KeyFrame *> sLoopEdges = pKF->GetLoopEdges();
for (set<KeyFrame *>::const_iterator sit = sLoopEdges.begin(), send = sLoopEdges.end(); sit != send; sit++)
{
KeyFrame *pLKF = *sit;
if (pLKF->mnId < pKF->mnId)
{
g2o::Sim3 Swl;
LoopClosing::KeyFrameAndPose::const_iterator itl = NonCorrectedSim3.find(pLKF);
if (itl != NonCorrectedSim3.end())
Swl = itl->second.inverse();
else
Swl = vScw[pLKF->mnId].inverse();
g2o::Sim3 Sil = Siw * Swl;
Eigen::Matrix4d Til;
Til.block<3, 3>(0, 0) = Sil.rotation().toRotationMatrix();
Til.block<3, 1>(0, 3) = Sil.translation();
Til(3, 3) = 1.;
// 同样，认为相对位姿比较准
Edge4DoF *e = new Edge4DoF(Til);
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(nIDi)));
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(pLKF->mnId)));
e->information() = matLambda;
optimizer.addEdge(e);
}
}
// 1.3 Covisibility graph edges
// 4.3 最有很好共视关系的关键帧也作为边进行优化
// 使用经过Sim3调整前关键帧之间的相对关系作为边
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 && pKFn != prevKF && pKFn != pKF->mNextKF && !pKF->hasChild(pKFn) && !sLoopEdges.count(pKFn))
{
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 Swn;
LoopClosing::KeyFrameAndPose::const_iterator itn = NonCorrectedSim3.find(pKFn);
if (itn != NonCorrectedSim3.end())
Swn = itn->second.inverse();
else
Swn = vScw[pKFn->mnId].inverse();
g2o::Sim3 Sin = Siw * Swn;
Eigen::Matrix4d Tin;
Tin.block<3, 3>(0, 0) = Sin.rotation().toRotationMatrix();
Tin.block<3, 1>(0, 3) = Sin.translation();
Tin(3, 3) = 1.;
Edge4DoF *e = new Edge4DoF(Tin);
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(nIDi)));
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(pKFn->mnId)));
e->information() = matLambda;
optimizer.addEdge(e);
}
}
}
}
// 5. 开始g2o优化
optimizer.initializeOptimization();
optimizer.computeActiveErrors();
optimizer.optimize(20);
unique_lock<mutex> lock(pMap->mMutexMapUpdate);
// SE3 Pose Recovering. Sim3:[sR t;0 1] -> SE3:[R t/s;0 1]
// 6. 设定优化后的位姿
for (size_t i = 0; i < vpKFs.size(); i++)
{
KeyFrame *pKFi = vpKFs[i];
const int nIDi = pKFi->mnId;
VertexPose4DoF *Vi = static_cast<VertexPose4DoF *>(optimizer.vertex(nIDi));
Eigen::Matrix3d Ri = Vi->estimate().Rcw[0];
Eigen::Vector3d ti = Vi->estimate().tcw[0];
g2o::Sim3 CorrectedSiw = g2o::Sim3(Ri, ti, 1.);
vCorrectedSwc[nIDi] = CorrectedSiw.inverse();
cv::Mat Tiw = Converter::toCvSE3(Ri, ti);
pKFi->SetPose(Tiw);
}
// Correct points. Transform to "non-optimized" reference keyframe pose and transform back with optimized pose
// 7. 步骤5和步骤6优化得到关键帧的位姿后，MapPoints根据参考帧优化前后的相对关系调整自己的位置
for (size_t i = 0, iend = vpMPs.size(); i < iend; i++)
{
MapPoint *pMP = vpMPs[i];
if (pMP->isBad())
continue;
int nIDr;
KeyFrame *pRefKF = pMP->GetReferenceKeyFrame();
nIDr = pRefKF->mnId;
// 得到MapPoint参考关键帧步骤5优化前的位姿
g2o::Sim3 Srw = vScw[nIDr];
// 得到MapPoint参考关键帧优化后的位姿
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();
}
pMap->IncreaseChangeIndex();
}
参考文献
【SLAM学习笔记】11-ORB_SLAM3关键源码分析⑨ Optimizer（六）地图回环优化_口哨糖youri的博客-CSDN博客
扫一扫
2448
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
