SLAM算法工程师之路：状态估计之非线性优化学习

整理了一段时间，让我们继续记录SLAM之路的学习。按照上一篇的记录点，今天的主题是非线性优化学习。SLAM的后端优化，一般分为滤波方法与非线性优化方法，其中部分思想重叠，具有很鲜明的对比特性。在前几章中，我们学习了KF系列与PF系列，并对一部分内容进行扩展，这些方法的本质是在一步步过程中进行优化收敛，本篇将引入后端整体优化思想，在建图，轨迹跟踪，系统状态估计中应用广泛。^[1]^[2]

本篇的学习大纲是：

状态估计问题与最大似然估计；
线性估计的最小二乘法；
非线性最小二乘法：梯度下降：最速/牛顿法；高斯-牛顿方法；列文伯格-马夸尔特方法学习；
ceres库的简介与应用学习。

1.状态估计问题与最大似然估计的引出

在SLAM中，我们可以通过滤波来单步进行优化状态估计的结果（马尔科夫思想），但需要知道的是，即使我们的传感器精度再高，运动方程与观测方程也只是近似成立。所以，与其假设数据符合目标方程，不如直接讨论如何在有噪声的数据中进行准确的状态估计。首先想起我们最基本的状态方程和观测方程。

令状态变量为 $X=\\left\\{ x_{1},x_{2},x_{3},··· \\right\\}$ ，观测变量为 $Z=\\left\\{ z_{1}, z_{2},z_{3},···\\right\\}$ 。

那么状态估计问题等同于求解条件概率 $P(X|Z)$ ，即在当前观测状态 $Z$ 下的状态 $X$ 的分布。由贝叶斯概率可以知道： $P(X|Z)=\\frac{P(Z|X)·P(X)}{P(Z)}$ 。直接求取后验分布是困难的，但是求一个状态最优估计，使得该状态下的后验概率最大(Maximize A Posterior,MAP)，是可以的：

$X_{MAP}^{\\ast}=argmaxP(X|Z)=argmax P(Z|X)·P(X)$

注意：贝叶斯概率的分母部分与待估计的的状态 $X$ 无关，因而可以忽略。

而求解最大后验概率相当于最大化似然和先验的乘积，假设我们不知道机器人的初始位置在什么地方，即没有先验，那么对目标的求解就变成了求 $X$ 的最大似然估计(Maximize Likelihood Estimation,MLE):

$X_{MLE}^{\\ast}=argmax P(Z|X)$

直观一点讲，似然是指“在现在的位姿下，可能会产生怎样的观测数据”。由于我们能够获得观测数据，所以最大似然估计可以理解为：“在什么样的状态下，最可能产生现在观测到的数据”。这就是SLAM中最大似然估计的直观意义。

2.线性估计最小二乘法的引出

最小二乘法的目标是：求解误差的最小平方和。根据Cost Function的对应，分为了线性与非线性。这个取决于对应的残差(Residual)是线性的还是非线性的。

先从模型角度入手，我们假设某次观测为：

$z_{k,j}=h(y_{j},x_{k}) + v_{k,j}$

而系统噪声都符合高斯分布，假设噪声项 $v_{k}\\sim N(0,Q_{k,j})$ ，所以观测数据的条件概率为：

$P(z_{j,k}|x_{k},y_{j})=N(h(y_{j},x_{k}),Q_{k,j})$

这样的一个概率分布依然符合高斯分布，按照高数和概率中的知识，对于该式的最大似然估计，可以使用最小化负对数来求取它的似然。考虑任意高维高斯分布 $x\\sim N(\\mu,\\Sigma)$ ，它的概率密度函数展开式为：

$P(x)=\\frac{1}{\\sqrt{(2\\pi)^Ndet(\\Sigma)}}exp(-\\frac{1}{2}(x-\\mu)^T\\Sigma^{-1}(x-\\mu))$

取负对数，则上式变为：

$-ln(P(x))=\\frac{1}{2}ln((2\\pi)^Ndet(\\Sigma)) + \\frac{1}{2}(x-\\mu)^T\\Sigma^{-1}(x-\\mu)$

