非线性求解器Ceres Solver

1 非线性最小二乘法

1.1 摘要

这个库主要用于求解具有边界约束的鲁棒非线性二乘问题，形式如下：
$$\begin{gathered} \underset{\mathbf{x}}{\min }=\frac{1}{2}\sum _i{\rho }_i(||f_i(x_{i1},...,x_{ik})|{|}^2) \\ \text{s.t.}{l}_j\le x_j\le u_j\text{\hspace{1em}(1)} \end{gathered}$$
其中，${\rho }_i(||f_i(x_{i1},...,x_{ik})|{|}^2)$被称为残差块；$f(\ast )$被称为代价函数；$x_{i1},...,x_{ik})$被称为参数块；${\rho }_i$为损失函数，目的是降低outlier对优化问题的影响。

如果定义；${\rho }_i(x)=x$为损失函数，同时不给出优化变量的边界，我们就得到了一个非线性最小二乘问题。

1.2 一个简单的例子

给出一个函数，求取其最小值

$$\frac{1}{2}(10-x{)}^2$$

STEP1:写出代价函数$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求解函数。

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

  // The variable to solve for with its initial value.
  double initial_x = 5.0;
  double x = initial_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.linear_solver_type = ceres::DENSE_QR;
  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;
}

1.3 导数

跟很多非线性优化器一样，优化器取决于目标函数的取值与函数来确定优化是否完成，但是在之前的例子中，我们使用的是自动求导，显然如果给出导数的显式表达，计算会更快。
考虑另外两种形式：1.解析求导；2.数值求导。

1.3.1 数值求导

在一些情况下，很难去定义模版化的代价函数，比如这个残差块涉及到别的库，在这种情况下，只能使用数值求导，即便将会付出很大的计算代价。
这个需要用到NumericDiffCostFunction，如

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);

1.3.2 解析求导

在某些情况下，使用自动微分是不可能的。例如，可能的情况是，以封闭形式计算导数比依靠自动微分代码使用的链式规则更有效率。

在解析求导的情形下，需要提供你自己的残差与雅可比计算的代码。可以定义一个CostFunction或者SizedCostFunction。

给一个解析求导的简例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通过输入参数确定，残差与雅可比依靠定义的residuals与jacobians。

1.4 鲍威尔函数

考虑一种更复杂的例子，最小化鲍威尔函数：
给定$x=[x_1,x_2,x_3,x_4]$

$$\begin{gathered} f_1(x)=x_1+10x_2 \\ {f}_2(x)=\sqrt{5}（x_3-x_4） \\ {f}_3(x)=(x_2-2x_3{)}^2 \\ {f}_4(x)=\sqrt{10}(x_1-x_4{)}^2 \\ F(x)=[f(x_1),f(x_2),f(x_3),f(x_4)] \end{gathered}$$

很显然，F(x)是一个四个参数的函数，含有四个残差，我们需要找到一个参数使得$||F(x)||$最小。

首先，先写出$f_4(x_1,x_4)$:

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

同样的，我们也可以定义F1,F2与F3。此时，问题可以构建如下：

double x1 =  3.0; double x2 = -1.0; double x3 =  0.0; double x4 = 1.0;

Problem problem;

// Add residual terms to the problem using the autodiff
// wrapper to get the derivatives automatically.
problem.AddResidualBlock(
  new AutoDiffCostFunction<F1, 1, 1, 1>(new F1), nullptr, &x1, &x2);
problem.AddResidualBlock(
  new AutoDiffCostFunction<F2, 1, 1, 1>(new F2), nullptr, &x3, &x4);
problem.AddResidualBlock(
  new AutoDiffCostFunction<F3, 1, 1, 1>(new F3), nullptr, &x2, &x3);
problem.AddResidualBlock(
  new AutoDiffCostFunction<F4, 1, 1, 1>(new F4), nullptr, &x1, &x4);

1.5 曲线拟合

这个例子出现在高翔的slam 14讲中。高翔给出了手写非线性优化。

1.6 鲁棒曲线拟合

如果在观测中，出现一些外点，这个时候非线性最小二乘就会出现拟合问题，就需要自己去拟合曲线。
在鲁棒曲线拟合中，通常的做法，是引入损失函数减少外点的影响。
为了把损失函数加入到残差块中，我们可以改变上面提到的加入残差块的方法

原先方式：

problem.AddResidualBlock(cost_function, nullptr , &m, &c);

加入损失函数的方式：

problem.AddResidualBlock(cost_function, new CauchyLoss(0.5) , &m, &c);

1.7 Bundle Adjustment

解决大规模BA问题是写ceres的动机之一。

BA是为了解决最小化重投影误差。
首先还是构建残差，类似于曲线拟合，每一张图像都构建重投影残差。
如下BA问题需要优化 相机的R、T以及焦距以及两个畸变系数。

struct SnavelyReprojectionError {
  SnavelyReprojectionError(double observed_x, double observed_y)
      : observed_x(observed_x), observed_y(observed_y) {}

  template <typename T>
  bool operator()(const T* const camera,
                  const T* const point,
                  T* residuals) const {
    // camera[0,1,2] are the angle-axis rotation.
    T p[3];
    ceres::AngleAxisRotatePoint(camera, point, p);
    // camera[3,4,5] are the translation.
    p[0] += camera[3]; p[1] += camera[4]; p[2] += camera[5];

    // Compute the center of distortion. The sign change comes from
    // the camera model that Noah Snavely's Bundler assumes, whereby
    // the camera coordinate system has a negative z axis.
    T xp = - p[0] / p[2];
    T yp = - p[1] / p[2];

    // Apply second and fourth order radial distortion.
    const T& l1 = camera[7];
    const T& l2 = camera[8];
    T r2 = xp*xp + yp*yp;
    T distortion = 1.0 + r2  * (l1 + l2  * r2);

    // Compute final projected point position.
    const T& focal = camera[6];
    T predicted_x = focal * distortion * xp;
    T predicted_y = focal * distortion * yp;

    // The error is the difference between the predicted and observed position.
    residuals[0] = predicted_x - T(observed_x);
    residuals[1] = predicted_y - T(observed_y);
    return true;
  }

   // Factory to hide the construction of the CostFunction object from
   // the client code.
   static ceres::CostFunction* Create(const double observed_x,
                                      const double observed_y) {
     return (new ceres::AutoDiffCostFunction<SnavelyReprojectionError, 2, 9, 3>(
                 new SnavelyReprojectionError(observed_x, observed_y)));
   }

  double observed_x;
  double observed_y;
};

以上只是一些Ceres最简单的例子，Ceres给出了很多示例：https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/