Global SLAM（一）—— 最小二乘问题以及ceres求解
Global SLAM（一）—— 最小二乘问题以及ceres求解
摘要：global slam 中的一项重要任务就是进行图优化，图优化主要就是为了求解最小二乘问题，这里介绍什么是最小二乘问题，以及如何使用开源库ceres求解。
一、最小二乘问题
最小二乘问题通常可以表述为，通过搜集到的一些数据(获取得到的样本)，对某一个模型进行拟合，并尽可能的使得模型结果和样本达到某种程度上的最佳拟合:
转换成通俗的例子：
假设我们有一个模型 y = a x + b y=ax+b y=ax+b，但是我们并不知道系数a、b为多少。然后，我们获得了一些列的采样 （ x 1 , y 1 ） （ x 2 , y 2 ） （ x 3 , y 3 ） . . . . . . （ x n , y n ） （x_{1}, y_{1}）（x_{2}, y_{2}）（x_{3}, y_{3}）......（x_{n}, y_{n}） （x1,y1）（x2,y2）（x3,y3）......（xn,yn）等等。
理想情况下，我们希望：
y k = a x k + b y_{k} = ax_{k}+b yk=axk+b
可是实际情况下：
y k ≠ a x k + b y_{k} \neq ax_{k}+b yk=axk+b
它们之间的差值，我们定义为：
e k = y k − a x k − b e_{k} =y_{k} -ax_{k}-b ek=yk−axk−b
然后我们的优化目标就是，让所有的差值之后最小，也就是尽考虑到所有采样点的感情。
m i n ( e 1 + e 2 + . . . e k + . . . e n ) min(e_{1}+e_{2}+...e_{k} +...e_{n} ) min(e1+e2+...ek+...en)
然而差值有正有负，为了避免它们相互抵消，所以应该把差值定义成平方的形式，也就是所谓的代价： e k 2 e_{k}^{2} ek2
所以我么的优化目标函数就变成：
m i n ( e 1 2 + e 2 2 + . . . e k 2 + . . . e n 2 ) min(e_{1}^{2}+e_{2}^{2}+...e_{k}^{2} +...e_{n}^{2} ) min(e12+e22+...ek2+...en2)
综上我们就可以得到更加一般的优化目标表达式了：
m i n 1 2 ∑ ρ k ( e k ) min \frac{1}{2}\sum \rho _{k} ( e_{k} ) min21∑ρk(ek)
其中称 ρ k ( e k ) \rho _{k} ( e_{k} ) ρk(ek) 为参差模块（residual block）， ρ k ( . ) \rho _{k}(.) ρk(.)称为损失函数（loss function）, e k e_{k} ek中涉及的 ( a , b ) ({a, b}) (a,b)称为参数模块（parameter blocks）。
二、损失函数（loss function）
上面的定义说明，应该可以很清楚的知道了参差模块（residual block）、参数模块（parameter blocks），损失函数（loss function）似乎还不是很明朗，这里再稍微介绍下损失函数。
上面举例中我们把每组的数据的误差平方，以相等的权重累和起来，那么对应的损失函数可以这样理解：
ρ k ( e k ) = 1 × e k 2 \rho _{k}(e_{k})=1\times e_{k}^{2} ρk(ek)=1×ek2
但是有时候，我们平等的看待每个数据点(线性回归)，其实并不好，比如数据整体分布很好，就是有小部分离群点，这些离群点会把拟合结果拉偏。所以可以做的更加精细一点。下面介绍一下常用的huber损失函数：
Huber Loss
Huber Loss 是一个用于回归问题的带参损失函数, 优点是能增强平方误差损失函数(MSE, mean square error)对离群点的鲁棒性。
当预测偏差小于 α \alpha α 时，它采用平方误差,
当预测偏差大于 α \alpha α时，采用的线性误差。
相比于最小二乘的线性回归，HuberLoss降低了对离群点的惩罚程度，所以 HuberLoss 是一种常用的鲁棒的回归损失函数。
L α ( e k ) = { 1 2 e k 2 if ∣ e k ∣ < = α α ( ∣ e k ∣ − 1 2 α ) if ∣ e k ∣ > α L_{\alpha }(e_{k})=\begin{cases} {\frac{1}{2}e_{k}^{2}} & \text{ if } |e_{k}|<=\alpha \\ \alpha (|e_{k}|-\frac{1}{2}\alpha)& \text{ if } |e_{k}|>\alpha \end{cases} Lα(ek)={21ek2α(∣ek∣−21α) if ∣ek∣<=α if ∣ek∣>α
三、ceres求解
Ceres的求解过程包括构建最小二乘和求解最小二乘问题两部分，其中构建最小二乘问题的相关方法均包含在Ceres::Problem类中，涉及的成员函数主要为Problem::AddResidualBlock()
ResidualBlockId Problem::AddResidualBlock(CostFunction *cost_function,
LossFunction *loss_function,
double *x0, double *x1, ...)
cost_function
与其他非线性优化工具包一样，ceres的性能很大程度上依赖于导数计算的精度和效率。这部分工作在ceres中称为CostFunction。这里只介绍一种最常用的costfunction。
自动导数（AutoDiffCostFunction）：由ceres自行决定导数的计算方式。
ceres::AutoDiffCostFunction<CostFunctor, int residualDim, int paramDim>(CostFunctor* functor);
模板参数依次为仿函数（functor）类型CostFunctor，残差维数residualDim和参数维数paramDim，接受参数类型为仿函数指针CostFunctor*。
四、实例演示
通过上面的铺垫，应该基本上对最小二乘问题、ceres API有了大致的了解。下面搬运一个《视觉SLAM十四讲》实例，对比实例来把上面的内容完全串起来。
问题描述
该代码主要是求解曲线 y = a x 2 + b x + c + w y=ax^{2}+bx+c+w y=ax2+bx+c+w; ( w w w是噪声） 假设有N个x和y的观测数据点，用来求解曲线的参数。则待估计变量实际上是a，b，c
解决思路
先用CV随机数产生器生成N个数据（包括噪声） 构造最小二乘问题 ； 配置求解器（配置项比较多）， 对问题进行优化。
#include 参考资料：
[1]. http://www.ceres-solver.org/
[2].《视觉slam十四讲》 高翔