1. g2o介绍

2. g2o库结构

2.1 g2o图优化库的组成结构

2.2 常用的类结构

a. 块求解器 BlockSolver

b. 块求解器特性 BlockSolverTraits

c. 块求解器特性中的线性求解器 Traits::LinearSolverType

d. 优化算法 OptimizationAlgorithm

e. 稀疏优化器 SparseOptimizer

f. 表示优化项的顶点—Vertex

g. 表示误差项的边—Edge

3. 示例：使用g2o进行曲线拟合

参考 6.4节参考SLAM十四讲

G2O的安装：参考SLAM十四讲，若安装在自定义路径，则在你的工程中的CMakeLists.txt中添加以下两行（指定include路径和 g2o库路径）即可。

# CMakeLists.txt中添加如下两行：
link_directories("/home/path-to-your-libs/lib")          # the file path of libg2o_core.so
set (G2O_INCLUDE_DIR "/home/path-to-your-libs/include")  # the file path of g2o folder# 例如：
cmake_minimum_required( VERSION 2.8 )
project( vo1 )set( CMAKE_BUILD_TYPE "Release" )
set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )# ...# 添加cmake模块以使用g2o
list( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules ) # 该目录下放入FindG2O.cmakelink_directories("/home/john/workspace/johnlibs/lib")          # libg2o_core.so 所在路径
set (G2O_INCLUDE_DIR "/home/john/workspace/johnlibs/include")  # g2o 文件夹所在路径find_package( G2O REQUIRED )
find_package( CSparse REQUIRED )
# ...

1. g2o介绍

g2o （General Graphic Optimization，G2O）。它是一个基于图优化的库。图优化是一种将非线

性优化与图论结合起来的理论。

非线性最小二乘的求解方式由很多个误差项之和组成的。然而，仅有一组优化变量和许多个误差项，我们并不清楚它们之间的关联。比方说，某一个优化变量 xj 存在于多少个误差项里呢？我们能保证对它的优化是有意义的吗？进一步，我们希望能够直观地看到该优化问题长什么样。于是，就说到了图优化。图优化，是把优化问题表现成图（Graph）的一种方式。这里的图是图论意义上的图。

一个图由若干个顶点（Vertex），以及连接着这些节点的边（Edge）组成。进而，用顶点表示优化变量，用边表示误差项。于是，对任意一个上述形式的非线性最小二乘问题，我们可以构建与之对应的一个图。上图是一个简单的图优化例子。我们用三角形表示相机位姿节点，用圆形表示路标点，它们构成了图优化的顶点；同时，蓝色线表示相机的运动模型，红色虚线表示观测模型，它们构成了图优化的边。此时，虽然整个问题的数学形式仍是式（6.12）那样，但现在我们可以直观地看到问题的结构了。

如果我们希望，也可以做去掉孤立顶点或优先优化边数较多（或按图论的术语，度数较大）的顶点这样的改进。但是最基本的图优化，是用图模型来表达一个非线性最小二乘的优化问题。而我们可以利用图模型的某些性质，做更好的优化。g2o 为 SLAM 提供了图优化所需的内容。

2. g2o库结构

2.1 g2o图优化库的组成结构

2.2 常用的类结构

a. 块求解器 BlockSolver

继承关系: BlockSolver -继承自-> BlockSolverBase -继承自-> Solver.

