ceres solver 08 例子： 圆拟合

输入一系列点（包括部分离群点），求拟合圆的圆心和半径。

1. 圆的模型

假设圆的方程如下：

圆心为, 半径为。为参与拟合的点。

令要求的半径（这样表示之后，就不可能是个非负数）。

所以我们要优化的参数是,因为。已知的是参与拟合的点。

2. Ceres 求解过程

2.1 定义代价函数

根据上述圆方程定义代价函数如下：

class DistanceFromCircleCost {
 public:
  DistanceFromCircleCost(double xx, double yy) : xx_(xx), yy_(yy) {}
  template <typename T> bool operator()(const T* const x,
                                        const T* const y,
                                        const T* const m,  // r = m^2
                                        T* residual) const {
    // Since the radius is parameterized as m^2, unpack m to get r.
    T r = *m * *m;

    // Get the position of the sample in the circle's coordinate system.
    T xp = xx_ - *x;
    T yp = yy_ - *y;
    
    residual[0] = r*r - xp*xp - yp*yp;
    return true;
  }

 private:
  // The measured x,y coordinate that should be on the circle.
  double xx_, yy_;
};

2.2 构建问题，添加残差项

  double x, y, r;
  if (scanf("%lg %lg %lg", &x, &y, &r) != 3) {
    fprintf(stderr, "Couldn't read first line.\n");
    return 1;
  }
  fprintf(stderr, "Got x, y, r %lg, %lg, %lg\n", x, y, r);
  
  // Save initial values for comparison.
  double initial_x = x;
  double initial_y = y;
  double initial_r = r;

  // Parameterize r as m^2 so that it can't be negative.
  double m = sqrt(r);

  Problem problem;

  // Configure the loss function.
  LossFunction* loss = NULL;
  if (FLAGS_robust_threshold) {
    loss = new CauchyLoss(FLAGS_robust_threshold);
  }

  // Add the residuals.
  double xx, yy;
  int num_points = 0;
  while (scanf("%lf %lf\n", &xx, &yy) == 2) {
    CostFunction *cost =
        new AutoDiffCostFunction<DistanceFromCircleCost, 1, 1, 1, 1>(
            new DistanceFromCircleCost(xx, yy));
    problem.AddResidualBlock(cost, loss, &x, &y, &m);
    num_points++;
  }

开始时，手动输入x、y、r的初始值。
然后为了减少离群点对拟合的影响，才用了损失函数$loss = new CauchyLoss(FLAGS_robust_threshold);$
然后手动输入各个点的数据，并添加残差项。

2.3 定义求解器的选项参数，并求解

  // Build and solve the problem.
  Solver::Options options;
  options.max_num_iterations = 500;
  options.linear_solver_type = ceres::DENSE_QR;
  Solver::Summary summary;
  Solve(options, &problem, &summary);

3. 完整代码

#include <cstdio>
#include <vector>

#include "ceres/ceres.h"
#include "gflags/gflags.h"
#include "glog/logging.h"

using ceres::AutoDiffCostFunction;
using ceres::CauchyLoss;
using ceres::CostFunction;
using ceres::LossFunction;
using ceres::Problem;
using ceres::Solve;
using ceres::Solver;

DEFINE_double(robust_threshold, 0.0, "Robust loss parameter. Set to 0 for "
              "normal squared error (no robustification).");


class DistanceFromCircleCost {
 public:
  DistanceFromCircleCost(double xx, double yy) : xx_(xx), yy_(yy) {}
  template <typename T> bool operator()(const T* const x,
                                        const T* const y,
                                        const T* const m,  // r = m^2
                                        T* residual) const {
    // Since the radius is parameterized as m^2, unpack m to get r.
    T r = *m * *m;

    // Get the position of the sample in the circle's coordinate system.
    T xp = xx_ - *x;
    T yp = yy_ - *y;
    
    residual[0] = r*r - xp*xp - yp*yp;
    return true;
  }

 private:
  // The measured x,y coordinate that should be on the circle.
  double xx_, yy_;
};

int main(int argc, char** argv) {
  CERES_GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true);
  google::InitGoogleLogging(argv[0]);

  double x, y, r;
  if (scanf("%lg %lg %lg", &x, &y, &r) != 3) {
    fprintf(stderr, "Couldn't read first line.\n");
    return 1;
  }
  fprintf(stderr, "Got x, y, r %lg, %lg, %lg\n", x, y, r);

  // Save initial values for comparison.
  double initial_x = x;
  double initial_y = y;
  double initial_r = r;

  // Parameterize r as m^2 so that it can't be negative.
  double m = sqrt(r);

  Problem problem;

  // Configure the loss function.
  LossFunction* loss = NULL;
  if (FLAGS_robust_threshold) {
    loss = new CauchyLoss(FLAGS_robust_threshold);
  }

  // Add the residuals.
  double xx, yy;
  int num_points = 0;
  while (scanf("%lf %lf\n", &xx, &yy) == 2) {
    CostFunction *cost =
        new AutoDiffCostFunction<DistanceFromCircleCost, 1, 1, 1, 1>(
            new DistanceFromCircleCost(xx, yy));
    problem.AddResidualBlock(cost, loss, &x, &y, &m);
    num_points++;
  }

  std::cout << "Got " << num_points << " points.\n";

  // Build and solve the problem.
  Solver::Options options;
  options.max_num_iterations = 500;
  options.linear_solver_type = ceres::DENSE_QR;
  Solver::Summary summary;
  Solve(options, &problem, &summary);

  // Recover r from m.
  r = m * m;

  std::cout << summary.BriefReport() << "\n";
  std::cout << "x : " << initial_x << " -> " << x << "\n";
  std::cout << "y : " << initial_y << " -> " << y << "\n";
  std::cout << "r : " << initial_r << " -> " << r << "\n";
  return 0;
}

4. 总结

总结使用步骤如下：

1. 定义代价函数。根据圆方程定义代价函数
2. 构建问题。添加残差项。这里要根据有噪声的数据点，不断地添加残差项。且使用new CauchyLoss()作为损失函数替代NULL
3. 求解问题。定义求解器的选项参数和求解器的结果报告