quaternion in eigen

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

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