2021SC@SDUSC

1.前言

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

单帧优化
局部地图优化
全局优化
尺度与重力优化
sim3优化
地图回环优化
地图融合优化

下面给出逐步注释分析

2.代码分析

Local Bundle Adjustment LoopClosing::MergeLocal() 融合地图时使用，纯视觉
优化目标：

vpAdjustKF
2.vpAdjustKF与vpFixedKF对应的MP点

优化所有的当前关键帧共视窗口里的关键帧和地图点, 固定所有融合帧共视窗口里的帧

在这里插入代码片void Optimizer::LocalBundleAdjustment(KeyFrame *pMainKF, vector<KeyFrame *> vpAdjustKF, vector<KeyFrame *> vpFixedKF, bool *pbStopFlag)
{bool bShowImages = false;// 1. 构建g2o优化器g2o::SparseOptimizer optimizer;g2o::BlockSolver_6_3::LinearSolverType *linearSolver;linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();g2o::BlockSolver_6_3 *solver_ptr = new g2o::BlockSolver_6_3(linearSolver);g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);optimizer.setAlgorithm(solver);optimizer.setVerbose(false);if (pbStopFlag)optimizer.setForceStopFlag(pbStopFlag);long unsigned int maxKFid = 0;set<KeyFrame *> spKeyFrameBA; // 存放关键帧，包含固定的与不固定的vector<MapPoint *> vpMPs;Map *pCurrentMap = pMainKF->GetMap();// Set fixed KeyFrame vertices// 2. 构建固定关键帧的节点，并储存对应的MPfor (KeyFrame *pKFi : vpFixedKF){if (pKFi->isBad() || pKFi->GetMap() != pCurrentMap){Verbose::PrintMess("ERROR LBA: KF is bad or is not in the current map", Verbose::VERBOSITY_NORMAL);continue;}pKFi->mnBALocalForMerge = pMainKF->mnId; // 防止重复添加g2o::VertexSE3Expmap *vSE3 = new g2o::VertexSE3Expmap();vSE3->setEstimate(Converter::toSE3Quat(pKFi->GetPose()));vSE3->setId(pKFi->mnId);vSE3->setFixed(true);optimizer.addVertex(vSE3);if (pKFi->mnId > maxKFid)maxKFid = pKFi->mnId;set<MapPoint *> spViewMPs = pKFi->GetMapPoints();for (MapPoint *pMPi : spViewMPs){if (pMPi)if (!pMPi->isBad() && pMPi->GetMap() == pCurrentMap)if (pMPi->mnBALocalForMerge != pMainKF->mnId){vpMPs.push_back(pMPi);pMPi->mnBALocalForMerge = pMainKF->mnId;}}spKeyFrameBA.insert(pKFi);}// Set non fixed Keyframe vertices// 3. 构建不固定关键帧的节点，并储存对应的MPset<KeyFrame *> spAdjustKF(vpAdjustKF.begin(), vpAdjustKF.end());for (KeyFrame *pKFi : vpAdjustKF){if (pKFi->isBad() || pKFi->GetMap() != pCurrentMap)continue;pKFi->mnBALocalForKF = pMainKF->mnId; // 防止重复添加g2o::VertexSE3Expmap *vSE3 = new g2o::VertexSE3Expmap();vSE3->setEstimate(Converter::toSE3Quat(pKFi->GetPose()));vSE3->setId(pKFi->mnId);optimizer.addVertex(vSE3);if (pKFi->mnId > maxKFid)maxKFid = pKFi->mnId;set<MapPoint *> spViewMPs = pKFi->GetMapPoints();for (MapPoint *pMPi : spViewMPs){if (pMPi){if (!pMPi->isBad() && pMPi->GetMap() == pCurrentMap){if (pMPi->mnBALocalForMerge != pMainKF->mnId){vpMPs.push_back(pMPi);pMPi->mnBALocalForMerge = pMainKF->mnId;}}}}spKeyFrameBA.insert(pKFi);}// 准备存放边的vectorconst int nExpectedSize = (vpAdjustKF.size() + vpFixedKF.size()) * vpMPs.size();vector<ORB_SLAM3::EdgeSE3ProjectXYZ *> vpEdgesMono;vpEdgesMono.reserve(nExpectedSize);vector<KeyFrame *> vpEdgeKFMono;vpEdgeKFMono.reserve(nExpectedSize);vector<MapPoint *> vpMapPointEdgeMono;vpMapPointEdgeMono.reserve(nExpectedSize);vector<g2o::EdgeStereoSE3ProjectXYZ *> vpEdgesStereo;vpEdgesStereo.reserve(nExpectedSize);vector<KeyFrame *> vpEdgeKFStereo;vpEdgeKFStereo.reserve(nExpectedSize);vector<MapPoint *> vpMapPointEdgeStereo;vpMapPointEdgeStereo.reserve(nExpectedSize);const float thHuber2D = sqrt(5.99);const float thHuber3D = sqrt(7.815);// Set MapPoint verticesmap<KeyFrame *, int> mpObsKFs;      // 统计每个关键帧对应的MP点数，调试输出用map<KeyFrame *, int> mpObsFinalKFs; // 统计每个MP对应的关键帧数，调试输出用map<MapPoint *, int> mpObsMPs;      // 统计每个MP被观测的图片数，双目就是两个，调试输出用// 4. 确定MP节点与边的连接for (unsigned int i = 0; i < vpMPs.size(); ++i){MapPoint *pMPi = vpMPs[i];if (pMPi->isBad())continue;g2o::VertexSBAPointXYZ *vPoint = new g2o::VertexSBAPointXYZ();vPoint->setEstimate(Converter::toVector3d(pMPi->GetWorldPos()));const int id = pMPi->mnId + maxKFid + 1;vPoint->setId(id);vPoint->setMarginalized(true);optimizer.addVertex(vPoint);const map<KeyFrame *, tuple<int, int>> observations = pMPi->GetObservations();int nEdges = 0;// SET EDGESfor (map<KeyFrame *, tuple<int, int>>::const_iterator mit = observations.begin(); mit != observations.end(); mit++){KeyFrame *pKF = mit->first;// 跳过的条件// 1. 帧坏了// 2. 帧靠后// 3. 不在参与优化帧的里面// 4. 在左相机上不存在这个三维点if (pKF->isBad() || pKF->mnId > maxKFid || pKF->mnBALocalForMerge != pMainKF->mnId || !pKF->GetMapPoint(get<0>(mit->second)))continue;nEdges++;const cv::KeyPoint &kpUn = pKF->mvKeysUn[get<0>(mit->second)];// 注意！！！这里没有考虑相机2if (pKF->mvuRight[get<0>(mit->second)] < 0) // Monocular{mpObsMPs[pMPi]++;Eigen::Matrix<double, 2, 1> obs;obs << kpUn.pt.x, kpUn.pt.y;ORB_SLAM3::EdgeSE3ProjectXYZ *e = new ORB_SLAM3::EdgeSE3ProjectXYZ();e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(id)));e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(pKF->mnId)));e->setMeasurement(obs);const float &invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave];e->setInformation(Eigen::Matrix2d::Identity() * invSigma2);g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber;e->setRobustKernel(rk);rk->setDelta(thHuber2D);e->pCamera = pKF->mpCamera;optimizer.addEdge(e);vpEdgesMono.push_back(e);vpEdgeKFMono.push_back(pKF);vpMapPointEdgeMono.push_back(pMPi);mpObsKFs[pKF]++;}else // RGBD or Stereo{mpObsMPs[pMPi] += 2;Eigen::Matrix<double, 3, 1> obs;const float kp_ur = pKF->mvuRight[get<0>(mit->second)];obs << kpUn.pt.x, kpUn.pt.y, kp_ur;g2o::EdgeStereoSE3ProjectXYZ *e = new g2o::EdgeStereoSE3ProjectXYZ();e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(id)));e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(pKF->mnId)));e->setMeasurement(obs);const float &invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave];Eigen::Matrix3d Info = Eigen::Matrix3d::Identity() * invSigma2;e->setInformation(Info);g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber;e->setRobustKernel(rk);rk->setDelta(thHuber3D);e->fx = pKF->fx;e->fy = pKF->fy;e->cx = pKF->cx;e->cy = pKF->cy;e->bf = pKF->mbf;optimizer.addEdge(e);vpEdgesStereo.push_back(e);vpEdgeKFStereo.push_back(pKF);vpMapPointEdgeStereo.push_back(pMPi);mpObsKFs[pKF]++;}}}// 这段没啥用，调试输出的，暂时不看map<int, int> mStatsObs;for (map<MapPoint *, int>::iterator it = mpObsMPs.begin(); it != mpObsMPs.end(); ++it){MapPoint *pMPi = it->first;int numObs = it->second;mStatsObs[numObs]++;}if (pbStopFlag)if (*pbStopFlag)return;// 5. 优化optimizer.initializeOptimization();optimizer.optimize(5);bool bDoMore = true;if (pbStopFlag)if (*pbStopFlag)bDoMore = false;// 6. 剔除误差大的边map<unsigned long int, int> mWrongObsKF;if (bDoMore){// Check inlier observationsint badMonoMP = 0, badStereoMP = 0;for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++){ORB_SLAM3::EdgeSE3ProjectXYZ *e = vpEdgesMono[i];MapPoint *pMP = vpMapPointEdgeMono[i];if (pMP->isBad())continue;if (e->chi2() > 5.991 || !e->isDepthPositive()){e->setLevel(1);badMonoMP++;}e->setRobustKernel(0);}for (size_t i = 0, iend = vpEdgesStereo.size(); i < iend; i++){g2o::EdgeStereoSE3ProjectXYZ *e = vpEdgesStereo[i];MapPoint *pMP = vpMapPointEdgeStereo[i];if (pMP->isBad())continue;if (e->chi2() > 7.815 || !e->isDepthPositive()){e->setLevel(1);badStereoMP++;}e->setRobustKernel(0);}Verbose::PrintMess("LBA: First optimization, there are " + to_string(badMonoMP) + " monocular and " + to_string(badStereoMP) + " sterero bad edges", Verbose::VERBOSITY_DEBUG);// Optimize again without the outliersoptimizer.initializeOptimization(0);optimizer.optimize(10);}// 7. 删除误差大的点与帧的连接关系vector<pair<KeyFrame *, MapPoint *>> vToErase;vToErase.reserve(vpEdgesMono.size() + vpEdgesStereo.size());set<MapPoint *> spErasedMPs;set<KeyFrame *> spErasedKFs;// Check inlier observationsint badMonoMP = 0, badStereoMP = 0;for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++){ORB_SLAM3::EdgeSE3ProjectXYZ *e = vpEdgesMono[i];MapPoint *pMP = vpMapPointEdgeMono[i];if (pMP->isBad())continue;if (e->chi2() > 5.991 || !e->isDepthPositive()){KeyFrame *pKFi = vpEdgeKFMono[i];vToErase.push_back(make_pair(pKFi, pMP));mWrongObsKF[pKFi->mnId]++;badMonoMP++;spErasedMPs.insert(pMP);spErasedKFs.insert(pKFi);}}for (size_t i = 0, iend = vpEdgesStereo.size(); i < iend; i++){g2o::EdgeStereoSE3ProjectXYZ *e = vpEdgesStereo[i];MapPoint *pMP = vpMapPointEdgeStereo[i];if (pMP->isBad())continue;if (e->chi2() > 7.815 || !e->isDepthPositive()){KeyFrame *pKFi = vpEdgeKFStereo[i];vToErase.push_back(make_pair(pKFi, pMP));mWrongObsKF[pKFi->mnId]++;badStereoMP++;spErasedMPs.insert(pMP);spErasedKFs.insert(pKFi);}}Verbose::PrintMess("LBA: Second optimization, there are " + to_string(badMonoMP) + " monocular and " + to_string(badStereoMP) + " sterero bad edges", Verbose::VERBOSITY_DEBUG);// Get Map Mutexunique_lock<mutex> lock(pMainKF->GetMap()->mMutexMapUpdate);if (!vToErase.empty()){map<KeyFrame *, int> mpMPs_in_KF;for (KeyFrame *pKFi : spErasedKFs){int num_MPs = pKFi->GetMapPoints().size();mpMPs_in_KF[pKFi] = num_MPs;}// 剔除链接关系，其他均为调试Verbose::PrintMess("LBA: There are " + to_string(vToErase.size()) + " observations whose will be deleted from the map", Verbose::VERBOSITY_DEBUG);for (size_t i = 0; i < vToErase.size(); i++){KeyFrame *pKFi = vToErase[i].first;MapPoint *pMPi = vToErase[i].second;pKFi->EraseMapPointMatch(pMPi);pMPi->EraseObservation(pKFi);}Verbose::PrintMess("LBA: " + to_string(spErasedMPs.size()) + " MPs had deleted observations", Verbose::VERBOSITY_DEBUG);Verbose::PrintMess("LBA: Current map is " + to_string(pMainKF->GetMap()->GetId()), Verbose::VERBOSITY_DEBUG);int numErasedMP = 0;for (MapPoint *pMPi : spErasedMPs){if (pMPi->isBad()){Verbose::PrintMess("LBA: MP " + to_string(pMPi->mnId) + " has lost almost all the observations, its origin map is " + to_string(pMPi->mnOriginMapId), Verbose::VERBOSITY_DEBUG);numErasedMP++;}}Verbose::PrintMess("LBA: " + to_string(numErasedMP) + " MPs had deleted from the map", Verbose::VERBOSITY_DEBUG);for (KeyFrame *pKFi : spErasedKFs){int num_MPs = pKFi->GetMapPoints().size();int num_init_MPs = mpMPs_in_KF[pKFi];Verbose::PrintMess("LBA: Initially KF " + to_string(pKFi->mnId) + " had " + to_string(num_init_MPs) + ", at the end has " + to_string(num_MPs), Verbose::VERBOSITY_DEBUG);}}// 没用，调试的for (unsigned int i = 0; i < vpMPs.size(); ++i){MapPoint *pMPi = vpMPs[i];if (pMPi->isBad())continue;const map<KeyFrame *, tuple<int, int>> observations = pMPi->GetObservations();for (map<KeyFrame *, tuple<int, int>>::const_iterator mit = observations.begin(); mit != observations.end(); mit++){KeyFrame *pKF = mit->first;if (pKF->isBad() || pKF->mnId > maxKFid || pKF->mnBALocalForKF != pMainKF->mnId || !pKF->GetMapPoint(get<0>(mit->second)))continue;const cv::KeyPoint &kpUn = pKF->mvKeysUn[get<0>(mit->second)];if (pKF->mvuRight[get<0>(mit->second)] < 0) // Monocular{mpObsFinalKFs[pKF]++;}else // RGBD or Stereo{mpObsFinalKFs[pKF]++;}}}// Recover optimized data// 8. 取出结果// Keyframesfor (KeyFrame *pKFi : vpAdjustKF){if (pKFi->isBad())continue;// 8.1 取出对应位姿g2o::VertexSE3Expmap *vSE3 = static_cast<g2o::VertexSE3Expmap *>(optimizer.vertex(pKFi->mnId));g2o::SE3Quat SE3quat = vSE3->estimate();cv::Mat Tiw = Converter::toCvMat(SE3quat);// 统计调试用int numMonoBadPoints = 0, numMonoOptPoints = 0;int numStereoBadPoints = 0, numStereoOptPoints = 0;vector<MapPoint *> vpMonoMPsOpt, vpStereoMPsOpt; // 存放mp内点vector<MapPoint *> vpMonoMPsBad, vpStereoMPsBad; // 存放mp外点// 8.2 卡方检验，调试用的貌似没别的作用for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++){ORB_SLAM3::EdgeSE3ProjectXYZ *e = vpEdgesMono[i];MapPoint *pMP = vpMapPointEdgeMono[i];KeyFrame *pKFedge = vpEdgeKFMono[i];if (pKFi != pKFedge){continue;}if (pMP->isBad())continue;if (e->chi2() > 5.991 || !e->isDepthPositive()){numMonoBadPoints++;vpMonoMPsBad.push_back(pMP);}else{numMonoOptPoints++;vpMonoMPsOpt.push_back(pMP);}}for (size_t i = 0, iend = vpEdgesStereo.size(); i < iend; i++){g2o::EdgeStereoSE3ProjectXYZ *e = vpEdgesStereo[i];MapPoint *pMP = vpMapPointEdgeStereo[i];KeyFrame *pKFedge = vpEdgeKFMono[i];if (pKFi != pKFedge){continue;}if (pMP->isBad())continue;if (e->chi2() > 7.815 || !e->isDepthPositive()){numStereoBadPoints++;vpStereoMPsBad.push_back(pMP);}else{numStereoOptPoints++;vpStereoMPsOpt.push_back(pMP);}}if (numMonoOptPoints + numStereoOptPoints < 50){Verbose::PrintMess("LBA ERROR: KF " + to_string(pKFi->mnId) + " has only " + to_string(numMonoOptPoints) + " monocular and " + to_string(numStereoOptPoints) + " stereo points", Verbose::VERBOSITY_DEBUG);}// 设置位姿pKFi->SetPose(Tiw);}// 更新点的坐标for (MapPoint *pMPi : vpMPs){if (pMPi->isBad())continue;g2o::VertexSBAPointXYZ *vPoint = static_cast<g2o::VertexSBAPointXYZ *>(optimizer.vertex(pMPi->mnId + maxKFid + 1));pMPi->SetWorldPos(Converter::toCvMat(vPoint->estimate()));pMPi->UpdateNormalAndDepth();}
}

