Ceres-Solver 官方文档-其一

1. Ceres Solver
2. Ceres Example

系列文章：

1. Ceres-Solver 官方文档-其一
2. Ceres-Solver 官方文档-其二
3. Ceres Manifolds/LocalParameterization 类

本文使用 Ceres-Solver 2.2.0 作为本文版本，后期官方文档的更新可能会与文中存在区别。

Non-linear Least Squares

Ceres Solver 是一个开源 C++ 库，用于建模和解决大型、复杂的优化问题。它可用于求解带边界约束的非线性最小二乘问题和一般的无约束优化问题。本章完整的示例代码都可以在Ceres Example中找到。

1. Introduction

Ceres 可以求解形为
$$\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}\text{\hspace{1em}(1)}$$的边界约束鲁棒非线性最小二乘问题。

表达式 ${\rho }_i\left({\Vert f_i\left(x_{i_1},\dots ,x_{i_k}\right)\Vert }^2\right)$ 被称为 ResidualBlock(残差块)，其中 $f_i(\cdot )$ 是基于参数块$[x_{i_1},\dots ,x_{i_k}]$的CostFunction(代价函数)。在大多数优化中问题一小群标量一起出现。例如一个由三个分量组成的平移向量和一个由四个分量组成的四元数定义了一个相机的位姿。我们将这样的一组标量称为ParameterBlock(参数块)。当然参数块也可以只是一个参数。$l_j$ 和 $u_j$ 为参数块 $x_j$ 的边界。

${\rho }_i$ 是 LossFunction(损失函数)。损失函数是一个标量函数，用于减少离群值对非线性最小二乘问题解的影响。

作为特例，当 ${\rho }_i(x)=x$，也就是恒等函数(identity function)，且 $l_j=-\infty$ 及 $u_j=\infty$ 时，能够得到更熟悉的非线性最小二乘问题：
$$\begin{array}{l}\frac{1}{2}{\sum }_i{\Vert f_i\left(x_{i_1},\dots ,x_{i_k}\right)\Vert }^2\end{array}\text{\hspace{1em}(2)}$$

2. Hello World!

首先，考虑寻找函数的最小值的问题：
$$\frac{1}{2}(10-x{)}^2.$$这是一个平凡的问题，其最小值位于 $x=10$，但它是一个很好的开始，阐明解决 Ceres 问题的基础知识。

第一步是写一个拟函数(functor)来求这个函数的值：
$$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>()，当只需要残差的值时T=double，当需要雅可比矩阵时使用特殊类型 T=Jet(Jet 是 Ceres 定义的一个特殊类型。如果使用自动求导时，会发现 T 的格式与传进去的格式不一致，就是这里的原因)。在下一小节中，我们将更详细地讨论向 Ceres 提供导数的各种方式。

一旦我们有了计算残差函数的方法，现在是时候用它来构造一个非线性最小二乘问题，并让 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;
}

AutoDiffCostFucntion 将 CostFunctor 作为输入，自动的对其进行微分，并赋予其一个 CostFunction 接口。(自动求导使用的链式法则，会对 operator() 内的残差求导)

编译并运行代码 examples/helloworld.cc 得：

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=5$ 开始，求解器(solver)经过两次迭代得到最终结果 10，细心的读者会注意到这是一个线性问题，一个线性解应该足以得到最优值。求解器的默认配置是针对非线性问题的，为了简单起见，我们在本例中没有对其进行更改。在一次迭代中使用 Ceres 确实有可能得到这个问题的解决方案。还要注意，在第一次迭代中，求解器确实非常接近最优函数值 0。我们将在讨论 Ceres 的收敛和参数设置时将会更详细地讨论这些问题。

3. Derivatives

Ceres-Solver 与大多数优化包一样，依赖于能够在任意参数下评估目标函数中每一项的函数值与导数，能够正确并有效的做到这一点对获得好的结果至关重要。Ceres Solver提供了几种不同方法，在上面的例子中你已经见到了其中的一种----自动微分(Automatic Differentiation)，我们现在考虑其他两种求导方式：解析求导(Analytic Derivatives)与数值求导(Numerical Derivatives)

