g2o图优化学习

g2o是一个最小二乘优化库。
可以将优化变量和误差项的关联表现为图的形式，直观的看到优化问题。
其余详见这里

基本概念

图优化，把优化问题表现成图的一种方式。

1. 定点（Vertex）：表示优化变量。
2. 边(Edge):表示误差项，连接到顶点。
3. 任意一个非线性最小二乘可以构建一个与之对应的图。

安装

1. 下载

git clone https://github.com/RainerKuemmerle/g2o.git

2. 安装依赖项

sudo apt-get install libeigen3-dev libsuitesparse-dev qtdeclarative5-dev qt5-qmake libqglviewer-dev

3. 编译安装

cd g2o/
mkdir build
cd build/
cmake ..
make -j4
sudo make install

4. CmakeLists 配置
   cmake list指令

LIST( APPEND CMAKE_MODULE_PATH /home/n1/g2o/cmake_modules )//下载的g2o文件中
SET( G2O_ROOT /usr/local/include/g2o )//设置库文件位置
FIND_PACKAGE( G2O )
FIND_PACKAGE( CSparse )
include_directories(${G2O_INCLUDE_DIR} ${CSPARSE_INCLUDE_DIR} )
include_directories(${CSPARSE_INCLUDE_DIR})
SET(G2O_LIBS g2o_cli g2o_ext_freeglut_minimal g2o_simulator g2o_solver_slam2d_linear g2o_types_icp g2o_types_slam2d g2o_core g2o_interface g2o_solver_csparse g2o_solver_structure_only g2o_types_sba g2o_types_slam3d g2o_csparse_extension g2o_opengl_helper g2o_solver_dense g2o_stuff g2o_types_sclam2d g2o_parser g2o_solver_pcg g2o_types_data g2o_types_sim3 cxsparse )
target_link_libraries(g2o_exe ${G2O_LIBS})
 

使用

1. 定义顶点

// 曲线模型的顶点，模板参数：优化变量维度和数据类型
class CurveFittingVertex : public g2o::BaseVertex<3, Eigen::Vector3d> {
//3：优化变量维数即Vertex维数为3维，
//Eigen::Vector3d：Vertex的数据类型
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW//字节对齐

  // 重置
  virtual void setToOriginImpl() override {
    _estimate << 0, 0, 0;// 重置，设定被优化变量的原始值  
    //_estimate :成员函数估计值 
  }

  // 更新
  virtual void oplusImpl(const double *update) override {
    _estimate += Eigen::Vector3d(update);//update强制类型转换为Vector3d，优化变量更新
    //用于优化过程中增量△x 的计算。根据增量方程计算出增量后，通过这个函数对估计值进行调整，因此该函数的内容要重视。
  }

  // 存盘和读盘：留空
  virtual bool read(istream &in) {}

  virtual bool write(ostream &out) const {}
};

2. 定义边

// 误差模型 模板参数：观测值维度，类型，连接顶点类型
class CurveFittingEdge : public g2o::BaseUnaryEdge<1, double, CurveFittingVertex> {
//边的模型，BaseUnaryEdge：一元边;BaseBinaryEdge：二元边，BaseMultiEdge：多元边
//1 连接顶点的个数
//double 测量值数据类型
//CurveFittingVertex 每个顶点类型
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  CurveFittingEdge(double x) : BaseUnaryEdge(), _x(x) {}

  // 计算曲线模型误差
  virtual void computeError() override {
    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 void linearizeOplus() override {
    const CurveFittingVertex *v = static_cast<const CurveFittingVertex *> (_vertices[0]);
    const Eigen::Vector3d abc = v->estimate();
    double y = exp(abc[0] * _x * _x + abc[1] * _x + abc[2]);
    _jacobianOplusXi[0] = -_x * _x * y;
    _jacobianOplusXi[1] = -_x * y;
    _jacobianOplusXi[2] = -y;
  }

  virtual bool read(istream &in) {}

  virtual bool write(ostream &out) const {}

public:
  double _x;  // x 值， y 值为 _measurement
};

边的几个重要成员变量：

_measurement； // 存储观测值
_error;  // 存储computeError() 函数计算的误差
_vertices[]; // 存储顶点信息，比如二元边,_vertices[]大小为2//存储顺序和调用setVertex(int, vertex) //设定的int有关（0或1）

边的几个重要成员函数：

setId(int);  // 定义边的编号（决定了在H矩阵中的位置）
setMeasurement(type);  // 定义观测值
setVertex(int, vertex);  // 定义顶点
setInformation();  // 定义协方差矩阵的逆

3. 配置BlockSolver(LinerSolerType)

 typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> BlockSolverType;  // 顶点的为纬度<观测值（x,y,z）3维，待优化值k的维度为1>

4. 创建BlockSolver，并用定义的线性求解器初始化

typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型
//其他
//LinearSolverCholmod  cholesky分解法
//LinearSolverCSparse  CSparse法
//LinearSolverPCG      preconditioned conjugate gradient 法
//LinearSolverDense    dense cholesky分解法
//LinearSolverEigen    使用eigen中sparse Cholesky 求解

5. 配置OptimizationAlgorithm:创建总求解器solver

auto solver = new g2o::OptimizationAlgorithmGaussNewton(
    g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
 //g2o::OptimizationAlgorithmLevenberg
 //g2o::OptimizationAlgorithmDogleg

6. 配置Optimzer:稀疏优化器

g2o::SparseOptimizer  optimizer;//创建稀疏优化器
SparseOptimizer::setAlgorithm(OptimizationAlgorithm* algorithm);//设置求解方法
optimizer.setVerbose(true);       // 打开调试输出

7. 添加顶点

  CurveFittingVertex *v=new CurveFittingVertex();
  v->setEstimate(Vector3d(ae,be,ce));//设置初始值
  v->setId(0);
  optimizer.addVertex(v);

8. 添加边

  //往图中添加边
  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(Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma));
    optimizer.addEdge(edge);  
  }

9. 启动优化

  optimizer.initializeOptimization();//初始化
  optimizer.optimize(10); 			//执行10次
  Vector3d abc_estimate = v->estimate();//获取当前值

10. 报错：
    error while loading shared libraries: libg2o_core.so: cannot open shared object file: No such file

sudo gedit /etc/ld.so.conf

添加如下代码：

/usr/local/lib

运行：

sudo ldconfig

11. 全部代码

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

using namespace std;
using namespace g2o;
using namespace Eigen;

//定义顶点
class CurveFittingVertex:public BaseVertex<3,Vector3d>{//3：雅克比矩阵纬度
  public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW//开启内存对齐
  //重置
  virtual void setToOriginImpl() override{
    _estimate<<0,0,0;//_estimate是BaseVertex内置成员变量
  }
  //更新
  virtual void oplusImpl(const double *update)override{
    _estimate+=Vector3d(update);
  }

  //读盘留空
  virtual bool read(istream &in){}
  virtual bool write(ostream &out)const{}
  //存盘留空
};

//定义边
class CurveFittingEdge:public BaseUnaryEdge<1,double,CurveFittingVertex>{//1：误差项的维度
  public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW//开启内存对齐
   CurveFittingEdge(double x):BaseUnaryEdge(),_x(x){};

    //计算曲线模型误差
    virtual void computeError() override{
      const CurveFittingVertex *v=static_cast<const CurveFittingVertex *> (_vertices[0]);//读取顶点信息
      const Vector3d abc=v->estimate();//返回对当前顶点姿态的估计
      
      _error(0,0)=_measurement-exp(abc(0,0)*_x*_x+abc(1,0)*_x+abc(2,0));//误差计算
    }

    //计算雅克比矩阵
    virtual void linearizeOplus() override{
      const CurveFittingVertex *v=static_cast<const CurveFittingVertex *>(_vertices[0]);
      const Vector3d abc=v->estimate();
      double y=exp(abc[0]*_x*_x+abc[1]*_x+abc[2]);//观测量
      _jacobianOplusXi[0]=-_x*_x*y;//求一阶导
      _jacobianOplusXi[1]=-_x*y;
      _jacobianOplusXi[2]=-y;
    }

    virtual bool read(istream &in){}
    virtual bool write(ostream &out)const{}
    public:
      double _x;//x值，y 值为 _measurement
};
int main(int argc, char const *argv[])
{
  double ar=1,br=2,cr=1;   //真实值
  double ae=2,be=0,ce=5.0;//参照值
  int N=100;              //数据个数
  double w_sigma=1;       //噪声sigma值
  double inv_sigma=1.0/w_sigma;
  cv::RNG rng;
  vector<double> x_data,y_data; //数据
  for(int i=0;i<N;i++){
    double x=i/100;
    x_data.push_back(x);
    y_data.push_back(exp(ar*x*x+br*x+cr)+rng.gaussian(w_sigma*w_sigma));
  }
  
  //构建图优化
  
  typedef BlockSolver<BlockSolverTraits<3,1>> BlockSolverType;
  //BlockSolverTraits对应一条边两个顶点的纬度<3:雅克比矩阵的纬度，1：观测量的纬度>
  typedef LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType;
  //线性求解器类型

  auto solver=new OptimizationAlgorithmGaussNewton(g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
  //make_unique 调用unique_ptr实现内存地址绑定
  SparseOptimizer optimizer;//构建一个图模型性类
  optimizer.setAlgorithm(solver);//设置求解器
  optimizer.setVerbose(true);//打开调试输出

  //往图中添加顶点
  CurveFittingVertex *v=new CurveFittingVertex();
  v->setEstimate(Vector3d(ae,be,ce));//设置初始值
  v->setId(0);
  optimizer.addVertex(v);

  //往图中添加边
  for(int i=0;i<N;i++){
    CurveFittingEdge *edge=new CurveFittingEdge(x_data[i]);//创建边
    edge->setId(i);                       //设置ID
    edge->setVertex(0, v);                // 设置连接的顶点
    edge->setMeasurement(y_data[i]);      //设置测量值
    edge->setInformation(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(20);  //执行10次
  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()<<endl;

  Vector3d abc_estimate = v->estimate();//获取当前值
  cout<<"estimated model:"<<abc_estimate.transpose()<<endl;
  return 0;
}