LoopClosing::MergeLocal() 融合地图时使用，优化当前帧没有参与融合的元素，本质图优化
优化目标：

vpNonFixedKFs
2.vpNonCorrectedMPs

void Optimizer::OptimizeEssentialGraph(KeyFrame *pCurKF, vector<KeyFrame *> &vpFixedKFs, vector<KeyFrame *> &vpFixedCorrectedKFs,vector<KeyFrame *> &vpNonFixedKFs, vector<MapPoint *> &vpNonCorrectedMPs)
{// 1. 优化器构建g2o::SparseOptimizer optimizer;optimizer.setVerbose(false);g2o::BlockSolver_7_3::LinearSolverType *linearSolver =new g2o::LinearSolverEigen<g2o::BlockSolver_7_3::PoseMatrixType>();g2o::BlockSolver_7_3 *solver_ptr = new g2o::BlockSolver_7_3(linearSolver);g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);solver->setUserLambdaInit(1e-16);optimizer.setAlgorithm(solver);Map *pMap = pCurKF->GetMap();const unsigned int nMaxKFid = pMap->GetMaxKFid();vector<g2o::Sim3, Eigen::aligned_allocator<g2o::Sim3>> vScw(nMaxKFid + 1);          // 存放每一帧优化前的sim3vector<g2o::Sim3, Eigen::aligned_allocator<g2o::Sim3>> vCorrectedSwc(nMaxKFid + 1); // 存放每一帧优化后的sim3，调试输出用，没有实际作用vector<g2o::VertexSim3Expmap *> vpVertices(nMaxKFid + 1);                           // 存放节点，没用，还占地方const int minFeat = 100; // pKFi->GetCovisiblesByWeight(minFeat);  essentialgraph 阈值就是共视大于100// 2. 确定固定关键帧的节点// vpMergeConnectedKFs 融合地图中的关键帧for (KeyFrame *pKFi : vpFixedKFs){if (pKFi->isBad())continue;g2o::VertexSim3Expmap *VSim3 = new g2o::VertexSim3Expmap();const int nIDi = pKFi->mnId;Eigen::Matrix<double, 3, 3> Rcw = Converter::toMatrix3d(pKFi->GetRotation());Eigen::Matrix<double, 3, 1> tcw = Converter::toVector3d(pKFi->GetTranslation());g2o::Sim3 Siw(Rcw, tcw, 1.0);vScw[nIDi] = Siw;vCorrectedSwc[nIDi] = Siw.inverse(); // This KFs mustn't be corrected 无实际作用VSim3->setEstimate(Siw);VSim3->setFixed(true);VSim3->setId(nIDi);VSim3->setMarginalized(false);VSim3->_fix_scale = true;optimizer.addVertex(VSim3);vpVertices[nIDi] = VSim3;}Verbose::PrintMess("Opt_Essential: vpFixedKFs loaded", Verbose::VERBOSITY_DEBUG);set<unsigned long> sIdKF;// vpLocalCurrentWindowKFs 当前地图中经过矫正的关键帧for (KeyFrame *pKFi : vpFixedCorrectedKFs){if (pKFi->isBad())continue;g2o::VertexSim3Expmap *VSim3 = new g2o::VertexSim3Expmap();const int nIDi = pKFi->mnId;Eigen::Matrix<double, 3, 3> Rcw = Converter::toMatrix3d(pKFi->GetRotation());Eigen::Matrix<double, 3, 1> tcw = Converter::toVector3d(pKFi->GetTranslation());g2o::Sim3 Siw(Rcw, tcw, 1.0);vCorrectedSwc[nIDi] = Siw.inverse(); // This KFs mustn't be corrected 无实际作用VSim3->setEstimate(Siw);cv::Mat Tcw_bef = pKFi->mTcwBefMerge;Eigen::Matrix<double, 3, 3> Rcw_bef = Converter::toMatrix3d(Tcw_bef.rowRange(0, 3).colRange(0, 3));Eigen::Matrix<double, 3, 1> tcw_bef = Converter::toVector3d(Tcw_bef.rowRange(0, 3).col(3));vScw[nIDi] = g2o::Sim3(Rcw_bef, tcw_bef, 1.0);VSim3->setFixed(true);VSim3->setId(nIDi);VSim3->setMarginalized(false);optimizer.addVertex(VSim3);vpVertices[nIDi] = VSim3;sIdKF.insert(nIDi);}Verbose::PrintMess("Opt_Essential: vpFixedCorrectedKFs loaded", Verbose::VERBOSITY_DEBUG);// 3. 确定待优化的关键帧节点for (KeyFrame *pKFi : vpNonFixedKFs){if (pKFi->isBad())continue;const int nIDi = pKFi->mnId;if (sIdKF.count(nIDi)) // It has already added in the corrected merge KFscontinue;g2o::VertexSim3Expmap *VSim3 = new g2o::VertexSim3Expmap();Eigen::Matrix<double, 3, 3> Rcw = Converter::toMatrix3d(pKFi->GetRotation());Eigen::Matrix<double, 3, 1> tcw = Converter::toVector3d(pKFi->GetTranslation());g2o::Sim3 Siw(Rcw, tcw, 1.0);vScw[nIDi] = Siw;VSim3->setEstimate(Siw);VSim3->setFixed(false);VSim3->setId(nIDi);VSim3->setMarginalized(false);optimizer.addVertex(VSim3);vpVertices[nIDi] = VSim3;sIdKF.insert(nIDi);}Verbose::PrintMess("Opt_Essential: vpNonFixedKFs loaded", Verbose::VERBOSITY_DEBUG);vector<KeyFrame *> vpKFs;vpKFs.reserve(vpFixedKFs.size() + vpFixedCorrectedKFs.size() + vpNonFixedKFs.size());vpKFs.insert(vpKFs.end(), vpFixedKFs.begin(), vpFixedKFs.end());vpKFs.insert(vpKFs.end(), vpFixedCorrectedKFs.begin(), vpFixedCorrectedKFs.end());vpKFs.insert(vpKFs.end(), vpNonFixedKFs.begin(), vpNonFixedKFs.end());set<KeyFrame *> spKFs(vpKFs.begin(), vpKFs.end());Verbose::PrintMess("Opt_Essential: List of KF loaded", Verbose::VERBOSITY_DEBUG);const Eigen::Matrix<double, 7, 7> matLambda = Eigen::Matrix<double, 7, 7>::Identity();// 4. 遍历所有帧for (KeyFrame *pKFi : vpKFs){int num_connections = 0; // 统计与pKFi连接的数量const int nIDi = pKFi->mnId;g2o::Sim3 Swi = vScw[nIDi].inverse();KeyFrame *pParentKFi = pKFi->GetParent();// Spanning tree edge// 4.1 找到pKFi的父帧且在这批关键帧里面，添加与其关联的sim3边，相对约束if (pParentKFi && spKFs.find(pParentKFi) != spKFs.end()){int nIDj = pParentKFi->mnId;g2o::Sim3 Sjw = vScw[nIDj];g2o::Sim3 Sji = Sjw * Swi;g2o::EdgeSim3 *e = new g2o::EdgeSim3();e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(nIDj)));e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(nIDi)));e->setMeasurement(Sji);e->information() = matLambda;optimizer.addEdge(e);num_connections++;}// Loop edges// 4.2 添加回环的边const set<KeyFrame *> sLoopEdges = pKFi->GetLoopEdges();for (set<KeyFrame *>::const_iterator sit = sLoopEdges.begin(), send = sLoopEdges.end(); sit != send; sit++){KeyFrame *pLKF = *sit;if (spKFs.find(pLKF) != spKFs.end() && pLKF->mnId < pKFi->mnId){g2o::Sim3 Slw = vScw[pLKF->mnId];g2o::Sim3 Sli = Slw * Swi;g2o::EdgeSim3 *el = new g2o::EdgeSim3();el->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(pLKF->mnId)));el->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(nIDi)));el->setMeasurement(Sli);el->information() = matLambda;optimizer.addEdge(el);num_connections++;}}// Covisibility graph edges// 4.3 建立essentialgraph（共视边）const vector<KeyFrame *> vpConnectedKFs = pKFi->GetCovisiblesByWeight(minFeat);for (vector<KeyFrame *>::const_iterator vit = vpConnectedKFs.begin(); vit != vpConnectedKFs.end(); vit++){KeyFrame *pKFn = *vit;// 1.这个帧存在且不是pKFi的父帧，防止重复添加// 2.pKFn不为pKFi的子帧// 3.pKFn不为回环帧// 4.pKFn要在这批关键帧里面// 防止重复添加if (pKFn && pKFn != pParentKFi && !pKFi->hasChild(pKFn) && !sLoopEdges.count(pKFn) && spKFs.find(pKFn) != spKFs.end()){if (!pKFn->isBad() && pKFn->mnId < pKFi->mnId){g2o::Sim3 Snw = vScw[pKFn->mnId];g2o::Sim3 Sni = Snw * Swi;g2o::EdgeSim3 *en = new g2o::EdgeSim3();en->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(pKFn->mnId)));en->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(nIDi)));en->setMeasurement(Sni);en->information() = matLambda;optimizer.addEdge(en);num_connections++;}}}}// Optimize!// 5. 开始优化optimizer.initializeOptimization();optimizer.optimize(20);unique_lock<mutex> lock(pMap->mMutexMapUpdate);// SE3 Pose Recovering. Sim3:[sR t;0 1] -> SE3:[R t/s;0 1]// 6. 取出结果for (KeyFrame *pKFi : vpNonFixedKFs){if (pKFi->isBad())continue;const int nIDi = pKFi->mnId;g2o::VertexSim3Expmap *VSim3 = static_cast<g2o::VertexSim3Expmap *>(optimizer.vertex(nIDi));g2o::Sim3 CorrectedSiw = VSim3->estimate();vCorrectedSwc[nIDi] = CorrectedSiw.inverse();Eigen::Matrix3d eigR = CorrectedSiw.rotation().toRotationMatrix();Eigen::Vector3d eigt = CorrectedSiw.translation();double s = CorrectedSiw.scale();eigt *= (1. / s); //[R t/s;0 1]cv::Mat Tiw = Converter::toCvSE3(eigR, eigt);pKFi->mTcwBefMerge = pKFi->GetPose();pKFi->mTwcBefMerge = pKFi->GetPoseInverse();pKFi->SetPose(Tiw);}// Correct points. Transform to "non-optimized" reference keyframe pose and transform back with optimized posefor (MapPoint *pMPi : vpNonCorrectedMPs){if (pMPi->isBad())continue;KeyFrame *pRefKF = pMPi->GetReferenceKeyFrame();g2o::Sim3 Srw;g2o::Sim3 correctedSwr;while (pRefKF->isBad()){if (!pRefKF){Verbose::PrintMess("MP " + to_string(pMPi->mnId) + " without a valid reference KF", Verbose::VERBOSITY_DEBUG);break;}pMPi->EraseObservation(pRefKF);pRefKF = pMPi->GetReferenceKeyFrame();}// 流程就是转到相机坐标系，在用新的rt转到世界cv::Mat TNonCorrectedwr = pRefKF->mTwcBefMerge;Eigen::Matrix<double, 3, 3> RNonCorrectedwr = Converter::toMatrix3d(TNonCorrectedwr.rowRange(0, 3).colRange(0, 3));Eigen::Matrix<double, 3, 1> tNonCorrectedwr = Converter::toVector3d(TNonCorrectedwr.rowRange(0, 3).col(3));Srw = g2o::Sim3(RNonCorrectedwr, tNonCorrectedwr, 1.0).inverse();cv::Mat Twr = pRefKF->GetPoseInverse();Eigen::Matrix<double, 3, 3> Rwr = Converter::toMatrix3d(Twr.rowRange(0, 3).colRange(0, 3));Eigen::Matrix<double, 3, 1> twr = Converter::toVector3d(Twr.rowRange(0, 3).col(3));correctedSwr = g2o::Sim3(Rwr, twr, 1.0);cv::Mat P3Dw = pMPi->GetWorldPos();Eigen::Matrix<double, 3, 1> eigP3Dw = Converter::toVector3d(P3Dw);Eigen::Matrix<double, 3, 1> eigCorrectedP3Dw = correctedSwr.map(Srw.map(eigP3Dw));cv::Mat cvCorrectedP3Dw = Converter::toCvMat(eigCorrectedP3Dw);pMPi->SetWorldPos(cvCorrectedP3Dw);pMPi->UpdateNormalAndDepth();}
}