Solver主要功能：主要是对优化算法进行求解。（官方：一个稀疏求解器（sparse solver）的通用接口，它在一个图上求解线性化目标函数的一次迭代）
BlockSolver ：实现了对Hessian的块进行操作的求解器。

  template <typename Traits>class BlockSolver: public BlockSolverBase {public:static const int PoseDim = Traits::PoseDim;static const int LandmarkDim = Traits::LandmarkDim;typedef typename Traits::PoseMatrixType PoseMatrixType;typedef typename Traits::LandmarkMatrixType LandmarkMatrixType; typedef typename Traits::PoseLandmarkMatrixType PoseLandmarkMatrixType;typedef typename Traits::PoseVectorType PoseVectorType;typedef typename Traits::LandmarkVectorType LandmarkVectorType;typedef typename Traits::PoseHessianType PoseHessianType;typedef typename Traits::LandmarkHessianType LandmarkHessianType;typedef typename Traits::PoseLandmarkHessianType PoseLandmarkHessianType;typedef typename Traits::LinearSolverType LinearSolverType;public:/*** allocate a block solver ontop of the underlying linear solver.* NOTE: The BlockSolver assumes exclusive access to the linear solver and will therefore free the pointer* in its destructor.*/BlockSolver(LinearSolverType* linearSolver); //注意这行！构造函数~BlockSolver();virtual bool init(SparseOptimizer* optmizer, bool online = false);virtual bool buildStructure(bool zeroBlocks = false);virtual bool updateStructure(const std::vector<HyperGraph::Vertex*>& vset, const HyperGraph::EdgeSet& edges);virtual bool buildSystem();virtual bool solve();virtual bool computeMarginals(SparseBlockMatrix<MatrixXD>& spinv, const std::vector<std::pair<int, int> >& blockIndices);virtual bool setLambda(double lambda, bool backup = false);virtual void restoreDiagonal();virtual bool supportsSchur() {return true;}virtual bool schur() { return _doSchur;}virtual void setSchur(bool s) { _doSchur = s;}LinearSolver<PoseMatrixType>* linearSolver() const { return _linearSolver;}virtual void setWriteDebug(bool writeDebug);virtual bool writeDebug() const {return _linearSolver->writeDebug();}virtual bool saveHessian(const std::string& fileName) const;virtual void multiplyHessian(double* dest, const double* src) const { _Hpp->multiplySymmetricUpperTriangle(dest, src);}protected:void resize(int* blockPoseIndices, int numPoseBlocks, int* blockLandmarkIndices, int numLandmarkBlocks, int totalDim);void deallocate();SparseBlockMatrix<PoseMatrixType>* _Hpp;SparseBlockMatrix<LandmarkMatrixType>* _Hll;SparseBlockMatrix<PoseLandmarkMatrixType>* _Hpl;SparseBlockMatrix<PoseMatrixType>* _Hschur;SparseBlockMatrixDiagonal<LandmarkMatrixType>* _DInvSchur;SparseBlockMatrixCCS<PoseLandmarkMatrixType>* _HplCCS;SparseBlockMatrixCCS<PoseMatrixType>* _HschurTransposedCCS;LinearSolver<PoseMatrixType>* _linearSolver;std::vector<PoseVectorType, Eigen::aligned_allocator<PoseVectorType> > _diagonalBackupPose;std::vector<LandmarkVectorType, Eigen::aligned_allocator<LandmarkVectorType> > _diagonalBackupLandmark;#    ifdef G2O_OPENMPstd::vector<OpenMPMutex> _coefficientsMutex;
#    endifbool _doSchur;double* _coefficients;double* _bschur;int _numPoses, _numLandmarks;int _sizePoses, _sizeLandmarks;};

从上述类的定义可以看出，BlockSolver 需要使用模板参数 Traits 中的线性求解器类型Traits::LinearSolverType 对象进行初始化。

b. 块求解器特性 BlockSolverTraits

BlockSolverTraits: 固定尺寸优化问题的特点结构， g2o中提供了两种该Traits。

  /*** \brief traits to summarize the properties of the fixed size optimization problem*/template <int _PoseDim, int _LandmarkDim>struct BlockSolverTraits{static const int PoseDim = _PoseDim;static const int LandmarkDim = _LandmarkDim;typedef Eigen::Matrix<double, PoseDim, PoseDim, Eigen::ColMajor> PoseMatrixType;typedef Eigen::Matrix<double, LandmarkDim, LandmarkDim, Eigen::ColMajor> LandmarkMatrixType;typedef Eigen::Matrix<double, PoseDim, LandmarkDim, Eigen::ColMajor> PoseLandmarkMatrixType;typedef Eigen::Matrix<double, PoseDim, 1, Eigen::ColMajor> PoseVectorType;typedef Eigen::Matrix<double, LandmarkDim, 1, Eigen::ColMajor> LandmarkVectorType;typedef SparseBlockMatrix<PoseMatrixType> PoseHessianType;typedef SparseBlockMatrix<LandmarkMatrixType> LandmarkHessianType;typedef SparseBlockMatrix<PoseLandmarkMatrixType> PoseLandmarkHessianType;typedef LinearSolver<PoseMatrixType> LinearSolverType; //注意这行！};/*** \brief traits to summarize the properties of the dynamic size optimization problem*/template <>struct BlockSolverTraits<Eigen::Dynamic, Eigen::Dynamic>{static const int PoseDim = Eigen::Dynamic;static const int LandmarkDim = Eigen::Dynamic;typedef MatrixXD PoseMatrixType;typedef MatrixXD LandmarkMatrixType;typedef MatrixXD PoseLandmarkMatrixType;typedef VectorXD PoseVectorType;typedef VectorXD LandmarkVectorType;typedef SparseBlockMatrix<PoseMatrixType> PoseHessianType;typedef SparseBlockMatrix<LandmarkMatrixType> LandmarkHessianType;typedef SparseBlockMatrix<PoseLandmarkMatrixType> PoseLandmarkHessianType;typedef LinearSolver<PoseMatrixType> LinearSolverType;};

c. 块求解器特性中的线性求解器 Traits::LinearSolverType

即为类 template <typename MatrixType> class LinearSolver{...}, 它的派生类有很多（在源文件include/g2o/solvers/文件夹下），比如 LinearSolverCCS类(派生出 LinearSolverCholmod类、LinearSolverCSparse类 )、LinearSolverPCG类、LinearSolverEigen类、LinearSolverDense类。

综上，定义一个BlockSolver，可以这样：

    typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block;  // 每个误差项优化变量维度为3，误差值维度为1Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); // 线性方程求解器Block* solver_ptr = new Block( linearSolver );      // 矩阵块求解器

d. 优化算法 OptimizationAlgorithm

继承关系：OptimizationAlgorithmLevenberg -继承自-> OptimizationAlgorithmWithHessian -继承自-> OptimizationAlgorithm。而LM优化器需要一个Solver来构造。

  class G2O_CORE_API OptimizationAlgorithmLevenberg : public OptimizationAlgorithmWithHessian{public:/*** construct the Levenberg algorithm, which will use the given Solver for solving the* linearized system.*/explicit OptimizationAlgorithmLevenberg(Solver* solver);//....}

e. 稀疏优化器 SparseOptimizer

SparseOptimizer是g2o的核心，它是一个Optimizable Graph，也是一个Hyper Graph（由上图可知），包含了许多的顶点和边。在优化之前，需要预先设置求解器和优化算法。

继承关系：SparseOptimizer -继承自-> OptimizableGraph -继承自-> HyperGraph

SparseOptimizer 的主要方法：

  class G2O_CORE_API SparseOptimizer : public OptimizableGraph {// Attention: _solver & _statistics is own by SparseOptimizer and will be// deleted in its destructor.SparseOptimizer();virtual ~SparseOptimizer();//! verbose information during optimizationbool verbose()  const {return _verbose;}void setVerbose(bool verbose);// new interface for the optimizer// the old functions will be dropped/*** Initializes the structures for optimizing a portion of the graph specified by a subset of edges.* Before calling it be sure to invoke marginalized() and fixed() to the vertices you want to include in the * schur complement or to set as fixed during the optimization.* @param eset: the subgraph to be optimized.* @returns false if somethings goes wrong*/virtual bool initializeOptimization(HyperGraph::EdgeSet& eset);/*** Initializes the structures for optimizing a portion of the graph specified by a subset of vertices.* Before calling it be sure to invoke marginalized() and fixed() to the vertices you want to include in the * schur complement or to set as fixed during the optimization.* @param vset: the subgraph to be optimized.* @param level: is the level (in multilevel optimization)* @returns false if somethings goes wrong*/virtual bool initializeOptimization(HyperGraph::VertexSet& vset, int level=0);/*** Initializes the structures for optimizing the whole graph.* Before calling it be sure to invoke marginalized() and fixed() to the vertices you want to include in the * schur complement or to set as fixed during the optimization.* @param level: is the level (in multilevel optimization)* @returns false if somethings goes wrong*/virtual bool initializeOptimization(int level=0);/*** starts one optimization run given the current configuration of the graph, * and the current settings stored in the class instance.* It can be called only after initializeOptimization*/int optimize(int iterations, bool online = false);

SparseOptimizer常用的继承自 OptimizableGraph 的方法：

  struct G2O_CORE_API OptimizableGraph : public HyperGraph {// .../*** adds a new vertex. The new vertex is then "taken".* @return false if a vertex with the same id as v is already in the graph, true otherwise.*/virtual bool addVertex(HyperGraph::Vertex* v, Data* userData);virtual bool addVertex(HyperGraph::Vertex* v) { return addVertex(v, 0);}/*** adds a new edge.* The edge should point to the vertices that it is connecting (setFrom/setTo).* @return false if the insertion does not work (incompatible types of the vertices/missing vertex). true otherwise.*/virtual bool addEdge(HyperGraph::Edge* e);// ...}

综上，定义一个稀疏优化器，可以这样：

    // 梯度下降方法，从GN, LM, DogLeg 中选g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );g2o::SparseOptimizer optimizer;     // 图模型optimizer.setAlgorithm( solver );   // 设置求解器optimizer.setVerbose( true );       // 打开调试输出// 另外类似这样使用：optimizer.addVertex( v );optimizer.addEdge( edge );optimizer.initializeOptimization();optimizer.optimize(100);

f. 表示优化项的顶点—Vertex

BaseVertex

每一个自定义的顶点，都应该继承自 BaseVertex类。

BaseVertex类的继承关系：BaseVertex -继承自-> OptimizableGraph::Vertex -继承自-> HyperGraph::Vertex （和 HyperGraph::DataContainer） -继承自-> HyperGraphElement。

  template <int D, typename T>class BaseVertex : public OptimizableGraph::Vertex {public:typedef T EstimateType;static const int Dimension = D;           ///< dimension of the estimate (minimal) in the manifold spacepublic:BaseVertex();// ...}

BaseVertex 有两个模板参数：

int D：维度，在流形空间（manifold）的最小维度。（优化项的维度）
typename T：顶点（状态变量）的类型。（优化项的数据类型）
头文件：g2o/core/base_vertex.h

VertexSE3Expmap

继承自 BaseVertex , 表示李代数的相机位姿。 class VertexSE3Expmap : public BaseVertex<6, SE3Quat> {...}

参数6 ：SE3Quat类型为六维，三维旋转，三维平移。
参数SE3Quat ：该类型旋转在前，平移在后，注意：类型内部使用的其实是四元数，不是李代数
头文件：g2o/types/slam3d/se3quat.h

需要设置的参数：

g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
//【1】设置待优化位姿（这是粗略位姿）
vSE3->setEstimate(Converter::toSE3Quat(pKFi->GetPose()));
//【2】设置Id号
vSE3->setId(pKFi->mnId);
//【3】设置是否固定，第一帧固定
vSE3->setFixed(pKFi->mnId==0)

VertexSBAPointXYZ

继承自 BaseVertex，表示空间点位置（地图点节点）。class VertexSBAPointXYZ : public BaseVertex<3, Vector3d> {//...}
需要设置的参数：

g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();
//【1】设置待优化空间点3D位置XYZ
vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));
//【2】设置Id号
vPoint->setId(id);
//【3】是否边缘化（以便稀疏化求解）
vPoint->setMarginalized(true);

自定义顶点，需要重写的函数

//顶点重置函数，设定被优化变量的原始值。
virtual void setToOriginImpl();//顶点更新函数。非常重要的一个函数，主要用于优化过程中增量△x 的计算。我们根据增量方
//程计算出增量之后，就是通过这个函数对估计值进行调整的，因此这个函数的内容一定要重视。
virtual void oplusImpl (const number_t* update);//读盘函数,仅仅声明一下就可以
virtual bool read(std::istream& is);//存盘函数，仅仅声明一下就可以 一般情况下不需要进行读/写操作的话，仅仅声明一下就可以
virtual bool write(std::ostream& os) const;

g. 表示误差项的边—Edge

边分为一元边、二元边、多元边（BaseUnaryEdge，BaseBinaryEdge，BaseMultiEdge）它们均是继承自BaseEdge类。

继承关系： BaseEdge -继承自-> OptimizableGraph::Edge -继承自-> HyperGraph::Edge （和HyperGraph::DataContainer） -继承自-> HyperGraphElement

EdgeSE3ProjectXYZOnlyPose （位姿一元边）（旧版本）

class EdgeSE3ProjectXYZOnlyPose: public BaseUnaryEdge<2, Vector2d, VertexSE3Expmap> {//...}

作用：仅优化相机位姿，为了构造出投影方程，需要按下面的方式把MapPoints的位置作为常量加入。需要设置的参数：

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); /** @attention 设置阈值，卡方自由度为2，内点概率95%对应的临界值*/e->fx = pFrame->fx;
e->fy = pFrame->fy;
e->cx = pFrame->cx;
e->cy = pFrame->cy;/** @attention 需要在这里设置<不做优化>的MapPoints的位置*/
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);

误差计算：（误差 = 图像2d观测值 - 3d点经估计的相机位姿和相机内参K投影到图像平面的2d点值）

  void computeError()  {const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[0]);Vector2d obs(_measurement);_error = obs-cam_project(v1->estimate().map(Xw));}

BaseBinaryEdge（二元边）

  template <int D, typename E, typename VertexXi, typename VertexXj>class BaseBinaryEdge : public BaseEdge<D, E>{public:typedef VertexXi VertexXiType;typedef VertexXj VertexXjType;static const int Di = VertexXiType::Dimension;static const int Dj = VertexXjType::Dimension;static const int Dimension = BaseEdge<D, E>::Dimension;typedef typename BaseEdge<D,E>::Measurement Measurement;BaseBinaryEdge() : BaseEdge<D,E>(),// ...}

参数D：测量值的维度。
参数E：测量值的数据类型。
VertexXi：顶点的类型。
VertexXj：顶点的类型。
注意：VertexX、VertexXj 是继承自BaseVertex等基础类的类！不是顶点的数据类。

EdgeSE3ProjectXYZ （空间点-位姿-二元边）（旧版本）

class EdgeSE3ProjectXYZ: public BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSE3Expmap> {//...}

作用：即要优化MapPoints的位置，又要优化相机的位姿

模板参数2 ：观测值（这里是3D点在像素坐标系下的投影坐标）的维度
参数Vector ：观测值类型，pixel.x，pixel.y
参数VertexSBAPointXYZ：第一个顶点类型
参数VertexSE3Expmap ：第二个顶点类型

需要设置的参数：

g2o::EdgeSE3ProjectXYZ* e = new g2o::EdgeSE3ProjectXYZ();//【1】设置第一个顶点，注意该顶点类型与模板参数第一个顶点类型对应
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
//【2】设置第二个顶点
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKFi->mnId)));
//【3】设置观测值，类型与模板参数对应
e->setMeasurement(obs);
const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave];
//【4】设置信息矩阵，协方差
e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);//【5】设置鲁棒核函数
g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
e->setRobustKernel(rk);
rk->setDelta(thHuberMono);//【6】设置相机内参
e->fx = pKFi->fx;
e->fy = pKFi->fy;
e->cx = pKFi->cx;
e->cy = pKFi->cy;