3.1 Numeric Derivatives

examples/helloworld_numeric_diff.cc
(如果函数 $f$ 是个黑盒子，即我们不知道它的解析形式，但给定任意的输入 $x$，我们都能得到唯一的输出 $f(x)$。这就是典型的计算机程序的特点，函数的内部实现被封装在库中，调用方得不到库的源码，这时候要想求函数的导数，就只能用数值求导方法。)

在某些情况下，我们无法定义一个模板代价拟函数，例如，当残差的求值涉及对无法控制的库函数的调用时。在这种情况下，可以使用数值微分法。用户定义一个仿函数来计算残差，并构建一个NumericDiffCostFunction 使用残差，以 $f(x)=10-x$ 为例，对应的仿函数为(与自动微分相比，前面得 T* 替换成了 double*)：

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 用来表明采用哪种有限差分方案来计算数值导数，详情可见 NumericDiffCostFunction。

一般来说，我们推荐自动微分而不是数值微分。c++模板的使用使自动微分变得高效，而数值微分代价高昂，容易出现数值错误，并导致较慢的收敛。

3.2 Analytic Derivatives

examples/helloworld_analytic_diff.cc
有些情况下可能无法使用自动微分。例如，在封闭形式下计算导数可能比依赖于自动微分代码使用的链式法则更有效。

在这种情况下，需要提供你自己的 残差 与 雅可比 计算代码。定义一个 CostFunction 的子类(subclass)，如果你知道 参数(parameter) 与 残差(residual) 在编译时的大小也可以定义 SizedCostFunction 的子类(实例中参数和残差大小确定，所以直接定义的 SizedCostFunction)。下面以实现 $f(x)=10-x$ 的 QuadraticCostFunction 为例：

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

QuadraticCostFunction::Evaluate 提供了 parameters 的输入数组，残差的输出数组 residuals 和雅可比矩阵的输出数组 jacobians。jacobians 数组是可选的，Evaluate 将检查它是否为非空，如果是非空，则用残差函数的导数值填充它。在这种情况下，由于残差函数是线性的，雅可比矩阵是常数。

从上面的代码片段可以看出，实现 CostFunction 对象有点繁琐。除非您有很好的理由自己管理雅可比矩阵计算，否则我们建议您使用AutoDiffCostFunction 或 NumericDiffCostFunction 来构造残差块。

3.3 More About Derivatives

计算导数是迄今为止使用 Ceres 最复杂的部分，根据具体情况，用户可能需要更复杂的计算导数的方法。这一部分只是浅显的介绍了如何向 Ceres 提供导数。一旦你熟悉了 NumericDiffCostFunction 和 AutoDiffCostFunction，我们建议你看看 DynamicAutoDiffCostFunction, CostFunctionToFunctor, NumericDiffFunctor 和 ConditionedCostFunction，以获得更高级的方法来构造和计算代价函数。

4. Powell’s Function

examples/powell.cc
现在考虑一个稍微复杂点的例子------最小化鲍威尔方程，令 $x=[x_1,x_2,x_3,x_4]$，且：
$$\begin{array}{rl}{f}_1(x)& =x_1+10x_2\\ f_2(x)& =\sqrt{5}\left(x_3-x_4\right)\\ f_3(x)& ={\left(x_2-2x_3\right)}^2\\ f_4(x)& =\sqrt{10}{\left(x_1-x_4\right)}^2\\ F(x)& =\left[f_1(x),f_2(x),f_3(x),f_4(x)\right]\end{array}$$$F(x)$ 是一个拥有四个参数、四组残差的方程，我们希望找到一个令 $\frac{1}{2}\Vert F(x){\Vert }^2$ 最小化的 $x$。

与前文相同，第一步是定义对目标仿函数中的项求值的仿函数。以下是评估 $f_1(x_1,x_2)$~$f_4(x_1,x_4)$ 的代码：

