ORB-SLAM2的源码阅读（十二）：Optimizer类

ORB_SLAM2主要都是进行图优化，把关键帧的位姿和路标点的坐标设为图的顶点，如果在对应关键帧中有观测到该路标点则产生一条边，这就是共视图的定义。然后Essential graph则还要对权重也就是观测到的路标点进行设定，文中是100。

当中还利用卡方分布判断是内点还是外点，对外点进行删除。

关于求解对应SE（3）的雅各比矩阵可以在g2o/types里面都已经定义好了，所以可以直接进行优化。

#ifndef OPTIMIZER_H
#define OPTIMIZER_H#include "Map.h"
#include "MapPoint.h"
#include "KeyFrame.h"
#include "LoopClosing.h"
#include "Frame.h"#include "Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h"namespace ORB_SLAM2
{class LoopClosing;class Optimizer
{
public://进行BAvoid static BundleAdjustment(const std::vector<KeyFrame*> &vpKF, const std::vector<MapPoint*> &vpMP,int nIterations = 5, bool *pbStopFlag=NULL, const unsigned long nLoopKF=0,const bool bRobust = true);//进行全局BAvoid static GlobalBundleAdjustemnt(Map* pMap, int nIterations=5, bool *pbStopFlag=NULL,const unsigned long nLoopKF=0, const bool bRobust = true);//局部BAvoid static LocalBundleAdjustment(KeyFrame* pKF, bool *pbStopFlag, Map *pMap);//对POSE进行优化int static PoseOptimization(Frame* pFrame);// if bFixScale is true, 6DoF optimization (stereo,rgbd), 7DoF otherwise (mono)//优化本质图，双目和深度只有六个自由度，单目有七个自由度，多一个尺度void static OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* pCurKF,const LoopClosing::KeyFrameAndPose &NonCorrectedSim3,const LoopClosing::KeyFrameAndPose &CorrectedSim3,const map<KeyFrame *, set<KeyFrame *> > &LoopConnections,const bool &bFixScale);// if bFixScale is true, optimize SE3 (stereo,rgbd), Sim3 otherwise (mono)static int OptimizeSim3(KeyFrame* pKF1, KeyFrame* pKF2, std::vector<MapPoint *> &vpMatches1,g2o::Sim3 &g2oS12, const float th2, const bool bFixScale);
};} //namespace ORB_SLAM#endif // OPTIMIZER_H


