Ceres是由Google开发的开源C++通用非线性优化库，与g2o并列为目前视觉SLAM中应用最广泛的优化算法库。

对于任何一个优化问题，我们首先需要对问题进行建模，之后采用合适的优化方法，进行求解。在求解的过程中，往往需要进行梯度下降求取最优，这里涉及了导数计算。所以在代码中使用Ceres进行优化时，需要包含基本内容：建模、优化、求导方法。

Ceres库主要用于求解无约束或者有界约束的最小二乘问题。其数学形式如下：

min ⁡ x 1 2 ∑ i ρ i ( ∥ f i ( x 1 , ⋯   , x k ) ∥ 2 ) s.t. l j ≤ x j ≤ u j \begin{split} \min_{\boldsymbol{x}} &\quad \frac{1}{2}\sum_{i} \rho_i\left(\left\|f_i\left(x_1, \cdots ,x_k\right)\right\|^2\right) \\ \text{s.t.} &\quad l_j \le x_j \le u_j \end{split} xmin​s.t.​21​i∑​ρi​(∥fi​(x1​,⋯,xk​)∥2)lj​≤xj​≤uj​​

我们的任务就是找到一组满足约束 l j ≤ x j ≤ u j l_j \le x_j \le u_j lj​≤xj​≤uj​的 x 1 , ⋯   , x k x_1, \cdots, x_k x1​,⋯,xk​，使得优化目标函数

1 2 ∑ i ρ i ( ∥ f i ( x 1 , ⋯   , x k ) ∥ 2 ) \begin{split}\frac{1}{2}\sum_{i} \rho_i\left(\left\|f_i\left(x_1, \cdots ,x_k\right)\right\|^2\right)\end{split} 21​i∑​ρi​(∥fi​(x1​,⋯,xk​)∥2)​

取值最小。

其中的一些变量的解释：

代价函数同时负责计算残差项（ f i ( x 1 , ⋯   , x k ) f_i\left(x_1, \cdots ,x_k\right) fi​(x1​,⋯,xk​)）和雅可比矩阵（ J i \mathcal{J}_i Ji​）。

J i = ∂ ∂ x i f i ( x 1 , ⋯   , x k ) ∀ i ∈ 1 , ⋯   , k \mathcal{J}_i = \frac{\partial}{\partial x_i} f_i\left(x_1, \cdots ,x_k\right) \qquad \forall i \in {1,\cdots,k} Ji​=∂xi​∂​fi​(x1​,⋯,xk​)∀i∈1,⋯,k

很多时候我们说最小二乘都是拿来做曲线拟合的，实际上只要能够把问题描述成上面的数学形式，就都可以使用Ceres来求解。使用起来也比较简单，只要按照教程介绍的套路，提供代价函数的计算方式，描述清楚每个残差块以及核函数即可。

如下面的示例代码所示，一般我们需要定义三个对象，problem用于描述将要求解的问题，options提供了很多配置项，而summary用于记录求解过程。

Ceres的求解过程包括构建最小二乘和求解最小二乘问题两部分，其中构建最小二乘问题的相关方法均包含在Ceres::Problem类中，涉及的成员函数主要包括Problem::AddResidualBlock()和Problem::AddParameterBlock()。

