Ceres Solvel翻译第一期-说明文档
非线性最小二乘法
介绍
Ceres可以解决有界约束形式的鲁棒非线性最小二乘问题，形式如下：
$$min_x\frac{1}{2}\sum_i\rho_i(\left |f_i(x_{i_1},\cdots,x_{i_k}) \right |^2)$$
$$s.t. l_i\le x_j\le u_j$$
这一表达式在工程和科学领域有非常广泛的应用，比如统计学中的曲线拟合，或者在计算机视觉中依据图像进行三维模型的构建等等。
$\rho_i(\left |f_i(x_{i_1},\cdots,x_{i_k}) \right |^2)$这一部分被称为残差块（ResidualBlock)，其中的$f_i(\cdot )$叫做代价函数（CostFuction)。代价函数依赖于一系列参数$[x_{i_1},\cdots,x_{i_k}]$,这一系列参数（均为标量）称为参数块（ParameterBlock)。当然参数块中也可以只含有一个变量。$l_j$和$u_j$分别是变量块$x_j$的上下边界。
$\rho_i$是损失函数（LossFuction)。损失函数是一个标量函数，其作用是减少异常值（Outliers)对优化结果的影响。其效果类似于对函数的过滤。
一个特殊情况是，$\rho_i(f
(x)) = f(x)$,也就是没有对函数进行任何过滤，损失函数的输出等于输入。若同时令$l_j = -\infty$并且$u_j = \infty$，即参数块的取值没有限制，那么此时问题变成了非线性最小二乘问题。表达式如下：
$$\frac{1}{2}\sum_i\left | f_i(x_{i_1},\cdots,x_{i_k}) \right |^2$$
Hello World
每个程序最简单的示例被称为Hello World。本节将简单描述Ceres的Hello World示例，以便让读者对库的使用步骤快速建立认识。
在Hello World这个例子中，待优化的函数是$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，然后仅仅以double类型输出残差值。也可以令T = Jet然后输出雅可比矩阵。这一部分在后面会有更详细的介绍。
雅可比矩阵实际上就是对一个含有多个参数的函数$f(x)$求一系列一阶偏微分
一旦残差方程建立，我们就可以用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;
}
AutoDiffCostFuction将刚刚建立的CostFuctor结构的一个实例作为输入，自动生成其微分并且赋予其一个CostFuction类型的接口。
编译完成，运行结果如下：（原文中$x$初始值输出错误，修正为上面代码中的$5$）
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 : 5 -> 10
初始值为$5$，最终通过两次循环之后到达最优解$10$。其实本例是一个线性问题，因为$f(x) = 10 - x$是一个线性函数，但是Ceres仍然可以应用。
导数
像大多数优化软件包一样，Ceres求解器的求解基于其能够在任意参数值下评估目标函数中每个项的值和导数。 正确而高效地做到这一点对于取得优秀的运算结果至关重要。Ceres提供了一系列解决方案，其中一个就是在Hello World中用到的Automatic Differentiation (自动微分算法)。这一部分我们将探讨另外两种可能性，即解析法（Analytic）和数值法（Numeric ）求导。
数值法求导（Numeric Derivatives)
在某些情况下，像在Hello World中一样定义一个代价函数是不可能的。比如在求解残差值（residual）的时候调用了一个库函数，而这个库函数的内部算法你根本无法干预。在这种情况下数值微分算法就派上用场了。用户定义一个CostFunctor来计算残差值，并且构建一个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);
这里我们引用一下Hello World中模板类函数以及自动微分算法（automatic)的代码进行比对：
//模板类函数
struct CostFunctor {
template <typename T>
bool operator()(const T* const x, T* residual) const {
residual[0] = T(10.0) - x[0];
return true;
}
};
//自动微分算法（automatic）
CostFunction* cost_function =
new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);
problem.AddResidualBlock(cost_function, NULL, &x);
可以发现两种算法在构建Problem时候基本差不多。但是在用Nummeric算法时需要额外给定一个参数ceres::CENTRAL。这个参数告诉计算机如何计算导数。更多具体介绍可以参看NumericDiffCostFunction的Doc文档。
Ceres官方更加推荐自动微分算法，因为C++模板类使自动算法有更高的效率。数值微分算法通常来说计算更复杂，收敛更缓慢。
解析法求导（Analytic Derivatives）
有些时候，应用自动求解算法时不可能的。比如在某些情况下，计算导数的时候，使用闭合解（closed form，也被称为解析解）会比使用自动微分算法中的链式法则（chain rule）更有效率。这里辨析一下解析解和数值解：
在解组件特性相关的方程式时，大多数的时候都要去解偏微分或积分式，才能求得其正确的解。依照求解方法的不同，可以分成以下两类：解析解和数值解。
- 解析解(analytical solution)：
就是一些严格的公式,给出任意的自变量就可以求出其因变量,也就是问题的解。他人可以利用这些公式计算各自的问题。所谓的解析解是一种包含：分式、三角函数、指数、对数甚至无限级数等基本函数的解的形式。用来求得解析解的方法称为解析法〈analytic techniques、analytic methods〉，解析法即是常见的微积分技巧，例如分离变量法等。解析解为一封闭形式〈closed-form〉的函数，因此对任一独立变量，我们皆可将其带入解析函数求得正确的相依变量。因此，解析解也被称为闭式解（closed-form solution）。- 数值解(numerical solution)：
是采用某种计算方法,如有限元的方法, 数值逼近,插值的方法, 得到的解。别人只能利用数值计算的结果, 而不能随意给出自变量并求出计算值。当无法藉由微积分技巧求得解析解时，这时便只能利用数值分析的方式来求得其数值解了。数值方法变成了求解过程重要的媒介。在数值分析的过程中，首先会将原方程式加以简化，以利后来的数值分析。例如，会先将微分符号（连续）改为差分符号（离散）等。然后再用传统的代数方法将原方程式改写成另一方便求解的形式。这时的求解步骤就是将一独立变量带入，求得相依变量的近似解。因此利用此方法所求得的相依变量为一个个分离的数值〈discrete values〉，不似解析解为一连续的分布，而且因为经过上述简化的动作，所以可以想见正确性将不如解析法来的好。
在这种情况下，你就可以自己编写残差计算代码和雅可比行列式的计算代码了。还是用Hello world中的$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 != nullptr && jacobians[0] != nullptr) {
jacobians[0][0] = -1;
}
return true;
}
};
Evaluate函数的参数包括：参数的输入数组、残差的输出数组以及雅可比矩阵的输出数组。其中雅可比矩阵是可选的，Evaluate会在它非空时进行检查，并且如果是非空则用残差的导数值进行填充。这种情况下残差函数是线性的，雅可比行列式是常数。
从上述代码片段可以看出，实现“CostFunction““其实有点乏味。所以除非有什么特殊原因需要自行构建雅可比的计算，否则最好还是直接使用自动微分法或者数值微分法来创建残差块。
其他导数计算方法
到目前为止，计算导数其实是Ceres最复杂的部分了。根据需要，用户有时候还需要更复杂的导数计算算法。这一节仅仅是大体介绍了如何使用Ceres进行导数计算最浅显的部分。对Numeric和Auto方法都很熟悉之后，还可以深入研究一下DynamicAutoDiffCostFunction , CostFunctionToFunctor, NumericDiffFunctor 和ConditionedCostFunction，从而实现更高级的代价函数的计算方法。
鲍威尔方程
在这一节我们使用一个复杂一些的例子——求解鲍威尔方程的最小值。我们定义参数块$x = [x_1,x_2,x_3,x_4]$，以及代价函数如下：
$$
\begin{aligned}
&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_1(x),f_2(x),f_3(x),f_4(x)]
\end{aligned}
$$
$F(x)$是关于上面四个残差值得方程。我们希望找到一组$x$，使得$\frac{1}{2}\left |F(x) \right |^2$取得最小值。
同样的，第一步仍然是定义残差方程。对于每一行方程都可以定义一个对应的结构体，如对于$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] = T(sqrt(10.0)) * (x1[0] - x4[0]) * (x1[0] - x4[0]);
return true;
}
};
类似的我们也可以实现$f_1$,$f_2$和$f_3$得代码。之后就可以通过下列代码，把各个残差块加入到Problem中。
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 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);
注意每个残差块只依赖与两个参数，而不是全部四个参数。运行结果如下：
Initial x1 = 3, x2 = -1, x3 = 0, x4 = 1
iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time
0 1.075000e+02 0.00e+00 1.55e+02 0.00e+00 0.00e+00 1.00e+04 0 4.95e-04 2.30e-03
1 5.036190e+00 1.02e+02 2.00e+01 2.16e+00 9.53e-01 3.00e+04 1 4.39e-05 2.40e-03
2 3.148168e-01 4.72e+00 2.50e+00 6.23e-01 9.37e-01 9.00e+04 1 9.06e-06 2.43e-03
3 1.967760e-02 2.95e-01 3.13e-01 3.08e-01 9.37e-01 2.70e+05 1 8.11e-06 2.45e-03
4 1.229900e-03 1.84e-02 3.91e-02 1.54e-01 9.37e-01 8.10e+05 1 6.91e-06 2.48e-03
5 7.687123e-05 1.15e-03 4.89e-03 7.69e-02 9.37e-01 2.43e+06 1 7.87e-06 2.50e-03
6 4.804625e-06 7.21e-05 6.11e-04 3.85e-02 9.37e-01 7.29e+06 1 5.96e-06 2.52e-03
7 3.003028e-07 4.50e-06 7.64e-05 1.92e-02 9.37e-01 2.19e+07 1 5.96e-06 2.55e-03
8 1.877006e-08 2.82e-07 9.54e-06 9.62e-03 9.37e-01 6.56e+07 1 5.96e-06 2.57e-03
9 1.173223e-09 1.76e-08 1.19e-06 4.81e-03 9.37e-01 1.97e+08 1 7.87e-06 2.60e-03
10 7.333425e-11 1.10e-09 1.49e-07 2.40e-03 9.37e-01 5.90e+08 1 6.20e-06 2.63e-03
11 4.584044e-12 6.88e-11 1.86e-08 1.20e-03 9.37e-01 1.77e+09 1 6.91e-06 2.65e-03
12 2.865573e-13 4.30e-12 2.33e-09 6.02e-04 9.37e-01 5.31e+09 1 5.96e-06 2.67e-03
13 1.791438e-14 2.69e-13 2.91e-10 3.01e-04 9.37e-01 1.59e+10 1 7.15e-06 2.69e-03
Ceres Solver v1.12.0 Solve Report
----------------------------------
Original Reduced
Parameter blocks 4 4
Parameters 4 4
Residual blocks 4 4
Residual 4 4
Minimizer TRUST_REGION
Dense linear algebra library EIGEN
Trust region strategy LEVENBERG_MARQUARDT
Given Used
Linear solver DENSE_QR DENSE_QR
Threads 1 1
Linear solver threads 1 1
Cost:
Initial 1.075000e+02
Final 1.791438e-14
Change 1.075000e+02
Minimizer iterations 14
Successful steps 14
Unsuccessful steps 0
Time (in seconds):
Preprocessor 0.002
Residual evaluation 0.000
Jacobian evaluation 0.000
Linear solver 0.000
Minimizer 0.001
Postprocessor 0.000
Total 0.005
Termination: CONVERGENCE (Gradient tolerance reached. Gradient max norm: 3.642190e-11 <= 1.000000e-10)
Final x1 = 0.000292189, x2 = -2.92189e-05, x3 = 4.79511e-05, x4 = 4.79511e-05
曲线拟合（Curve Fitting)
最小二乘法和非线性最小二乘分析得本来目的就是对一组数据进行曲线拟合。本节将介绍曲线拟合得问题。本节所用的采样点根据$y = e^{0.3x + 0.1}$生成，并且加入标准差为$\sigma = 0.2$高斯噪声。这$2n$个数据，存入data[]中。下面我们用下列带未知参数的方程来拟合这些采样点：
$$y = e^{mx+c}$$
同样的，我们定义一个用来计算残差的结构体。注意，对应每个采样点（观测点）都要计算一个残差。
struct ExponentialResidual {
ExponentialResidual(double x, double y)
: x_(x), y_(y) {}
template <typename T>
bool operator()(const T* const m, const T* const c, T* residual) const {
residual[0] = y_ - exp(m[0] * x_ + c[0]);
return true;
}
private:
// Observations for a sample.
const double x_;
const double y_;
};
下面构造Problem
double m = 0.0;
double c = 0.0;
Problem problem;
for (int i = 0; i < kNumObservations; ++i) {
CostFunction* cost_function =
new AutoDiffCostFunction<ExponentialResidual, 1, 1, 1>(
new ExponentialResidual(data[2 * i], data[2 * i + 1]));
problem.AddResidualBlock(cost_function, nullptr, &m, &c);
}
这里我们再次和Hello World进行对比：
struct CostFunctor {
template <typename T>
bool operator()(const T* const x, T* residual) const {
residual[0] = T(10.0) - x[0];
return true;
}
};
CostFunction* cost_function =
new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);
problem.AddResidualBlock(cost_function, NULL, &x);
通过对比，可以发现。在Hello World中，CostFunctor中是没有（显式）构造函数的，也就同样没有了初始值。所以在构造对象时，可以直接New CostFuntor。而在本节的曲线拟合例子中，构造对象时还要加上初始值new ExponentialResidual(data[2 * i], data[2 * i + 1]))。在方括号中的参数分别对应残差函数名<ExponentialResidual>，以及输出值（residual）的维度1，还有残差函数各个输入值（m,c）维度<1，1>。所以在本例中一共有三个1，而在Hello World中，只有两个1，即residual和x的维度。注意先是残差，后是输入参数，而且一一对应。
最后一点就是在把残差块加入problem的过程中，要把输入变量一一带入。比如&m，&c，&x等。以上就是在构建Problem的时候需要顾及到的三个方面。再就是在使用Numeric算法时，还要在方括号中指定计算机如何计算导数，如ceres::CENTRAL。
运行程序，得到下列结果：
iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time
0 1.211734e+02 0.00e+00 3.61e+02 0.00e+00 0.00e+00 1.00e+04 0 5.34e-04 2.56e-03
1 1.211734e+02 -2.21e+03 0.00e+00 7.52e-01 -1.87e+01 5.00e+03 1 4.29e-05 3.25e-03
2 1.211734e+02 -2.21e+03 0.00e+00 7.51e-01 -1.86e+01 1.25e+03 1 1.10e-05 3.28e-03
3 1.211734e+02 -2.19e+03 0.00e+00 7.48e-01 -1.85e+01 1.56e+02 1 1.41e-05 3.31e-03
4 1.211734e+02 -2.02e+03 0.00e+00 7.22e-01 -1.70e+01 9.77e+00 1 1.00e-05 3.34e-03
5 1.211734e+02 -7.34e+02 0.00e+00 5.78e-01 -6.32e+00 3.05e-01 1 1.00e-05 3.36e-03
6 3.306595e+01 8.81e+01 4.10e+02 3.18e-01 1.37e+00 9.16e-01 1 2.79e-05 3.41e-03
7 6.426770e+00 2.66e+01 1.81e+02 1.29e-01 1.10e+00 2.75e+00 1 2.10e-05 3.45e-03
8 3.344546e+00 3.08e+00 5.51e+01 3.05e-02 1.03e+00 8.24e+00 1 2.10e-05 3.48e-03
9 1.987485e+00 1.36e+00 2.33e+01 8.87e-02 9.94e-01 2.47e+01 1 2.10e-05 3.52e-03
10 1.211585e+00 7.76e-01 8.22e+00 1.05e-01 9.89e-01 7.42e+01 1 2.10e-05 3.56e-03
11 1.063265e+00 1.48e-01 1.44e+00 6.06e-02 9.97e-01 2.22e+02 1 2.60e-05 3.61e-03
12 1.056795e+00 6.47e-03 1.18e-01 1.47e-02 1.00e+00 6.67e+02 1 2.10e-05 3.64e-03
13 1.056751e+00 4.39e-05 3.79e-03 1.28e-03 1.00e+00 2.00e+03 1 2.10e-05 3.68e-03
Ceres Solver Report: Iterations: 13, Initial cost: 1.211734e+02, Final cost: 1.056751e+00, Termination: CONVERGENCE
Initial m: 0 c: 0
Final m: 0.291861 c: 0.131439
最终经过计算，结果是m=0.291861,c=0.131439。这个值和一开始的设定值（m=0.3,c=0.1）略有偏差。因为额外加入了高斯噪声，所以这个偏差的存在是合理的。拟合结果如下图所示，红圈即为拟合出的曲线。
鲁棒的曲线拟合（Robust Curve Fitting）
如果我们在数据集合中加入几个非常夸张的外围点Outliers，那么拟合结果会受到这几个点的明显影响。在这个时候需要应用损失函数（Loss function）来对异常数据进行过滤。比如在上文的例子中，我们对代码进行以下修改：
problem.AddResidualBlock(cost_function, NULL , &m, &c);
改为：
problem.AddResidualBlock(cost_function, new CauchyLoss(0.5) , &m, &c);
CauchyLoss是Ceres Solver附带的损失函数之一。 参数0.5指定了损失函数的规模。通过对下面两个图的对比，我们可以明显的发现损失函数的作用。
