ceres::GradientProblem求解一般最小化问题

对于最小二乘问题，ceres::Problem可以应对。

对于一般的无约束最小化问题，使用 ceres::GradientProblem求解。
对应的cost function有

• 继承 FirstOrderFunction，自定义factor，手写gradient和cost
• 使用 AutoDiffFirstOrderFunction，只需要手写cost

这个问题的一般最小化解法如下

// ----------------------------- 手动求导 ----------------------------
class FirstOrderFunctionExample : public ceres::FirstOrderFunction {
 public:
  FirstOrderFunctionExample(const std::vector<double>& x_data,
                            const std::vector<double>& y_data)
      : x_data_(x_data), y_data_(y_data) {}
  virtual ~FirstOrderFunctionExample() = default;

  virtual bool Evaluate(const double* parameters, double* cost,
                        double* gradient) const {
    assert(parameters != nullptr);
    assert(cost != nullptr);
    assert(gradient != nullptr);

    const double a = parameters[0];
    const double b = parameters[1];
    const double c = parameters[2];

    cost[0] = 0;
    if (gradient != nullptr) {
      gradient[0] = 0;
      gradient[1] = 0;
      gradient[2] = 0;
    }

    for (int i = 0; i < x_data_.size(); i++) {
      const double x = x_data_[i];
      const double y = y_data_[i];
      const double h_x = exp(a * x * x + b * x + c);
      const double temp_residual = h_x - y;
      cost[0] += 0.5 * temp_residual * temp_residual;

      gradient[0] += temp_residual * (x * x * h_x);
      gradient[1] += temp_residual * (x * h_x);
      gradient[2] += temp_residual * h_x;
    }

    return true;
  }

  virtual int NumParameters() const { return 3; };

 private:
  const std::vector<double>& x_data_;
  const std::vector<double>& y_data_;
};

// ----------------------------- 自动求导 ----------------------------
class AutoDiffFistOrderFunctionExample {
 public:
  AutoDiffFistOrderFunctionExample(const std::vector<double>& x_data,
                                   const std::vector<double>& y_data)
      : x_data_(x_data), y_data_(y_data) {}

  template <typename T>
  bool operator()(const T* parameters, T* cost) const {
    assert(parameters != nullptr);
    assert(cost != nullptr);

    const T a = parameters[0];
    const T b = parameters[1];
    const T c = parameters[2];

    cost[0] = T(0);

    for (int i = 0; i < x_data_.size(); i++) {
      const T x = T(x_data_[i]);
      const T y = T(y_data_[i]);
      const T h_x = exp(a * x * x + b * x + c);
      const T temp_residual = h_x - y;
      cost[0] += T(0.5) * temp_residual * temp_residual;
    }

    return true;
  }

  static ceres::FirstOrderFunction* Create(const std::vector<double>& x_data,
                                           const std::vector<double>& y_data) {
    return new ceres::AutoDiffFirstOrderFunction<
        AutoDiffFistOrderFunctionExample, 3>(
        new AutoDiffFistOrderFunctionExample(x_data, y_data));
  }

 private:
  const std::vector<double>& x_data_;
  const std::vector<double>& y_data_;
};


// ------------------------ 求解过程 ------------------

  Eigen::Vector3d state(ae, be, ce);
  ceres::GradientProblem gradient_problem(
      // new FirstOrderFunctionExample(x_data, y_data)
      AutoDiffFistOrderFunctionExample::Create(x_data, y_data));

  // 配置求解器
  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();

  ceres::GradientProblemSolver::Options options;
  options.minimizer_progress_to_stdout = true;
  ceres::GradientProblemSolver::Summary summary;
  ceres::Solve(options, gradient_problem, state.data(), &summary);

