当前位置： 首页 > news >正文

ORB-SLAM3 从理论到代码实现（六）：地图回环优化

news 2026/5/7 5:14:56

1. 前言

LoopClosing.cc这个文件是闭环检测与矫正的代码，其逻辑比较清晰。由于用到了多地图集，所以闭环检测不仅在当前地图中进行，还会在以前的地图中检测。如果是在当前地图中检测到了回环，则进行回环矫正；如果是在以前的地图中检测到了回环，则在回环处进行地图的融合，并矫正融合地图中所有的关键帧位姿和地图点。

LoopClosing流程图

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博客

ORB-SLAM3源码阅读笔记(9)-LoopClosing.cc_My.科研小菜鸡的博客-CSDN博客

http://www.jsqmd.com/news/768106/

相关文章：

  • 3步搞定GitHub中文界面的终极方案
  • 深度解析MDB Tools技术实现：跨平台Access数据库解决方案
  • 构建Excel技能知识库：从函数到Power Query的系统化实战指南
  • 从话题列表到3D点云：用RViz和Python玩转RealSense D435i的ROS数据流
  • 开源RTS游戏移植Godot引擎：架构重构与性能优化实战
  • 魔兽争霸3帧率优化：从卡顿到180帧流畅体验的完整指南
  • 用Arduino和热敏电阻模块DIY一个智能温控风扇（附完整代码与接线图）
  • Nez输入系统完全解析：虚拟按钮、摇杆和触摸输入的完美处理
  • 题库整理工具适合什么题型：从描述里对齐你的题库形态
  • Buck电路电感值、电容值计算
  • C++DFS深度优先搜索全解
  • AI原生安全平台OpenClaw-Security：LLM驱动的智能安全运营实战
  • [引]langchain docs 文档
  • OpenClaw Personas：214个开箱即用AI智能体，构建你的专属数字专家团队
  • RPG Maker Decrypter终极指南：三步解锁加密游戏资源
  • 视频处理前端(VPFE)架构与中断控制机制解析
  • 别再只会用AT指令了！用EC20 4G模块+移远串口助手，5分钟搞定MQTT物联网数据上报
  • 构建企业级.NET代码编辑器：ScintillaNET终极架构解析
  • 西门子PLC数据采集（一）：通过.net采集西门子PLC数据的方法
  • Navicat连不上MySQL？别慌！手把手教你排查2002错误（从服务状态到防火墙）
  • 别再只用默认参数了！mkfs.ext4格式化磁盘时，这几个参数调一调性能提升明显
  • 达梦DMRMAN备份集查看实战：从SHOW命令到XML导出，一份保姆级排查手册
  • Unity Timeline实战：用自定义对话轨道打造电影级游戏过场动画（附完整资源）
  • LinkSwift网盘直链下载助手：免费解锁九大网盘极速下载的终极指南
  • AI浏览器扩展开发实战：构建智能网页内容处理代理
  • 终极指南：C++20类类型非类型模板参数的创新应用
  • OCCT可视化系统揭秘：构建高性能3D图形渲染引擎
  • 2026高速四轴分切机厂家/高速分切机厂家推荐,精研分切技艺,赋能产业升级 - 栗子测评
  • 大语言模型在编程中的效率提升与风险防范
  • 终极Voyager代码统计报告：语言分布与复杂度深度分析