#include "Optimizer.h"#include "Thirdparty/g2o/g2o/core/block_solver.h"
#include "Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.h"
#include "Thirdparty/g2o/g2o/solvers/linear_solver_eigen.h"
#include "Thirdparty/g2o/g2o/types/types_six_dof_expmap.h"
#include "Thirdparty/g2o/g2o/core/robust_kernel_impl.h"
#include "Thirdparty/g2o/g2o/solvers/linear_solver_dense.h"
#include "Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h"#include<Eigen/StdVector>#include "Converter.h"#include<mutex>namespace ORB_SLAM2
{void Optimizer::GlobalBundleAdjustemnt(Map* pMap, int nIterations, bool* pbStopFlag, const unsigned long nLoopKF, const bool bRobust)
{vector<KeyFrame*> vpKFs = pMap->GetAllKeyFrames();vector<MapPoint*> vpMP = pMap->GetAllMapPoints();BundleAdjustment(vpKFs,vpMP,nIterations,pbStopFlag, nLoopKF, bRobust);
}void Optimizer::BundleAdjustment(const vector<KeyFrame *> &vpKFs, const vector<MapPoint *> &vpMP,int nIterations, bool* pbStopFlag, const unsigned long nLoopKF, const bool bRobust)
{vector<bool> vbNotIncludedMP;vbNotIncludedMP.resize(vpMP.size());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);//定义使用LM算法进行求解g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);optimizer.setAlgorithm(solver);if(pbStopFlag)optimizer.setForceStopFlag(pbStopFlag);long unsigned int maxKFid = 0;// Set KeyFrame vertices//设置关键帧顶点for(size_t i=0; i<vpKFs.size(); i++){KeyFrame* pKF = vpKFs[i];if(pKF->isBad())continue;//SE(3)位姿g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();vSE3->setEstimate(Converter::toSE3Quat(pKF->GetPose()));vSE3->setId(pKF->mnId);vSE3->setFixed(pKF->mnId==0);optimizer.addVertex(vSE3);if(pKF->mnId>maxKFid)maxKFid=pKF->mnId;}//5.991,7.815是按照卡方分布不同自由度设置的阈值const float thHuber2D = sqrt(5.99);const float thHuber3D = sqrt(7.815);// Set MapPoint vertices//设置地图点顶点for(size_t i=0; i<vpMP.size(); i++){MapPoint* pMP = vpMP[i];if(pMP->isBad())continue;//存储地图点坐标g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));//id是继续之前关键帧之后继续增加的const int id = pMP->mnId+maxKFid+1;vPoint->setId(id);vPoint->setMarginalized(true);optimizer.addVertex(vPoint);const map<KeyFrame*,size_t> observations = pMP->GetObservations();int nEdges = 0;//SET EDGESfor(map<KeyFrame*,size_t>::const_iterator mit=observations.begin(); mit!=observations.end(); mit++){KeyFrame* pKF = mit->first;if(pKF->isBad() || pKF->mnId>maxKFid)continue;nEdges++;const cv::KeyPoint &kpUn = pKF->mvKeysUn[mit->second];//判断是是否为单目if(pKF->mvuRight[mit->second]<0){Eigen::Matrix<double,2,1> obs;obs << kpUn.pt.x, kpUn.pt.y;//BA中的重投影误差，将地图点投影到相机坐标系下的相机平面。//地图点位置固定，是二元边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(pKF->mnId)));e->setMeasurement(obs);const float &invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave];e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);//添加核函数if(bRobust){g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;e->setRobustKernel(rk);rk->setDelta(thHuber2D);}e->fx = pKF->fx;e->fy = pKF->fy;e->cx = pKF->cx;e->cy = pKF->cy;optimizer.addEdge(e);}//如果是双目的情况else{Eigen::Matrix<double,3,1> obs;const float kp_ur = pKF->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(pKF->mnId)));e->setMeasurement(obs);const float &invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave];Eigen::Matrix3d Info = Eigen::Matrix3d::Identity()*invSigma2;e->setInformation(Info);if(bRobust){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);}}//如果没有添加到边，则删除对应顶点if(nEdges==0){optimizer.removeVertex(vPoint);vbNotIncludedMP[i]=true;}else{vbNotIncludedMP[i]=false;}}// Optimize!optimizer.initializeOptimization();optimizer.optimize(nIterations);// Recover optimized data//Keyframes//得到优化后的Keyframes的posefor(size_t i=0; i<vpKFs.size(); i++){KeyFrame* pKF = vpKFs[i];if(pKF->isBad())continue;g2o::VertexSE3Expmap* vSE3 = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(pKF->mnId));g2o::SE3Quat SE3quat = vSE3->estimate();if(nLoopKF==0){pKF->SetPose(Converter::toCvMat(SE3quat));}else{pKF->mTcwGBA.create(4,4,CV_32F);Converter::toCvMat(SE3quat).copyTo(pKF->mTcwGBA);pKF->mnBAGlobalForKF = nLoopKF;}}//Points//得到路标点优化后的坐标for(size_t i=0; i<vpMP.size(); i++){if(vbNotIncludedMP[i])continue;MapPoint* pMP = vpMP[i];if(pMP->isBad())continue;g2o::VertexSBAPointXYZ* vPoint = static_cast<g2o::VertexSBAPointXYZ*>(optimizer.vertex(pMP->mnId+maxKFid+1));if(nLoopKF==0){pMP->SetWorldPos(Converter::toCvMat(vPoint->estimate()));pMP->UpdateNormalAndDepth();}else{pMP->mPosGBA.create(3,1,CV_32F);Converter::toCvMat(vPoint->estimate()).copyTo(pMP->mPosGBA);pMP->mnBAGlobalForKF = nLoopKF;}}}//只优化当前帧的pose，地图点固定
//用于LocalTracking中运动模型跟踪，参考帧跟踪，地图跟踪TrackLocalMap,重定位
int Optimizer::PoseOptimization(Frame *pFrame)
{g2o::SparseOptimizer optimizer;g2o::BlockSolver_6_3::LinearSolverType * linearSolver;linearSolver = new g2o::LinearSolverDense<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);int nInitialCorrespondences=0;// Set Frame vertexg2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();vSE3->setEstimate(Converter::toSE3Quat(pFrame->mTcw));vSE3->setId(0);vSE3->setFixed(false);optimizer.addVertex(vSE3);// Set MapPoint verticesconst int N = pFrame->N;vector<g2o::EdgeSE3ProjectXYZOnlyPose*> vpEdgesMono;vector<size_t> vnIndexEdgeMono;vpEdgesMono.reserve(N);vnIndexEdgeMono.reserve(N);vector<g2o::EdgeStereoSE3ProjectXYZOnlyPose*> vpEdgesStereo;vector<size_t> vnIndexEdgeStereo;vpEdgesStereo.reserve(N);vnIndexEdgeStereo.reserve(N);const float deltaMono = sqrt(5.991);const float deltaStereo = sqrt(7.815);{unique_lock<mutex> lock(MapPoint::mGlobalMutex);for(int i=0; i<N; i++){MapPoint* pMP = pFrame->mvpMapPoints[i];if(pMP){// Monocular observationif(pFrame->mvuRight[i]<0){nInitialCorrespondences++;pFrame->mvbOutlier[i] = false;Eigen::Matrix<double,2,1> obs;const cv::KeyPoint &kpUn = pFrame->mvKeysUn[i];obs << kpUn.pt.x, kpUn.pt.y;//一元边g2o::EdgeSE3ProjectXYZOnlyPose* e = new g2o::EdgeSE3ProjectXYZOnlyPose();e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));e->setMeasurement(obs);const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave];e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;e->setRobustKernel(rk);rk->setDelta(deltaMono);e->fx = pFrame->fx;e->fy = pFrame->fy;e->cx = pFrame->cx;e->cy = pFrame->cy;cv::Mat Xw = pMP->GetWorldPos();e->Xw[0] = Xw.at<float>(0);e->Xw[1] = Xw.at<float>(1);e->Xw[2] = Xw.at<float>(2);optimizer.addEdge(e);vpEdgesMono.push_back(e);vnIndexEdgeMono.push_back(i);}else  // Stereo observation{nInitialCorrespondences++;pFrame->mvbOutlier[i] = false;//SET EDGEEigen::Matrix<double,3,1> obs;const cv::KeyPoint &kpUn = pFrame->mvKeysUn[i];const float &kp_ur = pFrame->mvuRight[i];obs << kpUn.pt.x, kpUn.pt.y, kp_ur;g2o::EdgeStereoSE3ProjectXYZOnlyPose* e = new g2o::EdgeStereoSE3ProjectXYZOnlyPose();e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));e->setMeasurement(obs);const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave];Eigen::Matrix3d Info = Eigen::Matrix3d::Identity()*invSigma2;e->setInformation(Info);g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;e->setRobustKernel(rk);rk->setDelta(deltaStereo);e->fx = pFrame->fx;e->fy = pFrame->fy;e->cx = pFrame->cx;e->cy = pFrame->cy;e->bf = pFrame->mbf;cv::Mat Xw = pMP->GetWorldPos();e->Xw[0] = Xw.at<float>(0);e->Xw[1] = Xw.at<float>(1);e->Xw[2] = Xw.at<float>(2);optimizer.addEdge(e);vpEdgesStereo.push_back(e);vnIndexEdgeStereo.push_back(i);}}}}if(nInitialCorrespondences<3)return 0;// We perform 4 optimizations, after each optimization we classify observation as inlier/outlier// At the next optimization, outliers are not included, but at the end they can be classified as inliers again.const float chi2Mono[4]={5.991,5.991,5.991,5.991};const float chi2Stereo[4]={7.815,7.815,7.815, 7.815};const int its[4]={10,10,10,10};    int nBad=0;for(size_t it=0; it<4; it++){vSE3->setEstimate(Converter::toSE3Quat(pFrame->mTcw));optimizer.initializeOptimization(0);optimizer.optimize(its[it]);nBad=0;for(size_t i=0, iend=vpEdgesMono.size(); i<iend; i++){g2o::EdgeSE3ProjectXYZOnlyPose* e = vpEdgesMono[i];const size_t idx = vnIndexEdgeMono[i];if(pFrame->mvbOutlier[idx]){e->computeError();}const float chi2 = e->chi2();if(chi2>chi2Mono[it]){                pFrame->mvbOutlier[idx]=true;e->setLevel(1);nBad++;}else{pFrame->mvbOutlier[idx]=false;e->setLevel(0);}if(it==2)e->setRobustKernel(0);}for(size_t i=0, iend=vpEdgesStereo.size(); i<iend; i++){g2o::EdgeStereoSE3ProjectXYZOnlyPose* e = vpEdgesStereo[i];const size_t idx = vnIndexEdgeStereo[i];if(pFrame->mvbOutlier[idx]){e->computeError();}const float chi2 = e->chi2();if(chi2>chi2Stereo[it]){pFrame->mvbOutlier[idx]=true;e->setLevel(1);nBad++;}else{                e->setLevel(0);pFrame->mvbOutlier[idx]=false;}if(it==2)e->setRobustKernel(0);}if(optimizer.edges().size()<10)break;}    // Recover optimized pose and return number of inliersg2o::VertexSE3Expmap* vSE3_recov = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(0));g2o::SE3Quat SE3quat_recov = vSE3_recov->estimate();cv::Mat pose = Converter::toCvMat(SE3quat_recov);pFrame->SetPose(pose);return nInitialCorrespondences-nBad;
}//局部BA
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();}
}//优化本质图
// EssentialGraph包括所有的关键帧顶点，但是优化的边大大减少，包括spanning tree，共识权重，以及闭环连接边
void Optimizer::OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* pCurKF,const LoopClosing::KeyFrameAndPose &NonCorrectedSim3,const LoopClosing::KeyFrameAndPose &CorrectedSim3,const map<KeyFrame *, set<KeyFrame *> > &LoopConnections, const bool &bFixScale)
{// Setup optimizerg2o::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);const vector<KeyFrame*> vpKFs = pMap->GetAllKeyFrames();const vector<MapPoint*> vpMPs = pMap->GetAllMapPoints();const unsigned int nMaxKFid = pMap->GetMaxKFid();vector<g2o::Sim3,Eigen::aligned_allocator<g2o::Sim3> > vScw(nMaxKFid+1);vector<g2o::Sim3,Eigen::aligned_allocator<g2o::Sim3> > vCorrectedSwc(nMaxKFid+1);vector<g2o::VertexSim3Expmap*> vpVertices(nMaxKFid+1);//权重阈值的设定const int minFeat = 100;// Set KeyFrame verticesfor(size_t i=0, iend=vpKFs.size(); i<iend;i++){KeyFrame* pKF = vpKFs[i];if(pKF->isBad())continue;g2o::VertexSim3Expmap* VSim3 = new g2o::VertexSim3Expmap();const int nIDi = pKF->mnId;LoopClosing::KeyFrameAndPose::const_iterator it = CorrectedSim3.find(pKF);if(it!=CorrectedSim3.end()){vScw[nIDi] = it->second;VSim3->setEstimate(it->second);}else{Eigen::Matrix<double,3,3> Rcw = Converter::toMatrix3d(pKF->GetRotation());Eigen::Matrix<double,3,1> tcw = Converter::toVector3d(pKF->GetTranslation());g2o::Sim3 Siw(Rcw,tcw,1.0);vScw[nIDi] = Siw;VSim3->setEstimate(Siw);}if(pKF==pLoopKF)VSim3->setFixed(true);VSim3->setId(nIDi);VSim3->setMarginalized(false);VSim3->_fix_scale = bFixScale;optimizer.addVertex(VSim3);vpVertices[nIDi]=VSim3;}set<pair<long unsigned int,long unsigned int> > sInsertedEdges;const Eigen::Matrix<double,7,7> matLambda = Eigen::Matrix<double,7,7>::Identity();// Set Loop edgesfor(map<KeyFrame *, set<KeyFrame *> >::const_iterator mit = LoopConnections.begin(), mend=LoopConnections.end(); mit!=mend; mit++){KeyFrame* pKF = mit->first;const long unsigned int nIDi = pKF->mnId;const set<KeyFrame*> &spConnections = mit->second;const g2o::Sim3 Siw = vScw[nIDi];const g2o::Sim3 Swi = Siw.inverse();for(set<KeyFrame*>::const_iterator sit=spConnections.begin(), send=spConnections.end(); sit!=send; sit++){const long unsigned int nIDj = (*sit)->mnId;if((nIDi!=pCurKF->mnId || nIDj!=pLoopKF->mnId) && pKF->GetWeight(*sit)<minFeat)continue;const g2o::Sim3 Sjw = vScw[nIDj];const 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);sInsertedEdges.insert(make_pair(min(nIDi,nIDj),max(nIDi,nIDj)));}}// Set normal edgesfor(size_t i=0, iend=vpKFs.size(); i<iend; i++){KeyFrame* pKF = vpKFs[i];const int nIDi = pKF->mnId;g2o::Sim3 Swi;LoopClosing::KeyFrameAndPose::const_iterator iti = NonCorrectedSim3.find(pKF);if(iti!=NonCorrectedSim3.end())Swi = (iti->second).inverse();elseSwi = vScw[nIDi].inverse();KeyFrame* pParentKF = pKF->GetParent();// Spanning tree edgeif(pParentKF){int nIDj = pParentKF->mnId;g2o::Sim3 Sjw;LoopClosing::KeyFrameAndPose::const_iterator itj = NonCorrectedSim3.find(pParentKF);if(itj!=NonCorrectedSim3.end())Sjw = itj->second;elseSjw = 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);}// Loop edgesconst set<KeyFrame*> sLoopEdges = pKF->GetLoopEdges();for(set<KeyFrame*>::const_iterator sit=sLoopEdges.begin(), send=sLoopEdges.end(); sit!=send; sit++){KeyFrame* pLKF = *sit;if(pLKF->mnId<pKF->mnId){g2o::Sim3 Slw;LoopClosing::KeyFrameAndPose::const_iterator itl = NonCorrectedSim3.find(pLKF);if(itl!=NonCorrectedSim3.end())Slw = itl->second;elseSlw = 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);}}// Covisibility graph edgesconst vector<KeyFrame*> vpConnectedKFs = pKF->GetCovisiblesByWeight(minFeat);for(vector<KeyFrame*>::const_iterator vit=vpConnectedKFs.begin(); vit!=vpConnectedKFs.end(); vit++){KeyFrame* pKFn = *vit;if(pKFn && pKFn!=pParentKF && !pKF->hasChild(pKFn) && !sLoopEdges.count(pKFn)){if(!pKFn->isBad() && pKFn->mnId<pKF->mnId){if(sInsertedEdges.count(make_pair(min(pKF->mnId,pKFn->mnId),max(pKF->mnId,pKFn->mnId))))continue;g2o::Sim3 Snw;LoopClosing::KeyFrameAndPose::const_iterator itn = NonCorrectedSim3.find(pKFn);if(itn!=NonCorrectedSim3.end())Snw = itn->second;elseSnw = 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);}}}}// Optimize!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]for(size_t i=0;i<vpKFs.size();i++){KeyFrame* pKFi = vpKFs[i];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->SetPose(Tiw);}// Correct points. Transform to "non-optimized" reference keyframe pose and transform back with optimized posefor(size_t i=0, iend=vpMPs.size(); i<iend; i++){MapPoint* pMP = vpMPs[i];if(pMP->isBad())continue;int nIDr;if(pMP->mnCorrectedByKF==pCurKF->mnId){nIDr = pMP->mnCorrectedReference;}else{KeyFrame* pRefKF = pMP->GetReferenceKeyFrame();nIDr = pRefKF->mnId;}g2o::Sim3 Srw = vScw[nIDr];g2o::Sim3 correctedSwr = vCorrectedSwc[nIDr];cv::Mat P3Dw = pMP->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);pMP->SetWorldPos(cvCorrectedP3Dw);pMP->UpdateNormalAndDepth();}
}//在用RANSAC求解过Sim3，以及通过Sim3匹配更多的地图点后，对当前关键帧，闭环关键帧，
// 以及匹配的地图点进行优化，获得更准确的Sim3位姿，再去下一步的闭环调整。
int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &vpMatches1, g2o::Sim3 &g2oS12, const float th2, const bool bFixScale)
{g2o::SparseOptimizer optimizer;g2o::BlockSolverX::LinearSolverType * linearSolver;linearSolver = new g2o::LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>();g2o::BlockSolverX * solver_ptr = new g2o::BlockSolverX(linearSolver);g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);optimizer.setAlgorithm(solver);// Calibrationconst cv::Mat &K1 = pKF1->mK;const cv::Mat &K2 = pKF2->mK;// Camera posesconst 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 vertexg2o::VertexSim3Expmap * vSim3 = new g2o::VertexSim3Expmap();    vSim3->_fix_scale=bFixScale;vSim3->setEstimate(g2oS12);vSim3->setId(0);vSim3->setFixed(false);vSim3->_principle_point1[0] = K1.at<float>(0,2);vSim3->_principle_point1[1] = K1.at<float>(1,2);vSim3->_focal_length1[0] = K1.at<float>(0,0);vSim3->_focal_length1[1] = K1.at<float>(1,1);vSim3->_principle_point2[0] = K2.at<float>(0,2);vSim3->_principle_point2[1] = K2.at<float>(1,2);vSim3->_focal_length2[0] = K2.at<float>(0,0);vSim3->_focal_length2[1] = K2.at<float>(1,1);optimizer.addVertex(vSim3);// Set MapPoint verticesconst int N = vpMatches1.size();const vector<MapPoint*> vpMapPoints1 = pKF1->GetMapPointMatches();vector<g2o::EdgeSim3ProjectXYZ*> vpEdges12;vector<g2o::EdgeInverseSim3ProjectXYZ*> vpEdges21;vector<size_t> vnIndexEdge;vnIndexEdge.reserve(2*N);vpEdges12.reserve(2*N);vpEdges21.reserve(2*N);const float deltaHuber = sqrt(th2);int nCorrespondences = 0;for(int i=0; i<N; i++){if(!vpMatches1[i])continue;MapPoint* pMP1 = vpMapPoints1[i];MapPoint* pMP2 = vpMatches1[i];const int id1 = 2*i+1;const int id2 = 2*(i+1);const int i2 = pMP2->GetIndexInKeyFrame(pKF2);if(pMP1 && pMP2){if(!pMP1->isBad() && !pMP2->isBad() && i2>=0){g2o::VertexSBAPointXYZ* vPoint1 = new g2o::VertexSBAPointXYZ();cv::Mat P3D1w = pMP1->GetWorldPos();cv::Mat P3D1c = R1w*P3D1w + t1w;vPoint1->setEstimate(Converter::toVector3d(P3D1c));vPoint1->setId(id1);vPoint1->setFixed(true);optimizer.addVertex(vPoint1);g2o::VertexSBAPointXYZ* vPoint2 = new g2o::VertexSBAPointXYZ();cv::Mat P3D2w = pMP2->GetWorldPos();cv::Mat P3D2c = R2w*P3D2w + t2w;vPoint2->setEstimate(Converter::toVector3d(P3D2c));vPoint2->setId(id2);vPoint2->setFixed(true);optimizer.addVertex(vPoint2);}elsecontinue;}elsecontinue;nCorrespondences++;// Set edge x1 = S12*X2Eigen::Matrix<double,2,1> obs1;const cv::KeyPoint &kpUn1 = pKF1->mvKeysUn[i];obs1 << kpUn1.pt.x, kpUn1.pt.y;g2o::EdgeSim3ProjectXYZ* e12 = new g2o::EdgeSim3ProjectXYZ();e12->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id2)));e12->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));e12->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*X1Eigen::Matrix<double,2,1> obs2;const cv::KeyPoint &kpUn2 = pKF2->mvKeysUn[i2];obs2 << kpUn2.pt.x, kpUn2.pt.y;g2o::EdgeInverseSim3ProjectXYZ* e21 = new g2o::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);}// Optimize!optimizer.initializeOptimization();optimizer.optimize(5);// Check inliersint nBad=0;for(size_t i=0; i<vpEdges12.size();i++){g2o::EdgeSim3ProjectXYZ* e12 = vpEdges12[i];g2o::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<g2o::EdgeSim3ProjectXYZ*>(NULL);vpEdges21[i]=static_cast<g2o::EdgeInverseSim3ProjectXYZ*>(NULL);nBad++;}}int nMoreIterations;if(nBad>0)nMoreIterations=10;elsenMoreIterations=5;if(nCorrespondences-nBad<10)return 0;// Optimize again only with inliersoptimizer.initializeOptimization();optimizer.optimize(nMoreIterations);int nIn = 0;for(size_t i=0; i<vpEdges12.size();i++){g2o::EdgeSim3ProjectXYZ* e12 = vpEdges12[i];g2o::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);}elsenIn++;}// Recover optimized Sim3g2o::VertexSim3Expmap* vSim3_recov = static_cast<g2o::VertexSim3Expmap*>(optimizer.vertex(0));g2oS12= vSim3_recov->estimate();return nIn;
}} //namespace ORB_SLAM

