相机姿态轨迹最小二乘多项式平滑优化（防抖动）

上一篇讲了利用随即一致性来减少错误匹配的情况，这一篇讲一下使用最小二乘多项式平滑方法来减少姿态的抖动问题。因为我们的设备是手持单目，相机在移动的过程中，会产生细微的抖动。虽然EKF或者G2O(通用图优化)会帮我们最小化错误，但是当我们得到正确的最接近真实的姿态后，我们会发现这些姿态是存在细微抖动的。下图为未平滑的相机姿态（Matlab）：

我们看到有大量的抖动在里面。体现在图像上则是AR物体会有小幅度的波动。这个波动其实很小，如果AR物体比较小的话，这个波动是不明显的（肉眼几乎察觉不到）。但是当我们拉远距离，呈现很大的AR物体时（如车展上的汽车），这个波动肉眼还是能察觉到。另外现在的VR/AR系统里，减小眩晕感是很重要的，所以有必要使呈现出的虚拟物体更加的稳定。因为AR系统是实时的，而复杂的滤波或者平滑方法会消耗更多的时间，所以我们采用基础的最小二乘法来消除抖动。这种方法参数少，计算量小，速度快，平滑效果不错。完整的名字是最小二乘移动窗口多项式平滑，我们用周围多个带有不同权重的点来表示其中的一个点，合理的选择平滑窗口的大小和多项式的次方数，对于消除抖动的效果和时间消耗极为重要。对于平滑后的数据与原始数据之间的误差，我们做了相应的数据补偿。

首先来看看最小二乘多项式平滑中最重要的两个参数：平滑窗口大小2m+1，多项式次数n。如下图所示:

我们先来计算一下m=2（移动窗口大小为5），n=2情况下的权重（系数）矩阵：

方程组（14）可以被表示成矩阵形式（15），然后利用通用解法解线性方程组。

得到b之后，带入到（14）里，就得到了每个点如何被周围2m+1个点表示，同时也得到了权重矩阵：

以下是不同平滑窗口大小和多项式次方的权重矩阵，用到系统中时只要选好参数就可以直接使用：

对于每一个新添加到平滑窗口里的姿态，使用前2m+1个姿态平滑，返回平滑后的数据，右移窗口（就是删第一个姿态，添加新姿态到最后）：

做了一些实验，对比分析实验结果后发现平滑窗口大小为7，多项式次数为1时，平滑效果最佳。大约平滑了20%：

平滑后的数据与原数据之间的误差是不可避免的，但还是有一些地方的误差是不必要的，比如一些本身平滑的轨迹，因为受到前后不平滑姿态的影响导致一定的不必要误差。遇到这种情况，我们便减少平滑后数据与原始数据之间的距离。比如连续k个姿态，波动角度小于一定值，我们便做这种处理：

经过一定的实验，我们发现连续5个姿态波动cos大于0.8（角度小于arccos(0.8)）时减少距离，效果最好：

最终整个平滑过程减少了姿态轨迹的波动，提高了约20%的平滑度，误差约0.01*50=5像素（图中一个unit为50pixel），耗时0.1毫秒。