最小二乘（Least Squares）-从数理统计到流形中的计算

前言

SLAM系统中，频繁地利用传感器测量信息，实际测得的数据总存在或大或小的误差，最小二乘法提供了一种十分有效的平差的方法，帧间匹配中要用到的梯度下降法、回环检测scan-to-map等等方法都需要用到最小二乘法，因此，重新自我学习了一遍最小二乘。

最大似然估计

最大似然估计就是最大可能性的估计，换句话说，就是所估计的事件最有可能是什么样子。 假设某件事情已经发生了，那么其中的怎样参数变化会产生这样的结果。
举个例子，箱子里有一堆两种颜色的球，抽取了10个球，10个球中有3个球是红色的，另外7个球是蓝色的（已经发生了的事件），那么箱子里蓝色球的比例是多少呢（怎样的概率参数可能产生前一种结果呢）？当然是蓝色球的比例是0.7时，这种事情才最有可能发生，这就是最大似然估计。

最小二乘的数理统计推导

测量中的观测值一般是服从正态分布的随机变量，可以用数理统计中的最大似然估计来解释最小二乘。
观测的向量为n维列向量L，L为随机正态分布向量，其数学期望和方差分别为：

由最大似然估计准则知，其似然函数（L的正态密度函数）为：

为便于分析，将其取对数，分析是G获得极大值时作为估计量的L-μL的取值变化。


当G取最大值时,（4）式取最小值，此即最小二乘表达式。式中P是协方差矩阵D的逆，又被称为信息矩阵或权阵。

高斯牛顿法（线性迭代）求解最小二乘解(Squared Error)

上述（4）式是最小二乘误差的表达式，但如何具体去取求解这个误差e，使得G满足最大似然估计呢？这里学习了一种简单的线性迭代求解的方法。此方法需要一个比较好的初值（就比如举例中的自己经验估计蓝色球可能的概率值为0.7），这样可以缩短迭代时间，避免陷入其他局部极值。初始值附近也应该连续，而且该误差函数模型是标量的，所以转置没有影响，只是一种习惯的表达。

1. 将误差函数在初值x附近一节泰勒展开，展开的步长为未知的△x，误差函数可以简化为：
   
2. 上述表达式对△x求导，可以求得误差函数取最小值时的△x：
   
3. 更新x=x+△x，将新的x继续按（5）、（6）式进行求解。

最小二乘在全局误差上的拓展（Global Error）

全局误差就是所有时刻最小二乘误差的累计，我们用F（x+△x）来表示全局的最小二乘误差的和，并继续沿用之前求解的思路，在初始解x处附近迭代求解△x。

上式就是全局误差函数的具体表达形式，之后将误差项△x进行微分求导，通过求解其局部极值，来计算△x的最优解。

之所以要将最小二乘在全局误差上进行拓展，是因为在SLAM中，有一个很重要的环节是回环检测，回环检测时，需要将当前位置成功辨识出是之前经过的点，当前时刻的点和之前某一时刻经过此处的点，在SLAM中，都是被记录与表达出来的。而理论上，这两个点应该是同一个点，也就是两个点的坐标差应为0或接近于0。所以，根据这个条件，可以把从上一时刻经过此处开始，到当前时刻间所有记录的点的数据，进行全局的最小二乘优化：目标是将两个点位置差接近于零，优化参数x（也就是两个时刻间的检测到的位置与约束等信息）。

最小二乘在流形（Manifold）中的拓展

流形是一种数学上的曲面曲线的概念在空间中的拓展，它在局部尺度范围内的尺度空间可以看成是欧式几何的，但在全局尺度尺度不一定是欧式几何。这个流形的概念比较拗口，不过引入它是为了描述在旋转变换时，使用既有的欧式空间的概念时，会出现一些表达上的缺陷，典型例子就是万向锁问题（Gimbal Lock），下面简单介绍下：

