g2o -- circle_fit代码解析

概述

本文介绍通过g2o框架，拟合二维圆形。其代码逻辑和前一篇的curve_fit相似，可以参照一起看。

代码解析

自定义顶点

/**
 * \brief a circle located at x,y with radius r
 */
class VertexCircle : public g2o::BaseVertex<3, Eigen::Vector3d> { // 顶点包含3个参数
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
  VertexCircle() {}

  bool read(std::istream& /*is*/) override { return false; }

  bool write(std::ostream& /*os*/) const override { return false; }

  void setToOriginImpl() override { // 设置顶点的初始值
    cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
  }

  void oplusImpl(const double* update) override { // 增量更新函数
    Eigen::Vector3d::ConstMapType v(update);
    _estimate += v;
  }
};

自定义边

/**
 * \brief measurement for a point on the circle
 *
 * Here the measurement is the point which is on the circle.
 * The error function computes the distance of the point to
 * the center minus the radius of the circle.
 */
class EdgePointOnCircle
    : public g2o::BaseUnaryEdge<1, Eigen::Vector2d, VertexCircle> { // 第一个参数表示是单一顶点的边，第二个参数表示观测值为二维参数，第三个参数表示顶点的类型
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  EdgePointOnCircle() {}
  bool read(std::istream& /*is*/) override { return false; }
  bool write(std::ostream& /*os*/) const override { return false; }

  template <typename T>
  bool operator()(const T* circle, T* error) const {
    typename g2o::VectorN<2, T>::ConstMapType center(circle);
    const T& radius = circle[2];

    error[0] = (measurement().cast<T>() - center).norm() - radius; // measurement()表示当前的观测值
    return true;
  }

  G2O_MAKE_AUTO_AD_FUNCTIONS  // use autodiff
};

生成待拟合点

  // generate random data
  Eigen::Vector2d center(4.0, 2.0);
  double radius = 2.0;
  Eigen::Vector2d* points = new Eigen::Vector2d[numPoints];

  g2o::Sampler::seedRand();
  for (int i = 0; i < numPoints; ++i) {
    double r = g2o::Sampler::gaussRand(radius, 0.05); // 模拟偏差
    double angle = g2o::Sampler::uniformRand(0.0, 2.0 * M_PI);
    points[i].x() = center.x() + r * cos(angle);
    points[i].y() = center.y() + r * sin(angle);
  }

初始化求解器

  // setup the solver
  g2o::SparseOptimizer optimizer;
  optimizer.setVerbose(false);

  // allocate the solver
  g2o::OptimizationAlgorithmProperty solverProperty;
  optimizer.setAlgorithm(
      g2o::OptimizationAlgorithmFactory::instance()->construct("lm_dense",
                                                               solverProperty));

添加顶点

  // 1. add the circle vertex
  VertexCircle* circle = new VertexCircle();
  circle->setId(0);
  circle->setEstimate(
      Eigen::Vector3d(3.0, 3.0, 3.0));  // 初始化参数，待优化
  optimizer.addVertex(circle);

添加边

  // 2. add the points we measured
  // 每个待拟合的点都可以形成一条边 
  for (int i = 0; i < numPoints; ++i) {
    EdgePointOnCircle* e = new EdgePointOnCircle;
    e->setInformation(Eigen::Matrix<double, 1, 1>::Identity());
    e->setVertex(0, circle);
    e->setMeasurement(points[i]);
    optimizer.addEdge(e);
  }

启动求解器

  optimizer.initializeOptimization();
  optimizer.setVerbose(verbose);
  optimizer.optimize(maxIterations); // 最大迭代次数

输出结果

  // print out the result
  cout << "Iterative least squares solution" << endl;
  cout << "center of the circle " << circle->estimate().head<2>().transpose()
       << endl;
  cout << "radius of the cirlce " << circle->estimate()(2) << endl;
  cout << "error " << errorOfSolution(numPoints, points, circle->estimate())
       << endl;
  cout << endl;