2021SC@SDUSC

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优化相关推荐

【SLAM学习笔记】6-ORB_SLAM3关键源码分析④ Optimizer（一）单帧优化
2021SC@SDUSC 目录 1.前言 2.代码分析 1.前言 Optimizer是非常重要的代码文件!! 这一部分代码量巨大,查阅了很多资料结合来看的代码,将分为以下部分进行分析 1. 单帧优化 ...
【SLAM学习笔记】12-ORB_SLAM3关键源码分析⑩ Optimizer（七）地图融合优化
2021SC@SDUSC 目录 1.前言 2.代码分析 1.前言这一部分代码量巨大,查阅了很多资料结合来看的代码,将分为以下部分进行分析单帧优化局部地图优化全局优化尺度与重力优化 sim3优 ...
【SLAM学习笔记】11-ORB_SLAM3关键源码分析⑨ Optimizer（六）地图回环优化
2021SC@SDUSC 目录 1.前言 2.代码分析 1.前言这一部分代码量巨大,查阅了很多资料结合来看的代码,将分为以下部分进行分析单帧优化局部地图优化全局优化尺度与重力优化 sim3优 ...
Netty学习笔记(一)Netty客户端源码分析
最近在学些BIO,NIO相关的知识,也学习了下Netty和它的源码,做个记录,方便以后继续学习,如果有错误的地方欢迎指正如果不了解BIO,NIO这些基础知识,可以看下我的如下博客 IO中的阻塞.非阻 ...
Android学习笔记-常用的一些源码，防止忘记了
Android学习笔记-常用的一些源码,防止忘记了... 设置拨打电话 StringdialUri="tell:"+m_currentTelNumble; IntentcallIn ...
Android Q 10.1 KeyMaster源码分析(二) - 各家方案的实现
写在之前这两篇文章是我2021年3月初看KeyMaster的笔记,本来打算等分析完KeyMaster和KeyStore以后再一起做成一系列贴出来,后来KeyStore的分析中断了,这一系列的文章就变 ...
Ceph 学习——OSD读写流程与源码分析（一）
消息从客户端发送而来,之前几节介绍了客户端下对象存储.块存储库的实现以及他们在客户端下API请求的发送过程(Ceph学习--Librados与Osdc实现源码解析 . Ceph学习--客户端读写操 ...
kube-scheduler源码分析（五）之 PrioritizeNodes
本文个人博客地址:https://www.huweihuang.com/kubernetes-notes/code-analysis/kube-scheduler/PrioritizeNodes.ht ...
Flume 1.7 源码分析（五）从Channel获取数据写入Sink
Flume 1.7 源码分析(一)源码编译 Flume 1.7 源码分析(二)整体架构 Flume 1.7 源码分析(三)程序入口 Flume 1.7 源码分析(四)从Source写数据到Channe ...
spring源码分析第五天------springAOP核心原理及源码分析
spring源码分析第五天------springAOP核心原理及源码分析 1. 面向切面编程.可以通过预编译方式和运行期动态代理实现在不修改源代码的情况下给程序动态统一添加功能的一种技术切面(A ...

【SLAM学习笔记】10-ORB_SLAM3关键源码分析⑧ Optimizer（五）sim3优化

目录

1.前言

2.代码分析

【SLAM学习笔记】10-ORB_SLAM3关键源码分析⑧ Optimizer（五）sim3优化相关推荐

最新文章

热门文章