struct F1
{
   template <typename T>
   bool operator()(const T* const x1, const T* const x2, T* residual) const
   {
   	residual[0] = x1[0] + 10 * x2[0];
   	return true;
   }
};
struct F2
{
   template <typename T>
   bool operator()(const T* const x3, const T* const x4, T* residual) const
   {
   	residual[0] = sqrt(5) * (x3[0] - x4[0]);
   }
};
struct F3
{
   template <typename T>
   bool operator()(const T* const x2, const T* const x3, T* residual) const
   {
   	residual[0] = (x2[0] - 2 * x3[0]) * (x2[0] - 2 * x3[0]);
   }
};
struct F4 
{
   template <typename T>
   bool operator()(const T* const x1, const T* const x4, T* residual)const
   {
   	residual = sqrt(10.0) * (x1[0] - x4[0]) * (x1[0] - x4[0]);
   	return true;
   }
};

利用上面定义的结构体按如下方法构建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 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);

注意，每个 ResidualBlock 只依赖对应的残差对象所依赖的两个参数，而不是所有四个参数。编译并运行 examples/powell.cc：

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

5. Curve Fitting

examples/curve_fitting.cc
到目前为止，我们看到的例子都是没有数据的简单优化问题(比如下面示例中的 x_ 和 y_)。最小二乘和非线性最小二乘分析的最初目的是拟合曲线与数据。现在正适合我们考虑这个问题 examples/curve_fitting.cc。它包含通过对曲线 $y=e^{0.3x+0.1}$ 采样生成的数据，并向数据添加带有标准差 $\sigma =0.2$ 的高斯噪声。现在我们用一些数据来拟合曲线：
$$y=e^{mx+c}$$我们首先定义一个模板对象来计算残差。每次观测(observation)都会有一个残差。(新增成员变量 和 构造函数(带输入) 来使用观测)

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_;
};

假设观测数据存在一个 $2n$ 大小的数组 data 当中，problem 的构建就是简单的对每一个观测数据创建一个 CostFunction：

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

编译并运行 examples/curve_fitting.cc 得：

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$ 开始，初始的目标函数值是 $121.173$。Ceres找到了一组结果 $m=0.291861,c=0.131439$ 对应的目标函数值是 $1.05675$。这些值与原始的模型参数 $m=0.3,c=0.1$ 稍有不同，但是这个是可以预料到的。当使用有噪声的数据构建拟合曲线时，我们希望看到这样的偏差(deviation)。事实上如果使用 $m=0.3,c=0.1$ 来评估目标函数时，会得到一个大于 $1.05675$ 的目标函数值 $1.082425$。下图阐明了拟合：

6. Robust Curve Fitting

examples/robust_curve_fitting.cc
现在假设我们得到的数据有一些离群值(outlier)，也就是说，我们有一些不服从噪声模型的点。如果我们使用上面的代码来拟合这些数据，我们将得到如下所示的拟合结果。请注意拟合曲线是如何偏离基本事实的：

要处理离群值，常规的方法是使用 LossFunction(因为有离群点，为了减少离群点噪声带来的影响，需要引入核函数)。损失函数减少残差高的残差块的影响，通常是与离群值对应的残差块。为了将损失函数与残差块联系起来，我们改变：

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

至

problem.AddResidualBlock(cost_function, new CauchyLoss(0.5), &m, &c); //新增 new CauchyLoss(0.5) 核函数

CauchyLoss 是 Ceres Solver 附带的损失函数之一。参数 0.5 指定损失函数的尺度。因此，我们在 examples/robust_curve_fitting.cc 得到以下拟合结果。注意拟合曲线是如何向真实曲线靠拢的。

使用 LossFunction 来减少离群值对最小二乘拟合的影响。

7. Bundle Adjustment

examples/simple_bundle_adjuster.cc
编写Ceres的主要原因之一是我们需要解决大规模的BA问题。

给定一组测量的图像特征位置和对应关系，BA的目标是找到最小重投影误差的三维点位置和相机参数。这种优化问题通常被表述为非线性最小二乘问题，其中误差是观测到的特征位置与相应的三维点在相机图像平面上的投影之差的平方 $L_2$ 范数。Ceres 对解决BA问题有广泛的支持。

让我们从BAL数据集中解决一个问题 examples/simple_bundle_adjuster.cc。

