ORB_SLAM2原理及代码解析:Optimizer::LocalBundleAdjustment

1 作用

2 运行逻辑

                                       

3 参数及含义

4 代码

void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap)
{    // Local KeyFrames: First Breath Search from Current Keyframelist<KeyFrame*> lLocalKeyFrames;lLocalKeyFrames.push_back(pKF);pKF->mnBALocalForKF = pKF->mnId;const vector<KeyFrame*> vNeighKFs = pKF->GetVectorCovisibleKeyFrames();for(int i=0, iend=vNeighKFs.size(); i<iend; i++){KeyFrame* pKFi = vNeighKFs[i];pKFi->mnBALocalForKF = pKF->mnId;if(!pKFi->isBad())lLocalKeyFrames.push_back(pKFi);}// Local MapPoints seen in Local KeyFrameslist<MapPoint*> lLocalMapPoints;for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin() , lend=lLocalKeyFrames.end(); lit!=lend; lit++){vector<MapPoint*> vpMPs = (*lit)->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 Keyframes. Keyframes that see Local MapPoints but that are not Local Keyframeslist<KeyFrame*> lFixedCameras;for(list<MapPoint*>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++){map<KeyFrame*,size_t> observations = (*lit)->GetObservations();for(map<KeyFrame*,size_t>::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())lFixedCameras.push_back(pKFi);}}}// Setup optimizerg2o::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);if(pbStopFlag)optimizer.setForceStopFlag(pbStopFlag);unsigned long maxKFid = 0;// Set Local KeyFrame verticesfor(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==0);optimizer.addVertex(vSE3);if(pKFi->mnId>maxKFid)maxKFid=pKFi->mnId;}// Set Fixed KeyFrame verticesfor(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 verticesconst int nExpectedSize = (lLocalKeyFrames.size()+lFixedCameras.size())*lLocalMapPoints.size();vector<g2o::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 thHuberMono = sqrt(5.991);const float thHuberStereo = sqrt(7.815);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()));int id = pMP->mnId+maxKFid+1;vPoint->setId(id);vPoint->setMarginalized(true);optimizer.addVertex(vPoint);const map<KeyFrame*,size_t> observations = pMP->GetObservations();//Set edgesfor(map<KeyFrame*,size_t>::const_iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++){KeyFrame* pKFi = mit->first;if(!pKFi->isBad()){                const cv::KeyPoint &kpUn = pKFi->mvKeysUn[mit->second];// Monocular observationif(pKFi->mvuRight[mit->second]<0){Eigen::Matrix<double,2,1> obs;obs << kpUn.pt.x, kpUn.pt.y;g2o::EdgeSE3ProjectXYZ* e = new g2o::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->fx = pKFi->fx;e->fy = pKFi->fy;e->cx = pKFi->cx;e->cy = pKFi->cy;optimizer.addEdge(e);vpEdgesMono.push_back(e);vpEdgeKFMono.push_back(pKFi);vpMapPointEdgeMono.push_back(pMP);}else // Stereo observation{Eigen::Matrix<double,3,1> obs;const float kp_ur = pKFi->mvuRight[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);}}}}if(pbStopFlag)if(*pbStopFlag)return;optimizer.initializeOptimization();optimizer.optimize(5);bool bDoMore= true;if(pbStopFlag)if(*pbStopFlag)bDoMore = false;if(bDoMore){// Check inlier observationsfor(size_t i=0, iend=vpEdgesMono.size(); i<iend;i++){g2o::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];MapPoint* pMP = vpMapPointEdgeMono[i];if(pMP->isBad())continue;if(e->chi2()>5.991 || !e->isDepthPositive()){e->setLevel(1);}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);}e->setRobustKernel(0);}// Optimize again without the outliersoptimizer.initializeOptimization(0);optimizer.optimize(10);}vector<pair<KeyFrame*,MapPoint*> > vToErase;vToErase.reserve(vpEdgesMono.size()+vpEdgesStereo.size());// Check inlier observations       for(size_t i=0, iend=vpEdgesMono.size(); i<iend;i++){g2o::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=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//Keyframesfor(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; lit++){KeyFrame* pKF = *lit;g2o::VertexSE3Expmap* vSE3 = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(pKF->mnId));g2o::SE3Quat SE3quat = vSE3->estimate();pKF->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();}
}

5 解析

5.1 构建局部关键帧集合

    // Local KeyFrames: First Breath Search from Current Keyframelist<KeyFrame*> lLocalKeyFrames;lLocalKeyFrames.push_back(pKF);pKF->mnBALocalForKF = pKF->mnId;//获取与当前关键帧共视度最高的关键帧集合const vector<KeyFrame*> vNeighKFs = pKF->GetVectorCovisibleKeyFrames();for(int i=0, iend=vNeighKFs.size(); i<iend; i++){KeyFrame* pKFi = vNeighKFs[i];pKFi->mnBALocalForKF = pKF->mnId;if(!pKFi->isBad())lLocalKeyFrames.push_back(pKFi);}

5.1.1 GetVectorCovisibleKeyFrames()

vector<KeyFrame*> KeyFrame::GetVectorCovisibleKeyFrames()
{unique_lock<mutex> lock(mMutexConnections);return mvpOrderedConnectedKeyFrames;
}

5.2 收集局部地图点

    // Local MapPoints seen in Local KeyFrameslist<MapPoint*> lLocalMapPoints;for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin() , lend=lLocalKeyFrames.end(); lit!=lend; lit++){//获取关键帧观测到的地图点vector<MapPoint*> vpMPs = (*lit)->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;}}}