因为对数函数是单调递增的，所以对原函数最大化相当于求取了负对数最小化，而最小化该式 $x$ 时，第一项与 $x$ 无关，相当于只最小化右侧的二次型项就可以，就得到了对状态的最大似然估计。带入SLAM观测模型，相当于：

$(x_{k},y_{j})^{*}=argmaxN(h(y_{j},x_{k}),Q_{k,j})$

$=argmin((z_{k,j}-h(x_{k},y_{j}))^{T}Q_{k,j}^{-1}(z_{k,j}-h(x_{k},y_{j})))$

上述式子中的二次型称为马哈拉诺比斯距离（Mahalanobis distance），也称为马氏距离。可以认为是由 $Q_{k,j}^{-1}$ 加权后的欧氏距离，而 $Q_{k,j}^{-1}$ 称为信息矩阵，即高斯分布的协方差矩阵的逆。

当考虑到批量数据输入时，假设各个时刻输入和观测相互独立，联合分布进行因式分解，有：

$P(z,u|x,y)=\\prod_{k}P(u_{k}|x_{k-1},x_{k})\\prod_{k,j}P(z_{k,j}|x_{k},y_{j})$

这样我们就可以独立地处理各时刻的运动和观测。定义各次输入和观测数据与模型之间的误差：

$e_{u,k}=x_{k}-f(x_{k-1},u_{k})$

$e_{z,j,k}=z_{k,j}- h(x_{k},y_{j})$

那么，最小化所有时刻的估计值与真实读数之间的马氏距离，等价于求解最大似然估计。而负对数允许我们把乘积变为求和：

$minJ(x,y)=\\sum_{k}{e_{u,k}^{T}}R_{k}^{-1}e_{u,k}+ \\sum_{k}\\sum_{j}{e_{z,k,j}^{T}}Q_{k,j}^{-1}e_{z,k,j}$

这样便获得了模型上的最小二乘问题。直观上看，由于噪声的存在，我们把估计的轨迹与地图带入SLAM的运动，观测方程中时，他们并不会完美成立，因此需要对状态估计值进行微调，使得误差下降，而如何使这个误差下降到最小，就是一个非线性优化过程考虑的问题，我们稍后再说；

从SLAM计算角度入手，通常上述概念的核心，是对应到了矩阵方程 $Ax=b$ 的求解， $A$ 是一个矩阵， $b$ 是一个列向量，根据学过的线代知识，不难得出它的解是 $x=(A^{T}A)^{-1}A^{T}b$ 。

实质上该矩阵方程就是求 $||Ax-b||^{2}$ 的最小值，所以用该式子对 $x$ 进行求导，便可以得到极小值。由于 $Ax-b$ 是一个列向量，所以有： $||Ax-b||^{2}=(Ax-b)^{T}·(Ax-b)$ 。将右侧式子展开（转置符号代入并结合）得：

$||Ax-b||^{2}=(x^{T}A^{T}-b^{T})·(Ax-b)=x^{T}A^{T}Ax - x^{T}A^{T}b - b^{T}Ax + b^{T}b$

上式中，因为 $x^{T}A^{T}b$ 是一个1维度的标量，所以其转置就是其本身。( $x^{T}$ 是一个 $1*n$ 的行向量， $A^{T}$ 是一个 $n*n$ 的矩阵)，即 $x^{T}A^{T}b=b^{T}Ax$ 。所以，上式右侧替换为 $x^{T}A^{T}Ax -2x^{T}A^{T}b + b^{T}b$ 。令 $S(x)=b^{T}b -2x^{T}A^{T}b +x^{T}A^{T}Ax$ (这么写是为了求导按项遍历时理解方便)。对 $x$ 求导，令其为0。得： $x=(A^{T}A)^{-1}A^{T}b$ 。

附上一些矩阵与向量求导的基本公式：(式中出现的字母均为矩阵或向量)