与前面一样，第一步是定义一个计算重投影误差/残差的模板化拟函数。拟函数的结构类似于ExponentialResidual，里面有一个该对象的实例负责每个图像观测。

BAL问题中的每个残差都依赖于一个三维点和一个九参数相机。定义相机的9个参数是：3个用于罗德里格斯轴角矢量的旋转，3个用于平移，1个用于焦距，2个用于径向畸变。该相机型号的详细信息可以在Bundler主页和BAL主页上找到。

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

注意，与前面的例子不同，这是一个非平凡的函数，计算它的解析雅可比矩阵有点麻烦。自动求导简单得多。函数 AngleAxisRotatePoint() 和其他用于操作旋转的函数可以在 include/ceres/rotate.h 中找到。

给定此拟函数，BA问题可构造为：

ceres::Problem problem;
for (int i = 0; i < bal_problem.num_observations(); ++i) {
  ceres::CostFunction* cost_function =
      SnavelyReprojectionError::Create(
           bal_problem.observations()[2 * i + 0],
           bal_problem.observations()[2 * i + 1]);
  problem.AddResidualBlock(cost_function,
                           nullptr /* squared loss */,
                           bal_problem.mutable_camera_for_observation(i),
                           bal_problem.mutable_point_for_observation(i));
}

注意，BA的问题构造与曲线拟合示例非常相似----每个观测向目标函数中添加一项。

由于这是一个大型稀疏问题(对于DENSE_QR来说很大)，解决这个问题的一种方法是将Solver::Options::linear_solver_type设置为SPARSE_NORMAL_CHOLESKY并调用Solve()。虽然这是一件合理的事情，但BA问题具有特殊的稀疏性结构，可以利用它来更有效地解决这些问题。Ceres 为这个任务提供了三个专门的求解器(统称为基于Schur的求解器)。示例代码使用其中最简单的 DENSE_SCHUR。

ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_SCHUR;
options.minimizer_progress_to_stdout = true;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
std::cout << summary.FullReport() << "\n";

有关更复杂的BA示例，该示例演示了使用 Ceres 的更高级功能，包括各种线性求解器，鲁棒损失函数和流形，请参阅examples/ bundle_adjuter .cc。

8. Other Examples

除了上面章节的几个示例，example 目录包含了其他几个示例：

1. examples/ bundle_adjuter .cc 展示了如何使用 Ceres 的各种特性来解决BA问题。

2. circle_fit.cc 显示了如何将数据拟合到圆上。

3. ellipse_approximation.cc 拟合随机分布在具有近似线段轮廓的椭圆上的点。这是通过共同优化线段轮廓的控制点以及数据点的预像位置来实现的。这个示例的目的是展示 Solver::Options::dynamic_sparsity 的一个示例用例，以及它如何使数字密集但动态稀疏的问题受益。

4. denoising.cc 使用 Fields of Experts 模型实现图像去噪。

5. nist.cc 实现并试图解决 NIST 的非线性回归问题。

6. more_garbow_hillstrom.cc 一篇论文中测试问题的子集。

7. libmv_bundle_adjuster.cc 是Blender/libmv使用的BA算法。

8. libmv_homography.cc 该文件演示了解决两组点之间的单应性问题，并通过对图像空间错误进行回调检查来使用自定义退出条件。

9. robot_pose_mle.cc 这个例子演示了如何使用 CostFunction 的 DynamicAutoDiffCostFunction 变体。DynamicAutoDiffCostFunction 用于在编译时不知道参数块的数量或大小的情况。

   这个例子模拟了一个机器人在一维走廊上穿行，其中包含噪声里程计读数和走廊末端的噪声范围读数。通过融合噪声里程计和传感器读数，本例演示了如何计算机器人在每个时间步的姿态的最大似然估计(MLE)。

