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

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

, in which

当 很小时，可以近似表达为：

四元数小量和李代数小量有很简单的对应关系，所以在使用四元数的优化问题中，往往也取 小量作为更新量，例如 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 量，如某个误差函数 ，则

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

尾声

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

可以证明：

。

代入过程就不贴了。

参考文献

[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.