（上图来自杨亮博士关于SLAM的PPT）
欧式空间的欧拉角表示的旋转的变换，分为yaw（偏航角）、pitch（俯仰角）、roll（滚转角），我们可以用这三个旋转的角度，描述空间中刚体的旋转，任何转动都可以理解成三个轴的依次旋转。可是当两个坐标是旋转到重合时，也就是从上图中左图旋转到右图时，我们疑惑地发现，右图少了一个描述的自由度。具体而言，就是，右图中只有pitch（俯仰角）、roll(滚转角)，而丢失了对yaw(偏航角)的描述。
按照高翔博士的说法，这种类似万向锁的奇异性问题，只要我们用三个实数来描述三维旋转，这种奇异性问题就不可避免。而且由于这种奇异性，将旋转表示成矩阵式，对应的旋转矩阵就是奇异矩阵（狭义上不可逆，行列式为0），这种矩阵在迭代和插值运算时，会使运算的对应向量，也缺失对应的自由度（按照曹荣美老师的说法，矩阵就是线性组合，就是一种线性映射）。下举例说明：

上式是我随手写的一个奇异矩阵和向量的运算，可以明显的看到，上式经过奇异矩阵运算后，缺失了对y的描述，我目前将其理解为，奇异矩阵应理解为在两个空间的映射R3->R2的映射关系，具体的我之后再咨询老师了解下。

既然我们了解了欧拉角这种欧式几何有这个弱点，我们就想着如何解决它。目前有两种优秀的表达方式，一种是四元数（quaternion），另一种是李群（如纯旋转时，SO（3），特殊正交群，列向量单位正交的集合）与李代数（其对应的李代数so(3)）。这两种方法都是通过添加冗余的参数，过盈地表达旋转，这样就不会出现奇异性。而且这两种方式在表达旋转时，都可以忽略旋转的先后顺序，不用再像欧拉角一样，先转x轴、再转y轴，如果顺序相反，旋转矩阵的表达式就会不一样，这对计算会带来极大简化。而且引入类似李代数时，通过李群与李代数的转换关系，就可以把位姿约束转换（如旋转矩阵正交且行列式为1）为无约束的问题，具体的转换原理，我现在也没搞明白。之后再说吧。

在三维旋转中，因为欧式几何描述会有缺陷，所以我们需要引入其他的表述方法，这就会使空间由欧式空间拓展到更广义的空间。其中的曲面曲线的表达就成为流形，也就是我们在表达位置关系变换时，需要借助流形这个概念来完善我们的描述。这类非奇异描述方法都是参数冗余的，会带来额外的自由度与自由度对应的误差（误差我还不知道如何评估）。因此，这种过参数的最小二乘运算会有相应的形式。

参考Giorgio Grisetti论文中的定义与推导，首先定义一种空间映射的运算符（如下：+、口两种符号的叠加）

论文中还严谨地表达了将x增广到过参数化的空间后的符号，我没找到对应的符号在MathType中如何输入，先就都不打了，之后我会再对此修正。劳烦大家先行默认此刻的x、△x都是被增广到过参数空间。

上式与最小二乘的误差表达类似，都是先进行一阶线性化，此时的雅克比矩阵J可以如下计算为：

之后，将雅克比矩阵J**迭代到式（5）**中，就可得到类似（6）式的计算结果：

上述就是在流形空间中的最小二乘求解的表达，讲述了流形（Manifold）的概念，以及解释了因为欧式旋转表达的缺点，带来了过参数化形式的参数表达的过程，具体的推导细节建议看论文《 C. Hertzberg, “A framework for sparse, non-linear least squares problems on manifolds,” Master’s thesis, Univ. Bremen, 2008. 》

小结

从数理统计的角度讲述了最小二乘原理;
学习了最简单的高斯牛顿方法，通过一阶求导雅克比矩阵J，求解最小二乘解;
之后为图优化的学习，理解了最小二乘求解误差在全局空间的表达；
之后介绍了流形的概念及由来；
之后为说明流形，详述了万向锁及为此引入四元数与李群李代数；
李群李代数、四元数的表述均不详细，因为我也没理解透，之后会进一步补充；
最后，介绍了最小二乘在过参数形式下的表达，其实，只是为对旋转矩阵的表达进行了丰富，因此而带来多余的参数，为了带入计算这些参数，需要将优化量x也进行相应的拓展或增广。

参考

1. （《参考误差理论与测量平差基础》）
2. Giorgio Grisetti《Least Squares and SLAM》
3. 《视觉SLAM十四讲》高翔
4. 《A tutorial on Graph-based SLAM》Giorgio Grisetti, Rainer Kümmerle, Cyrill Stachniss, and Wolfram Burgard