10. slam/pose_graph_2d/pose_graph_2d.cc SLAM 问题包括构建未知环境的地图，同时根据该地图进行定位。这个问题的主要困难在于没有任何额外的外部辅助信息，如 GPS。SLAM 被认为是机器人技术的基本挑战之一。位姿图优化问题是 SLAM 问题的一个例子。下面介绍了如何在具有相对姿态约束的二维空间中建立基于姿态图的 SLAM 问题。

    考虑一个在二维平面上运动的机器人。机器人可以使用一组传感器，如车轮里程计或激光距离扫描仪。从这些原始测量中，我们想要估计机器人的轨迹，并建立一个环境地图。为了降低问题的计算复杂度，姿态图方法将原始测量数据抽象出来。具体来说，它创建了一个表示机器人姿态的节点图，以及表示两个节点之间的相对变换(delta position and orientation)的边。边是来自原始传感器测量的虚拟测量，例如，通过整合原始车轮里程计或对齐从机器人获得的激光距离扫描。结果图的可视化如下所示：
    图中机器人的姿态用三角形表示，测量值用连接线表示，闭环测量值用虚线表示。闭环检测是在非顺序机器人状态之间的测量，它们减少了随时间累积的误差。下面将描述位姿图问题的数学公式：

    在时间 $t$ 的机器人有状态 $x_t={\left[p^T,\psi \right]}^T$，其中 $p$ 是表示位置的二维向量、$\psi$ 是以弧度为单位的方向。机器人状态在两个时间戳 $a$ 和 $b$ 处的相对变换的测量值为：$z_{ab}=\left[{\hat{p}}_{ab}^T,{\hat{\psi }}_{ab}\right]$。计算测量值与预测值之间误差的 Ceres 代价函数中实现的残差为：
    $$r_{ab}=\left[\begin{array}{c}{R}_a^T\left(p_b-p_a\right)-{\hat{p}}_{ab}\\ \text{ Normalize }\left({\psi }_b-{\psi }_a-{\hat{\psi }}_{ab}\right)\end{array}\right]$$其中函数 Normalize() 归一化角度至范围：$[-\pi ，\pi )$， $R$ 是由以下旋转矩阵给出：
    $$R_a=\left[\begin{array}{cc}\cos {\psi }_a& -\sin {\psi }_a\\ \sin {\psi }_a& \cos {\psi }_a\end{array}\right]$$为了完成代价函数，我们需要根据测量的不确定度对残差进行加权。因此，我们将残差预乘为测量的协方差矩阵的倒数平方根，即 ${\Sigma }_{ab}^{-\frac{1}{2}}{r}_{ab}$，其中 ${\Sigma }_{ab}$ 是协方差。

    最后，我们使用流形对方向进行归一化至范围 $[-\pi ，\pi )$。特别地，我们定义 AngleManifold::Plus() 函数为：$\mathrm{Normalize}(\psi +\Delta )$ 和 AngleManifold::Minus() 函数为：$\mathrm{Normalize}(y)-\mathrm{Normalize}(x)$。(Minus 的这个感觉有点问题？咋感觉是 $\mathrm{Normalize}(y-x)$)

    这个功能包包含一个可执行文件 pose_graph_2d，它将读取一个问题定义文件。这个可执行文件可以处理任何使用 g2o 格式的2D问题定义。pose_graph_2d 将打印 Ceres 求解器的完整总结，然后以以下格式将机器人的原始和优化姿势(分别为poses_original.txt和poses_optimized.txt)输出：

    pose_id x y yaw_radians
    pose_id x y yaw_radians
    pose_id x y yaw_radians

    其中 pose_id 是文件定义中对应的整数ID。注意，该文件将按照升序对 pose_id 进行排序。

    可执行文件 pose_graph_2d 期望第一个参数是问题定义的路径。运行可执行文件:

    /path/to/bin/pose_graph_2d /path/to/dataset/dataset.g2o

    提供了一个python脚本来可视化结果输出文件：

    /path/to/repo/examples/slam/pose_graph_2d/plot_results.py --optimized_poses ./poses_optimized.txt --initial_poses ./poses_original.txt

    举个例子，Edwin Olson创建了一个 benchmark dataset，该数据集在网格世界中有 $3500$ 个节点，总共有 $5598$ 条边。使用提供的脚本可视化产生的结果：
    绿色为原始姿态，蓝色为优化后的姿态。如图所示，优化后的姿态更接近底层网格世界。注意，由于缺乏提供足够信息来重建轨迹的相对约束，图的左侧有一个小的 yaw drift。

    另：在老版本内，示例中使用 ceres::LocalParameterization* 表示这里的流形，而在 Ceres V2.2.0，Ceres 将 LocalParameterization 删了，并使用 Manifold 作为替代(V2.1.0 新增)：Version History。所以文中使用的流形创建方式为 ceres::Manifold* angle_manifold = AngleManifold::Create() 而不是 ceres::LocalParameterization* angle_local_parameterization = AngleLocalParameterization::Create()。不得不说，老版本的命名方式确实不如新版，至少新版又短一眼又能看出其用途。