$\\frac{\\partial Ax}{\\partial x}=A^{T}$
$\\frac{\\partial Ax}{\\partial x^{T}}=A$
$\\frac{\\partial (xA)}{\\partial x}=A$
$\\frac{\\partial y}{\\partial x}=\\frac{\\partial (x^{T}Ax)}{\\partial x}=(A^{T}+A)x$
特别地，当 $A$ 为对阵矩阵时，有 $\\frac{\\partial (x^{T}Ax)}{\\partial x}=2Ax$ 。(此时 $x^{T}Ax$ 是一个标量，也就是一个数)

3.非线性最小二乘法

考虑一个简单的最小二乘问题：

$\\min_{x}F(x)=\\frac{1}{2}||f(x)||_{2}^{2}$

其中，自变量 $x \\in \\R^{n}$ ， $f$ 是一个任意标量的非线性函数 $f(x)$ ： $\\R^{n}\\rightarrow \\R$ 。注意这里的系数 $\\frac{1}{2}$ 是无关紧要的，并不会影响结论。对该问题进行讨论：由高中数学得知，求解一个函数的最小值，就是求取函数零点，求导后令其为0，判断各个解对应的结果大小即可获得最小值。但假如 $f$ 函数难以获取精准表达式或不易求解，便需要从导数的原理角度入手求解了。

首先是关于目标函数的全局性质，我们不一定能够获得，但是我们从某一个初值进行一步步推进，其递增或递减特性在区间是固定的，由此我们不断优化当前的步长与迭代次数，使得目标函数值下降，便可以求得一个较好的结果，这便是本篇提到的第一个非线性优化的求解方法：梯度下降法。

梯度下降法的步骤如下：

寻找一个初值 $x_{0}$ ；
对于第 $k$ 次求解(迭代)，确定一个增量 $\\Delta x$ ，使得 $||f(x_{k}+\\Delta x_{k})||_{2}^{2}$ 达到极小值；
若 $\\Delta x$ 足够小，则停止；
否则，令 $x=x+ \\Delta x$ ，继续迭代运算；

这让对函数直接求导取0的方法变成了用使结果下降的增量 $\\Delta x$ 不断逼近的方法。由于可以对 $f$ 函数进行线性化，计算上也会变得相对容易一些。当函数下降到增量足够小的时候，就认为此时的结果收敛，目标达到了极小值。

在这个求解过程中，问题在于如何确定每次迭代的增量值，这是一个局部问题，我们只需要关心 $f$ 在迭代处的局部性质就可以，这类思想在最优化，机器学习中广泛应用。

考虑在第 $k$ 次迭代，假设在 $x_{k}$ 处，想要寻求增量 $\\Delta x$ ，最直观的方法是将函数在 $x_{k}$ 处进行泰勒展开：

$F(x+\\Delta x) \\approx F(x_{k})+J(x_{k})^{T}\\Delta x_{k}+ \\frac{1}{2}\\Delta x_{k}^{T}H(x_{k})\\Delta x$

其中 $J(x_{k})$ 是 $F(x)$ 关于 $x$ 的一阶导数，也称为梯度，其形式为雅可比(Jacobin)矩阵， $H(x_{k})$ 是二阶导数，其形式为海塞(Hessian)矩阵，他们都在 $x_{k}$ 处取值，具体计算可以参考微积分。我们选择保留泰勒展开的一阶或二阶项，对应的求解方法称为一阶梯度法或二阶梯度法。如果保留一阶梯度，那么取增量的反向梯度，则可以让原函数结果下降，即：

$\\Delta x^{*}=-J(x_{k})$

这种方法被称为最速下降法，它的集合含义也很好理解，我们沿着反向梯度前进，在一阶近似情况下，目标函数结果必定下降(趋于极值点)。

当保留二阶梯度信息时，增量方程扩展为：

