ceres 基本使用-自动求导、数值导数、解析导数-代码示例

Ceres Solver 教程

1.0 非线性优化问题一般形式

​ 下式子就是非线性优化的最基本的形式：
$$\begin{array}{ll}{\min }_{\mathbf{x}}& \frac{1}{2}{\sum }_i{\rho }_i\left({\Vert f_i\left(x_{i_1},\dots ,x_{i_k}\right)\Vert }^2\right)\\ \text{ s.t. }& l_j\le x_j\le u_j\end{array}$$
​ 这种形式的问题广泛出现在科学和工程领域。从统计学中的拟合曲线，到用计算机视觉从照片构建3D模型。

​ 表达式 ${\rho }_i\left({\Vert f_i\left(x_{i_1},\dots ,x_{i_k}\right)\Vert }^2\right)$ 被称为 ResidualBlock 。$f_i(\dot{})$ 被称为 CostFunction ，取决于参数块 $\left[x_{i_1},\dots ,x_{i_k}\right]$ 。在大多数优化问题中，大量的标量同时成群出现。例如，平移矢量的三个分量和四元数的四个分量定义了摄像机的姿态。我们将这样一组小标量称为 ParameterBlock。当然，ParameterBlock 可以只是一个参数。$l_j{u}_j$ 是参数块 $x_j$ 的边界。

​ ${\rho }_i$ 是一个LossFunction。LossFunction 是一个标量函数，用于减少异常值对非线性最小二乘问题解的影响。就是我们通常说的核函数。

​ 作为一种特殊情况，当${\rho }_i(x)=x$，即为恒等函数，并且 $l_j=-\infty and{u}_j=+\infty$ 我们得到了更熟悉的非线性最小二乘问题。
$$\frac{1}{2}\sum _i{\Vert f_i\left(x_{i_1},\dots ,x_{i_k}\right)\Vert }^2$$

2.0 Hello World!

​ 找出如下函数的最小值点：
$$\frac{1}{2}(10-x{)}^2$$
​ 当然，我们一眼就能看出其最小值点在 $x=10$ 的位置，但是怎么让计算机通过计算得出呢，实际上也是可以自己写类似于梯度下降之类的方法去得到最优解，但是很明显当参数量过大或者解析倒数难以表达的时候，我们的工作量就会非常的大，此时借助第三方优化库来加快我们的开发速度无疑是最好的选择。

​ 下面展示如何使用 Ceres Solver 去求解该问题。

​ 第一步是写一个函子来计算 $f(x)=10-x$ 这个函数的值

struct CostFunctor {
   template <typename T>
   bool operator()(const T* const x, T* residual) const {
     residual[0] = 10.0 - x[0];
     return true;
   }
};

​ 需要着重注意的是这里的 operator() 是一个模板函数。它假设所有的输入和输出都是某种 $T$ 类型。这里的模板使用允许 Ceres 调用 CostFunctor::operator()，当只需要残差的值时，T=double，当需要雅可比矩阵时，使用特殊类型 T=Jet。在后面，我们将详细讨论 Ceres 提供的各种求导方式。

​ 一旦我们有了计算残差函数的方法，现在是时候用它来构造一个非线性最小二乘问题，并让 Ceres 来解决它。

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

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

// A templated cost functor that implements the residual r = 10 -
// x. The method operator() is templated so that we can then use an
// automatic differentiation wrapper around it to generate its
// derivatives.
struct CostFunctor {
  template <typename T>
  bool operator()(const T* const x, T* residual) const {
    residual[0] = 10.0 - x[0];
    return true;
  }
};

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

  // The variable to solve for with its initial value. It will be
  // mutated in place by the solver.
  double x = 0.5;
  const double initial_x = x;

  // Build the problem.
  Problem problem;

  // Set up the only cost function (also known as residual). This uses
  // auto-differentiation to obtain the derivative (jacobian).
  CostFunction* cost_function =
      new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);
  problem.AddResidualBlock(cost_function, nullptr, &x);

  // Run the solver!
  Solver::Options options;
  options.minimizer_progress_to_stdout = true;
  Solver::Summary summary;
  Solve(options, &problem, &summary);

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

• 输出

iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  4.512500e+01    0.00e+00    9.50e+00   0.00e+00   0.00e+00  1.00e+04       0    5.33e-04    3.46e-03
   1  4.511598e-07    4.51e+01    9.50e-04   9.50e+00   1.00e+00  3.00e+04       1    5.00e-04    4.05e-03
   2  5.012552e-16    4.51e-07    3.17e-08   9.50e-04   1.00e+00  9.00e+04       1    1.60e-05    4.09e-03
Ceres Solver Report: Iterations: 2, Initial cost: 4.512500e+01, Final cost: 5.012552e-16, Termination: CONVERGENCE
x : 0.5 -> 10

​ 从 $x=0.5$ 开始，求解器在两次迭代中达到10。细心的读者会注意到这是一个线性问题，一个线性解应该足以得到最优值。解算器的默认配置是针对非线性问题的，为了简单起见，在本例中没有更改它。使用 Ceres 在一次迭代中确实有可能获得这个问题的最终解。还需要注意的是，解算器在第一次迭代中确实非常接近函数的最优值0。当我们讨论谷神星的收敛性和参数设置时，我们会更详细地讨论这些问题。

3.0 求导

​ Ceres Solver 与大多数优化软件包一样，依赖于能够在任意参数值下评估目标函数中每一项的值和导数。正确而有效地这样做对于获得好的结果是必不可少的。Ceres Solver 提供了许多这样做的方法。

​ 我们现在考虑另外两种可能性。解析导数和数值导数。

3.1 数值导数

