quaternion in eigen math

Representations
Product
Optimization on a manifold
- oplus
- oplusJacobian
- quaternion error

Representations eigen中quaternion的构造函数为Quaternion (const Scalar &w, const Scalar &x, const Scalar &y, const Scalar &z)，注意w在前。然而在内部存储时eigen将四元数的w放在最后，例如通过

Eigen::Vector4d q = q_AB.coeffs();

访问时，q中的最后一个元素才是w。
在论文中，比较常见的四元数分为两种hamilton quaternion和JPL quaternion。主要的区别在于前者是右手系 ij=k ，后者是左手系 ij=?k ，eigen中的四元数为hamilton quaternion，然而有点不一样的是四元数内的顺序定义不一样，即w在最后。
Q=pw+pxi+pyj+pzk?Q=pw+pv
q=[pxpypzpw]T
Product 四元数的乘积具有以下形式，在eigen中直接调用*即可。
p?q=???????pwqx+pxqw+pyqz?pzqypwqy?pxqz+pyqw+pzqxpwqz+pxqy?pyqx+pzqwpwqw?pxqx?pyqy?pzqz???????
同时也可以将四元数的乘法写成矩阵的乘法，这里定义四元数的左乘矩阵 []L 和右乘矩阵 []R 。
p?q=[p]Lq=[q]Rp
[q]L=???????qwqz?qy?qx?qzqwqx?qyqy?qxqw?qzqxqyqzqw???????
[q]R=???????qw?qzqy?qxqzqw?qx?qy?qyqxqw?qzqxqyqzqw???????
Optimization on a manifold 表示旋转的空间维度是3，而四元数表示的维度为4，因此需要加入模为1的约束。在最优化时，如果对这四个维度都增加需要对四元数增加一个微小增量，会打破这个约束。最优的方法是在流形空间上进行优化，此时增量存在于正切空间（维度为3）。我们不断修正这个增量，从而得到修正后的四元数以及cost function。这里需要定义一个广义的加 ? ，同时也需要对增量进行求导。
oplus 【quaternion in eigen】 q′=q?Δq=δq?q
其中 Δq 时一个三维的向量，我们定义的扰动叠加在单位四元数 q 的左边（右边也是可以的）。
Δq=[w1 w2 w3]T
δq=exp([12Δq0])=???Δq∥Δq∥sin(∥Δq∥2)cos(∥Δq∥2)???
δq≈[12Δq1]
oplusJacobian q′=q+J4×3Δq
下面求解雅可比矩阵：
J4×3=?q?Δq?Δq∣∣∣Δq=0=?δq?q?Δq∣∣∣Δq=0=?δq?q?δq∣∣∣Δq=0?δq?Δq∣∣∣?Δq=0=?[q]Rδq?δq∣∣∣Δq=0?δq?Δq∣∣∣Δq=0=[q]R????Δq∥Δq∥sin(∥Δq∥2)cos(∥Δq∥2)????Δq∣∣∣Δq=0=12[q]R?????100001000010?????=[12[q]R]c1:c3
即右乘矩阵的前3列除以2。
quaternion error 给定两个四元数 p 和 q ， p 作为真值， q 作为估计值，我们需要估计它们之间的误差，这时可以定义误差为：
e(q)=2[p?q?1]r1:r3≈2[12Δe1]r1:r3=Δe
其中 r1:r3 表示取第一行至第三行。
下面求解雅可比矩阵：
e(q?Δq)≈e(q)+J3×3Δq
J3×3=?e(q?Δq)?Δq∣∣∣Δq=0=?2[p?(δq?q)?1]r1:r3?Δq∣∣∣Δq=0=2?[p?q?1?δq?1]r1:r3?Δq∣∣∣Δq=0=2[?(p?q?1?δq?1)?Δq∣∣∣Δq=0]r1:r3=2[?[p?q?1]Lδq?1]?δq?1?q?1?q?δq?Δq∣∣∣Δq=0]r1:r3=?????[p?q?1]L??????10000?10000?100001??????????100001000010??????????r1:r3=?[[p?q?1]L]topleft3×3