误差计算：重投影误差，使用的是估计的相机位姿 + 估计的地图3D点坐标

  void computeError()  {const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[1]);const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);Vector2d obs(_measurement);_error = obs-cam_project(v1->estimate().map(v2->estimate()));}

EdgeSE3Expmap（位姿-位姿-二元边）

class G2O_TYPES_SBA_API EdgeSE3Expmap : public BaseBinaryEdge<6, SE3Quat, VertexSE3Expmap, VertexSE3Expmap> { //... }

作用：优化变量是相邻相机两个关键帧位姿，约束来自对这两个关键帧位姿变换的测量（里程计、IMU等）,变换关系：detaT = T1 * inv(T2); 需要设置的参数：

Se2 measure_se2 = pMsrOdo->se2;
// 【1】里程计测量的协方差
g2o::Matrix3D covariance = toEigenMatrixXd(pMsrOdo->info).inverse(); // 【2】将里程计测量转换成g2o::SE3Quat类型
Eigen::AngleAxisd rotz(measure_se2.theta, Eigen::Vector3d::UnitZ());
g2o::SE3Quat relativePose_SE3Quat(rotz.toRotationMatrix(), Eigen::Vector3d(measure_se2.x, measure_se2.y, 0));// 【3】将`里程计测量协方差`转换为`相机坐标系下协方差`
// 注意：g2o::SE3Quat是旋转在前，平移在后
g2o::Matrix6d covariance_6d = g2o::Matrix6d::Identity();
covariance_6d(0,0) = covariance(2,2);
covariance_6d(0,4) = covariance(2,0); covariance_6d(0,5) = covariance(2,1);
covariance_6d(4,0) = covariance(0,2); covariance_6d(5,0) = covariance(1,2);
covariance_6d(3,3) = covariance(0,0);
covariance_6d(4,4) = covariance(1,1);
covariance_6d(1,1) = 0.00001;
covariance_6d(2,2) = 0.01;
covariance_6d(5,5) = 0.0001;g2o::Matrix6d Info = g2o::Matrix6d::Identity();
Info = covariance_6d.inverse();// 【4】构造边
g2o::EdgeOnlineCalibration* e = new g2o::EdgeOnlineCalibration;
e->vertices()[0] = optimizer.vertex(id0);
e->vertices()[1] = optimizer.vertex(id1);
e->setMeasurement(relativePose_SE3Quat);
e->setInformation(Info);
optimizer.addEdge(e);

误差计算：相机2的位姿的逆 * 观测的相机位姿值 * 相机1的位姿