这里面进行visual inertial ba
LoopClosing::MergeLocal2 中用到
优化目标：

相关帧的位姿，速度，偏置，还有涉及点的坐标，可以理解为跨地图的局部窗口优化

void Optimizer::MergeInertialBA(KeyFrame *pCurrKF, KeyFrame *pMergeKF, bool *pbStopFlag, Map *pMap, LoopClosing::KeyFrameAndPose &corrPoses)
{const int Nd = 6;const unsigned long maxKFid = pCurrKF->mnId;// 1. 准备所有被优化的关键帧, 完全固定的帧// 被优化的帧, 当前帧和融合匹配帧加起来一共12个vector<KeyFrame *> vpOptimizableKFs;vpOptimizableKFs.reserve(2 * Nd);// For cov KFS, inertial parameters are not optimized// 共视帧, 只优化位姿，不优化速度与偏置const int maxCovKF = 15;vector<KeyFrame *> vpOptimizableCovKFs;vpOptimizableCovKFs.reserve(2 * maxCovKF);// Add sliding window for current KF// 当前关键帧先加到容器中vpOptimizableKFs.push_back(pCurrKF);// 后面用这个变量避免重复pCurrKF->mnBALocalForKF = pCurrKF->mnId;// 添加5个与当前关键帧相连的时序上相连的关键帧进容器// 1.1 一直放一直放，一直放到没有上一个关键帧，这里面包含了pCurrKF最近的6帧，从晚到早排列，不过有可能存不满for (int i = 1; i < Nd; i++){if (vpOptimizableKFs.back()->mPrevKF){// 用mPrevKF访问时序上的上一个关键帧vpOptimizableKFs.push_back(vpOptimizableKFs.back()->mPrevKF);vpOptimizableKFs.back()->mnBALocalForKF = pCurrKF->mnId;}elsebreak;}// 1.2 如果 vpOptimizableKFs 中最早的一帧前面还有，往不更新惯导参数的序列中添加// 否则把最后一个取出来放到不更新惯导参数的序列中if (vpOptimizableKFs.back()->mPrevKF){vpOptimizableCovKFs.push_back(vpOptimizableKFs.back()->mPrevKF);vpOptimizableKFs.back()->mPrevKF->mnBALocalForKF = pCurrKF->mnId;}else{// 如果没找到时序相连的帧,把被优化的窗口缩减一个给到固定的变量vpOptimizableCovKFs.push_back(vpOptimizableKFs.back());vpOptimizableKFs.pop_back();}// 没用到KeyFrame *pKF0 = vpOptimizableCovKFs.back();cv::Mat Twc0 = pKF0->GetPoseInverse();// Add temporal neighbours to merge KF (previous and next KFs)// 2.  同样, 对于融合帧也把它和时序上的几个邻居加到可优化的容器里// 2.1 把匹配的融合关键帧也放进来准备一起优化vpOptimizableKFs.push_back(pMergeKF);pMergeKF->mnBALocalForKF = pCurrKF->mnId;// Previous KFs// 把融合帧时序上的邻居添加到可优化的容器里, 因为融合帧左右都有时序上的邻居,所以这里先取一半 Nd/2// 2.2 再放进来3个pMergeKF之前的关键帧，有可能放不满for (int i = 1; i < (Nd / 2); i++){if (vpOptimizableKFs.back()->mPrevKF){vpOptimizableKFs.push_back(vpOptimizableKFs.back()->mPrevKF);vpOptimizableKFs.back()->mnBALocalForKF = pCurrKF->mnId;}elsebreak;}// We fix just once the old map// 记录与融合帧窗口时序上紧邻的帧为完全固定的帧// 2.3 类似于上面的做法如果有前一个关键帧放入lFixedKeyFrames，否则从vpOptimizableKFs取出，注意这里防止重复添加的标识又多了一个变量// 记录完全固定的帧list<KeyFrame *> lFixedKeyFrames;if (vpOptimizableKFs.back()->mPrevKF){lFixedKeyFrames.push_back(vpOptimizableKFs.back()->mPrevKF);vpOptimizableKFs.back()->mPrevKF->mnBAFixedForKF = pCurrKF->mnId;}// 如果没找到,则从窗口内拿出一帧给到完全固定的帧else{vpOptimizableKFs.back()->mnBALocalForKF = 0;vpOptimizableKFs.back()->mnBAFixedForKF = pCurrKF->mnId;lFixedKeyFrames.push_back(vpOptimizableKFs.back());vpOptimizableKFs.pop_back();}// Next KFs// 2.4 再添加一个pMergeKF的下一个关键帧if (pMergeKF->mNextKF){vpOptimizableKFs.push_back(pMergeKF->mNextKF);vpOptimizableKFs.back()->mnBALocalForKF = pCurrKF->mnId;}// 2.5 数量不够时，添加最后一个的下一帧。继续添加直到达到2Nd个可优化关键帧,或没有新的可以添加了while (vpOptimizableKFs.size() < (2 * Nd)){if (vpOptimizableKFs.back()->mNextKF){vpOptimizableKFs.push_back(vpOptimizableKFs.back()->mNextKF);vpOptimizableKFs.back()->mnBALocalForKF = pCurrKF->mnId;}elsebreak;}int N = vpOptimizableKFs.size();// Optimizable points seen by optimizable keyframes// 3. 帧弄完了该弄点了，将优化的帧的点存入lLocalMapPoints (所有被可优化关键帧看到的点)// 添加用来优化的地图点list<MapPoint *> lLocalMapPoints;// 统计了在这些帧中点被观测的次数map<MapPoint *, int> mLocalObs;// 遍历所有可优化的关键帧for (int i = 0; i < N; i++){vector<MapPoint *> vpMPs = vpOptimizableKFs[i]->GetMapPointMatches();// 遍历每个关键帧所有的地图点for (vector<MapPoint *>::iterator vit = vpMPs.begin(), vend = vpMPs.end(); vit != vend; vit++){// Using mnBALocalForKF we avoid redundance here, one MP can not be added several times to lLocalMapPointsMapPoint *pMP = *vit;if (pMP)if (!pMP->isBad())// 用这个变量记录是否重复添加if (pMP->mnBALocalForKF != pCurrKF->mnId){mLocalObs[pMP] = 1;lLocalMapPoints.push_back(pMP);pMP->mnBALocalForKF = pCurrKF->mnId; // 防止重复添加}elsemLocalObs[pMP]++;}}// 4. 把pCurrKF的共视帧加进来 只优化位姿，不优化速度与偏置// 固定所有观察到地图点,但没有被加到优化变量中的关键帧int i = 0;const int min_obs = 10;vector<KeyFrame *> vNeighCurr = pCurrKF->GetCovisiblesByWeight(min_obs);// 遍历所有的pair<地图点指针,观测次数>for (vector<KeyFrame *>::iterator lit = vNeighCurr.begin(), lend = vNeighCurr.end(); lit != lend; lit++){// 拿到所有的观测// 上限，添加15个if (i >= maxCovKF)break;KeyFrame *pKFi = *lit;if (pKFi->mnBALocalForKF != pCurrKF->mnId && pKFi->mnBAFixedForKF != pCurrKF->mnId) // If optimizable or already included...{pKFi->mnBALocalForKF = pCurrKF->mnId;if (!pKFi->isBad()){i++;vpOptimizableCovKFs.push_back(pKFi);}}}i = 0;// 把pMergeKF的共视帧加进来 只优化位姿，不优化速度与偏置// BUG ！！！！！这里不知道是bug还是啥vNeighCurr是不是应该改成vNeighMerge// vpOptimizableCovKFs 改成 lFixedKeyFramesvector<KeyFrame *> vNeighMerge = pMergeKF->GetCovisiblesByWeight(min_obs);for (vector<KeyFrame *>::iterator lit = vNeighCurr.begin(), lend = vNeighCurr.end(); lit != lend; lit++, i++){// 上限，添加15个if (i >= maxCovKF)break;KeyFrame *pKFi = *lit;// 如果还没有被添加到共视帧容器里if (pKFi->mnBALocalForKF != pCurrKF->mnId && pKFi->mnBAFixedForKF != pCurrKF->mnId) // If optimizable or already included...{pKFi->mnBALocalForKF = pCurrKF->mnId;if (!pKFi->isBad()){i++;vpOptimizableCovKFs.push_back(pKFi);}}}// 5. 构建优化器g2o::SparseOptimizer optimizer;g2o::BlockSolverX::LinearSolverType *linearSolver;linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolverX::PoseMatrixType>();g2o::BlockSolverX *solver_ptr = new g2o::BlockSolverX(linearSolver);g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);solver->setUserLambdaInit(1e3);optimizer.setAlgorithm(solver);optimizer.setVerbose(false);// Set Local KeyFrame verticesN = vpOptimizableKFs.size();// 5.1 设置所有的可优化关键帧顶点for (int i = 0; i < N; i++){KeyFrame *pKFi = vpOptimizableKFs[i];VertexPose *VP = new VertexPose(pKFi);VP->setId(pKFi->mnId);VP->setFixed(false);// 位姿optimizer.addVertex(VP);if (pKFi->bImu){VertexVelocity *VV = new VertexVelocity(pKFi);VV->setId(maxKFid + 3 * (pKFi->mnId) + 1);VV->setFixed(false);optimizer.addVertex(VV);VertexGyroBias *VG = new VertexGyroBias(pKFi);VG->setId(maxKFid + 3 * (pKFi->mnId) + 2);VG->setFixed(false);optimizer.addVertex(VG);VertexAccBias *VA = new VertexAccBias(pKFi);VA->setId(maxKFid + 3 * (pKFi->mnId) + 3);VA->setFixed(false);optimizer.addVertex(VA);}}// Set Local cov keyframes vertices// 5.2 只优化位姿，不优化速度与偏置int Ncov = vpOptimizableCovKFs.size();for (int i = 0; i < Ncov; i++){KeyFrame *pKFi = vpOptimizableCovKFs[i];VertexPose *VP = new VertexPose(pKFi);VP->setId(pKFi->mnId);VP->setFixed(false);optimizer.addVertex(VP);if (pKFi->bImu){VertexVelocity *VV = new VertexVelocity(pKFi);VV->setId(maxKFid + 3 * (pKFi->mnId) + 1);VV->setFixed(true);optimizer.addVertex(VV);VertexGyroBias *VG = new VertexGyroBias(pKFi);VG->setId(maxKFid + 3 * (pKFi->mnId) + 2);VG->setFixed(true);optimizer.addVertex(VG);VertexAccBias *VA = new VertexAccBias(pKFi);VA->setId(maxKFid + 3 * (pKFi->mnId) + 3);VA->setFixed(true);optimizer.addVertex(VA);}}// Set Fixed KeyFrame vertices// 5.3 设置所有完全固定的关键帧顶点for (list<KeyFrame *>::iterator lit = lFixedKeyFrames.begin(), lend = lFixedKeyFrames.end(); lit != lend; lit++){KeyFrame *pKFi = *lit;VertexPose *VP = new VertexPose(pKFi);VP->setId(pKFi->mnId);VP->setFixed(true);optimizer.addVertex(VP);if (pKFi->bImu){VertexVelocity *VV = new VertexVelocity(pKFi);VV->setId(maxKFid + 3 * (pKFi->mnId) + 1);VV->setFixed(true);optimizer.addVertex(VV);VertexGyroBias *VG = new VertexGyroBias(pKFi);VG->setId(maxKFid + 3 * (pKFi->mnId) + 2);VG->setFixed(true);optimizer.addVertex(VG);VertexAccBias *VA = new VertexAccBias(pKFi);VA->setId(maxKFid + 3 * (pKFi->mnId) + 3);VA->setFixed(true);optimizer.addVertex(VA);}}// 6 设置所有的inertial的边// Create intertial constraints// 预积分边vector<EdgeInertial *> vei(N, (EdgeInertial *)NULL);// 角速度bias边vector<EdgeGyroRW *> vegr(N, (EdgeGyroRW *)NULL);// 加速地bias边vector<EdgeAccRW *> vear(N, (EdgeAccRW *)NULL);// 6.1 第一阶段优化vpOptimizableKFs里面的帧，遍历所有可优化的关键帧for (int i = 0; i < N; i++){// cout << "inserting inertial edge " << i << endl;KeyFrame *pKFi = vpOptimizableKFs[i];if (!pKFi->mPrevKF){Verbose::PrintMess("NOT INERTIAL LINK TO PREVIOUS FRAME!!!!", Verbose::VERBOSITY_NORMAL);continue;}if (pKFi->bImu && pKFi->mPrevKF->bImu && pKFi->mpImuPreintegrated){pKFi->mpImuPreintegrated->SetNewBias(pKFi->mPrevKF->GetImuBias());// 位姿g2o::HyperGraph::Vertex *VP1 = optimizer.vertex(pKFi->mPrevKF->mnId);// 速度g2o::HyperGraph::Vertex *VV1 = optimizer.vertex(maxKFid + 3 * (pKFi->mPrevKF->mnId) + 1);// 角速度biasg2o::HyperGraph::Vertex *VG1 = optimizer.vertex(maxKFid + 3 * (pKFi->mPrevKF->mnId) + 2);// 加速度biasg2o::HyperGraph::Vertex *VA1 = optimizer.vertex(maxKFid + 3 * (pKFi->mPrevKF->mnId) + 3);// 同上g2o::HyperGraph::Vertex *VP2 = optimizer.vertex(pKFi->mnId);g2o::HyperGraph::Vertex *VV2 = optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 1);g2o::HyperGraph::Vertex *VG2 = optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 2);g2o::HyperGraph::Vertex *VA2 = optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 3);if (!VP1 || !VV1 || !VG1 || !VA1 || !VP2 || !VV2 || !VG2 || !VA2){cerr << "Error " << VP1 << ", " << VV1 << ", " << VG1 << ", " << VA1 << ", " << VP2 << ", " << VV2 << ", " << VG2 << ", " << VA2 << endl;continue;}vei[i] = new EdgeInertial(pKFi->mpImuPreintegrated);// 设置顶点 2*Pose + 2*Velocity + 1 角速度bias + 1 加速度biasvei[i]->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VP1));vei[i]->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VV1));vei[i]->setVertex(2, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VG1));vei[i]->setVertex(3, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VA1));vei[i]->setVertex(4, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VP2));vei[i]->setVertex(5, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VV2));// TODO Uncomment// 设置优化参数g2o::RobustKernelHuber *rki = new g2o::RobustKernelHuber;vei[i]->setRobustKernel(rki);rki->setDelta(sqrt(16.92));// 添加边optimizer.addEdge(vei[i]);// 角速度bias的边vegr[i] = new EdgeGyroRW();// 设置顶点两个角速度biasvegr[i]->setVertex(0, VG1);vegr[i]->setVertex(1, VG2);// 设置infomation matrixcv::Mat cvInfoG = pKFi->mpImuPreintegrated->C.rowRange(9, 12).colRange(9, 12).inv(cv::DECOMP_SVD);Eigen::Matrix3d InfoG;for (int r = 0; r < 3; r++)for (int c = 0; c < 3; c++)InfoG(r, c) = cvInfoG.at<float>(r, c);vegr[i]->setInformation(InfoG);optimizer.addEdge(vegr[i]);// 设置加速度的边vear[i] = new EdgeAccRW();vear[i]->setVertex(0, VA1);vear[i]->setVertex(1, VA2);// 设置information matrixcv::Mat cvInfoA = pKFi->mpImuPreintegrated->C.rowRange(12, 15).colRange(12, 15).inv(cv::DECOMP_SVD);Eigen::Matrix3d InfoA;for (int r = 0; r < 3; r++)for (int c = 0; c < 3; c++)InfoA(r, c) = cvInfoA.at<float>(r, c);vear[i]->setInformation(InfoA);optimizer.addEdge(vear[i]);}elseVerbose::PrintMess("ERROR building inertial edge", Verbose::VERBOSITY_NORMAL);}Verbose::PrintMess("end inserting inertial edges", Verbose::VERBOSITY_DEBUG);// Set MapPoint vertices// 6.2 设置所有地图的顶点// 设置地图点顶点const int nExpectedSize = (N + Ncov + lFixedKeyFrames.size()) * lLocalMapPoints.size();// 对于双目单目设置不同// Monovector<EdgeMono *> vpEdgesMono;vpEdgesMono.reserve(nExpectedSize);vector<KeyFrame *> vpEdgeKFMono;vpEdgeKFMono.reserve(nExpectedSize);vector<MapPoint *> vpMapPointEdgeMono;vpMapPointEdgeMono.reserve(nExpectedSize);// Stereovector<EdgeStereo *> vpEdgesStereo;vpEdgesStereo.reserve(nExpectedSize);vector<KeyFrame *> vpEdgeKFStereo;vpEdgeKFStereo.reserve(nExpectedSize);vector<MapPoint *> vpMapPointEdgeStereo;vpMapPointEdgeStereo.reserve(nExpectedSize);const float thHuberMono = sqrt(5.991);const float chi2Mono2 = 5.991;const float thHuberStereo = sqrt(7.815);const float chi2Stereo2 = 7.815;const unsigned long iniMPid = maxKFid * 5;// 遍历所有被可优化关键帧观测到的的地图点// 7. 添加重投影的边for (list<MapPoint *>::iterator lit = lLocalMapPoints.begin(), lend = lLocalMapPoints.end(); lit != lend; lit++){MapPoint *pMP = *lit;if (!pMP)continue;g2o::VertexSBAPointXYZ *vPoint = new g2o::VertexSBAPointXYZ();vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));unsigned long id = pMP->mnId + iniMPid + 1;vPoint->setId(id);vPoint->setMarginalized(true);// 添加顶点optimizer.addVertex(vPoint);const map<KeyFrame *, tuple<int, int>> observations = pMP->GetObservations();// Create visual constraints// 添加重投影边for (map<KeyFrame *, tuple<int, int>>::const_iterator mit = observations.begin(), mend = observations.end(); mit != mend; mit++){KeyFrame *pKFi = mit->first;if (!pKFi)continue;if ((pKFi->mnBALocalForKF != pCurrKF->mnId) && (pKFi->mnBAFixedForKF != pCurrKF->mnId))continue;if (pKFi->mnId > maxKFid){Verbose::PrintMess("ID greater than current KF is", Verbose::VERBOSITY_NORMAL);continue;}if (optimizer.vertex(id) == NULL || optimizer.vertex(pKFi->mnId) == NULL)continue;if (!pKFi->isBad()){// 3D点的观测const cv::KeyPoint &kpUn = pKFi->mvKeysUn[get<0>(mit->second)];// 如果是单目观测if (pKFi->mvuRight[get<0>(mit->second)] < 0) // Monocular observation{// 投影Eigen::Matrix<double, 2, 1> obs;obs << kpUn.pt.x, kpUn.pt.y;EdgeMono *e = new EdgeMono();// 设置边的顶点// 3D点e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(id)));// 关键帧位姿e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(pKFi->mnId)));e->setMeasurement(obs);const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave];// 设置信息矩阵e->setInformation(Eigen::Matrix2d::Identity() * invSigma2);g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber;e->setRobustKernel(rk);rk->setDelta(thHuberMono);// 添加边optimizer.addEdge(e);vpEdgesMono.push_back(e);vpEdgeKFMono.push_back(pKFi);vpMapPointEdgeMono.push_back(pMP);}//双目else // stereo observation{const float kp_ur = pKFi->mvuRight[get<0>(mit->second)];Eigen::Matrix<double, 3, 1> obs;obs << kpUn.pt.x, kpUn.pt.y, kp_ur;EdgeStereo *e = new EdgeStereo();e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(id)));e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(pKFi->mnId)));e->setMeasurement(obs);const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave];e->setInformation(Eigen::Matrix3d::Identity() * invSigma2);g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber;e->setRobustKernel(rk);rk->setDelta(thHuberStereo);optimizer.addEdge(e);vpEdgesStereo.push_back(e);vpEdgeKFStereo.push_back(pKFi);vpMapPointEdgeStereo.push_back(pMP);}}}}// 如果要停止就直接返回if (pbStopFlag)if (*pbStopFlag)return;// 8. 开始优化optimizer.initializeOptimization();optimizer.optimize(3);if (pbStopFlag)if (!*pbStopFlag)optimizer.optimize(5);optimizer.setForceStopFlag(pbStopFlag);// 更具优化结果挑选删除外点vector<pair<KeyFrame *, MapPoint *>> vToErase;vToErase.reserve(vpEdgesMono.size() + vpEdgesStereo.size());// Check inlier observations// Mono// 9. 处理外点// 更具卡方检测来记录要删除的外点for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++){EdgeMono *e = vpEdgesMono[i];MapPoint *pMP = vpMapPointEdgeMono[i];if (pMP->isBad())continue;if (e->chi2() > chi2Mono2){KeyFrame *pKFi = vpEdgeKFMono[i];vToErase.push_back(make_pair(pKFi, pMP));}}// Stereofor (size_t i = 0, iend = vpEdgesStereo.size(); i < iend; i++){EdgeStereo *e = vpEdgesStereo[i];MapPoint *pMP = vpMapPointEdgeStereo[i];if (pMP->isBad())continue;if (e->chi2() > chi2Stereo2){KeyFrame *pKFi = vpEdgeKFStereo[i];vToErase.push_back(make_pair(pKFi, pMP));}}// Get Map Mutex and erase outliers// 移除外点unique_lock<mutex> lock(pMap->mMutexMapUpdate);if (!vToErase.empty()){for (size_t i = 0; i < vToErase.size(); i++){KeyFrame *pKFi = vToErase[i].first;MapPoint *pMPi = vToErase[i].second;pKFi->EraseMapPointMatch(pMPi);pMPi->EraseObservation(pKFi);}}// Recover optimized data// 10. 取数// 根据优化的结果,修改每个被优化的变量// Keyframes// 对于每个可优化关键帧for (int i = 0; i < N; i++){KeyFrame *pKFi = vpOptimizableKFs[i];// 修改位姿VertexPose *VP = static_cast<VertexPose *>(optimizer.vertex(pKFi->mnId));cv::Mat Tcw = Converter::toCvSE3(VP->estimate().Rcw[0], VP->estimate().tcw[0]);pKFi->SetPose(Tcw);// 在corrPoses记录每个关键帧融合后的位姿cv::Mat Tiw = pKFi->GetPose();cv::Mat Riw = Tiw.rowRange(0, 3).colRange(0, 3);cv::Mat tiw = Tiw.rowRange(0, 3).col(3);g2o::Sim3 g2oSiw(Converter::toMatrix3d(Riw), Converter::toVector3d(tiw), 1.0);corrPoses[pKFi] = g2oSiw;if (pKFi->bImu){// 速度VertexVelocity *VV = static_cast<VertexVelocity *>(optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 1));// 修改速度pKFi->SetVelocity(Converter::toCvMat(VV->estimate()));// 角速度biasVertexGyroBias *VG = static_cast<VertexGyroBias *>(optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 2));// 加速度biasVertexAccBias *VA = static_cast<VertexAccBias *>(optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 3));Vector6d b;// 修改biasb << VG->estimate(), VA->estimate();pKFi->SetNewBias(IMU::Bias(b[3], b[4], b[5], b[0], b[1], b[2]));}}// 遍历所有的共视帧for (int i = 0; i < Ncov; i++){KeyFrame *pKFi = vpOptimizableCovKFs[i];VertexPose *VP = static_cast<VertexPose *>(optimizer.vertex(pKFi->mnId));cv::Mat Tcw = Converter::toCvSE3(VP->estimate().Rcw[0], VP->estimate().tcw[0]);// 修改位姿pKFi->SetPose(Tcw);// 记录融合后的位姿cv::Mat Tiw = pKFi->GetPose();cv::Mat Riw = Tiw.rowRange(0, 3).colRange(0, 3);cv::Mat tiw = Tiw.rowRange(0, 3).col(3);g2o::Sim3 g2oSiw(Converter::toMatrix3d(Riw), Converter::toVector3d(tiw), 1.0);corrPoses[pKFi] = g2oSiw;// 这部分固定了，跟没做一样~if (pKFi->bImu){VertexVelocity *VV = static_cast<VertexVelocity *>(optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 1));pKFi->SetVelocity(Converter::toCvMat(VV->estimate()));VertexGyroBias *VG = static_cast<VertexGyroBias *>(optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 2));VertexAccBias *VA = static_cast<VertexAccBias *>(optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 3));Vector6d b;b << VG->estimate(), VA->estimate();pKFi->SetNewBias(IMU::Bias(b[3], b[4], b[5], b[0], b[1], b[2]));}}// Points// 对于所有的地图点for (list<MapPoint *>::iterator lit = lLocalMapPoints.begin(), lend = lLocalMapPoints.end(); lit != lend; lit++){// 跟新位置和normal等信息MapPoint *pMP = *lit;g2o::VertexSBAPointXYZ *vPoint = static_cast<g2o::VertexSBAPointXYZ *>(optimizer.vertex(pMP->mnId + iniMPid + 1));pMP->SetWorldPos(Converter::toCvMat(vPoint->estimate()));pMP->UpdateNormalAndDepth();}pMap->IncreaseChangeIndex();
}

