2021SC@SDUSC

目录

• 1.前言
• 2.代码分析

1.前言

这一部分代码量巨大，查阅了很多资料结合来看的代码，将分为以下部分进行分析

单帧优化

局部地图优化

全局优化

尺度与重力优化

sim3优化

地图回环优化

地图融合优化

下面给出逐步注释分析

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 vertexORB_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表示对应的回环的mpMapPoint *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关键帧中对应的特征点idconst 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 existif (!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*X2Eigen::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))); // g2oS12e12->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 resulte12->setRobustKernel(0);e21->setRobustKernel(0);}// 如果有坏点，迭代次数更多int nMoreIterations;if (nBad > 0)nMoreIterations = 10;elsenMoreIterations = 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优化的全部内容，希望文章能够帮你解决所遇到的问题。

如果觉得生活随笔网站内容还不错，欢迎将生活随笔推荐给好友。