    void computeError()  {const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[0]);const VertexSE3Expmap* v2 = static_cast<const VertexSE3Expmap*>(_vertices[1]);SE3Quat C(_measurement);SE3Quat error_= v2->estimate().inverse()*C*v1->estimate();_error = error_.log();}

EdgeProjectXYZ2UV （投影方程边）

class G2O_TYPES_SBA_API EdgeProjectXYZ2UV : public BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>{ //... }

EdgeSE3ProjectXYZ （空间点-位姿-二元边）的新版本，同样是即要优化MapPoints的位置，又要优化相机的位姿。
模板参数2 ：观测值（这里是3D点在像素坐标系下的投影坐标）的维度
参数Vector2D ：观测值类型，pixel.x，pixel.y
参数VertexSBAPointXYZ：第一个顶点类型 (3D空间点)
参数VertexSE3Expmap ：第二个顶点类型 (相机位姿)

需要设置的参数：

g2o::CameraParameters* camera = new g2o::CameraParameters (K.at<double> ( 0,0 ), Eigen::Vector2d ( K.at<double> ( 0,2 ), K.at<double> ( 1,2 ) ), 0);
camera->setId ( 0 );
optimizer.addParameter ( camera )

误差计算：同 EdgeSE3ProjectXYZ；

_error = obs-cam->cam_map(v1->estimate().map(v2->estimate()));

自定义边需要重写的函数

virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;　//分别是读盘、存盘函数，一般情况下不需要进行读/写操作的话，仅仅声明一下就可以
virtual void computeError();　　　　　　　　　//非常重要，是使用当前顶点的值计算的测量值与真实的测量值之间的误差
virtual void linearizeOplus();　　　　　　　　//非常重要，是在当前顶点的值下，该误差对优化变量的偏导数，也就是我们说的Jacobian
_measurement：　　　　　　　　　　　　　　　　　//存储观测值
_error：　　　　　　　　　　　　　　　　　　　　//存储computeError() 函数计算的误差
_vertices[]：　　　　　　　　　　　　　　　　   //存储顶点信息，比如二元边的话，_vertices[] 的大小为2，存储顺序和调用setVertex(int, vertex)  //是设定的int 有关（0 或1）
setId(int)：　　　　　　　　　　　　　　　　    //来定义边的编号（决定了在H矩阵中的位置）
setMeasurement(type) 　　　　　　　　　　      //函数来定义观测值
setVertex(int, vertex)　　　　　　 　　        //来定义顶点
setInformation()

相关的 outliner检测

优化完成后，对每一条边都进行检查，剔除误差较大的边（认为是错误的边），并设置setLevel为0，即下次不再对该边进行优化 , 第二次优化完成后，会对连接偏差比较大的边，在关键帧中剔除对该MapPoint的观测，在MapPoint中剔除对该关键帧的观测，具体实现参考orb-slam源码Optimizer::LocalBundleAdjustment

optimizer.optimize ( 100 );
// 优化完成后，进行Edge的检查
for(size_t i=0, iend=vpEdgesMono.size(); i<iend;i++)
{g2o::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];MapPoint* pMP = vpMapPointEdgeMono[i];if(pMP->isBad())continue;// 基于卡方检验计算出的阈值（假设测量有一个像素的偏差）// 第二个判断条件，用于检查构成该边的MapPoint在该相机坐标系下的深度是否为正？if(e->chi2()>5.991 || !e->isDepthPositive()){e->setLevel(1);// 不优化}// 因为剔除了错误的边，所以下次优化不再使用核函数e->setRobustKernel(0);}

3. 示例：使用g2o进行曲线拟合

为了使用 g2o，首先要做的是将曲线拟合问题抽象成图优化。这个过程中，只要记住 节点为优化变量，边为误差项即可。因此，曲线拟合的图优化问题可以画成图 6-3 的形式。

作为 g2o 的用户，我们要做的事主要有以下几个步骤：

1. 定义顶点和边的类型；
2. 构建图；
3. 选择优化算法；
4. 调用 g2o 进行优化，返回结果。

#include <iostream>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <Eigen/Core>
#include <opencv2/core/core.hpp>
#include <cmath>
#include <chrono>
using namespace std; // 曲线模型的顶点，模板参数：优化变量维度和数据类型
class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d> // 顶点定义评估的向量(模型参数)
{
public:EIGEN_MAKE_ALIGNED_OPERATOR_NEWvirtual void setToOriginImpl() // 重置{_estimate << 0,0,0;}virtual void oplusImpl( const double* update ) // 更新{_estimate += Eigen::Vector3d(update);}// 存盘和读盘：留空virtual bool read( istream& in ) {}virtual bool write( ostream& out ) const {}
};// 误差模型 模板参数：观测值维度，类型，连接顶点类型
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex> // 边定义和计算误差
{
public:EIGEN_MAKE_ALIGNED_OPERATOR_NEWCurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {} //子类显试调用父类构造函数 // x 也是观测值// 计算曲线模型误差void computeError(){const CurveFittingVertex* v = static_cast<const CurveFittingVertex*> (_vertices[0]);const Eigen::Vector3d abc = v->estimate();_error(0,0) = _measurement - std::exp( abc(0,0)*_x*_x + abc(1,0)*_x + abc(2,0) ) ;}virtual bool read( istream& in ) {}virtual bool write( ostream& out ) const {}
public:double _x;  // x 值， y 值为 _measurement
};int main( int argc, char** argv )
{double a=1.0, b=2.0, c=1.0;         // 真实参数值int N=100;                          // 数据点double w_sigma=1.0;                 // 噪声Sigma值cv::RNG rng;                        // OpenCV随机数产生器double abc[3] = {0,0,0};            // abc参数的估计值vector<double> x_data, y_data;      // 数据cout<<"generating data: "<<endl;for ( int i=0; i<N; i++ ){double x = i/100.0;x_data.push_back ( x );y_data.push_back (exp ( a*x*x + b*x + c ) + rng.gaussian ( w_sigma ));cout<<x_data[i]<<" "<<y_data[i]<<endl;}// 构建图优化，先设定g2otypedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block;  // 每个误差项优化变量维度为3，误差值维度为1Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); // 线性方程求解器Block* solver_ptr = new Block( linearSolver );      // 矩阵块求解器// 梯度下降方法，从GN, LM, DogLeg 中选g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );// g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr );// g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( solver_ptr );g2o::SparseOptimizer optimizer;     // 图模型optimizer.setAlgorithm( solver );   // 设置求解器optimizer.setVerbose( true );       // 打开调试输出// 往图中增加顶点CurveFittingVertex* v = new CurveFittingVertex();v->setEstimate( Eigen::Vector3d(0,0,0) ); //v->setId(0);optimizer.addVertex( v );// 往图中增加边for ( int i=0; i<N; i++ ){CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );edge->setId(i);edge->setVertex( 0, v );                // 设置连接的顶点 edge->setMeasurement( y_data[i] );      // 观测数值edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) ); // 信息矩阵：协方差矩阵之逆optimizer.addEdge( edge );}// 执行优化cout<<"start optimization"<<endl;chrono::steady_clock::time_point t1 = chrono::steady_clock::now();optimizer.initializeOptimization();optimizer.optimize(100);chrono::steady_clock::time_point t2 = chrono::steady_clock::now();chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );cout<<"solve time cost = "<<time_used.count()<<" seconds. "<<endl;// 输出优化值Eigen::Vector3d abc_estimate = v->estimate();cout<<"estimated model___: "<<abc_estimate.transpose()<<endl;return 0;
}

CMakeLists.txt

cmake_minimum_required( VERSION 2.8 )
project( g2o_curve_fitting )set( CMAKE_BUILD_TYPE "Release" )
set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )# 添加cmake模块以使用ceres库
list( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules )
# 寻找G2O
find_package( G2O REQUIRED )include_directories( ${G2O_INCLUDE_DIRS}"/usr/include/eigen3""/home/指向你的g2o安装路径/workspace/johnlibs/include"
)link_directories ("/home/指向你的g2o安装路径/workspace/johnlibs/lib")# OpenCV
find_package( OpenCV REQUIRED )
include_directories( ${OpenCV_DIRS} )add_executable( curve_fitting main.cpp )
# 与G2O和OpenCV链接
target_link_libraries( curve_fitting ${OpenCV_LIBS}g2o_core g2o_stuff)add_executable( curve_fitting_john  main_john.cpp )
# 与G2O和OpenCV链接
target_link_libraries( curve_fitting_john ${OpenCV_LIBS}g2o_core g2o_stuff)