$\\Delta x^{*}=argmin (F(x)+J(x^{T}\\Delta x + \\frac{1}{2}\\Delta x^{T}H\\Delta x)$

而上式的右侧包含 $\\Delta x$ 的零次( $F(x)$ )，一次( $x^{T}\\Delta x$ )和二次项 ( $\\frac{1}{2}\\Delta x^{T}H\\Delta x$ )，对其求导令其等于0，则有：

$J+H\\Delta x=0 \\Rightarrow H\\Delta x=-J$

求解这个线性方程，就得到了增量，这种方法称为牛顿法。

另：该方程与前述求解 $Ax=b$ 结构相同，求解方法可以自行带入。

由此，我们可以获悉：非线性最小二乘的优化思想，是一种按照导数的定义对目标函数局部性质进行探索，采用步长迭代寻求极小值点，直到收敛的方法。其中在泰勒展开一阶截断求取雅克比矩阵的方法称为一阶梯度法，又被称为最速下降法，展开式二阶截断的计算雅克比与海塞矩阵关系的方法称为二阶梯度法，又被称为牛顿法。

高斯-牛顿法

高斯牛顿法是优化方法里最具代表的一种，也是比较简单的方法。它的核心思想是把 $f(x)$ 进行一阶泰勒展开。需要注意：这里展开的不是目标函数 $F(x)$ 而是 $f(x)$ (否则就变成了牛顿法)。

$f(x+\\Delta x) \\approx f(x) + J(x)^{T}\\Delta x$

这里的 $J(x)^{T}$ 是 $f(x)$ 关于 $x$ 的导数，为一个 $n\ imes1$ 的列向量。根据前面的概念，当前的目标是寻求最小的 $\\Delta x$ ，使得 $||f(x+\\Delta x)||^{2}$ 达到最小。为了求 $\\Delta x$ ，构建一个线性的最小二乘问题：

$\\Delta x^{*}=arg\\min_{\\Delta x}||f(x)+(J(x)^{T}\\Delta x )||^{2}$

根据极值条件，对上述目标函数对 $\\Delta x$ 求导，并令其导数为0。为此，先展开目标函数的平方项：

$\\frac{1}{2}||f(x)+(J(x)^{T}\\Delta x )||^{2}=\\frac{1}{2}(f(x)+(J(x)^{T}\\Delta x ))^{T}(f(x)+(J(x)^{T}\\Delta x ))$

$=\\frac{1}{2}(||f(x)||_{2}^{2}+2f(x)J(x)^{T}\\Delta x +\\Delta x ^{T}J(x)J(x)^{T}\\Delta x )$

上式关于 $\\Delta x$ 求导，并令其为0，有：

$J(x)f(x) + J(x)J^{T}(x)\\Delta x=0$

可以得到如下方程组：

$J(x)J^{T}(x)\\Delta x=-J(x)f(x)$
这个方程式关于变量 $\\Delta x$ 的线性方程组，我们称它为增量方程，也称之为高斯牛顿方程(Gauss-Newton equation)或者正规方程(Normal euqation)。将方程左侧的 $J(x)J^{T}(x)$ 系数定义为 $H(x)$ ，右侧 $-J(x)f(x)$ 定义为 $g(x)$ ，那么上式变为：

$H\\Delta x=g$

对比牛顿法可以得出：高斯牛顿法用 $JJ^{T}$ 的形式作为了牛顿法中二阶(Hessian)矩阵的近似，从而省略了计算 $H$ 的过程。而求解增量方程是整个优化问题的核心所在，如果我们能顺利解出该方程，那么高斯牛顿法的算法步骤可以写成：

寻找一个初值 $x_{0}$ ；
对于第 $k$ 次迭代，求出当前的雅可比矩阵 $J(x_{k})$ 以及误差 $f(x_{k})$ ；
求解增量方程 $H\\Delta x=g$ ；
若 $\\Delta x$ 足够小则停止，否则，令 $x=x+ \\Delta x$ ，继续迭代运算；

为了求解增量方程，我们还需要求解 $H^{-1}$ ，这需要 $H$ 矩阵可逆，但实际数据中计算的 $JJ^{T}$ 却只有半正定性。也就是说，在使用高斯牛顿法的时候，可能会出现 $JJ^{T}$ 为奇异矩阵的情况，此时增量的稳定性变差，导致无法收敛。直观的说，就是原函数在局部看起来并不像一个二次函数，导致其斜率(导数)不稳定或不存在，况且假设此时计算出了 $H$ 矩阵，但得到的步长 $\\Delta x$ 比较大导致结果不够准确，也会出现发散的可能。

虽然高斯牛顿法有上述的缺点，但其依然是非线性优化方面较为好理解与实现的方法，值得我们学习与改进并加入SLAM的应用中。

列文伯格-马夸尔特方法(Levenberg-Marquart，LM)

高斯牛顿法对 $f(x)$ 进行二阶泰勒展开，只能在步长 $\\Delta x$ 附近有较好的结果，如果给 $\\Delta x$ 添加一个范围，应该可以弥补一部分高斯牛顿法的缺点。那么列文伯格-马夸尔特方法就是给步长添加一个信赖区域(Trust Region)，而这个信赖区域定义了在不同情况下二阶阶段的函数是近似有效的，这种方法也称为信赖区域方法(Trust Region Method)。在信赖区域里，可以认为方法的近似与结果都是有效的，而出了这个区域，则需要其他方法来弥补。

定义信赖区域的一种比较好的方法，就是用近似模型与实际函数之间的差异来确定：如果差异越小，则说明近似效果好，我们迭代就可以扩大近似的范围，而当差异很大，则需要缩小近似的范围。由此，引入一个指标 $\\rho$ 来刻画近似的好坏程度：

$\\rho=\\frac{f(x+\\Delta x) - f(x)}{J(x)^{T}\\Delta x}$

$\\rho$ 的分子是实际函数的下降值，而分母是近似模型的下降值。如果 $\\rho$ 接近于1，则认为近似是好的；如果 $\\rho$ 太小，说明实际减小的值远少于近似减小的值，则此时近似会比较差，需要缩小近似范围；反之，如果 $\\rho$ 比较大，则说明实际下降的比预计下降的大，我们应放大近似范围来加快收敛速度。

于是，我们构建这样一个改良的非线性优化框架，该框架会比高斯牛顿法有更好的效果，给出算法步骤：

寻找一个初值 $x_{0}$ ，以及一个初始的优化半径 $\\mu$ ；
对于第 $k$ 次迭代，在高斯牛顿法的基础上增加信赖区域，求解式子变为： $\\min_{\\Delta x_{k}}\\frac{1}{2}||f(x_{k}+J(x_{k})^{T}\\Delta x_{k})||^{2},s.t. ||D\\Delta x_{k}||^{2}\\leq \\mu$ ，其中 $\\mu$ 是信赖区域的半径， $D$ 为系数矩阵(几何理解是这个矩阵将空间中原本的一个半径确定的球体信赖区域转换成一个椭球体，可以根据需求来寻求空间中的有效部分。而在一般的使用中，这个系数矩阵都为单位矩阵 $I$ )。
计算 $\\rho$ 值；
若 $\\rho > \\frac{3}{4}$ ，则设置 $\\mu=2\\mu$ ；( $\\frac{3}{4}$ 是一个经验值，这个值可以在代码中进行调试来确保不同场景下的应用情况)
若 $\\rho < \\frac{1}{4}$ ，则设置 $\\mu=\\frac{1}{2}\\mu$ ；
若 $\\rho$ 大于某阈值，则近似认为可行。令 $x_{k+1}=x_{k}+ \\Delta x_{k}$ ；
判断结果是否收敛，如果不收敛则返回第2步继续，否则结束。

附：如果将 $D$ 矩阵取为非负数对角阵——实际中通常用 $JJ^{T}$ 的对角元素平方根，使得在梯度小的维度上约束范围更大一些(马夸尔特提出)。

无论如何，在列文伯格-马夸尔特优化中，我们都需要求解 $\\min_{\\Delta x_{k}}\\frac{1}{2}||f(x_{k}+J(x_{k})^{T}\\Delta x_{k})||^{2},s.t. ||D\\Delta x_{k}||^{2}\\leq \\mu$ 式子这样的一个子问题来获得梯度。这个子问题是带不等式约束的优化问题，我们用拉格朗日乘子把约束项放到目标函数中，构建成拉格朗日函数：

$\\iota(\\Delta x_{k},\\lambda)=\\frac{1}{2}||f(x_{k}+J(x_{k})^{T}\\Delta x_{k})||^{2}+ \\frac{\\lambda}{2}(||D\\Delta x_{k}|| - \\mu)$

这里 $\\lambda$ 为拉格朗日乘子。类似于高斯牛顿中的做法，令该拉格朗日函数关于 $\\Delta x$ 的导数为0，它的核心仍是计算增量的线性方程：

$(H + \\lambda D^{T}D)\\Delta x_{k}=g$

相比高斯牛顿法，增量方程多了一项 $\\lambda D^{T}D$ ，如果考虑它的简化形式，即 $D=I$ ，那么相当于求解： $(H + \\lambda I)\\Delta x_{k}=g$

理解：当参数 $\\lambda$ 比较小的时候， $H$ 占据主要地位，这说明二次近似模型在该范围内是比较好的，列文伯格-马夸尔特方法更接近于高斯牛顿法。另一方面，当 $\\lambda$ 比较大时， $\\lambda I$ 占据主要地位，列文伯格-马夸尔特方法更接近于一阶梯度下降法(最速下降法)，这说明附近的二次近似不够好，需要快速寻求收敛域。这样的求解方式，可以在一定程度上避免线性方程组的系数矩阵与非奇异的病态问题，提供更稳定，更准确的增量 $\\Delta x$ 。

实际SLAM应用中，还有许多方式求解增量，本篇讲解的是较为广泛使用的基本方法。通常选择高斯牛顿或者LM方法，当问题性质比较好，就用高斯牛顿法，如果问题出现的奇异点比较多或问题存在病态情况，用LM方法较好。

4.Ceres的简介与学习

Ceres是一个广泛使用的最小二乘问题求解库，其代码基于C++实现。我们使用时只需要按照一定的步骤定义待求解的优化问题，然后运行代码就可以。Crese求解的最小二乘问题最一般的形式如下(带边界的核函数最小二乘)：

$\\min_{x}\\frac{1}{2}\\sum_{i}{\\rho_{i}(||f_{i}(x_{i_{1},···,x_{i_{n}}})||^{2})}$

$s.t. ( l_{j}\\leq x_{j}\\leq u_{j})$

在这个问题中， $x_{1},···,x_{n}$ 为优化变量，又称为参数块(Parameter blocks)， $f_{i}$ 称为代价函数(Cost function)，也称为残差块(Residual blocks)，在SLAM中也可以理解为误差项。受限条件为优化变量的上下限。在最简单的情况下，可以取正负无穷，即认为不限制优化变量的界限。此时，目标函数由许多平方项经过一个核函数 $\\rho(·)$ 之后求和组成。类似的，可以取这个核函数为一个恒等函数，那么这个问题就就变成了无约束的最小二乘问题，就和前面介绍的理论对应上了。

所以，为了使用Ceres更好的做这件事，我们的工作步骤如下：

定义每个参数块。参数块通常都是普通的向量，在SLAM中的理解可以认为是四元数等这类特殊的结构。在代码层级，我们需要对这样的向量定义一个double类型的数组；
定义残差块的计算方式。残差块通常关联着若干个参数块，对它们进行一些自定义的计算，然后获得残差值。Ceres对它们求平方和之后，作为目标函数的值；
残差块计算需要定义雅克比矩阵的计算方式，一般选用“自动求导”功能，也可以手写一个，详细可以参考前述的EKF中的代码片段。手写的话，注意计算过程格式：通过一个带模板的括号运算符实现；
将所有上述内容带入到对象中，调用solve函数求解即可。求解时可以传入一些配置信息，例如：迭代次数，终止条件等，也可以设为默认。

Ceres安装

代码库地址：

https://github.com/ceres-solver/ceres-solver

下载后cmake，make，make install编译安装就行。在/usr/local/lib/目录下能够看到libceres.a的库就说明安装完成。

Ceres的使用^[3]：

//代码借鉴于高翔博士的SLAM专题 ch6

#include <iostream>
#include <opencv2/core/core.hpp>
#include <ceres/ceres.h>
#include <chrono>

using namespace std;

// 代价函数的计算模型
struct CURVE_FITTING_COST {
  CURVE_FITTING_COST(double x, double y) : _x(x), _y(y) {}

  // 残差的计算
  template<typename T>
  bool operator()(
    const T *const abc, // 模型参数，有3维
    T *residual) const {
    residual[0] = T(_y) - ceres::exp(abc[0] * T(_x) * T(_x) + abc[1] * T(_x) + abc[2]); // y-exp(ax^2+bx+c)
    return true;
  }

  const double _x, _y;    // x,y数据
};

int main(int argc, char **argv) {
  double ar = 1.0, br = 2.0, cr = 1.0;         // 真实参数值
  double ae = 2.0, be = -1.0, ce = 5.0;        // 估计参数值
  int N = 100;                                 // 数据点
  double w_sigma = 1.0;                        // 噪声Sigma值
  double inv_sigma = 1.0 / w_sigma;
  cv::RNG rng;                                 // OpenCV随机数产生器

  vector<double> x_data, y_data;      // 数据
  for (int i = 0; i < N; i++) {
    double x = i / 100.0;
    x_data.push_back(x);
    y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));
  }

  double abc[3] = {ae, be, ce};

  // 构建最小二乘问题
  ceres::Problem problem;
  for (int i = 0; i < N; i++) {
    problem.AddResidualBlock(     // 向问题中添加误差项
      // 使用自动求导，模板参数：误差类型，输出维度，输入维度，维数要与前面struct中一致
      new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3>(
        new CURVE_FITTING_COST(x_data[i], y_data[i])
      ),
      nullptr,            // 核函数，这里不使用，为空
      abc                 // 待估计参数
    );
  }

  // 配置求解器
  ceres::Solver::Options options;     // 这里有很多配置项可以填
  options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;  // 增量方程如何求解
  options.minimizer_progress_to_stdout = true;   // 输出到cout

  ceres::Solver::Summary summary;                // 优化信息
  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  ceres::Solve(options, &problem, &summary);  // 开始优化
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "solve time cost=" << time_used.count() << " seconds. " << endl;

  // 输出结果
  cout << summary.BriefReport() << endl;
  cout << "estimated a,b,c=";
  for (auto a:abc) cout << a << " ";
  cout << endl;

  return 0;
}

