旋转矩阵,四元数(quaternion),Rodrigues向量
四元数转换成旋转矩阵
四元数 q = ( cos θ 2 , sin θ 2 n ^ T ) T = ( q 0 , q 1 , q 2 , q 3 ) T \mathbf{q} = (\cos\frac{\theta}{2}, \sin\frac{\theta}{2} \hat{\mathbf{n}}^T)^T = (q_0, q_1, q_2, q_3)^T q=(cos2θ,sin2θn^T)T=(q0,q1,q2,q3)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−q322(q1q2+q0q3)2(q1q