1. 前言

Optimizer是非常重要的代码文件！!
这一部分代码量巨大，查阅了很多资料结合来看的代码，将分为以下部分进行分析

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

2. 代码分析

2.1. LocalBundleAdjustment

用于LocalMapping线程中剔除关键帧之前的局部地图优化。局部BA的共视图（其中帧都是关键帧）。用数据结构的术语这个图表示的数据结构称为无向加权图，又叫网，权重就是共视地图点的个数。

Local Bundle Adjustment LocalMapping::Run() 使用，纯视觉
总结下与ORBSLAM2的不同：
前面操作基本一样，但优化时2代去掉了误差大的点又进行优化了，3代只是统计但没有去掉继续优化，而后都是将误差大的点干掉

void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool *pbStopFlag, Map *pMap, int &num_fixedKF, int &num_OptKF, int &num_MPs, int &num_edges)
{// 该优化函数用于LocalMapping线程的局部BA优化// Local KeyFrames: First Breath Search from Current Keyframelist<KeyFrame *> lLocalKeyFrames;// 步骤1：将当前关键帧加入lLocalKeyFrameslLocalKeyFrames.push_back(pKF);pKF->mnBALocalForKF = pKF->mnId;Map *pCurrentMap = pKF->GetMap();// 步骤2：找到关键帧连接的关键帧（一级相连），加入lLocalKeyFrames中const vector<KeyFrame *> vNeighKFs = pKF->GetVectorCovisibleKeyFrames();for (int i = 0, iend = vNeighKFs.size(); i < iend; i++){KeyFrame *pKFi = vNeighKFs[i];// 记录局部优化id，该数为不断变化，数值等于局部化的关键帧的id，该id用于防止重复添加pKFi->mnBALocalForKF = pKF->mnId;if (!pKFi->isBad() && pKFi->GetMap() == pCurrentMap)lLocalKeyFrames.push_back(pKFi);}// Local MapPoints seen in Local KeyFramesnum_fixedKF = 0;// 步骤3：遍历lLocalKeyFrames中关键帧，将它们观测的MapPoints加入到lLocalMapPointslist<MapPoint *> lLocalMapPoints;set<MapPoint *> sNumObsMP;for (list<KeyFrame *>::iterator lit = lLocalKeyFrames.begin(), lend = lLocalKeyFrames.end(); lit != lend; lit++){KeyFrame *pKFi = *lit;if (pKFi->mnId == pMap->GetInitKFid()){num_fixedKF = 1;}vector<MapPoint *> vpMPs = pKFi->GetMapPointMatches();for (vector<MapPoint *>::iterator vit = vpMPs.begin(), vend = vpMPs.end(); vit != vend; vit++){MapPoint *pMP = *vit;if (pMP)if (!pMP->isBad() && pMP->GetMap() == pCurrentMap){if (pMP->mnBALocalForKF != pKF->mnId){lLocalMapPoints.push_back(pMP);pMP->mnBALocalForKF = pKF->mnId;}}}}num_MPs = lLocalMapPoints.size();// Fixed Keyframes. Keyframes that see Local MapPoints but that are not Local Keyframes// 步骤4：得到能被局部MapPoints观测到，但不属于局部关键帧的关键帧，这些关键帧在局部BA优化时不优化list<KeyFrame *> lFixedCameras;for (list<MapPoint *>::iterator lit = lLocalMapPoints.begin(), lend = lLocalMapPoints.end(); lit != lend; lit++){map<KeyFrame *, tuple<int, int>> observations = (*lit)->GetObservations();for (map<KeyFrame *, tuple<int, int>>::iterator mit = observations.begin(), mend = observations.end(); mit != mend; mit++){KeyFrame *pKFi = mit->first;if (pKFi->mnBALocalForKF != pKF->mnId && pKFi->mnBAFixedForKF != pKF->mnId){pKFi->mnBAFixedForKF = pKF->mnId;if (!pKFi->isBad() && pKFi->GetMap() == pCurrentMap)lFixedCameras.push_back(pKFi);}}}// 步骤4.1：相比ORBSLAM2多出了判断固定关键帧的个数，最起码要两个固定的,如果实在没有就把lLocalKeyFrames中最早的KF固定，还是不够再加上第二早的KF固定num_fixedKF = lFixedCameras.size() + num_fixedKF;if (num_fixedKF < 2){list<KeyFrame *>::iterator lit = lLocalKeyFrames.begin();int lowerId = pKF->mnId;KeyFrame *pLowerKf;int secondLowerId = pKF->mnId;KeyFrame *pSecondLowerKF;for (; lit != lLocalKeyFrames.end(); lit++){KeyFrame *pKFi = *lit;if (pKFi == pKF || pKFi->mnId == pMap->GetInitKFid()){continue;}if (pKFi->mnId < lowerId){lowerId = pKFi->mnId;pLowerKf = pKFi;}else if (pKFi->mnId < secondLowerId){secondLowerId = pKFi->mnId;pSecondLowerKF = pKFi;}}lFixedCameras.push_back(pLowerKf);lLocalKeyFrames.remove(pLowerKf);num_fixedKF++;if (num_fixedKF < 2){lFixedCameras.push_back(pSecondLowerKF);lLocalKeyFrames.remove(pSecondLowerKF);num_fixedKF++;}}if (num_fixedKF == 0){Verbose::PrintMess("LM-LBA: There are 0 fixed KF in the optimizations, LBA aborted", Verbose::VERBOSITY_QUIET);return;}// Setup optimizer// 步骤5：构造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);if (pMap->IsInertial())solver->setUserLambdaInit(100.0);optimizer.setAlgorithm(solver);optimizer.setVerbose(false);if (pbStopFlag)optimizer.setForceStopFlag(pbStopFlag);unsigned long maxKFid = 0;// Set Local KeyFrame vertices// 步骤6：添加顶点：Pose of Local KeyFramefor (list<KeyFrame *>::iterator lit = lLocalKeyFrames.begin(), lend = lLocalKeyFrames.end(); lit != lend; lit++){KeyFrame *pKFi = *lit;g2o::VertexSE3Expmap *vSE3 = new g2o::VertexSE3Expmap();vSE3->setEstimate(Converter::toSE3Quat(pKFi->GetPose()));vSE3->setId(pKFi->mnId);vSE3->setFixed(pKFi->mnId == pMap->GetInitKFid());optimizer.addVertex(vSE3);if (pKFi->mnId > maxKFid)maxKFid = pKFi->mnId;}num_OptKF = lLocalKeyFrames.size();// Set Fixed KeyFrame vertices// 步骤7：添加顶点：Pose of Fixed KeyFrame，注意这里调用了vSE3->setFixed(true)。for (list<KeyFrame *>::iterator lit = lFixedCameras.begin(), lend = lFixedCameras.end(); lit != lend; lit++){KeyFrame *pKFi = *lit;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 vertices// 步骤7：添加3D顶点// 存放的方式(举例)// 边id: 1 2 3 4 5 6 7 8 9// KFid: 1 2 3 4 1 2 3 2 3// MPid: 1 1 1 1 2 2 2 3 3// 所以这个个数大约是点数×帧数，实际肯定比这个要少const int nExpectedSize = (lLocalKeyFrames.size() + lFixedCameras.size()) * lLocalMapPoints.size();// 存放单目时的边vector<ORB_SLAM3::EdgeSE3ProjectXYZ *> vpEdgesMono;vpEdgesMono.reserve(nExpectedSize);// 存放双目鱼眼时另一个相机的边vector<ORB_SLAM3::EdgeSE3ProjectXYZToBody *> vpEdgesBody;vpEdgesBody.reserve(nExpectedSize);// 存放单目时的KFvector<KeyFrame *> vpEdgeKFMono;vpEdgeKFMono.reserve(nExpectedSize);// 存放单目时的MP// 存放双目鱼眼时另一个相机的KFvector<KeyFrame *> vpEdgeKFBody;vpEdgeKFBody.reserve(nExpectedSize);// 存放单目时的MPvector<MapPoint *> vpMapPointEdgeMono;vpMapPointEdgeMono.reserve(nExpectedSize);// 存放双目鱼眼时另一个相机的MPvector<MapPoint *> vpMapPointEdgeBody;vpMapPointEdgeBody.reserve(nExpectedSize);// 存放双目时的边vector<g2o::EdgeStereoSE3ProjectXYZ *> vpEdgesStereo;vpEdgesStereo.reserve(nExpectedSize);// 存放双目时的KFvector<KeyFrame *> vpEdgeKFStereo;vpEdgeKFStereo.reserve(nExpectedSize);// 存放双目时的MPvector<MapPoint *> vpMapPointEdgeStereo;vpMapPointEdgeStereo.reserve(nExpectedSize);const float thHuberMono = sqrt(5.991);const float thHuberStereo = sqrt(7.815);int nPoints = 0;int nKFs = lLocalKeyFrames.size() + lFixedCameras.size(), nEdges = 0;// 添加顶点：MapPointfor (list<MapPoint *>::iterator lit = lLocalMapPoints.begin(), lend = lLocalMapPoints.end(); lit != lend; lit++){MapPoint *pMP = *lit;g2o::VertexSBAPointXYZ *vPoint = new g2o::VertexSBAPointXYZ();vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));int id = pMP->mnId + maxKFid + 1;vPoint->setId(id);// 这里的边缘化与滑动窗口不同，而是为了加速稀疏矩阵的计算BlockSolver_6_3默认了6维度的不边缘化，3自由度的三维点被边缘化，所以所有三维点都设置边缘化vPoint->setMarginalized(true);optimizer.addVertex(vPoint);nPoints++;const map<KeyFrame *, tuple<int, int>> observations = pMP->GetObservations();// Set edges//  步骤8：对每一对关联的MapPoint和KeyFrame构建边for (map<KeyFrame *, tuple<int, int>>::const_iterator mit = observations.begin(), mend = observations.end(); mit != mend; mit++){KeyFrame *pKFi = mit->first;if (!pKFi->isBad() && pKFi->GetMap() == pCurrentMap){const int leftIndex = get<0>(mit->second);// Monocular observation// 单目if (leftIndex != -1 && pKFi->mvuRight[get<0>(mit->second)] < 0){const cv::KeyPoint &kpUn = pKFi->mvKeysUn[leftIndex];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(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);e->pCamera = pKFi->mpCamera;optimizer.addEdge(e);vpEdgesMono.push_back(e);vpEdgeKFMono.push_back(pKFi);vpMapPointEdgeMono.push_back(pMP);nEdges++;}else if (leftIndex != -1 && pKFi->mvuRight[get<0>(mit->second)] >= 0) // Stereo observation{const cv::KeyPoint &kpUn = pKFi->mvKeysUn[leftIndex];Eigen::Matrix<double, 3, 1> obs;const float kp_ur = pKFi->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(pKFi->mnId)));e->setMeasurement(obs);const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave];Eigen::Matrix3d Info = Eigen::Matrix3d::Identity() * invSigma2;e->setInformation(Info);g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber;e->setRobustKernel(rk);rk->setDelta(thHuberStereo);e->fx = pKFi->fx;e->fy = pKFi->fy;e->cx = pKFi->cx;e->cy = pKFi->cy;e->bf = pKFi->mbf;optimizer.addEdge(e);vpEdgesStereo.push_back(e);vpEdgeKFStereo.push_back(pKFi);vpMapPointEdgeStereo.push_back(pMP);nEdges++;}if (pKFi->mpCamera2){int rightIndex = get<1>(mit->second);if (rightIndex != -1){rightIndex -= pKFi->NLeft;Eigen::Matrix<double, 2, 1> obs;cv::KeyPoint kp = pKFi->mvKeysRight[rightIndex];obs << kp.pt.x, kp.pt.y;ORB_SLAM3::EdgeSE3ProjectXYZToBody *e = new ORB_SLAM3::EdgeSE3ProjectXYZToBody();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[kp.octave];e->setInformation(Eigen::Matrix2d::Identity() * invSigma2);g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber;e->setRobustKernel(rk);rk->setDelta(thHuberMono);e->mTrl = Converter::toSE3Quat(pKFi->mTrl);e->pCamera = pKFi->mpCamera2;optimizer.addEdge(e);vpEdgesBody.push_back(e);vpEdgeKFBody.push_back(pKFi);vpMapPointEdgeBody.push_back(pMP);nEdges++;}}}}}num_edges = nEdges;if (pbStopFlag)if (*pbStopFlag)return;// 步骤9：开始优化optimizer.initializeOptimization();std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now();optimizer.optimize(5);std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now();bool bDoMore = true;if (pbStopFlag)if (*pbStopFlag)bDoMore = false;if (bDoMore){// Check inlier observationsint nMonoBadObs = 0;// 步骤10：检测outlier，并设置下次不优化，上面展示了怎么存储的，i是共享的，第i个边是由第i个MP与第i个KF组成的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()){nMonoBadObs++;}}int nBodyBadObs = 0;for (size_t i = 0, iend = vpEdgesBody.size(); i < iend; i++){ORB_SLAM3::EdgeSE3ProjectXYZToBody *e = vpEdgesBody[i];MapPoint *pMP = vpMapPointEdgeBody[i];if (pMP->isBad())continue;if (e->chi2() > 5.991 || !e->isDepthPositive()){nBodyBadObs++;}}int nStereoBadObs = 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()){nStereoBadObs++;}}// Optimize again// 步骤11：排除误差较大的outlier后再次优化，但这里没有去掉，相当于接着优化了10次，如果上面不去掉应该注释掉，浪费了计算时间optimizer.initializeOptimization(0);optimizer.optimize(10);}vector<pair<KeyFrame *, MapPoint *>> vToErase;vToErase.reserve(vpEdgesMono.size() + vpEdgesBody.size() + vpEdgesStereo.size());// Check inlier observations// 步骤12：在优化后重新计算误差，剔除连接误差比较大的关键帧和MapPointfor (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));}}for (size_t i = 0, iend = vpEdgesBody.size(); i < iend; i++){ORB_SLAM3::EdgeSE3ProjectXYZToBody *e = vpEdgesBody[i];MapPoint *pMP = vpMapPointEdgeBody[i];if (pMP->isBad())continue;if (e->chi2() > 5.991 || !e->isDepthPositive()){KeyFrame *pKFi = vpEdgeKFBody[i];vToErase.push_back(make_pair(pKFi, pMP));}}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));}}// Get Map Mutexunique_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// Keyframesbool bShowStats = false;for (list<KeyFrame *>::iterator lit = lLocalKeyFrames.begin(), lend = lLocalKeyFrames.end(); lit != lend; lit++){KeyFrame *pKFi = *lit;g2o::VertexSE3Expmap *vSE3 = static_cast<g2o::VertexSE3Expmap *>(optimizer.vertex(pKFi->mnId));g2o::SE3Quat SE3quat = vSE3->estimate();pKFi->SetPose(Converter::toCvMat(SE3quat));}// Pointsfor (list<MapPoint *>::iterator lit = lLocalMapPoints.begin(), lend = lLocalMapPoints.end(); lit != lend; lit++){MapPoint *pMP = *lit;g2o::VertexSBAPointXYZ *vPoint = static_cast<g2o::VertexSBAPointXYZ *>(optimizer.vertex(pMP->mnId + maxKFid + 1));pMP->SetWorldPos(Converter::toCvMat(vPoint->estimate()));pMP->UpdateNormalAndDepth();}// TODO Check this changeindexpMap->IncreaseChangeIndex();
}

