四元数矩阵与 so(3) 左右雅可比

2018-05-22

目录

我们知道,单位四元数 q 和 so(3) 向量 (即 rotation vector)的对应关系为:

, in which

当 $\phi$ 很小时,可以近似表达为:

四元数小量和李代数小量有很简单的对应关系,所以在使用四元数的优化问题中,往往也取 小量作为更新量,例如 OKVIS [1]。

具体到优化过程中,使用李群李代数(如预积分论文 [2])和使用四元数之间还是有区别的。使用李群李代数,在 residual 推导时会用到 so3 的左雅可比或右雅可比;使用四元数,会用到四元数矩阵(或共轭四元数矩阵)。不过既然两者使用的小量一致,那四元数矩阵和 so3 的左右雅可比的关系如何呢?

之所以有这个疑问,是因为 Xingyin 兄在博客里指出 [3],OKVIS 代码中使用了预积分,代码和预计分论文里的思路一致。不过实际上,虽然预积分流程一致,两者在雅可比推导上有所不同:预积分使用 so3 的右雅可比;OKVIS 使用四元数矩阵。所以,我们有必要把这两者的关系厘清。

四元数矩阵

两个旋转叠加,可以表达为两个四元数相乘,或者一个 4*4 矩阵与一个四元数相乘:

这里的 为四元数矩阵, 为共轭四元数矩阵:

两种矩阵对于求导非常方便:

现在,假设有个一个待估计的旋转量 q ,其更新方式为 为 so3 小量。假定有 q 的测量量 ,定义误差函数

则其相对于更新量的雅可比为

so(3) 雅可比

据 Barfoot [4],若 的左雅可比为 ,右雅可比为

于是,假设有待估计量 C,更新方式为 为 so3 小量。假定有 C 的测量量 ,定义误差函数

则其相对于更新量的雅可比为:

讨论

我们来比较一下

首先,

如果可以认为误差函数 很小,则

另一方面,据 Barfoot [4],有

如果可以认为误差函数很小,即 ,则 ,于是

可以看到,当可以认为误差函数很小时, 是相等的。这其实很好理解,因为当 很小时, 估计成立,所以 实际上就表示同一个量,都是误差旋转量的李代数,推导出来的雅可比自然就一样了,我们只是通过不同的途径去算同一个量而已。这个例子的价值在于,展示了四元数矩阵和 so3 雅可比之间的具体关系,即:

如果有可以认为很小的 so3 量,如某个误差函数 ,则

上面用的是右扰动模型;如果使用左扰动,可类似地得到

尾声

如果 $\phi$ 不能认为很小, 的关系是怎样?并不复杂。区别仅在于此时 的估计不能成立, 不能当作一个量而已;但它们之间的相互关系是已知的:。在上面的演算中,我们实际上得到的结果是

可以证明:

代入过程就不贴了。

参考文献

[1] S. Leutenegger, S. Lynen, M. Bosse, R. Siegwart, P. Furgale, “Keyframe-based visual-inertial odometry using nonlinear optimization”, Int. Journal of Robotics Research (IJRR), 2014.

[2] C. Forster, L. Carlone, F. Dellaert and D. Scaramuzza, “On-Manifold Preintegration for Real-Time Visual–Inertial Odometry,” in IEEE Transactions on Robotics, vol. 33, no. 1, pp. 1-21, Feb. 2017.

[3] OKVIS IMU 误差公式代码版本

[4] T. Barfoot, State Estimation for Robotics.


标签: roboticsSLAM

留言请用 Github Issues

聊天请在 Gitter/fan-farm

授权协议 (CC) BY-NC-SA | 订阅 RSS | 邮箱 fzheng@link.cuhk.edu.hk