Ceres使用（二）

非线性最小二乘

Introduction

Ceres可以解决这种形式的边界约束鲁棒非线性最小二乘问题：

$$\begin{split}\min_{\mathbf{x}} &\quad \frac{1}{2}\sum_{i} \rho_i\left(\left\|f_i\left(x_{i_1}, ... ,x_{i_k}\right)\right\|^2\right) \quad\text{s.t.} &\quad l_j \le x_j \le u_j\end{split}\quad\text(1)$$

这种形式的问题出现在科研工程的很多场合，从统计学中的曲线拟合，到计算机视觉中的三维重建。
在这章中，我们将要学习如何用Ceres Solver解决问题（1）。本章中的所有例子的完整工程代码可以在这里获取。
表达式$\rho_i\left(\left\|f_i\left(x_{i_1},...,x_{i_k}\right)\right\|^2\right)$是参差模块ResidualBlock，其中$f_i(\cdot)$是一个代价函数CostFunction，依赖于参数块$[x_{i_1}, ... ,x_{i_k}]$。在大部分的优化问题中，小群体的标量一起出现。例如，平移向量的三个元素，四元数的四个元素定义里一个相机的位姿。我们把这样一组标量称为参数块ParameterBlock。当然，一个参数块可以只有一个参数，$l_j$和$u_j$是$x_j$的边界约束。
$\rho_i$是误差函数LossFunction。误差函数是一个标量函数，用于减少异常值对非线性最小二乘问题的影响。
特例：$\rho_i(x)=x\quad-\infty<x<\infty$，我们可以获取更多熟悉的非线性最小二乘问题。

$$\frac{1}{2}\sum_{i} \left\|f_i\left(x_{i_1}, ... ,x_{i_k}\right)\right\|^2.\quad\text(2)$$

Hello World!

1.考虑找到函数的最小值：

$$\frac{1}{2}(10-x)^2$$
该问题很简单，可以一眼看出，它的最小值位于 $x=10$处。可以通过这个问题作为使用ceres的示例。
第一步：写出这个函数的评估项函数 $f(x)=10-x$：

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

需要注意的是operator()是一个模板类函数，假设所有的输入和输出参数都是T类型。模板的使用使得Ceres能够在多种情况调用CostFunctor::operator<\T>()，当T=double型，T=Jet型（此时是需要用雅克比矩阵）。
一旦有能够计算残差的方程了，就可以用它构建非线性最小二乘问题，并且使用Ceres求解。
整个代码为：

#include <iostream>
#include <ceres/ceres.h>
#include <glog/logging.h>

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

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

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

    // 设定求解的未知数的初值
    double initial_x = 5.0;
    double x = initial_x;

    //建立Problem
    Problem problem;

    //设置唯一的残差方程CostFunction。用自动求导获得导数（或者雅克比矩阵）
    //自动求导的模板参数<误差类型， 输出维度， 输入维度>
    CostFunction* cost_function = new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);
    //设置残差模块，参数(代价函数，核函数，待估计参数)
    problem.AddResidualBlock(cost_function, NULL, &x);

    //运行求解器
    Solver::Options options;
    options.linear_solver_type = ceres::DENSE_QR;   //增量方程的求解方式
    options.minimizer_progress_to_stdout = true;    //是否把优化最小二乘的过程输出到cout
    Solver::Summary summary;    //优化信息
    Solve(options, &problem, &summary); //开始优化

    std::cout << summary.BriefReport() << std::endl;    //输出结果
    std::cout << "x : " << initial_x << " -> " << x << std::endl;

    return 0;
}

对应的CMakeLists.txt文件为：

cmake_minimum_required(VERSION 3.8)
project(ceres_study)

set(CMAKE_CXX_STANDARD 11)

find_package(Ceres REQUIRED)
include_directories( ${CERES_INCLUDE_DIRS} )

set(SOURCE_FILES main.cpp)
add_executable(ceres_study ${SOURCE_FILES})
target_link_libraries(ceres_study ${CERES_LIBRARIES})