2.2. LocalInertialBA

局部地图＋惯导BA LocalMapping IMU中使用，地图经过imu初始化时用这个函数代替LocalBundleAdjustment

void Optimizer::LocalInertialBA(KeyFrame *pKF, bool *pbStopFlag, Map *pMap, int &num_fixedKF, int &num_OptKF, int &num_MPs, int &num_edges, bool bLarge, bool bRecInit)
{// Step 1. 确定待优化的关键帧们Map *pCurrentMap = pKF->GetMap();int maxOpt = 10; // 最大优化关键帧数目int opt_it = 10; // 每次优化的迭代次数if (bLarge){maxOpt = 25;opt_it = 4;}// 预计待优化的关键帧数，min函数是为了控制优化关键帧的数量const int Nd = std::min((int)pCurrentMap->KeyFramesInMap() - 2, maxOpt);const unsigned long maxKFid = pKF->mnId;vector<KeyFrame *> vpOptimizableKFs;const vector<KeyFrame *> vpNeighsKFs = pKF->GetVectorCovisibleKeyFrames();list<KeyFrame *> lpOptVisKFs;vpOptimizableKFs.reserve(Nd);vpOptimizableKFs.push_back(pKF);pKF->mnBALocalForKF = pKF->mnId;for (int i = 1; i < Nd; i++){if (vpOptimizableKFs.back()->mPrevKF){vpOptimizableKFs.push_back(vpOptimizableKFs.back()->mPrevKF);vpOptimizableKFs.back()->mnBALocalForKF = pKF->mnId;}elsebreak;}int N = vpOptimizableKFs.size();// Optimizable points seen by temporal optimizable keyframes// Step 2. 确定这些关键帧对应的地图点，存入lLocalMapPointslist<MapPoint *> lLocalMapPoints;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++){MapPoint *pMP = *vit;if (pMP)if (!pMP->isBad())if (pMP->mnBALocalForKF != pKF->mnId){lLocalMapPoints.push_back(pMP);pMP->mnBALocalForKF = pKF->mnId;}}}// Fixed Keyframe: First frame previous KF to optimization window)// Step 3. 固定一帧，为vpOptimizableKFs中最早的那一关键帧的上一关键帧，如果没有上一关键帧了就用最早的那一帧，毕竟目前得到的地图虽然有尺度但并不是绝对的位置list<KeyFrame *> lFixedKeyFrames;if (vpOptimizableKFs.back()->mPrevKF){lFixedKeyFrames.push_back(vpOptimizableKFs.back()->mPrevKF);vpOptimizableKFs.back()->mPrevKF->mnBAFixedForKF = pKF->mnId;}else{vpOptimizableKFs.back()->mnBALocalForKF = 0;vpOptimizableKFs.back()->mnBAFixedForKF = pKF->mnId;lFixedKeyFrames.push_back(vpOptimizableKFs.back());vpOptimizableKFs.pop_back();}// Optimizable visual KFs// 4. 做了一系列操作发现最后lpOptVisKFs为空。这段应该是调试遗留代码，如果实现的话其实就是把共视图中在前面没有加过的关键帧们加进来，但作者可能发现之前就把共视图的全部帧加进来了，也有可能发现优化的效果不好浪费时间// 获得与当前关键帧有共视关系的一些关键帧，大于15个点，排序为从小到大const int maxCovKF = 0;for (int i = 0, iend = vpNeighsKFs.size(); i < iend; i++){if (lpOptVisKFs.size() >= maxCovKF)break;KeyFrame *pKFi = vpNeighsKFs[i];if (pKFi->mnBALocalForKF == pKF->mnId || pKFi->mnBAFixedForKF == pKF->mnId)continue;pKFi->mnBALocalForKF = pKF->mnId;if (!pKFi->isBad() && pKFi->GetMap() == pCurrentMap){lpOptVisKFs.push_back(pKFi);vector<MapPoint *> vpMPs = pKFi->GetMapPointMatches();for (vector<MapPoint *>::iterator vit = vpMPs.begin(), vend = vpMPs.end(); vit != vend; vit++){MapPoint *pMP = *vit;if (pMP)if (!pMP->isBad())if (pMP->mnBALocalForKF != pKF->mnId){lLocalMapPoints.push_back(pMP);pMP->mnBALocalForKF = pKF->mnId;}}}}// Fixed KFs which are not covisible optimizable// 5. 将所有mp点对应的关键帧（除了前面加过的）放入到固定组里面，后面优化时不改变const int maxFixKF = 200;for (list<MapPoint *>::iterator lit = lLocalMapPoints.begin(), lend = lLocalMapPoints.end(); lit != lend; lit++){map<KeyFrame *, tuple<int, int>> observations = (*lit)->GetObservations();for (map<KeyFrame *, tuple<int, int>>::iterator mit = observations.begin(), mend = observations.end(); mit != mend; mit++){KeyFrame *pKFi = mit->first;if (pKFi->mnBALocalForKF != pKF->mnId && pKFi->mnBAFixedForKF != pKF->mnId){pKFi->mnBAFixedForKF = pKF->mnId;if (!pKFi->isBad()){lFixedKeyFrames.push_back(pKFi);break;}}}if (lFixedKeyFrames.size() >= maxFixKF)break;}bool bNonFixed = (lFixedKeyFrames.size() == 0);// Setup optimizer// 6. 构造优化器，正式开始优化g2o::SparseOptimizer optimizer;g2o::BlockSolverX::LinearSolverType *linearSolver;linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolverX::PoseMatrixType>();g2o::BlockSolverX *solver_ptr = new g2o::BlockSolverX(linearSolver);if (bLarge){g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);solver->setUserLambdaInit(1e-2); // to avoid iterating for finding optimal lambdaoptimizer.setAlgorithm(solver);}else{g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);solver->setUserLambdaInit(1e0);optimizer.setAlgorithm(solver);}// Set Local temporal KeyFrame vertices// 7. 建立关于关键帧的节点，其中包括，位姿，速度，以及两个偏置N = vpOptimizableKFs.size();num_fixedKF = 0;num_OptKF = 0;num_MPs = 0;num_edges = 0;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);}num_OptKF++;}// Set Local visual KeyFrame vertices// 8. 建立关于共视关键帧的节点，但这里为空for (list<KeyFrame *>::iterator it = lpOptVisKFs.begin(), itEnd = lpOptVisKFs.end(); it != itEnd; it++){KeyFrame *pKFi = *it;VertexPose *VP = new VertexPose(pKFi);VP->setId(pKFi->mnId);VP->setFixed(false);optimizer.addVertex(VP);num_OptKF++;}// Set Fixed KeyFrame vertices// 9. 建立关于固定关键帧的节点，其中包括，位姿，速度，以及两个偏置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) // This should be done only for keyframe just before temporal window{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);}num_fixedKF++;}// Create intertial constraints// 暂时没看到有什么用vector<EdgeInertial *> vei(N, (EdgeInertial *)NULL);vector<EdgeGyroRW *> vegr(N, (EdgeGyroRW *)NULL);vector<EdgeAccRW *> vear(N, (EdgeAccRW *)NULL);// 10. 建立惯性边，没有imu跳过for (int i = 0; i < N; i++){KeyFrame *pKFi = vpOptimizableKFs[i];if (!pKFi->mPrevKF){cout << "NOT INERTIAL LINK TO PREVIOUS FRAME!!!!" << endl;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);g2o::HyperGraph::Vertex *VG1 = optimizer.vertex(maxKFid + 3 * (pKFi->mPrevKF->mnId) + 2);g2o::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);vei[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));// 到最早的一个可优化的关键帧或者地图没有ba2时添加鲁棒核函数if (i == N - 1 || bRecInit){// All inertial residuals are included without robust cost function, but not that one linking the// last optimizable keyframe inside of the local window and the first fixed keyframe out. The// information matrix for this measurement is also downweighted. This is done to avoid accumulating// error due to fixing variables.// 所有惯性残差都没有鲁棒核，但不包括窗口内最早一个可优化关键帧与第一个固定关键帧链接起来的惯性残差。// 该度量的信息矩阵也被降权。这样做是为了避免由于固定变量而累积错误。g2o::RobustKernelHuber *rki = new g2o::RobustKernelHuber;vei[i]->setRobustKernel(rki);if (i == N - 1)vei[i]->setInformation(vei[i]->information() * 1e-2);rki->setDelta(sqrt(16.92));}optimizer.addEdge(vei[i]);vegr[i] = new EdgeGyroRW();vegr[i]->setVertex(0, VG1);vegr[i]->setVertex(1, VG2);cv::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]);num_edges++;vear[i] = new EdgeAccRW();vear[i]->setVertex(0, VA1);vear[i]->setVertex(1, VA2);cv::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]);num_edges++;}elsecout << "ERROR building inertial edge" << endl;}// Set MapPoint verticesconst int nExpectedSize = (N + 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);// 11. 建立视觉边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;map<int, int> mVisEdges;for (int i = 0; i < N; i++){KeyFrame *pKFi = vpOptimizableKFs[i];mVisEdges[pKFi->mnId] = 0;}for (list<KeyFrame *>::iterator lit = lFixedKeyFrames.begin(), lend = lFixedKeyFrames.end(); lit != lend; lit++){mVisEdges[(*lit)->mnId] = 0;}num_MPs = lLocalMapPoints.size();for (list<MapPoint *>::iterator lit = lLocalMapPoints.begin(), lend = lLocalMapPoints.end(); lit != lend; lit++){MapPoint *pMP = *lit;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 constraintsfor (map<KeyFrame *, tuple<int, int>>::const_iterator mit = observations.begin(), mend = observations.end(); mit != mend; mit++){KeyFrame *pKFi = mit->first;if (pKFi->mnBALocalForKF != pKF->mnId && pKFi->mnBAFixedForKF != pKF->mnId)continue;if (!pKFi->isBad() && pKFi->GetMap() == pCurrentMap){const int leftIndex = get<0>(mit->second);cv::KeyPoint kpUn;// Monocular left observationif (leftIndex != -1 && pKFi->mvuRight[leftIndex] < 0){mVisEdges[pKFi->mnId]++;kpUn = pKFi->mvKeysUn[leftIndex];Eigen::Matrix<double, 2, 1> obs;obs << kpUn.pt.x, kpUn.pt.y;EdgeMono *e = new EdgeMono(0);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);// Add here uncerteintyconst float unc2 = pKFi->mpCamera->uncertainty2(obs);const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave] / unc2;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);num_edges++;}// Stereo-observationelse if (leftIndex != -1) // Stereo observation{kpUn = pKFi->mvKeysUn[leftIndex];mVisEdges[pKFi->mnId]++;const float kp_ur = pKFi->mvuRight[leftIndex];Eigen::Matrix<double, 3, 1> obs;obs << kpUn.pt.x, kpUn.pt.y, kp_ur;EdgeStereo *e = new EdgeStereo(0);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);// Add here uncerteintyconst float unc2 = pKFi->mpCamera->uncertainty2(obs.head(2));const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave] / unc2;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);num_edges++;}// Monocular right observationif (pKFi->mpCamera2){int rightIndex = get<1>(mit->second);if (rightIndex != -1){rightIndex -= pKFi->NLeft;mVisEdges[pKFi->mnId]++;Eigen::Matrix<double, 2, 1> obs;cv::KeyPoint kp = pKFi->mvKeysRight[rightIndex];obs << kp.pt.x, kp.pt.y;EdgeMono *e = new EdgeMono(1);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);// Add here uncerteintyconst float unc2 = pKFi->mpCamera->uncertainty2(obs);const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave] / unc2;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);num_edges++;}}}}}// cout << "Total map points: " << lLocalMapPoints.size() << endl;for (map<int, int>::iterator mit = mVisEdges.begin(), mend = mVisEdges.end(); mit != mend; mit++){assert(mit->second >= 3);}// 12. 开始优化optimizer.initializeOptimization();optimizer.computeActiveErrors();float err = optimizer.activeRobustChi2();optimizer.optimize(opt_it); // Originally to 2float err_end = optimizer.activeRobustChi2();if (pbStopFlag)optimizer.setForceStopFlag(pbStopFlag);vector<pair<KeyFrame *, MapPoint *>> vToErase;vToErase.reserve(vpEdgesMono.size() + vpEdgesStereo.size());// 13. 开始确认待删除的连接关系// Check inlier observations// Monofor (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++){EdgeMono *e = vpEdgesMono[i];MapPoint *pMP = vpMapPointEdgeMono[i];bool bClose = pMP->mTrackDepth < 10.f;if (pMP->isBad())continue;if ((e->chi2() > chi2Mono2 && !bClose) || (e->chi2() > 1.5f * chi2Mono2 && bClose) || !e->isDepthPositive()){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 outliersunique_lock<mutex> lock(pMap->mMutexMapUpdate);if ((2 * err < err_end || isnan(err) || isnan(err_end)) && !bLarge){cout << "FAIL LOCAL-INERTIAL BA!!!!" << endl;return;}// 14. 删除连接关系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);}}// Display main statistcis of optimizationVerbose::PrintMess("LIBA KFs: " + to_string(N), Verbose::VERBOSITY_DEBUG);Verbose::PrintMess("LIBA bNonFixed?: " + to_string(bNonFixed), Verbose::VERBOSITY_DEBUG);Verbose::PrintMess("LIBA KFs visual outliers: " + to_string(vToErase.size()), Verbose::VERBOSITY_DEBUG);for (list<KeyFrame *>::iterator lit = lFixedKeyFrames.begin(), lend = lFixedKeyFrames.end(); lit != lend; lit++)(*lit)->mnBAFixedForKF = 0;// 15. 取出结果// Recover optimized data// Local temporal KeyframesN = vpOptimizableKFs.size();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);pKFi->mnBALocalForKF = 0;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]));}}// Local visual KeyFrame// 空for (list<KeyFrame *>::iterator it = lpOptVisKFs.begin(), itEnd = lpOptVisKFs.end(); it != itEnd; it++){KeyFrame *pKFi = *it;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);pKFi->mnBALocalForKF = 0;}// Pointsfor (list<MapPoint *>::iterator lit = lLocalMapPoints.begin(), lend = lLocalMapPoints.end(); lit != lend; lit++){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学习笔记】7-ORB_SLAM3关键源码分析⑤ Optimizer（二）局部地图优化_口哨糖youri的博客-CSDN博客

ORBSLAM3的g2o优化之局部BA（LocalBundleAdjustment）_宛如新生的博客-CSDN博客_局部ba优化

ORB-SLAM3从理论到代码实现（二）：Optimizer局部地图优化相关推荐

ORB-SLAM3从理论到代码实现（一）：Optimizer单帧优化
1. 前言 Optimizer是非常重要的代码文件!! 这一部分代码量巨大,查阅了很多资料结合来看的代码,将分为以下部分进行分析单帧优化局部地图优化全局优化尺度与重力优化 sim3优化地图回 ...
ORB SLAM3论文大致翻译，手动整理
ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM I. INTRODU ...
felzenszwalb算法_学习图像场景解析的理论和应用（二）场景解析的经典算法分析之SLIC...
2003 年,任晓峰教授在图像分割技术层面上提出了超像素分割的这一概念,是指具有相似纹理.颜色.亮度等特征的相邻像素构成的有一定视觉意义的不规则像素块.它利用像素之间特征的相似性将像素分组,用少量的超 ...
数字IC秋招手撕代码（二）50%占空比的三分频
数字IC秋招手撕代码(二)50%占空比的三分频题目设计思路与逻辑分频代码或逻辑分频代码异或逻辑分频代码题目用verilog实现三分频电路,要求输出50%占空比设计思路如果不限制 ...
ORB SLAM3——IMU优化部分精读-VertexPose(ImuCamPose)的更新量到底是什么？
ORB SLAM3--IMU优化部分精读-VertexPose(ImuCamPose)的更新量到底是什么? 先说答案 void ImuCamPose::Update(const double *pu) ...
【深度学习】pix2pix GAN理论及代码实现与理解
灵感:最近也是在看关于GAN方面的代码,也是看到了很多篇博客,都写的挺好的,让我醍醐灌顶,理解了GAN的原理以及代码实现.所以写一下来记载一下,最后有其他好文章的链接. 灵感来源:pix2pixGAN ...
无人机航拍图像匹配——ORB算法实践（含代码）
无人机航拍图像匹配--ORB算法实践(含代码) 一.摘要二.ORB算法的原理 1. FAST角点检测 2.BRIEF描述子描述子生成 3.方向计算(角点检测和描述子之间的步骤) 4.尺度金字塔三 ...
Openai神作Dalle2理论和代码复现
Openai神作Dalle2 注:大家觉得博客好的话,别忘了点赞收藏呀,本人每周都会更新关于人工智能和大数据相关的内容,内容多为原创,Python Java Scala SQL 代码,CV NLP 推 ...
Part-1 ORB SLAM3初始化-1
初始化 ORB SLAM3的初始化主要是创建ORB词袋.关键帧数据库.多地图等对象,其步骤如下: 检测配置文件能否打开加载ORB词袋(ORBVocabulary) 创建关键帧数据库(KeyFrame ...

ORB-SLAM3从理论到代码实现（二）：Optimizer局部地图优化