【SLAM学习笔记】12-ORB_SLAM3关键源码分析⑩ Optimizer（七）地图融合优化相关推荐

【SLAM学习笔记】6-ORB_SLAM3关键源码分析④ Optimizer（一）单帧优化
2021SC@SDUSC 目录 1.前言 2.代码分析 1.前言 Optimizer是非常重要的代码文件!! 这一部分代码量巨大,查阅了很多资料结合来看的代码,将分为以下部分进行分析 1. 单帧优化 ...
【SLAM学习笔记】11-ORB_SLAM3关键源码分析⑨ Optimizer（六）地图回环优化
2021SC@SDUSC 目录 1.前言 2.代码分析 1.前言这一部分代码量巨大,查阅了很多资料结合来看的代码,将分为以下部分进行分析单帧优化局部地图优化全局优化尺度与重力优化 sim3优 ...
Nginx学习笔记（五）源码分析内存模块内存对齐
Nginx源码分析&内存模块今天总结了下C语言的内存分配问题,那么就看看Nginx的内存分配相关模型的具体实现.还有内存对齐的内容~~不懂的可以看看~~ src/os/unix/Ngx_al ...
Netty学习笔记(一)Netty客户端源码分析
最近在学些BIO,NIO相关的知识,也学习了下Netty和它的源码,做个记录,方便以后继续学习,如果有错误的地方欢迎指正如果不了解BIO,NIO这些基础知识,可以看下我的如下博客 IO中的阻塞.非阻 ...
Android学习笔记-常用的一些源码，防止忘记了
Android学习笔记-常用的一些源码,防止忘记了... 设置拨打电话 StringdialUri="tell:"+m_currentTelNumble; IntentcallIn ...
Ceph 学习——OSD读写流程与源码分析（一）
消息从客户端发送而来,之前几节介绍了客户端下对象存储.块存储库的实现以及他们在客户端下API请求的发送过程(Ceph学习--Librados与Osdc实现源码解析 . Ceph学习--客户端读写操 ...
JStorm与Storm源码分析（七）--BasicBoltExecutor与装饰模式
在Storm中IBasicBolt的主要作用是为用户提供一种更为简单的Bolt编写方式,更为简单体现在Storm框架本身帮你处理了所发出消息的Ack.Fail和Anchor操作,而这部分操作是由执行器 ...
Java的wait()、notify()学习三部曲之一：JVM源码分析
原文链接:https://blog.csdn.net/boling_cavalry/article/details/77793224 综述 Java的wait().notify()学习三部曲由三篇文章 ...
【安卓学习积累】IntentService的源码分析
今天主要总结一下IntentService的源码,里面是如何实现的,为什么IntentService在执行完耗时操作后会自动停止. 1.the service is started as needed ...

【SLAM学习笔记】12-ORB_SLAM3关键源码分析⑩ Optimizer（七）地图融合优化

目录

1.前言

2.代码分析

【SLAM学习笔记】12-ORB_SLAM3关键源码分析⑩ Optimizer（七）地图融合优化相关推荐

最新文章

热门文章