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=p_w+p_xi+p_yj+p_zk \qquad \Longleftrightarrow \qquad Q=p_w+ \textbf p_v$
$\textbf q =\begin{bmatrix}p_x &p_y&p_z&p_w \end{bmatrix}^T$
Product
四元数的乘积具有以下形式，在eigen中直接调用*即可。
$\textbf p \otimes \textbf q = \begin{bmatrix} p_wq_x + p_xq_w + p_yq_z − p_zq_y \\ p_wq_y − p_xq_z + p_yq_w + p_zq_x \\ p_wq_z + p_xq_y − p_yq_x + p_zq_w \\ p_wq_w − p_xq_x − p_yq_y − p_zq_z \end{bmatrix}$
同时也可以将四元数的乘法写成矩阵的乘法，这里定义四元数的左乘矩阵$[]_L$和右乘矩阵$[]_R$。
$\textbf p \otimes \textbf q = [\textbf p]_L \textbf q = [\textbf q]_R \textbf p$
$[\textbf p]_L = \begin{bmatrix} q_w & -q_z & q_y & q_x \\ q_z & q_w & -q_x & q_y \\ -q_y & q_x & q_w & q_z \\ -q_x & -q_y & -q_z & q_w \end{bmatrix}$
$[\textbf p]_R = \begin{bmatrix} q_w & q_z & -q_y & q_x \\ -q_z & q_w & q_x & q_y \\ q_y & -q_x & q_w & q_z \\ -q_x & -q_y & -q_z & q_w \end{bmatrix}$
Optimization on a manifold
表示旋转的空间维度是3，而四元数表示的维度为4，因此需要加入模为1的约束。在最优化时，如果对这四个维度都增加需要对四元数增加一个微小增量，会打破这个约束。最优的方法是在流形空间上进行优化，此时增量存在于正切空间（维度为3）。我们不断修正这个增量，从而得到修正后的四元数以及cost function。这里需要定义一个广义的加$\boxplus$，同时也需要对增量进行求导。
oplus
$\textbf q’ = \textbf q \boxplus \Delta \textbf q = \delta \textbf q \otimes \textbf q$
其中$ \Delta \textbf q$时一个三维的向量，我们定义的扰动叠加在单位四元数$\textbf q$的左边（右边也是可以的）。
$ \Delta \textbf q = [w_1 \ w_2 \ w_3]^T$
$ \delta \textbf q = exp \bigg( \begin{bmatrix}\frac{1}{2} \Delta \textbf q\\0\end{bmatrix} \bigg) = \begin{bmatrix} \frac{\Delta \textbf q }{| \Delta \textbf q |}sin(\frac{| \Delta \textbf q |}{2} ) \\ cos(\frac{| \Delta \textbf q |}{2} ) \end{bmatrix}$
$ \delta \textbf q \approx \begin{bmatrix}\frac{1}{2} \Delta \textbf q\\1\end{bmatrix}$
oplusJacobian
$q’ = q + J_{4\times3} \Delta \textbf q $
下面求解雅可比矩阵：
$
\begin{align}
J_{4\times3} &= \frac{\partial \textbf q \boxplus \Delta \textbf q}{\partial \Delta \textbf q} \bigg| _{\Delta \textbf q=0} = \frac{\partial{\delta \textbf q \otimes \textbf q}}{\partial \Delta \textbf q} \bigg| _{\Delta \textbf q=0} = \frac{\partial \delta \textbf q \otimes \textbf q }{\partial \delta \textbf q} \bigg| _{\Delta \textbf q=0} \frac{\partial \delta \textbf q}{\partial \Delta \textbf q}\bigg| _{\partial \Delta \textbf q=0} \\
&= \frac{\partial [\textbf q]_R \delta \textbf q}{\partial \delta \textbf q} \bigg| _{\Delta \textbf q=0} \frac{\partial \delta \textbf q}{\partial \Delta \textbf q}\bigg| _{\Delta \textbf q=0} \\
&=[\textbf q]_R \frac{\partial \begin{bmatrix} \frac{\Delta \textbf q }{| \Delta \textbf q |}sin(\frac{| \Delta \textbf q |}{2} ) \\ cos(\frac{| \Delta \textbf q |}{2} ) \end{bmatrix}}{\partial \Delta \textbf q}\bigg| _{\Delta \textbf q=0}\\
&= \frac{1}{2} [\textbf q]_R \begin{bmatrix} 1&0&0 \\ 0&1&0 \\ 0&0&1 \\ 0&0&0 \end{bmatrix} \\
&= \left[ \frac{1}{2} [\textbf q]_{R} \right]_{c_1:c_3}
\end{align}
$
即右乘矩阵的前3列除以2。
quaternion error
给定两个四元数$\textbf p$和$\textbf q$，$\textbf p$作为真值，$\textbf q$作为估计值，我们需要估计它们之间的误差，这时可以定义误差为：
$e(\textbf q) =2 [\textbf p \otimes \textbf q^{-1}]_{r1:r3} \approx 2\begin{bmatrix}\frac{1}{2} \Delta \textbf e\\1\end{bmatrix}_{r1:r3} = \Delta \textbf e$
其中$r1:r3$表示取第一行至第三行。
下面求解雅可比矩阵：
$e( \textbf q \boxplus \Delta \textbf q) \approx e(\textbf q) + J_{3\times3} \Delta \textbf q$
$
\begin{align}
J_{3\times3} &= \frac{\partial e( \textbf q \boxplus \Delta \textbf q)}{\partial \Delta \textbf q}\bigg|_{\Delta \textbf q=0} = \frac{\partial 2[\textbf p \otimes (\delta \textbf q \otimes \textbf q)^{-1}]_{r1:r3}}{\partial \Delta \textbf q}\bigg\vert _{\Delta \textbf q=0} \\
&= 2 \frac{\partial [\textbf p \otimes \textbf q^{-1} \otimes \delta \textbf q^{-1} ]_{r1:r3}}{\partial \Delta \textbf q}\bigg| _{\Delta \textbf q=0} \\
&= 2 \left[ \frac{\partial (\textbf p \otimes \textbf q^{-1} \otimes \delta \textbf q^{-1} )}{\partial \Delta \textbf q} \bigg| _{\Delta \textbf q=0}\right] _{r1:r3} \\
&= 2 \left[ \frac{\partial [\textbf p \otimes \textbf q^{-1}]_L \delta \textbf q^{-1} ]}{\partial \delta \textbf q^{-1}} \frac{\partial \textbf q^{-1}}{\partial \textbf q} \frac{\partial \delta \textbf q}{\partial \Delta \textbf q} \bigg| _{\Delta \textbf q=0} \right] _{r1:r3} \\
&=\left[ [\textbf p \otimes \textbf q^{-1}]_L \begin{bmatrix} -1&0&0&0\\0&-1&0&0\\0&0&-1&0\\0&0&0&1\end{bmatrix} \begin{bmatrix} 1&0&0 \\ 0&1&0 \\ 0&0&1 \\ 0&0&0 \end{bmatrix} \right] _{r1:r3}\\
&=-\left[ [\textbf p \otimes \textbf q^{-1}]_L\right] _{topleft 3 \times 3}
\end{align}
$