求解结果：

iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  1.250000e+01    0.00e+00    5.00e+00   0.00e+00   0.00e+00  1.00e+04        0    2.11e-05    5.43e-05
   1  1.249750e-07    1.25e+01    5.00e-04   5.00e+00   1.00e+00  3.00e+04        1    3.93e-05    1.42e-04
   2  1.388518e-16    1.25e-07    1.67e-08   5.00e-04   1.00e+00  9.00e+04        1    9.86e-06    1.61e-04
Ceres Solver Report: Iterations: 3, Initial cost: 1.250000e+01, Final cost: 1.388518e-16, Termination: CONVERGENCE
x : 5 -> 10

从$x=5$开始，求解器用了两次迭代就得到了10。细心的读者会注意到这是一个线性问题，一个线性求解问题应该能够得到最优值。求解器的默认配置是针对非线性问题的，为了方便我们没有在这个例子中修改参数。其实是可以用Ceres经过一次迭代就可获得最终结果。注意到，求解器在第一次迭代中确实已经非常接近最优函数值0。在之后讨论Ceres的收敛和参数设置时，我们将更详细的讨论这些问题。
实际上，求解器运行了三次迭代，通过查看线性求解器在第三次迭代中返回的值，发现参数块的更新太小并且声明了收敛。Ceres只在迭代结束时打印出显示，一旦检测到收敛就终止，这就是你为什么只能在这里看到两个迭代，而不是三个迭代。

Derivatives

和其他优化包一样，Ceres求解器依赖于能够在任意参数值处评估目标函数中每个项的值和导数。正确而有效的做到这一点，对于取得好的结果至关重要。Ceres求解器提供了很多方法，在上例中已经展示了一种——Automatic Differentiation自动求导。
此外，还有另外两种方法：分析法Analytic和数字求导numeric derivatives。

Numeric Derivatives

在某些情况下，不可能定义一个模板类的代价函数，例如，当残差的估计涉及一个你无法控制的库函数的调用时。在这种情况下，可以使用数字求导。用户定义了一个函数计算残差值，并且构建了一个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, NULL, &x);

注意，在自动求导中，这部分代码是：

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

和自动求导的构造函数几乎相同，除了一个额外的模板参数，指明用于计算数值导数的有限差分格式的类型。
一般而言，我们推荐使用自动求导而不是数值求导。C++模板类的使用使得自动求导效率高，而数值求导消耗大，容易出现数字错误，并导致收敛速度变慢。

Analytic Derivatives

在有些例子中，不可能使用自动求导。例如，可能是以闭合形式计算导数而不是用自动求导中的链式法则更有效。
在这种情况下，需要自行提供残差和雅克比矩阵的计算代码。为此，如果在编译时知道参数和残差的大小，则定义一个CostFunction或者SizedCostFunction的子类。这里是$f(x)=10-x$的一个例子：

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 != NULL && jacobians[0] != NULL) {
      jacobians[0][0] = -1;
    }
    return true;
  }
};

SimpleCostFunction::Evaluate提供了参数parameters输入数组，残差输出数组residuals，雅克比矩阵的输出数组jacobians。jacobians数组是可选的，当Evaluate非空时，需要检验以下，且在此情况下，需要用残差方程的导数来填充。在这个例子中，因为残差方程是线性的，所以jacobians是常数。
从上面的代码片段可以看出，实现CostFunction有些乏味。我们建议，除非你有足够的理由自己计算雅克比，你可以使用AutoDiffCostFunction或NumericDiffCostFunction来构造你的残差块。

More About Derivatives

计算导数是使用Ceres中最复杂的一部分，并且根据情况，用户可能需要更复杂的方式计算导数。这部分只是简单介绍了如何给Ceres提供导数。当你习惯了NumericDiffCostFunction和AutoDiffCostFunction时，我们建议你也看一下DynamicAutoDiffCostFunction，CostFunctionToFunctor，NumericDiffFunctor和ConditionedCostFunction，从而能够用更先进的方式构造和计算代价函数。