slambook2/ch6/g2oCurveFitting.cpp的一些总结

前言

我们来看看十四讲上第六章这个曲线拟合的例子
设曲线方程为

a,b,c为待估计参数，w为噪声。假设有N个关于x,y的观测数据点，构建一个非线性优化问题。可以通过求解下面的最小二乘问题来估计曲线参数

可以将本问题转化成图优化问题。用顶点表示优化变量，边表示误差项，使用g2o进行曲线拟合，g2o的编译安装过程这里不进行赘述。g2o编程主要部分在于顶点和边的定义，此外就是构建图模型与将顶点和边添加到图中，执行优化即可。

1.定义曲线模型的顶点

class CurveFittingVertex : public g2o::BaseVertex<3, Eigen::Vector3d> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  // 重置
  virtual void setToOriginImpl() override {
    _estimate << 0, 0, 0;
  }
  // 更新
  virtual void oplusImpl(const double *update) override {
    _estimate += Eigen::Vector3d(update);
  }
  // 存盘和读盘：留空
  virtual bool read(istream &in) {}
  virtual bool write(ostream &out) const {}
};

1. CurveFittingVertex继承BaseVertex<优化变量维度,顶点数据类型>
2. 成员变量 _estimate 类型为顶点的类型,表示待估计的a,b,c
   源码为typedef T EstimateType; EstimateType _estimate;
3. 成员函数setToOriginImpl()将_estimate重置为初始值
4. 成员函数oplusImpl(const double *update)
   源码为virtual void oplusImpl(const number_t* v) = 0;
   使用v中的参数来更新节点。实际上，number_t是double类型。将更新量以数组形式表示，返回该数组首地址，我们需要将update数组转化成顶点类型。书中顶点类型（Vector3d类型）可直接由数组初始化，不要被迷惑。
5. 成员函数setEstimate()为成员变量_estimate赋值，初始化了顶点值，同时调用updateCache()函数（暂时没有弄明白）

2.定义边

class CurveFittingEdge : public g2o::BaseUnaryEdge<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
};

1. CurveFittingEdge继承BaseUnaryEdge<观测值维度，观测值类型，顶点数据类型>
2. 成员函数computeError()函数，用来计算边的误差。其中，成员变量_vertices源码为typedef std::vector<Vertex*> VertexContainer; VertexContainer _vertices;即_vertices是由Vertex* 构成的vector，此处_vertices的类型为BaseVertex*.因此要对 _vertices[0]进行强制类型转换 static_cast<const CurveFittingVertex *> (_vertices[0])将其转成CurveFittingVertex类型
3. 成员函数estimate(),返回_estimate的值
4. 成员变量_measurement保存了观测值的值（error = measurement - estimate）
5. 成员函数setVertex()原型为void setVertex(size_t i, Vertex* v),设置该边要连接的顶点。由于本例只有一个顶点，所有边都连接该顶点。
6. 成员函数setMeasurement()为观测值_measurement赋值
7. void linearizeOplus()函数，计算Jacobian矩阵，将结果保存至 _jacobianOplusXi中
8. setInformation矩阵，源码为void setInformation(const InformationType& information) { _information = information; }，定义协方差矩阵，这里为高斯噪声方差的逆（1×1），是为了计算高斯牛顿法的增量方程的

3.其他

剩下的工作就是创建求解器、创建图模型、设置求解器等工作了。之后就需要向图中添加顶点和边。
在分析代码的时候，心中要对高斯牛顿法流程有一个清晰的认识，添加顶点设置了待估计参数的初值，添加N条边，确立了顶点与顶点的连接关系（当然，本例只有一个顶点，就都是unary edge），同时也通过计算每个error对estimate的Jacobian矩阵，列写出了总的高斯牛顿增量方程。于是在执行optimize(10)时，重复求解增量，更新估计值。

// 构建图优化
//先设定g2o
  typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> BlockSolverType;  // 每个误差项优化变量维度为3，误差值维度为1
  typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型
  // 梯度下降方法，可以从GN, LM, DogLeg 中选
  auto solver = new g2o::OptimizationAlgorithmGaussNewton(
    g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
  g2o::SparseOptimizer optimizer;     // 图模型
  optimizer.setAlgorithm(solver);   // 设置求解器
  optimizer.setVerbose(true);       // 打开调试输出

// 往图中增加顶点
  CurveFittingVertex *v = new CurveFittingVertex();
  v->setEstimate(Eigen::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);
    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(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() << " seconds. " << endl;

  // 输出优化值
  Eigen::Vector3d abc_estimate = v->estimate();
  cout << "estimated model: " << abc_estimate.transpose() << endl;