5.3 收集固定关键帧

    // Fixed Keyframes. Keyframes that see Local MapPoints but that are not Local Keyframeslist<KeyFrame*> lFixedCameras;for(list<MapPoint*>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++){//获取当前地图点所有观测map<KeyFrame*,size_t> observations = (*lit)->GetObservations();for(map<KeyFrame*,size_t>::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())lFixedCameras.push_back(pKFi);}}}

固定关键帧作用：

没有被选为局部关键帧；

但这些关键帧也能观测到本地地图点；

它们的位姿保持不动；

只用于提供约束关系，防止优化时整体漂移。

5.3.1 GetObservations()

详见4.2.1：

https://blog.csdn.net/weixin_45728280/article/details/152809802?spm=1011.2415.3001.5331

5.4 构建g2o优化器

    // Setup optimizerg2o::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);

5.5 添加关键帧顶点

   //如果为真，BA停止，传入g2o，详见5.5.1if(pbStopFlag)optimizer.setForceStopFlag(pbStopFlag);unsigned long maxKFid = 0;// Set Local KeyFrame verticesfor(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()));//设置顶点IDvSE3->setId(pKFi->mnId);//将ID为0的第一个关键帧固定，不参与优化，作为世界坐标系原点vSE3->setFixed(pKFi->mnId==0);//将顶点加入优化图optimizer.addVertex(vSE3);//更新当前最大关键帧IDif(pKFi->mnId>maxKFid)maxKFid=pKFi->mnId;}// Set Fixed KeyFrame verticesfor(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;}

5.5.1 setForceStopFlag()

（1）声明：SparseOptimizer.h

（2）定义：SparseOptimizer.cpp

  void SparseOptimizer::setForceStopFlag(bool* flag){_forceStopFlag=flag;}

5.6 添加地图点顶点+投影观测边

    // Set MapPoint verticesconst int nExpectedSize = (lLocalKeyFrames.size()+lFixedCameras.size())*lLocalMapPoints.size();//vpEdgesMono：存放所有“单目投影误差边（g2o::EdgeSE3ProjectXYZ*）”的指针vector<g2o::EdgeSE3ProjectXYZ*> vpEdgesMono;//分配内存vpEdgesMono.reserve(nExpectedSize);//vpEdgeKFMono：存放与每条边对应的关键帧//边：相机投影该点后的像素位置和实际检测到的像素位置的差距，即重投影误差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);//鲁棒核函数，详情5.6.1const float thHuberMono = sqrt(5.991);const float thHuberStereo = sqrt(7.815);for(list<MapPoint*>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++){MapPoint* pMP = *lit;//EdgeSE3ProjectXYZ或EdgeStereoSE3ProjectXYZ:边//g2o::VertexSE3Expmap：相机位姿（节点），g2o::VertexSBAPointXYZ：地图点（节点）g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();//地图点在世界坐标下的初始值vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));//给每个地图点顶点（MapPoint）分配一个独立的、且不与关键帧冲突的唯一IDint id = pMP->mnId+maxKFid+1;vPoint->setId(id);//顶点求解时可以被边缘化，稀疏矩阵优化原理vPoint->setMarginalized(true);optimizer.addVertex(vPoint);const map<KeyFrame*,size_t> observations = pMP->GetObservations();//Set edgesfor(map<KeyFrame*,size_t>::const_iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++){KeyFrame* pKFi = mit->first;if(!pKFi->isBad()){                //mit->second：地图点在该帧中的特征点索引//取出地图点在该帧中的观测点位置const cv::KeyPoint &kpUn = pKFi->mvKeysUn[mit->second];// Monocular observationif(pKFi->mvuRight[mit->second]<0){Eigen::Matrix<double,2,1> obs;obs << kpUn.pt.x, kpUn.pt.y;g2o::EdgeSE3ProjectXYZ* e = new g2o::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->fx = pKFi->fx;e->fy = pKFi->fy;e->cx = pKFi->cx;e->cy = pKFi->cy;optimizer.addEdge(e);vpEdgesMono.push_back(e);vpEdgeKFMono.push_back(pKFi);vpMapPointEdgeMono.push_back(pMP);}else // Stereo observation{Eigen::Matrix<double,3,1> obs;const float kp_ur = pKFi->mvuRight[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);}}}}

5.6.1 鲁棒核函数

通俗理解：重投影误差小于阀值，好点；大于，坏点。

5.7 第一次优化

if(pbStopFlag)if(*pbStopFlag)return;optimizer.initializeOptimization();optimizer.optimize(5);

5.8 筛选外点，再次优化

    bool bDoMore= true;if(pbStopFlag)if(*pbStopFlag)bDoMore = false;if(bDoMore){// Check inlier observationsfor(size_t i=0, iend=vpEdgesMono.size(); i<iend;i++){g2o::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];MapPoint* pMP = vpMapPointEdgeMono[i];if(pMP->isBad())continue;if(e->chi2()>5.991 || !e->isDepthPositive()){e->setLevel(1);}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);}e->setRobustKernel(0);}// Optimize again without the outliersoptimizer.initializeOptimization(0);optimizer.optimize(10);}

5.9 删除外点观测关系

    vector<pair<KeyFrame*,MapPoint*> > vToErase;vToErase.reserve(vpEdgesMono.size()+vpEdgesStereo.size());// Check inlier observations       for(size_t i=0, iend=vpEdgesMono.size(); i<iend;i++){g2o::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=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);}}

5.10 更新优化后结果

    // Recover optimized data//Keyframesfor(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; lit++){KeyFrame* pKF = *lit;g2o::VertexSE3Expmap* vSE3 = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(pKF->mnId));g2o::SE3Quat SE3quat = vSE3->estimate();pKF->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();}
}