ORB-SLAM2的源码阅读（十二）：Optimizer类相关推荐

【vue-router源码】十二、useRoute、useRouter、useLink源码分析
[vue-rouer源码]系列文章 [vue-router源码]一.router.install解析 [vue-router源码]二.createWebHistory.createWebHashHis ...
Alibaba Druid 源码阅读（二）数据库连接池实现初步探索
Alibaba Druid 源码阅读(二) 数据库连接池实现初步探索简介在上篇文章中,了解了连接池的应用场景和本地运行了示例,本篇文章中,我们尝试来探索下Alibaba Druid数据库连接池的整 ...
Soul 网关源码阅读（二）代码初步运行
Soul 源码阅读(二)代码初步运行简介基于上篇:Soul 源码阅读(一) 概览,这部分跑一下Soul网关的示例过程记录现在我们可以根据地图,稍微探索一下周边,摸一摸 ...
价值4500的国际版多语言点赞抖音分享点赞任务平台源码（十二种语言）
介绍: 平台会员分享给我的,他自己搭建成功了,测试可用!我就不测试了,需要的拿! 九种语言 :西班牙语,泰语.日语,印度尼西亚语言.越南语言.英文.繁体中文,简体中文,印度语前台支持更换5种颜色风格 ...
12 哈希表相关类——Live555源码阅读(一)基本组件类
12 哈希表相关类--Live555源码阅读(一)基本组件类这是Live555源码阅读的第一部分,包括了时间类,延时队列类,处理程序描述类,哈希表类这四个大类. 本文由乌合之众 lym瞎编,欢迎转载 ...
Mybatis源码阅读之二——模板方法模式与Executor
[系列目录] Mybatis源码阅读之一--工厂模式与SqlSessionFactory 文章目录一. 模板方法模式二. 同步回调与匿名函数三. Executor BaseExecutor与其子 ...
gin context和官方context_gin 源码阅读（二）路由和路由组
" 上一篇讲的是gin 框架的启动原理,今天来讲一下 gin 路由的实现. 1 用法还是老样子,先从使用方式开始: func main() { r := gin.Default() r.G ...
Rpc框架dubbo-client（v2.6.3）源码阅读（二）
接上一篇 dubbo-server 之后,再来看一下 dubbo-client 是如何工作的. dubbo提供者服务示例, 其结构是这样的! dubbo://192.168.11.6:20880/co ...
Lidar_imu自动标定源码阅读（二）——calibration部分
源码阅读,能力有限,如有某处理解错误,请指出,谢谢. Lidar_parser_base.h:激光雷达分析器基础 #pragma once#include <pcl/point_cloud.h& ...
16 BasicHashTable基本哈希表类(三)——Live555源码阅读(一)基本组件类
这是Live555源码阅读的第一部分,包括了时间类,延时队列类,处理程序描述类,哈希表类这四个大类. 本文由乌合之众 lym瞎编,欢迎转载 http://www.cnblogs.com/oloroso ...

ORB-SLAM2的源码阅读（十二）：Optimizer类

ORB-SLAM2的源码阅读（十二）：Optimizer类相关推荐

最新文章

热门文章