quaternion Jacobian

Quaternion

• $q=[w,x,y,z]$ 其中，$w$为实部，$x,y,z$为虚部
• 4维表示3维旋转，故Pose 7维，实际6维度。对于继承ceres:: LocalParameterization中
  • virtual int GlobalSize() const { return 7; }
  • virtual int LocalSize() const { return 6; }
• Eigen 四元数构造 inline Quaternion(const Scalar& w, const Scalar& x, const Scalar& y, const Scalar& z) : m_coeffs(x, y, z, w){}
  • 构造时， $w$ 在前
  • 存储时， $w$ 在后
• 四元数乘积：
  • $p\otimes q=[p{]}_{L}q=[q{]}_{R}p$
  • 由于 w 在前和后，对于左乘和右乘矩阵是不一样的，因此：
  • 

四元数左右乘求解 Jacobian

• 四元数表示旋转的空间维度是3，本身的维度为4，因此需要加入模为1的约束。在最优化时，如果对这四个维度都增加需要对四元数增加一个微小增量，会打破这个约束。最优方法是在流行空间进行优化，此时增量存在于正切空间(维度是3)。我们不断修正这个增量，从而得到修正后的四元数和cost function。这里定义一个广义的加 $⊞$，同时也需要对增量求导。
  • 左乘：
    • 加法 oplus：
      • $q^{\prime }=q⊞\Delta q=\delta q\otimes q$
        • 其中，$\Delta q$ 是三维向量，我们定义的扰动叠加在单位四元数 $q$ 的左边
        • $\Delta q=[w_1,w_2,w_3{]}^T$
        • $\delta q=exp(\left[\begin{array}{c}\frac{1}{2}\Delta q\\ 0\end{array}\right])=\left[\begin{array}{c}\frac{\Delta q}{||\Delta q||}\sin \frac{||\Delta q||}{2}\\ \cos \frac{||\Delta q||}{2}\end{array}\right]$
      • $\delta q\approx \left[\begin{array}{c}\frac{1}{2}\Delta q\\ 0\end{array}\right]$
    • 加法雅克比 oplusJacobian：
      • $q^{\prime }=q+J_{4\times 3}\Delta q$
      • $J_{4\times 3}=\frac{d\vec{q}}{d\vec{v}}={\begin{array}{c}\frac{\partial q⊞\Delta q}{\partial \Delta q}\end{array}|}_{\Delta q=0}={\begin{array}{c}\frac{\partial \delta q\otimes q}{\partial \Delta q}\end{array}|}_{\Delta q=0}={\begin{array}{c}\frac{\partial \delta q\otimes q}{\partial \delta q}\frac{\partial \delta q}{\partial \Delta q}\end{array}|}_{\Delta q=0}=\frac{1}{2}[q{]}_R\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\\ 0& 0& 0\end{array}\right]=[\frac{1}{2}[q{]}_R{]}_{c1:c3}$
  • 右乘：
    • 加法 oplus：
      • $q^{\prime }=q⊞\Delta q=q\otimes \delta q$
      • $\delta q\approx \left[\begin{array}{c}\frac{1}{2}\Delta q\\ 0\end{array}\right]$
    • 加法雅克比 oplusJacobian：
      • $q^{\prime }=q+J_{4\times 3}\Delta q$
      • $J_{4\times 3}=\frac{d\vec{q}}{d\vec{v}}={\begin{array}{c}\frac{\partial q⊞\Delta q}{\partial \Delta q}\end{array}|}_{\Delta q=0}={\begin{array}{c}\frac{\partial q\otimes \delta q}{\partial \Delta q}\end{array}|}_{\Delta q=0}={\begin{array}{c}\frac{\partial q\otimes \delta q}{\partial \delta q}\frac{\partial \delta q}{\partial \Delta q}\end{array}|}_{\Delta q=0}=\frac{1}{2}[q{]}_L\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\\ 0& 0& 0\end{array}\right]=[\frac{1}{2}[q{]}_L{]}_{c1:c3}$
• quaternion error
  • 给定两个四元数 $p,q$，$p$ 作为真值，$q$ 作为估计值，需估计他们之间的误差，这是可定义误差为：
  • $e(q)=2[p\otimes q^{-1}{]}_{r_1:r_3}\approx 2{\left[\begin{array}{c}\frac{1}{2}\Delta e\\ 1\end{array}\right]}_{r_1:r_3}=\Delta e$
  • 求解雅克比：
    • $e(q⊞\Delta q)\approx e(q)+J_{3\times 3}\Delta q$
    • $J_{3\times 3}={\begin{array}{c}\frac{\partial q⊞\Delta q}{\partial \Delta q}\end{array}|}_{\Delta q=0}={\begin{array}{c}\frac{\partial 2[p\otimes (\delta q\otimes q{)}^{-1}{]}_{r_1:r_3}}{\partial \Delta q}\end{array}|}_{\Delta q=0}$ 左乘
    • 也可以右乘。

Quaternion and Rotation Matrix