运行结果：

参考：

《视觉SLAM十四讲精品总结》7：VO—— 3D-2D：PnP+BA_try_again_later的博客-CSDN博客
g2o库的学习与使用_火柴的初心的博客-CSDN博客_g2o库

非线性优化_曲线拟合_g2o图优化_最小二乘优化示例相关推荐

c++分治法求最大最小值实现_最优化计算与matlab实现（12）——非线性最小二乘优化问题——G-N法...
参考资料 <精通MATLAB最优化计算(第二版)> 编程工具 Matlab 2019a 目录石中居士:最优化计算与Matlab实现--目录zhuanlan.zhihu.com 非线性最 ...
louvian算法缺点优化_机器学习中的优化算法(1)-优化算法重要性,SGD,Momentum(附Python示例)...
本系列文章已转至机器学习的优化器zhuanlan.zhihu.com 优化算法在机器学习中扮演着至关重要的角色,了解常用的优化算法对于机器学习爱好者和从业者有着重要的意义. 这系列文章先讲述优化算 ...
深度学习将灰度图着色_通过深度学习为视频着色
深度学习将灰度图着色零本地设置/ DeOldify / Colab笔记本 (Zero Local Setup / DeOldify / Colab Notebook) "Haal Kais ...
漏斗模型_绘制漏斗图
漏斗模型_绘制漏斗图漏斗思维,它是一种线性的思考方式,一般按照任务的完成路径,识别出几个关键的行为转化节点,然后分析行为点间的转化与流失情况,进而定位问题,指导决策. 漏斗模型是指多个自定义事件序列 ...
【Excel】绘图案例_常见复合图：簇状图+堆积图+折线图
[Excel]绘图案例_常见复合图:簇状图+堆积图+折线图前言最近有朋友让我帮忙用excel画图,老实说我很讨厌用excel画图,点来点去,复杂一些还不能复用,非常繁琐.当然,入门也很简单.需求时 ...
python实现贝叶斯优化_贝叶斯优化的并行实现
python实现贝叶斯优化 The concept of 'optimization' is central to data science. We minimize loss by optimizi ...
高级FPGA设计结构实现和优化_(一)结构设计
高级FPGA设计结构实现和优化_结构设计高速度结构设计高流量设计低时滞结构设计时序设计改进策略添加寄存器层次并行结构展平逻辑结构寄存器平衡重新安排路径面积结构设计折叠流水线基 ...
多重背包单调队列优化思路_动态规划入门——多重背包与单调优化
本文始发于个人公众号:TechFlow,原创不易,求个关注今天是算法与数据结构的第14篇文章,也是动态规划专题的第三篇. 在之前的文章当中,我们介绍了多重背包的二进制拆分的解法.在大多数情况下,这种 ...
mysql not in优化_实践中如何优化MySQL(收藏)
SQL语句的优化: 1.尽量避免使用子查询 3.用IN来替换OR 4.LIKE前缀%号.双百分号._下划线查询非索引列或*无法使用到索引,如果查询的是索引列则可以 5.读取适当的记录LIMIT M,N ...
diag开关什么意思_双控开关接线图_一灯双控开关接线图_单联双控开关接线图_双控开关接线图实物图...
电工学习网:www.diangon.com 关注电工学习网官方微信公众号"电工电气学习",收获更多经验知识. 双控开关接线图_一灯双控开关接线图_单联双控开关接线图_双控开关接线图 ...

非线性优化_曲线拟合_g2o图优化_最小二乘优化示例

1. g2o介绍

2. g2o库结构

2.1 g2o图优化库的组成结构

2.2 常用的类结构

a. 块求解器 BlockSolver

b. 块求解器特性 BlockSolverTraits

c. 块求解器特性中的线性求解器 Traits::LinearSolverType

d. 优化算法 OptimizationAlgorithm

e. 稀疏优化器 SparseOptimizer

f. 表示优化项的顶点—Vertex

g. 表示误差项的边—Edge

3. 示例：使用g2o进行曲线拟合

非线性优化_曲线拟合_g2o图优化_最小二乘优化示例相关推荐

最新文章

热门文章