求解结果

   0: f: 1.597873e+06 d: 0.00e+00 g: 3.52e+06 h: 0.00e+00 s: 0.00e+00 e:  0 it: 5.96e-06 tt: 5.96e-06
   1: f: 3.997422e+04 d: 1.56e+06 g: 9.70e+04 h: 1.32e+00 s: 2.84e-07 e:  1 it: 1.22e-05 tt: 6.01e-05
   2: f: 3.982660e+04 d: 1.48e+02 g: 1.33e+05 h: 3.24e+00 s: 7.59e-05 e:  7 it: 3.48e-05 tt: 1.10e-04
   3: f: 3.715282e+04 d: 2.67e+03 g: 1.26e+05 h: 1.15e+00 s: 4.47e-04 e:  2 it: 5.01e-06 tt: 1.19e-04
   4: f: 3.214174e+04 d: 5.01e+03 g: 1.13e+05 h: 2.05e-01 s: 1.76e-01 e:  1 it: 1.91e-06 tt: 1.25e-04
   5: f: 2.342511e+04 d: 8.72e+03 g: 8.85e+04 h: 9.50e-01 s: 2.92e-01 e:  1 it: 2.15e-06 tt: 1.32e-04
   6: f: 1.149555e+04 d: 1.19e+04 g: 5.11e+04 h: 1.86e+00 s: 7.82e-01 e:  1 it: 1.91e-06 tt: 1.37e-04
   7: f: 7.297522e+03 d: 4.20e+03 g: 3.52e+04 h: 8.34e-01 s: 1.00e+00 e:  1 it: 1.91e-06 tt: 1.41e-04
   8: f: 5.291364e+03 d: 2.01e+03 g: 2.62e+04 h: 1.52e+00 s: 1.00e+00 e:  1 it: 1.91e-06 tt: 1.46e-04
   9: f: 4.090081e+03 d: 1.20e+03 g: 1.92e+04 h: 7.94e-01 s: 1.00e+00 e:  1 it: 1.91e-06 tt: 1.50e-04
  10: f: 2.981728e+03 d: 1.11e+03 g: 1.22e+04 h: 7.03e-01 s: 1.00e+00 e:  1 it: 1.91e-06 tt: 1.55e-04
  11: f: 2.041256e+03 d: 9.40e+02 g: 5.95e+03 h: 5.04e-01 s: 1.00e+00 e:  1 it: 1.91e-06 tt: 1.60e-04
  12: f: 1.343468e+03 d: 6.98e+02 g: 1.85e+03 h: 2.86e-01 s: 1.00e+00 e:  1 it: 2.15e-06 tt: 1.64e-04
  13: f: 7.680185e+02 d: 5.75e+02 g: 2.58e+03 h: 3.09e-01 s: 1.00e+00 e:  1 it: 2.15e-06 tt: 1.69e-04
  14: f: 2.648358e+02 d: 5.03e+02 g: 2.04e+03 h: 5.23e-01 s: 1.00e+00 e:  1 it: 2.15e-06 tt: 1.73e-04
  15: f: 7.679229e+01 d: 1.88e+02 g: 4.73e+02 h: 4.46e-01 s: 1.00e+00 e:  1 it: 2.15e-06 tt: 1.78e-04
  16: f: 6.966103e+01 d: 7.13e+00 g: 1.14e+02 h: 1.05e-01 s: 1.00e+00 e:  1 it: 2.15e-06 tt: 1.83e-04
  17: f: 6.340726e+01 d: 6.25e+00 g: 1.41e+02 h: 2.47e-01 s: 1.00e+00 e:  1 it: 2.15e-06 tt: 1.88e-04
  18: f: 5.529499e+01 d: 8.11e+00 g: 3.61e+02 h: 5.61e-01 s: 1.00e+00 e:  1 it: 1.91e-06 tt: 1.93e-04
  19: f: 5.251237e+01 d: 2.78e+00 g: 2.82e+02 h: 3.17e-01 s: 1.00e+00 e:  1 it: 1.91e-06 tt: 1.98e-04
  20: f: 5.134446e+01 d: 1.17e+00 g: 1.16e+02 h: 1.71e-01 s: 1.00e+00 e:  1 it: 2.86e-06 tt: 2.03e-04
  21: f: 5.103081e+01 d: 3.14e-01 g: 1.58e+01 h: 4.87e-02 s: 1.00e+00 e:  1 it: 1.91e-06 tt: 2.07e-04
  22: f: 5.100139e+01 d: 2.94e-02 g: 2.15e+01 h: 3.83e-02 s: 9.71e-05 e:  2 it: 5.01e-06 tt: 2.15e-04
  23: f: 5.099233e+01 d: 9.06e-03 g: 3.60e+01 h: 2.09e-02 s: 1.55e-03 e:  2 it: 4.05e-06 tt: 2.22e-04
  24: f: 5.097767e+01 d: 1.47e-02 g: 2.22e+01 h: 6.78e-03 s: 3.82e-01 e:  1 it: 2.86e-06 tt: 2.29e-04
  25: f: 5.096851e+01 d: 9.16e-03 g: 4.38e-02 h: 1.11e-02 s: 1.00e+00 e:  1 it: 2.15e-06 tt: 2.36e-04
solve time cost = 0.000267951 seconds. 
Ceres GradientProblemSolver Report: Iterations: 27, Initial cost: 1.597873e+06, Final cost: 5.096851e+01, Termination: CONVERGENCE
estimated a,b,c = 0.890912   2.1719 0.943629

注意在 GradientProblem的构造函数中可以传入参数的Manifold，用于更新非欧式空间的参数

class CERES_EXPORT GradientProblem {
 public:
  // Takes ownership of the function.
  explicit GradientProblem(FirstOrderFunction* function);

  // Takes ownership of the function and the manifold.
  GradientProblem(FirstOrderFunction* function, Manifold* manifold);