大意是使用CV的生成器随机生成100个带高斯噪声的数据，利用Ceres进行拟合。代码中涉及到的注意点(与前述工作步骤相似)有：

书写了一个结构体，并在类中定义带模板参数的()运算符，形成了一个拟函数，这样直接用Ceres的对象调用该函数就可以；
参数块求导部分，采用自动求导函数Auto diff，类似的选择还有数值求导Numeric diff或自行编写求导部分，出于方便，使用自动求导；
优化的量的单位及维度，本篇中的误差是标量，所以单位与维度均为1，所以Auto diff中对a、b、c三个优化量的参数设置为1，3(表示单位标量1，维度3个);
设定好后，在options中配置优化选项。例如，是否使用线搜索，或是置信区间，迭代步长，次数等。

Ceres的简介与学习说到这里，更多的学习可以参考文献书籍与代码。关于SLAM框架，也有许多应用，计划在后期进行学习记录。

至此，本篇的记录即将结束，再次回顾学习记录的内容：状态估计问题的优化之后端非线性优化，与单步马尔科夫式滤波(如EKF,UKF等)不同的是这种方法会对整体的模型函数进行非线性最小二乘思想的问题构建，通过导数定义的方式去寻求目标函数的极小值，来确保结果收敛。在求解极小值过程中，我们学习了在分布函数一阶/二阶泰勒展开截断的梯度下降法，分别是最速下降法与牛顿法；并推理对其概率密度函数的泰勒展开进行截断，得到了高斯牛顿方法；随后在其步长上做出优化，将置信区间的思想加入高斯牛顿法，得到列文伯格-马夸尔特方法(LM方法)。最后，简单介绍学习了SLAM中用于处理非线性优化的Ceres库。

好啦，本次的对SLAM的记录学习暂时告一段落。

由于先前介绍滤波算法时，其应用较为广泛，就没有对类似于Gmapping的框架进行学习。而考虑到本篇的非线性优化内容，计划下一篇记录学习一个较为经典的SLAM框架：LOAM系列之A-LOAM框架原理学习。