ORB-SLAM3 从理论到代码实现（五）：sim3 优化

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投22投1这么来只优化g2oS12int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vectorMapPoint * vpMatches1, g2o::Sim3 g2oS12, const float th2, const bool bFixScale, Eigen::Matrixdouble, 7, 7 mAcumHessian, const bool bAllPoints) { // 1. 初始化g2o优化器 // 先构造求解器 g2o::SparseOptimizer optimizer; // 构造线性方程求解器Hx -b的求解器 g2o::BlockSolverX::LinearSolverType *linearSolver; // 使用dense的求解器常见非dense求解器有cholmod线性求解器和shur补线性求解器 linearSolver new g2o::LinearSolverDenseg2o::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 vectorMapPoint * vpMapPoints1 pKF1-GetMapPointMatches(); vectorORB_SLAM3::EdgeSim3ProjectXYZ * vpEdges12; // pKF2对应的MapPoints到pKF1的投影 vectorORB_SLAM3::EdgeInverseSim3ProjectXYZ * vpEdges21; // pKF1对应的MapPoints到pKF2的投影 vectorsize_t vnIndexEdge; vectorbool 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; // 输出信息用 vectorint vIdsOnlyInKF2; // 2.2 遍历当前关键帧的所有MP点 for (int i 0; i N; i) { if (!vpMatches1[i]) continue; // pMP1和pMP2是匹配的MapPointspMP1表示当前帧正常对应的mppMP2表示对应的回环的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 get0(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 doesnt 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.atfloat(2) 0) { Verbose::PrintMess(Sim3: Z coordinate is negative, Verbose::VERBOSITY_DEBUG); continue; } nCorrespondences; // 2.4 添加两个顶点3D点到相机投影的边 // Set edge x1 S12*X2 Eigen::Matrixdouble, 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_castg2o::OptimizableGraph::Vertex *(optimizer.vertex(id2))); // 2相机坐标系下的三维点 e12-setVertex(1, dynamic_castg2o::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::Matrixdouble, 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.atfloat(2); float x P3D2c.atfloat(0) * invz; float y P3D2c.atfloat(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_castg2o::OptimizableGraph::Vertex *(optimizer.vertex(id1))); e21-setVertex(1, dynamic_castg2o::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_castMapPoint *(NULL); optimizer.removeEdge(e12); optimizer.removeEdge(e21); vpEdges12[i] static_castORB_SLAM3::EdgeSim3ProjectXYZ *(NULL); vpEdges21[i] static_castORB_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_castMapPoint *(NULL); } else { nIn; } } // Recover optimized Sim3、 // 6.得到优化后的结果 g2o::VertexSim3Expmap *vSim3_recov static_castg2o::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博客