旋转矩阵，四元数(quaternion)，Rodrigues向量

旋转矩阵，四元数(quaternion)，Rodrigues向量

四元数转换成旋转矩阵

四元数 $\mathbf{q}=(\cos \frac{\theta }{2},\sin \frac{\theta }{2}{\hat{\mathbf{n}}}^T{)}^T=(q_0,q_1,q_2,q_3{)}^T$
对应旋转矩阵
R = [ q 0 2 + q 1 2 − q 2 2 − q 3 2 2 ( q 1 q 2 − q 0 q 3 ) 2 ( q 1 q 3 + q 0 q 2 ) 2 ( q 1 q 2 + q 0 q 3 ) q 0 2 − q 1 2 + q 2 2 − q 3 2 2 ( q 2 q 3 − q 0 q 1 ) 2 ( q 1 q 3 − q 0 q 2 ) 2 ( q 2 q 3 + q 0 q 1 ) q 0 2 − q 1 2 − q 2 2 + q 3 2 ] \mathbf{R} = \begin{bmatrix} q_0^2+q_1^2-q_2^2-q_3^2 & 2(q_1q_2-q_0q_3) & 2(q_1q_3+q_0q_2)\\ 2(q_1q_2+q_0q_3) & q_0^2-q_1^2+q_2^2-q_3^2 & 2(q_2q_3-q_0q_1)\\ 2(q_1q_3-q_0q_2) & 2(q_2q_3+q_0q_1) & q_0^2-q_1^2-q_2^2+q_3^2\\ \end{bmatrix} R=⎣⎡​q02​+q12​−q22​−q32​2(q1​q2​+q0​q3​)2(q1​q