1. 前言
该函数实现于 src/Optimizer.cc 文件中，被 src/LoopClosing.cc 文件中的LoopClosing::ComputeSim3() 调用。如果当前关键帧，与某一候选关键帧匹配时，则会计算两帧之间的 Sim3 变换 gScm（候选关键帧到当前帧的Sim3变换）。
获得候选关键帧到当前帧的Sim3变换gScm之后，然后再利用两帧之间的匹配关系，对gScm进行优化，首先看下图：
KF1代表当前关键帧，KF2代表匹配成功的候选关键帧。g2oS12 表示 KF2→KF1 的Sim变换（g2oS12 KF→KF2 的Sim变换）。大家需要注意，闭环线程中调用 LoopClosing::ComputeSim3() 时，还未进行地图点融合。也就是说KF1的关键点KP1与KF1的关键点KP2匹配，但是他们对应的地图点未必相同。
2. 代码分析
LoopClosing::DetectAndReffineSim3FromLastKF使用1投2，2投1这么来只优化g2oS12
int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &vpMatches1, g2o::Sim3 &g2oS12, const float th2, const bool bFixScale, Eigen::Matrix<double, 7, 7> &mAcumHessian, const bool bAllPoints) { // 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); // Camera poses // 2.1 添加Sim3顶点 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 ORB_SLAM3::VertexSim3Expmap *vSim3 = new ORB_SLAM3::VertexSim3Expmap(); vSim3->_fix_scale = bFixScale; vSim3->setEstimate(g2oS12); vSim3->setId(0); vSim3->setFixed(false); vSim3->pCamera1 = pKF1->mpCamera; vSim3->pCamera2 = pKF2->mpCamera; optimizer.addVertex(vSim3); // Set MapPoint vertices // 2.1 添加MP顶点 const int N = vpMatches1.size(); const vector<MapPoint *> vpMapPoints1 = pKF1->GetMapPointMatches(); vector<ORB_SLAM3::EdgeSim3ProjectXYZ *> vpEdges12; // pKF2对应的MapPoints到pKF1的投影 vector<ORB_SLAM3::EdgeInverseSim3ProjectXYZ *> vpEdges21; // pKF1对应的MapPoints到pKF2的投影 vector<size_t> vnIndexEdge; vector<bool> vbIsInKF2; vnIndexEdge.reserve(2 * N); vpEdges12.reserve(2 * N); vpEdges21.reserve(2 * N); vbIsInKF2.reserve(2 * N); const float deltaHuber = sqrt(th2); int nCorrespondences = 0; int nBadMPs = 0; // 没有实际用处，没有输出信息 int nInKF2 = 0; // 输出信息用 int nOutKF2 = 0; // 输出信息用 int nMatchWithoutMP = 0; // 输出信息用 vector<int> vIdsOnlyInKF2; // 2.2 遍历当前关键帧的所有MP点 for (int i = 0; i < N; i++) { if (!vpMatches1[i]) continue; // pMP1和pMP2是匹配的MapPoints，pMP1表示当前帧正常对应的mp，pMP2表示对应的回环的mp MapPoint *pMP1 = vpMapPoints1[i]; MapPoint *pMP2 = vpMatches1[i]; // (1, 2) (3, 4) (5, 6) const int id1 = 2 * i + 1; const int id2 = 2 * (i + 1); // 返回这个点在pKF2关键帧中对应的特征点id const int i2 = get<0>(pMP2->GetIndexInKeyFrame(pKF2)); cv::Mat P3D1c; // 点1在当前关键帧相机坐标系下的坐标 cv::Mat P3D2c; // 点2在候选关键帧相机坐标系下的坐标 if (pMP1 && pMP2) { if (!pMP1->isBad() && !pMP2->isBad()) { // 2.3 添加PointXYZ顶点， 且设为了固定 g2o::VertexSBAPointXYZ *vPoint1 = new g2o::VertexSBAPointXYZ(); cv::Mat P3D1w = pMP1->GetWorldPos(); P3D1c = R1w * P3D1w + t1w; vPoint1->setEstimate(Converter::toVector3d(P3D1c)); // 点1在当前关键帧下的三维点坐标 vPoint1->setId(id1); vPoint1->setFixed(true); optimizer.addVertex(vPoint1); g2o::VertexSBAPointXYZ *vPoint2 = new g2o::VertexSBAPointXYZ(); cv::Mat P3D2w = pMP2->GetWorldPos(); P3D2c = R2w * P3D2w + t2w; vPoint2->setEstimate(Converter::toVector3d(P3D2c)); // 点2在候选关键帧下的三维点坐标 vPoint2->setId(id2); vPoint2->setFixed(true); optimizer.addVertex(vPoint2); } else { nBadMPs++; continue; } } else { nMatchWithoutMP++; // The 3D position in KF1 doesn't exist if (!pMP2->isBad()) { // 执行到这里意味着特征点没有对应的原始MP，却有回环MP，将其投到候选帧里面 g2o::VertexSBAPointXYZ *vPoint2 = new g2o::VertexSBAPointXYZ(); cv::Mat P3D2w = pMP2->GetWorldPos(); P3D2c = R2w * P3D2w + t2w; vPoint2->setEstimate(Converter::toVector3d(P3D2c)); vPoint2->setId(id2); vPoint2->setFixed(true); optimizer.addVertex(vPoint2); vIdsOnlyInKF2.push_back(id2); } continue; } if (i2 < 0 && !bAllPoints) // bAllPoints = true { Verbose::PrintMess(" Remove point -> i2: " + to_string(i2) + "; bAllPoints: " + to_string(bAllPoints), Verbose::VERBOSITY_DEBUG); continue; } if (P3D2c.at<float>(2) < 0) { Verbose::PrintMess("Sim3: Z coordinate is negative", Verbose::VERBOSITY_DEBUG); continue; } nCorrespondences++; // 2.4 添加两个顶点（3D点）到相机投影的边 // Set edge x1 = S12*X2 Eigen::Matrix<double, 2, 1> obs1; const cv::KeyPoint &kpUn1 = pKF1->mvKeysUn[i]; obs1 << kpUn1.pt.x, kpUn1.pt.y; // 这个边的误差计算方式 // 1. 将点2通过g2oS12计算到当前关键帧下 // 2. 点2在当前关键帧下投影到图像上与观测求误差 ORB_SLAM3::EdgeSim3ProjectXYZ *e12 = new ORB_SLAM3::EdgeSim3ProjectXYZ(); e12->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(id2))); // 2相机坐标系下的三维点 e12->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(0))); // g2oS12 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 // 2.5 另一个边 Eigen::Matrix<double, 2, 1> obs2; cv::KeyPoint kpUn2; bool inKF2; // 投之前要确定下这个点的像素坐标 if (i2 >= 0) { kpUn2 = pKF2->mvKeysUn[i2]; obs2 << kpUn2.pt.x, kpUn2.pt.y; inKF2 = true; nInKF2++; // 输出信息，表示在kf2中找到MP2的点数 } else // BUG 如果没找到，使用三维点投影到KF2中，表示并没有特征点与之对应（把这个结果当做obs2是不是会带来一些误差，而且还不通过内参吗？？？，重大bug） { float invz = 1 / P3D2c.at<float>(2); float x = P3D2c.at<float>(0) * invz; float y = P3D2c.at<float>(1) * invz; // float u = pKF2->fx * x + pKF2->cx; // float v = pKF2->fy * y + pKF2->cy; // obs2 << u, v; // kpUn2 = cv::KeyPoint(cv::Point2f(u, v), pMP2->mnTrackScaleLevel); obs2 << x, y; kpUn2 = cv::KeyPoint(cv::Point2f(x, y), pMP2->mnTrackScaleLevel); inKF2 = false; nOutKF2++; } // 1相机坐标系下的三维点经过g2oS12投影到kf2下计算重投影误差 ORB_SLAM3::EdgeInverseSim3ProjectXYZ *e21 = new ORB_SLAM3::EdgeInverseSim3ProjectXYZ(); 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); vbIsInKF2.push_back(inKF2); } // Optimize! // 3. 开始优化 optimizer.initializeOptimization(); optimizer.optimize(5); // Check inliers // 4.剔除一些误差大的边，因为e12与e21对应的是同一个三维点，所以只要有一个误差太大就直接搞掉 // Check inliers // 进行卡方检验，大于阈值的边剔除，同时删除鲁棒核函数 int nBad = 0; int nBadOutKF2 = 0; for (size_t i = 0; i < vpEdges12.size(); i++) { ORB_SLAM3::EdgeSim3ProjectXYZ *e12 = vpEdges12[i]; ORB_SLAM3::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<ORB_SLAM3::EdgeSim3ProjectXYZ *>(NULL); vpEdges21[i] = static_cast<ORB_SLAM3::EdgeInverseSim3ProjectXYZ *>(NULL); nBad++; if (!vbIsInKF2[i]) { nBadOutKF2++; } continue; } // Check if remove the robust adjustment improve the result e12->setRobustKernel(0); e21->setRobustKernel(0); } // 如果有坏点，迭代次数更多 int nMoreIterations; if (nBad > 0) nMoreIterations = 10; else nMoreIterations = 5; if (nCorrespondences - nBad < 10) return 0; // Optimize again only with inliers // 5. 再一次优化 optimizer.initializeOptimization(); optimizer.optimize(nMoreIterations); int nIn = 0; mAcumHessian = Eigen::MatrixXd::Zero(7, 7); // 更新vpMatches1，删除外点，统计内点数量 for (size_t i = 0; i < vpEdges12.size(); i++) { ORB_SLAM3::EdgeSim3ProjectXYZ *e12 = vpEdges12[i]; ORB_SLAM3::EdgeInverseSim3ProjectXYZ *e21 = vpEdges21[i]; if (!e12 || !e21) continue; e12->computeError(); e21->computeError(); if (e12->chi2() > th2 || e21->chi2() > th2) { size_t idx = vnIndexEdge[i]; vpMatches1[idx] = static_cast<MapPoint *>(NULL); } else { nIn++; } } // Recover optimized Sim3、 // 6.得到优化后的结果 g2o::VertexSim3Expmap *vSim3_recov = static_cast<g2o::VertexSim3Expmap *>(optimizer.vertex(0)); g2oS12 = vSim3_recov->estimate(); return nIn; }参考文献
【SLAM学习笔记】10-ORB_SLAM3关键源码分析⑧ Optimizer（五）sim3优化_口哨糖youri的博客-CSDN博客_sim3优化
(01)ORB-SLAM2源码无死角解析-(64) BA优化(g2o)→闭环线程:Optimizer::OptimizeSim3→Sim3变换优化_江南才尽，年少无知！的博客-CSDN博客
