我已经给你们算好了

　　他直接指着屏幕上的一个矩阵说道：

　　“世界坐标系到相机坐标系的变换用的是欧拉角表示旋转矩阵，然后用对极几何约束求本质矩阵，再用SVD分解做运动恢复。”

　　“问题就出在这一步，在高速动态场景下，相邻帧之间的旋转不是在欧几里得空间里发生的，而是在一个弯曲的流形上。

　　你的SVD分解把非线性约束强制投影到了线性空间里，相当于把球面坐标硬塞进直角坐标系，畸变就是这么来的。”

　　沈凌的瞳孔缩了一下。

　　他隐隐约约感觉到问题出在几何框架上，但一直没能准确定位到，想不到肖宿一下子就看出来了。

　　“正确的做法应该是放弃欧拉角，改用四元数表示旋转，并且在单位四元数构成的S^3球面上直接做优化。”

　　肖宿再次拿起白板笔，在图纸的另一块空白区域写了起来。

　　“首先，设第k帧到第k+1帧的旋转四元数为q_k，那么帧间运动导致的深度估计误差肯定满足一个辛结构保持的Hamilton系统。

　　然后构造这样一个辛矩阵M，它的作用是把高维的投影误差映射到一个低维的辛商流形上，在这个商流形上，误差传播方程就会退化成一个可积系统。”

　　“这个辛矩阵的定义是M=J·∇²H，其中J是标准辛形式，H是系统的Hamilton函数，取为投影误差在深度方向上的二范数。

　　∇²H是H的Hessian矩阵，在这个特定的几何设定下，它的闭式解是∇²H=(I-n·n^T)/d^2，其中，n是视线方向的单位向量，d是深度值。

　　这两个一乘，M的特征值就会全部落在单位圆上，所以数值稳定性是自然的，不需要任何额外的正则化。”

　　沈凌的嘴唇微微颤抖着。

　　他浑身上下都激动的泛起了鸡皮疙瘩。

　　那个困扰了他一年半的帧间畸变问题，肖宿只是用一组辛矩阵轻轻描淡写地把它拆解了。

　　每一步推导都写得那么简单，组合在一起，却让他有些头皮发麻。

　　这种震撼不是来自结论本身有多复杂，恰恰相反。

　　它太简单了。

　　简单到让人觉得，在正确的视角下，这个问题本来就不应该有困难。

　　“可是我还有一个问题，”沈凌强迫自己冷静下来，“辛矩阵的方案解决了旋转部分的畸变，但平移部分的漂移呢？

　　我们的相机在快速大范围移动时，深度图会出现一个缓慢的累积漂移，这个漂移在用了您的辛矩阵修正旋转后还是会残留的。”

　　“那个漂移不是平移带来的，而是你的深度图初始值在高速运动下没有及时收敛。”

　　肖宿的回答几乎没有任何停顿，“可以用非线性互补滤波器来解决。

　　在低频段，可以信任惯性测量单元的姿态数据，而高频段则可以信任视觉里程计的相对运动估计。至于交叉频率，直接设在视差数据的噪声带宽和IMU漂移带宽的交点处就可以。

　　传递函数就取n阶巴特沃斯（Butterworth）形式，阶数n的取值，是由姿态误差的截止频率和深度估计的收敛速率共同决定的。

　　我已经给你们算好了，n取3