注意：Ceres Solver只接受最小二乘优化，也就是 min ⁡ r T r \min r^{T} r minrTr；若要对残差加权重，使用马氏距离，即 min ⁡ r T P − 1 r \min r^{T} P^{-1} r minrTP−1r，则要对信息矩阵 P − 1 P^{-1} P−1做Cholesky分解，即 L L T = P − 1 L L^{T} = P^{-1} LLT=P−1，则 d = r T ( L L T ) r = ( L T r ) T ( L T r ) d = r^{T} (L L^{T}) r = (L^T r)^T (L^T r) d=rT(LLT)r=(LTr)T(LTr) ，令 r ′ = L T r r^{'} = L^T r r′=LTr ，最终 min ⁡ r ′ T r ′ \min r^{'T} r^{'} minr′Tr′。

使用 Ceres Solver 求解非线性优化问题，主要包括以下几部分：

【STEP 1】构建优化问题（ceres::Problem）

【STEP 2】构建代价函数（ceres::CostFunction）或残差（residual）

【STEP 3】添加代价函数、核函数：通过ceres::Problem::AddResidualBlock添加代价函数（cost function）、核函数（loss function）和输入参数块（即待优化状态量块）

【STEP 4】配置求解器（ceres::Solver::Options）

【STEP 5】运行求解器（ceres::Solve(options, &problem, &summary)）

一个简单的代码实现是：

下面将会对其中的重要的几个STEP进行详细讲解。

代价函数最重要的功能就是计算残差向量和雅可比矩阵。Ceres提供了三种求导方法，分别是：解析求导、自动求导、数值求导。

在SLAM中，使用的一般都是解析求导，这种方法需要自己填入雅克比函数。

先看下最基础的代价函数类的声明：

CostFunction的输入参数块的数量和大小以及输出的数量，分别存储在CostFunctio::parameter_block_sizes_和CostFunction::num_residuals_中。从此类继承的子类应使用对应的访问器设置这两个成员。当使用Problem::AddResidualBlock()添加代价函数时，此信息将由Problem验证。

CostFunction中有一个纯虚函数需要子类进行重写，该纯虚函数的定义为：

纯虚函数的形参的解释：

jacobians的详细表达式为：

jacobians[i][r * parameter_block_sizes_[i] + c] = ∂ residual [ r ] ∂ parameters [ i ] [ c ] \text{jacobians[i][r * parameter\_block\_sizes\_[i] + c]}=\frac{\displaystyle \partial \text{residual}[r]}{\displaystyle \partial \text{parameters}[i][c]} jacobians[i][r * parameter_block_sizes_[i] + c]=∂parameters[i][c]∂residual[r]​

parameters和residuals不能为nullptr，jacobians可能为nullptr。如果jacobians为nullptr，则用户只需计算残差即可。

纯虚函数的返回值指示残差或雅可比的计算是否成功。

如果输入参数块的大小和残差向量的大小在编译时已知（这是常见情况），则可以使用SizedCostFunction。这些已知值可以指定为模板参数，并且用户只需要实现CostFunction::Evaluate()。

该模板参数的含义为：

Ceres提供的三种求导方法，都是基于上面的基础代价函数类实现的。

解析求导（Analytic Derivatives），也被称为自定义求导。解析求导需要自行定义迭代求导时的雅克比矩阵和残差。需要以下几个步骤：

在ceres中的代价函数里面Evaluate()采用的是纯虚函数：

下面以最小二乘 min ⁡ x 1 2 ( 10 − x ) 2 \underset{x}{\min} \frac{1}{2}(10-x)^{2} xmin​21​(10−x)2为例解析求导。显然残差为 ( 10 − x ) (10−x) (10−x)，而雅可比矩阵维度为 1 ∗ 1 1∗1 1∗1且为常数 − 1 -1 −1。在求导的Evaluate()函数中定义残差和雅克比如下：

下面以最小二乘 min ⁡ x 1 2 ( k − x T y ) 2 \underset{x}{\min} \frac{1}{2}(k-x^Ty)^{2} xmin​21​(k−xTy)2为例解析求导，其中 x x x、 y y y为二维向量变量， k k k为常数。显然残差为 k − x T y k-x^Ty k−xTy：

对于，ceres::SizedCostFunction的几个模板参数与Evaluate函数的形参对应关系：

x x x决定了residuals的维度

y , z , . . . y, z, ... y,z,...的个数决定了输入参数块的个数，即parameters的第一维度，而 y y y、 z z z本身的值决定了每个输入参数块的维度，即parameters的第二维度

y , z , . . . y, z, ... y,z,...的个数决定了输入参数块的个数，即jacobians的第一维度，而 x x x的值（行数）和 y y y、 z z z本身的值（列数）的乘积决定了每个输入参数块的雅可比矩阵的维度，即jacobians的第二维度（按行优先的顺序排列）

自动求导（Automatic Derivatives）是Ceres很神奇的一个功能，它能够对于一些数据形式较为基础的表达式，自动求解出导数形式。采用的原理是对偶数（dual numbers）和Jets格式实现的，不理解具体细节也不影响使用。

如果想要了解自动求导的原理，可以参考文章：Ceres的数值求导与自动求导的原理。

自动求导的代价函数类定义为AutoDiffCostFunction类，其函数声明为：

想要使用自动求导，就必须构造代价函数结构体CostFunctor（采用类模板形式），并在其结构体内对模板括号运算符() 重载，定义残差。在重载的括号运算符() 形参中，最后一个为残差（唯一的非常量形参），前面几个为输入参数块。函数返回true表示成功。

注意：采用自动求导时，即使是整数也要写成浮点型，否则会报错Jet类型匹配错误。

下面以最小二乘 min ⁡ x 1 2 ( 10 − x ) 2 \underset{x}{\min} \frac{1}{2}(10-x)^{2} xmin​21​(10−x)2为例自动求导。显然残差为 ( 10 − x ) (10−x) (10−x)：

下面以最小二乘 min ⁡ x 1 2 ( k − x T y ) 2 \underset{x}{\min} \frac{1}{2}(k-x^Ty)^{2} xmin​21​(k−xTy)2为例自动求导，其中 x x x、 y y y为二维向量变量， k k k为常数。显然残差为 k − x T y k-x^Ty k−xTy：

对于，ceres::AutoDiffCostFunction的几个模板参数与CostFunctor的模板括号运算符() 的形参对应关系：

x x x决定了residuals的维度

y , z , . . . y, z, ... y,z,...的个数决定了输入参数块的个数，即模板括号运算符() 的常量形参（除最后一个形参）个数，而 y y y、 z z z本身的值决定了每个输入参数块的维度，即每个常量形参的维度

x x x决定了模板括号运算符() 的非常量形参（最后一个形参）的维度

数值求导（Numeric derivatives）核心思想是，当增量很小时，可以采用 lim ⁡ Δ x → 0 f ( x + Δ x ) − f ( x ) Δ x \underset{\Delta x \to 0}{\lim} \frac{f(x+\Delta x)-f(x)}{\Delta x} Δx→0lim​Δxf(x+Δx)−f(x)​近似求导，由此我们不需要实际计算导数的表达式，也可以从数值上进行近似。为此，求导方式的代码只需要写清楚残差怎么计算即可。

数值求导的代价函数类定义为NumericDiffCostFunction类，其函数声明为：

数值求导法也是构造代价函数结构体CostFunctor，来定义残差。但在重载括号运算符() 时，没有使用模板，类型都固定为double。在重载的括号运算符() 形参中，最后一个为残差（唯一的非常量形参），前面几个为输入参数块。函数返回true表示成功。另一个不同点是，在实例化代价函数类时需要额外指定一个模板参数NumericDiffMethodType。

数值求导方法NumericDiffMethodType又具体分为：前向求导、中心求导和Ridders方法，对应的精度和耗时依次增加。官方建议在不考虑时间约束时采用Ridders方法，中心求导是一个均衡的选择。

下面以最小二乘 min ⁡ x 1 2 ( 10 − x ) 2 \underset{x}{\min} \frac{1}{2}(10-x)^{2} xmin​21​(10−x)2为例自动求导。显然残差为 ( 10 − x ) (10−x) (10−x)：

下面以最小二乘 min ⁡ x 1 2 ( k − x T y ) 2 \underset{x}{\min} \frac{1}{2}(k-x^Ty)^{2} xmin​21​(k−xTy)2为例数值求导，其中 x x x、 y y y为二维向量变量， k k k为常数。显然残差为 k − x T y k-x^Ty k−xTy：

对于，ceres::NumericDiffCostFunction的几个模板参数与CostFunctor的括号运算符() 的形参对应关系：

x x x决定了residuals的维度

y , z , . . . y, z, ... y,z,...的个数决定了输入参数块的个数，即括号运算符() 的常量形参（除最后一个形参）个数，而 y y y、 z z z本身的值决定了每个输入参数块的维度，即每个常量形参的维度

x x x决定了模板括号运算符() 的非常量形参（最后一个形参）的维度

求导方法的选择的依据是多方面的，一方面是精度和速度，另一方面是使用的便捷性。毫无疑问，在代码优化的情况下，解析求导方式求解速度最快，但缺点是需要人工计算雅克比矩阵，失去了便捷性。数值求导几乎是万能的，只是时间稍慢。而自动求导，虽然较为方便，但也存在问题，例如传入的变量类型若不是较为基础的数据格式则不可用自动求导。

官方给出了几种方法的计算时间如下：

（多种求导方法耗时比较。从上到下：解析求导、优化后的解析求导、向前数值求导、中心数值求导、Ridders数值求导、自动求导）

可以看出，代码优化较好的解析法求导速度最快，其次是自动求导，数值求导相对较慢。

声明ceres::Problem problem

通过AddResidualBlock()将代价函数（cost function）、核函数（loss function）和输入参数块添加到problem

这里以自动求导为例：

problem对象也可以调用AddParameterBlock()显式添加输入参数块，这会导致额外的正确性检查；但是，如果参数块不存在，AddResidualBlock()会隐式添加它们，因此不需要显式调用AddParameterBlock()。

也就是说，AddResidualBlock()在添加残差块的同时，该函数内部已经隐式地调用了AddParameterBlock()函数进行参数块的设置。但在一些情况下，如果想要显示地传入参数模（通常出现在需要对优化参数进行重新参数化的情况），可以使用AddParameterBlock()函数：

其中，第一种函数原型除了会增加一些额外的参数检查之外，功能上和隐式地进行参数块的设置并没有太大区别。第二/三种函数原型则会额外传入LocalParameterization/Manifold，用于重构优化参数的维数。在2.2.0版本之后，LocalParameterization接口被弃用，需要使用Manifold代替。

注意：problem内的参数块使用map进行管理，无论添加多少次，都不会重复。

对于最小二乘问题，最小化可能会遇到包含异常值的输入项，即完全虚假的测量值，因此使用损失函数来减少其影响非常重要。

自定义核函数类需要继承基类LossFunction，并对其中的纯虚函数进行实现。当然也可以使用Ceres预先定义好的较常用的子类。

关键方法是LossFunction::Evaluate()，它给定一个非负标量 s s s，核函数 ρ \rho ρ，计算：

o u t = [ ρ ( s ) , ρ ′ ( s ) , ρ ′ ′ ( s ) ] out = \begin{bmatrix}\rho(s), & \rho'(s), & \rho''(s)\end{bmatrix} out=[ρ(s),​ρ′(s),​ρ′′(s)​]

其中， s s s是残差的平方。

下面以柯西核函数（ceres预先定义好的CauchyLoss）为例：

Ceres预定义的一些LossFunction。为了简单起见，我们描述了它们的未缩放版本。下图以图形方式说明了它们的形状。

下面梳理一下常见的损失函数：

LocalParameterization类的作用是解决非线性优化中的过参数化问题。所谓过参数化，即输入参数块的实际自由度小于参数本身的自由度。

除了上文的AddParameterBlock()函数之外，还可以用下面的函数设置：

例如，在SLAM中，许多优化问题中，尤其是传感器融合问题，必须对存在于被称为流形的空间中的量进行建模。流形空间，局部看起来像欧几里得空间。更准确地说，流形上的每个点都有一个与流形相切的线性空间。它的尺寸等于流形本身的固有尺寸，该固有尺寸小于或等于嵌入流形的周围空间。例如，三维空间中球体上一点的切线空间是与该点处的球体相切的二维平面。切线空间有趣的原因有两个：

例如：在SLAM的优化问题中，采用单位四元数表示位姿时，由于单位四元数本身的约束（模长为1），实际的自由度为3而非4。同时，单位四元数/旋转矩阵并不支持广义的加法运算，即使用普通的加法就会打破其constraint，比如，旋转矩阵加旋转矩阵得到的就不再是旋转矩阵，单位四元数加上单位四元数就不再是单位四元数。

因此，我们在使用ceres对其进行迭代更新的时候就需要自定义其更新方式了，此时就需要使用到LocalParameterization。

LocalParaneterization本身是一个虚基类，用户在自行定义使用的子类时，需要将所有的纯虚函数都实现一遍。当然也可以使用Ceres预先定义好的较常用的子类。

下面以单位四元数（ceres预先定义好的QuaternionParameterization）为例，解释如何实现参数本地化。

由于由单位四元数表示的传感器的旋转/方向，它的实际维数为4，但是正切空间上的维数为3。因此，增量 d e l t a delta delta就是一个3维的。那单位四元数怎么和3维表示的 d e l t a delta delta进行计算呢？旋转向量（它正好就是3维的，且与单位四元数之间可以相互转化）。流型中的加法可以用符号 ⊞ \boxplus ⊞ 表示。

设单位四元数 s = [ w , x , y , z ] s=[w,x,y,z] s=[w,x,y,z]，对应的旋转向量为 ϕ = [ p , q , r ] \phi = [p,q,r] ϕ=[p,q,r] ，定义 θ = p 2 + q 2 + r 2 \theta=\sqrt{p^{2}+q^{2}+r^{2}} θ=p2+q2+r2 ​ ，是 ϕ \phi ϕ 的模（旋转角）， ∣ ∣ a ∧ ∣ ∣ = 1 ||a^{\wedge}|| = 1 ∣∣a∧∣∣=1，是 ϕ \phi ϕ 的方向向量（旋转轴）， 即 ϕ = θ a ∧ \phi = \theta a^{\wedge} ϕ=θa∧，其中 ∧ \wedge ∧为向量到反对称矩阵的转换符。

旋转向量到单位四元数的映射为：

[ c o s ( θ ) , s i n ( θ ) θ p , s i n ( θ ) θ q , s i n ( θ ) θ r ] \Big[cos(\theta), \frac{sin(\theta)}{\theta}p, \frac{sin(\theta)}{\theta}q, \frac{sin(\theta)}{\theta}r \Big] [cos(θ),θsin(θ)​p,θsin(θ)​q,θsin(θ)​r]

单位四元数相对于旋转矢量的雅克比矩阵计算方法，即：

J 4 × 3 = d s / d ϕ = d [ w , x , y , z ] T / d [ p , q , r ] \mathcal{J}_{4 \times 3}=d \mathcal{s} / d \mathcal{\phi} = d [w, x, y, z]^T /d [p,q,r] J4×3​=ds/dϕ=d[w,x,y,z]T/d[p,q,r]

对应Jacobian维数为4行3列。这部分的详细推导可以参考：【Ceres】（二）LocalParameterization参数化、[ceres-solver] From QuaternionParameterization to LocalParameterization 、优化库——ceres（二）深入探索ceres::Problem 。

下面看下LocalParaneterization的具体用法。例如：在SLAM中，若使用四元数进行优化，可以使用Ceres预先定义的QuaternionParameterization对优化参数进行重构：

Ceres预定义的一些LocalParaneterization：

在2.2.0版本之后，LocalParameterization接口被弃用，需要使用Manifold代替。它们之间的使用场景/使用方法很多都是类似的。这里就不详细介绍了。

除了上文的AddParameterBlock()函数之外，还可以用下面的函数设置：

Solver::Options含有的参数种类繁多，但是在大多数情况下，绝大多数参数我们都会使用Ceres的默认设置，这里只列出一些常用或较为重要的参数：

Solver::Summary包含了求解器本身和求解中各变量的信息，许多成员函数与Solver::Options一致，这里只给出两个常用的成员函数：

一个简单的求解 min ⁡ x 1 2 ( 10 − x ) 2 \underset{x}{\min} \frac{1}{2}(10-x)^{2} xmin​21​(10−x)2 的优化问题，代码如下：

构造优化问题，并求解相机位姿：