11. slam/pose_graph_3d/pose_graph_3d.cc 下面介绍了如何在三维空间中基于相对姿态约束的位姿图 SLAM 问题。该示例还说明了如何将 Eigen 的几何模块与 Ceres 的自动微分功能一起使用。

    时间戳 $t$ 处的机器人具有状态 $x_t={\left[p^T,q^T\right]}^T$，其中 $p$ 是表示位置的三维向量，$q$ 是表示为 Eigen 四元数的方向。机器人状态在两个时间戳 $a$ 和 $b$ 处的相对变换的测量值为：$z_{ab}={\left[{\hat{p}}_{ab}^T,{\hat{q}}_{ab}^T\right]}^T$。计算测量值与预测值之间误差的Ceres代价函数中实现的残差为：
    $$r_{ab}=\left[\begin{array}{c}R{\left(q_a\right)}^T\left(p_b-p_a\right)-{\hat{p}}_{ab}\\ 2.0\mathrm{vec}\left(\left(q_a^{-1}{q}_b\right){\hat{q}}_{ab}^{-1}\right)\end{array}\right]$$其中函数 $\mathrm{vec}()$ 返回四元数的向量部分，即 $\left[q_x,q_y,q_z\right]$，$R(q)$ 是四元数的旋转矩阵。

    为了完成代价函数，我们需要根据测量的不确定度对残差进行加权。因此，我们将残差预乘为测量的协方差矩阵的倒数平方根，即 ${\Sigma }_{ab}^{-\frac{1}{2}}{r}_{ab}$，其中 ${\Sigma }_{ab}$ 是协方差。

    假设我们使用四元数来表示方向，我们需要使用一个流形 EigenQuaternionManifold 来只应用与定义四元数的四向量正交的更新。Eigen 的四元数使用不同于常用的四元数元素的内部内存布局。具体来说，Eigen 将元素存储为 $[x,y,z,w]$，其中实部在最后，而通常实部储存在前面。注意，当通过构造函数创建一个 Eigen 四元数时，元素按 $w$、$x$、$y$、$z$ 的顺序接受。由于 Ceres 操作的参数块是原始double指针，因此这种差异很重要，需要不同的参数化。

    这个功能包包含一个可执行文件 pose_graph_3d，它将读取一个问题定义文件。这个可执行文件可以处理任何使用 g2o 格式和四元数表示方向的 3D 问题定义。pose_graph_3d 将打印 Ceres 求解器的完整摘要，然后将机器人的原始位姿和优化后的位姿(分别为poses_original.txt和poses_optimized.txt)以以下格式输出到磁盘：

    pose_id x y z q_x q_y q_z q_w
    pose_id x y z q_x q_y q_z q_w
    pose_id x y z q_x q_y q_z q_w
    …

    其中pose_id是文件定义中对应的整数ID。注意，该文件将按照升序对pose_id进行排序。

    可执行文件 pose_graph_3d 期望第一个参数是问题定义的路径。可执行文件可以通过以下方式运行：

    /path/to/bin/pose_graph_3d /path/to/dataset/dataset.g2o

    提供了一个脚本来可视化结果输出文件。还有一个选项--axes_equal来启用相等轴：

    /path/to/repo/examples/slam/pose_graph_3d/plot_results.py --optimized_poses ./poses_optimized.txt --initial_poses ./poses_original.txt

    以机器人在一个有 $2500$ 个节点、$4949$ 条边的球面上行走为例，求解了一个标准的合成基准数据集。使用提供的脚本可视化结果产生：