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优化的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。