​ 在某些情况下，定义模板代价函子是不可能的，例如，当残差的计算涉及到一个库函数的调用，这时是没办法在代价函数内调用其他库函数中的方法的，比如你是不能使用 std::pow() 函数的，相应的只能使用 ceres::pow() 函数，这是因为 ceres 并不知道如何计算 std::pow() 的导数，但是知道 ceres::pow() 的导数如何算。在这种情况下，可以使用数值微分（导数）。用户定义一个函子来计算剩余值，并使用它构造一个NumericDiffCostFunction。

​ 例如 $f(x)=10-x$ 对应的数值导数如下所示：

struct NumericDiffCostFunctor {
  bool operator()(const double* const x, double* residual) const {
    residual[0] = 10.0 - x[0];
    return true;
  }
};

​ 通过如下方式对 Problem 添加残差块：

CostFunction* cost_function =
  new NumericDiffCostFunction<NumericDiffCostFunctor, ceres::CENTRAL, 1, 1>(
      new NumericDiffCostFunctor);
problem.AddResidualBlock(cost_function, nullptr, &x);

​ 与前面使用自动微分时候的进行比较，这是前面使用自动微分的时候定义的方式：

CostFunction* cost_function =
    new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);
problem.AddResidualBlock(cost_function, nullptr, &x);

​ 很明显的发现多了一个模板参数 ceres::CENTRAL 表示用于计算数值导数的有限差分格式3。更多详细信息请参见NumericDiffCostFunction 的文档。

• 总结

​ 一般来说，Google 建议使用自动微分而不是数值微分。 使用 C++ 模板可以提高自动微分效率，而数值微分成本高，容易出现数值错误，并导致收敛速度较慢。

3.2 解析求导

​ 在某些情况下，无法使用自动微分。 例如，以封闭形式计算导数而不是依赖于自动微分代码使用的链式规则可能会更有效。

​ 在这种情况下，可以提供您自己的残差和雅可比计算代码。 为此，如果您在编译时知道参数和残差的大小，请定义 CostFunction 或 SizedCostFunction 的子类。 例如，这里是$f(x)=10-x$实现的 SimpleCostFunction

class QuadraticCostFunction : public ceres::SizedCostFunction<1, 1> {
 public:
  virtual ~QuadraticCostFunction() {}
  virtual bool Evaluate(double const* const* parameters,
                        double* residuals,
                        double** jacobians) const {
    const double x = parameters[0][0];
    residuals[0] = 10 - x;

    // Compute the Jacobian if asked for.
    if (jacobians != nullptr && jacobians[0] != nullptr) {
      jacobians[0][0] = -1;
    }
    return true;
  }
};

​ SimpleCostFunction::Evaluate 提供了一个参数输入数组、一个用于残差的输出数组残差和一个用于雅可比矩阵的输出数组 jacobians。 jacobians 数组是可选的，Evaluate 应该检查它何时为非空，如果是，则用残差函数的导数的值填充它。 在这种情况下，由于残差函数是线性的，雅可比是常数。 从上面的代码片段可以看出，实现 CostFunction 对象有点繁琐。 建议除非有充分的理由和能力自己管理雅可比计算，否则您可以使用 AutoDiffCostFunction 或 NumericDiffCostFunction 来构造残差块。

​ 计算导数是迄今为止使用 Ceres 最复杂的部分，根据情况，用户可能需要更复杂的方法来计算导数。 本节仅涉及最为简单的部分。 一旦您对使用 NumericDiffCostFunction 和 AutoDiffCostFunction 感到满意，建议查看 DynamicAutoDiffCostFunction、CostFunctionToFunctor、NumericDiffFunctor 和 ConditionedCostFunction，了解构建和计算成本函数的更高级方法。

• 这里给出解析求导计算上述函数最小值的代码

#include <vector>

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

using ceres::CostFunction;
using ceres::Problem;
using ceres::SizedCostFunction;
using ceres::Solve;
using ceres::Solver;

// A CostFunction implementing analytically derivatives for the
// function f(x) = 10 - x.
class QuadraticCostFunction
    : public SizedCostFunction<1 /* number of residuals */,
                               1 /* size of first parameter */> {
 public:
  bool Evaluate(double const* const* parameters,
                double* residuals,
                double** jacobians) const override {
    double x = parameters[0][0];

    // f(x) = 10 - x.
    residuals[0] = 10 - x;

    // f'(x) = -1. Since there's only 1 parameter and that parameter
    // has 1 dimension, there is only 1 element to fill in the
    // jacobians.
    //
    // Since the Evaluate function can be called with the jacobians
    // pointer equal to nullptr, the Evaluate function must check to see
    // if jacobians need to be computed.
    //
    // For this simple problem it is overkill to check if jacobians[0]
    // is nullptr, but in general when writing more complex
    // CostFunctions, it is possible that Ceres may only demand the
    // derivatives w.r.t. a subset of the parameter blocks.
    if (jacobians != nullptr && jacobians[0] != nullptr) {
      jacobians[0][0] = -1;
    }

    return true;
  }
};

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

  // The variable to solve for with its initial value. It will be
  // mutated in place by the solver.
  double x = 0.5;
  const double initial_x = x;

  // Build the problem.
  Problem problem;

  // Set up the only cost function (also known as residual).
  CostFunction* cost_function = new QuadraticCostFunction;
  problem.AddResidualBlock(cost_function, nullptr, &x);

  // Run the solver!
  Solver::Options options;
  options.minimizer_progress_to_stdout = true;
  Solver::Summary summary;
  Solve(options, &problem, &summary);

  std::cout << summary.BriefReport() << "\n";
  std::cout << "x : " << initial_x << " -> " << x << "\n";

  return 0;
}