四元数，旋转矩阵，轴角，欧拉角的相互转换（原理+Eigen代码实现）

注：文章涉及的坐标系都为右手系

1. 旋转矩阵

1.1. 旋转矩阵 -> 四元数

假设有单位四元数$\mathbf{q}=q_0+q_1\mathbf{i}+q_2\mathbf{j}+q_3\mathbf{k}=s+\mathbf{v}$,三维点$\mathbf{p}$经过旋转之后变为${\mathbf{p}}^{\prime }$,如果使用矩阵描述，那么有${\mathbf{p}}^{\prime }=\mathbf{Rp}$ 。 如果用四元数描述旋转，则表示为${\mathbf{p}}^{\prime }=\mathbf{qp}{\mathbf{q}}^{-1}$。

$${\mathbf{p}}^{\prime }=\mathbf{qp}{\mathbf{q}}^{-1}=\mathcal{L}(\mathbf{q})\mathcal{R}({\mathbf{q}}^{-1})\mathbf{p}=\mathbf{Rp}$$

${\mathbf{q}}^{-1}={\mathbf{q}}^{\ast }/||\mathbf{q}|{|}^2$，所以单位四元数的逆即为${\mathbf{q}}^{\ast }$

$$\begin{gathered} \mathcal{L}(\mathbf{q})\mathcal{R}({\mathbf{q}}^{\ast })=\left[\begin{array}{cc}s& -{\mathbf{v}}^T\\ \mathbf{v}& s{\mathbf{I}}_{3\times 3}+[\mathbf{v}\times ]\end{array}\right]\left[\begin{array}{cc}s& {\mathbf{v}}^T\\ -\mathbf{v}& s{\mathbf{I}}_{3\times 3}+[\mathbf{v}\times ]\end{array}\right] \\ =\left[\begin{array}{cc}a& b\\ c& \mathbf{v}{\mathbf{v}}^T+s^2\mathbf{I}+2s[\mathbf{v}\times ]+(\mathbf{v}\times {)}^2\end{array}\right] \end{gathered}$$
因为$\mathbf{p}$和${\mathbf{p}}^{\prime }$都是虚四元数，所以该矩阵的右下角即为从四元数到旋转矩阵的变换关系：
$$R=\mathbf{v}{\mathbf{v}}^T+s^2\mathbf{I}+2s[\mathbf{v}\times ]+(\mathbf{v}\times {)}^2$$

代入$s=q_0,\mathbf{v}=q_1\mathbf{i}+q_2\mathbf{j}+q_3\mathbf{k}$.
根据旋转矩阵的表达式，可以由旋转矩阵得到四元数：
R = [ r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 ] = [ 2 ( q 0 2 + q 1 2 ) − 1 2 ( q 1 q 2 − q 0 q 3 ) 2 ( q 1 q 3 + q 0 q 1 ) 2 ( q 1 q 2 + q 0 q 3 ) 2 ( q 0 2 + q 2 2 ) − 1 2 ( q 2 q 3 − q 0 q 1 ) 2 ( q 1 q 3 − q 0 q 1 ) 2 ( q 2 q 3 + q 0 q 1 ) 2 ( q 0 2 + q 3 2 ) − 1 ] R =\left[\begin{array}{lll} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{array}\right] =\left[\begin{array}{lll} 2(q_0^2 + q_1^2)-1 & 2(q_1q_2-q_0q_3) & 2(q_1q_3+q_0q_1) \\ 2(q_1q_2+q_0q_3) & 2(q_0^2 + q_2^2)-1 & 2(q_2q_3-q_0q_1) \\ 2(q_1q_3-q_0q_1) & 2(q_2q_3+q_0q_1) & 2(q_0^2 + q_3^2)-1 \end{array}\right] R= ​r11​r21​r31​​r12​r22​r32​​r13​r23​r33​​ ​= ​2(q02​+q12​)−12(q1​q2​+q0​q3​)2(q1​q3​−q0​q1​)​2(q1​q2​−q0​q3​)2(q02​+q22​)−12(q2​q3​+q0​q1​)​2(q1​q3​+q0​q1​)2(q2​q3​−q0​q1​)2(q02​+q32​)−1​ ​

$q_0=\frac{\sqrt{1+r_{11}+r_{22}+r_{33}}}{2}$

$q_1=\frac{r_{32}-r_{23}}{4q_0}$

$q_2=\frac{r_{13}-r_{31}}{4q_0}$

$q_3=\frac{r_{21}-r_{12}}{4q_0}$

	Eigen::Quaterniond quaternion(rotation_matrix);

或者

	Eigen::Quaterniond quaternion;
	quaternion = rotation_matrix;

1.2. 旋转矩阵 -> 欧拉角

R x ( θ ) = [ 1 0 0 0 cos ⁡ θ − sin ⁡ θ 0 sin ⁡ θ cos ⁡ θ ] R_{x}(\theta)=\left[\begin{array}{ccc}1 & 0 & 0 \\ 0 & \cos \theta & -\sin \theta \\ 0 & \sin \theta & \cos \theta\end{array}\right] Rx​(θ)= ​100​0cosθsinθ​0−sinθcosθ​ ​

R y ( θ ) = [ cos ⁡ θ 0 sin ⁡ θ 0 1 0 − sin ⁡ θ 0 cos ⁡ θ ] R_{y}(\theta)=\left[\begin{array}{ccc}\cos \theta & 0 & \sin \theta \\ 0 & 1 & 0 \\ -\sin \theta & 0 & \cos \theta\end{array}\right] Ry​(θ)= ​cosθ0−sinθ​010​sinθ0cosθ​ ​

R z ( θ ) = [ cos ⁡ θ − sin ⁡ θ 0 sin ⁡ θ cos ⁡ θ 0 0 0 1 ] R_{z}(\theta)=\left[\begin{array}{ccc}\cos \theta & -\sin \theta & 0 \\ \sin \theta & \cos \theta & 0 \\ 0 & 0 & 1 \end{array}\right] Rz​(θ)= ​cosθsinθ0​−sinθcosθ0​001​ ​

R = R z ( ϕ ) R y ( θ ) R x ( ψ ) = [ r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 ] = R=R_{z}(\phi) R_{y}(\theta) R_{x}(\psi)= \left[\begin{array}{lll} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{array}\right]= R=Rz​(ϕ)Ry​(θ)Rx​(ψ)= ​r11​r21​r31​​r12​r22​r32​​r13​r23​r33​​ ​=
[ cos ⁡ θ cos ⁡ ϕ sin ⁡ ψ sin ⁡ θ cos ⁡ ϕ − cos ⁡ ψ sin ⁡ ϕ cos ⁡ ψ sin ⁡ θ cos ⁡ ϕ + sin ⁡ ψ sin ⁡ ϕ cos ⁡ θ sin ⁡ ϕ sin ⁡ ψ sin ⁡ θ sin ⁡ ϕ + cos ⁡ ψ cos ⁡ ϕ cos ⁡ ψ sin ⁡ θ sin ⁡ ϕ − sin ⁡ ψ cos ⁡ ϕ − sin ⁡ θ sin ⁡ ψ cos ⁡ θ cos ⁡ ψ cos ⁡ θ ] \left[\begin{array}{ccc}\cos \theta \cos \phi & \sin \psi \sin \theta \cos \phi-\cos \psi \sin \phi & \cos \psi \sin \theta \cos \phi+\sin \psi \sin \phi \\ \cos \theta \sin \phi & \sin \psi \sin \theta \sin \phi+\cos \psi \cos \phi & \cos \psi \sin \theta \sin \phi-\sin \psi \cos \phi \\ -\sin \theta & \sin \psi \cos \theta & \cos \psi \cos \theta\end{array}\right] ​cosθcosϕcosθsinϕ−sinθ​sinψsinθcosϕ−cosψsinϕsinψsinθsinϕ+cosψcosϕsinψcosθ​cosψsinθcosϕ+sinψsinϕcosψsinθsinϕ−sinψcosϕcosψcosθ​ ​

则对于绕ZYX定轴得到的旋转矩阵可以如下表示欧拉角：
$$\begin{array}{c}{\theta }_x=\mathrm{atan}2\left(r_{32},r_{33}\right)\\ {\theta }_y=\mathrm{atan}2\left(-r_{31},\sqrt{r_{32}^2+r_{33}^2}\right)\\ {\theta }_z=\mathrm{atan}2\left(r_{21},r_{11}\right)\end{array}$$

	Eigen::Vector3d R_ZYX = R.eulerAngles(2,1,0);
	std::cout << yaw, pitch, roll" << R_ZYX.transpose() *180/ M_PI << std::endl;

1.3. 旋转矩阵->轴角

利用罗德里格旋转公式可以获得绕某过原点一轴 $\mathbf{n}=\left[r_x,r_y,r_z\right]$ 旋转某一角度 $\theta$ 的旋转矩阵
R = c o s θ I + ( 1 − c o s θ ) n n T + s i n θ n ^ = [ r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 ] = R=cos\theta \mathbf I+(1-cos\theta )\mathbf n\mathbf n^T+sin\theta \mathbf n \hat{}=\left[\begin{array}{lll} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{array}\right]= R=cosθI+(1−cosθ)nnT+sinθn^= ​r11​r21​r31​​r12​r22​r32​​r13​r23​r33​​ ​=
[ r x 2 ( 1 − cos ⁡ θ ) + cos ⁡ θ r x r y ( 1 − cos ⁡ θ ) − r z s i n θ r x r z ( 1 − cos ⁡ θ ) + r y s i n θ r x r y ( 1 − cos ⁡ θ ) + r z s i n θ r y 2 ( 1 − cos ⁡ θ ) + cos ⁡ θ r y r z ( 1 − cos ⁡ θ ) − r x s i n θ r x r z ( 1 − cos ⁡ θ ) − r y s i n θ r y r z ( 1 − cos ⁡ θ ) + r x s i n θ r z 2 ( 1 − cos ⁡ θ ) + c o s θ ] \left[\begin{array}{ccc} r_{x}^{2}(1-\cos \theta) + \cos \theta& r_{x} r_{y}(1-\cos \theta)-r_zsin\theta & r_{x} r_{z}(1-\cos \theta)+r_ysin\theta \\ r_{x} r_{y}(1-\cos \theta)+r_zsin\theta & r_{y}^{2}(1-\cos \theta) +\cos \theta& r_{y} r_{z}(1-\cos \theta)-r_xsin\theta \\ r_{x} r_{z}(1-\cos \theta)-r_ysin\theta & r_{y} r_{z}(1-\cos \theta)+r_xsin\theta & r_{z}^2(1-\cos \theta)+cos\theta \end{array}\right] ​rx2​(1−cosθ)+cosθrx​ry​(1−cosθ)+rz​sinθrx​rz​(1−cosθ)−ry​sinθ​rx​ry​(1−cosθ)−rz​sinθry2​(1−cosθ)+cosθry​rz​(1−cosθ)+rx​sinθ​rx​rz​(1−cosθ)+ry​sinθry​rz​(1−cosθ)−rx​sinθrz2​(1−cosθ)+cosθ​ ​

$$\theta =acos\frac{r_{11}+r_{22}+r_{33}-1}{2}$$

r ⃗ = 1 2 s i n θ [ r 32 − r 23 r 13 − r 31 r 21 − r 12 ] \vec r = \frac{1}{2sin\theta}\left[\begin{array}{lll} r_{32} - r_{23} \\ r_{13} - r_{31} \\ r_{21} - r_{12} \end{array}\right] r =2sinθ1​ ​r32​−r23​r13​−r31​r21​−r12​​ ​

	Eigen::AngleAxisd angel_axisd(rotation_matrix);

2. 旋转向量

2.1. 旋转向量->轴角

  Eigen::AngleAxisd angel_axisd;
  angel_axisd =
      Eigen::AngleAxisd(extrinsic_params[0], Eigen::Vector3d::UnitZ()) *
      Eigen::AngleAxisd(extrinsic_params[1], Eigen::Vector3d::UnitY()) *
      Eigen::AngleAxisd(extrinsic_params[2], Eigen::Vector3d::UnitX());

或

    Eigen::Vector3d vec(jvalue[sensor_type][j]["extrinsic_param"]["rotation"][0].asDouble(),
                                  jvalue[sensor_type][j]["extrinsic_param"]["rotation"][1].asDouble(),
                                  jvalue[sensor_type][j]["extrinsic_param"]["rotation"][2].asDouble());
    Eigen::AngleAxisd rot_vec(vec.norm(), vec.normalized());

2.2. 旋转向量->旋转矩阵

    Eigen::Matrix3d rotation_matrix;
    rotation_matrix=rotation_vector.matrix();

或

	Eigen::Matrix3d rotation_matrix;
	rotation_matrix=rotation_vector.toRotationMatrix();

2.3. 旋转向量->欧拉角

	Eigen::Vector3d eulerAngle=rotation_vector.matrix().eulerAngles(2,1,0);

2.4. 旋转向量->四元数

	Eigen::Quaterniond quaternion(rotation_vector);

3. 轴角

3.1. 轴角->旋转向量

轴角和旋转向量本质上是一个东西, 轴角用四个元素表达旋转, 其中的三个元素用来描述旋转轴, 另外一个元素描述旋转的角度,如 下所示：
$$r=[x,y,z,\theta ]$$
其中单位向量 $n=[x,y,z]$对应的是旋转轴 $\theta$ 对应的是旋转角度。旋转向量与轴角相同, 只是旋转向量用三个元素来描述旋转, 它把 $\theta$ 角 乘到了旋转轴上, 如下:
$$r_v=[x\ast \theta ,y\ast \theta ,z\ast \theta ]$$

3.2. 轴角 -> 四元数

假设有单位四元数$\mathbf{q}=q_0+q_1\mathbf{i}+q_2\mathbf{j}+q_3\mathbf{k}=s+\mathbf{v}$,三维点$\mathbf{p}$经过旋转之后变为${\mathbf{p}}^{\prime }$,如果使用矩阵描述，那么有${\mathbf{p}}^{\prime }=\mathbf{Rp}$ 。 如果用四元数描述旋转，则表示为${\mathbf{p}}^{\prime }=\mathbf{qp}{\mathbf{q}}^{-1}$。

$${\mathbf{p}}^{\prime }=\mathbf{qp}{\mathbf{q}}^{-1}=\mathcal{L}(\mathbf{q})\mathcal{R}({\mathbf{q}}^{-1})\mathbf{p}=\mathbf{Rp}$$

${\mathbf{q}}^{-1}={\mathbf{q}}^{\ast }/||\mathbf{q}|{|}^2$，所以单位四元数的逆即为${\mathbf{q}}^{\ast }$

$$\begin{gathered} \mathcal{L}(\mathbf{q})\mathcal{R}({\mathbf{q}}^{\ast })=\left[\begin{array}{cc}s& -{\mathbf{v}}^T\\ \mathbf{v}& s{\mathbf{I}}_{3\times 3}+[\mathbf{v}\times ]\end{array}\right]\left[\begin{array}{cc}s& {\mathbf{v}}^T\\ -\mathbf{v}& s{\mathbf{I}}_{3\times 3}+[\mathbf{v}\times ]\end{array}\right] \\ =\left[\begin{array}{cc}a& b\\ c& \mathbf{v}{\mathbf{v}}^T+s^2\mathbf{I}+2s[\mathbf{v}\times ]+(\mathbf{v}\times {)}^2\end{array}\right] \end{gathered}$$
因为$\mathbf{p}$和${\mathbf{p}}^{\prime }$都是虚四元数，所以该矩阵的右下角即为从四元数到旋转矩阵的变换关系：
$$R=\mathbf{v}{\mathbf{v}}^T+s^2\mathbf{I}+2s[\mathbf{v}\times ]+(\mathbf{v}\times {)}^2$$

对上式两侧求迹，得到
$$\begin{gathered} tr(R)=tr(\mathbf{v}{\mathbf{v}}^T)+s^{2}tr({\mathbf{I}}_{3\times 3})+2str([\mathbf{v}\times ])+tr((\mathbf{v}\times {)}^2) \\ =q_1^2+q_2^2+q_3^2+3s^3-2q_1^2-2q_2^2-2q_3^2 \\ =3s^3-(q_1^2+q_2^2+q_3^2) \\ =3s^3-(1-s^2) \\ =4s^2-1 \end{gathered}$$

v v T = [ q 1 q 2 q 3 ] [ q 1 q 2 q 3 ] = [ q 1 2 q 1 q 2 q 1 q 3 q 2 q 1 q 2 2 q 2 q 3 q 3 q 1 q 3 q 2 q 3 2 ] \mathbf v \mathbf v^T= \begin{bmatrix}q_1\\q_2\\q_3\end{bmatrix} \begin{bmatrix}q_1&q_2&q_3\end{bmatrix}= \begin{bmatrix}q_1^2&q_1q_2&q_1q_3\\q_2q_1&q_2^2 &q_2q_3\\q_3q_1&q_3q_2&q_3^2\end{bmatrix} vvT= ​q1​q2​q3​​ ​[q1​​q2​​q3​​]= ​q12​q2​q1​q3​q1​​q1​q2​q22​q3​q2​​q1​q3​q2​q3​q32​​ ​
( v × ) 2 = [ 0 − q 3 q 2 q 3 0 − q 1 − q 2 q 1 0 ] [ 0 − q 3 q 2 q 3 0 − q 1 − q 2 q 1 0 ] = [ − q 2 2 − q 3 2 q 1 q 2 q 1 q 3 q 1 q 2 − q 2 2 − q 3 2 q 2 q 3 q 1 q 3 q 2 q 3 − q 1 2 − q 2 2 ] (\mathbf v\times)^2=\begin{bmatrix}0&-q_3&q_2\\q_3&0&-q_1\\-q_2&q_1&0\end{bmatrix}\begin{bmatrix}0&-q_3&q_2\\q_3&0&-q_1\\-q_2&q_1&0\end{bmatrix} = \begin{bmatrix}-q_2^2-q_3^2&q_1q_2&q_1q_3\\q_1q_2&-q_2^2-q_3^2&q_2q_3\\q_1q_3&q_2q_3&-q_1^2-q_2^2\end{bmatrix} (v×)2= ​0q3​−q2​​−q3​0q1​​q2​−q1​0​ ​ ​0q3​−q2​​−q3​0q1​​q2​−q1​0​ ​= ​−q22​−q32​q1​q2​q1​q3​​q1​q2​−q22​−q32​q2​q3​​q1​q3​q2​q3​−q12​−q22​​ ​

又有了罗德里格斯公式可以得到旋转向量到旋转矩阵的转换关系：
$$\mathbf{R}=cos\theta \mathbf{I}+(1-cos\theta )\mathbf{n}{\mathbf{n}}^T+sin\theta [\mathbf{n}\times ]$$
对上式子求迹可得：
$$\begin{gathered} tr(\mathbf{R})=cos\theta tr(\mathbf{I})+(1-cos\theta )tr(\mathbf{n}{\mathbf{n}}^T)+sin\theta [(\mathbf{n}\times ]) \\ =3cos\theta +(1-cos\theta )+0 \\ =1+2cos\theta \\ =>\theta =arccos\frac{tr(\mathbf{R})-1}{2}=arccos(2s^2-1) \\ =>cos\theta =2co{s}^2\frac{\theta }{2}-1=2s^2-1 \\ =>\theta =2arccos(s) \end{gathered}$$
轴角绕单位轴 $\mathbf{u}$ 旋转 $\theta$ 的四元数是：

至于旋转轴，如果在四元数转换的时候用$\mathbf{q}$的虚部代替$\mathbf{p}$，易知$\mathbf{q}$的虚部组成的向量在旋转时是不动的，即构成旋转轴。于是只要将它除以它的模长，即得。
$\mathbf{u}=[n_x,n_y,n_z{]}^T=[q_1,q_2,q_3{]}^T/sin\frac{\theta }{2}$

所以，轴角绕单位轴 $\mathbf{u}$ 旋转 $\theta$ 的四元数是：

$\mathbf{q}(w,\mathbf{v})=\left(\cos \frac{\theta }{2},\mathbf{u}\sin \frac{\theta }{2}\right)$

3.3. 轴角->旋转矩阵

罗德里格斯形式旋转角（轴角）使用一个单位旋转轴k和绕轴旋转的角度$\theta$（正方向右手定则）来描述

如上图，我们可以知道，向量 $v$ 在 $k$ 的作用下其实只旋转了垂直于旋转轴的分量，我们将 $v$ 做分解，有
$$v=v_{\Vert }+v_{\perp }$$
其中平行分量使用投影和旋转轴表示为：（$v\cdot k$等价于将v向量在k向量上的投影（$|v|\ast |k|\ast cos\theta$），k是一个长度为1的旋转轴）
$$v_{\Vert }=(v\cdot k)k$$
垂直分量表示为($-$表示方向相反)
$$v_{\perp }=v-v_{\Vert }=v-(v\cdot k)k=-k\times (k\times v)$$
平行于轴的分量在旋转时不会改变幅度和方向（如图所示）,
$$v_{\Vert rot}=v_{\Vert }$$
垂直于轴的分量只会改变方向，但不会改变大小
$$\begin{array}{c}\left|v_{\perp rot}\right|=\left|v_{\perp }\right|\\ v_{\perp }=\cos \theta v_{\perp }+\sin \theta k\times v_{\perp }\end{array}$$
代入分解式
$$k\times v_{\perp }=k\times \left(v-v_{\Vert }\right)=k\times v-k\times v_{\Vert }=k\times v$$
因此上式可以修改为
$$v_{\perp }=\cos \theta v_{\perp }+\sin \theta k\times v$$
则
$$\begin{array}{rl}{\mathbf{v}}_{\mathrm{rot}}& ={\mathbf{v}}_{\Vert }+\cos (\theta ){\mathbf{v}}_{\perp }+\sin (\theta )\mathbf{k}\times \mathbf{v}\\ & ={\mathbf{v}}_{\Vert }+\cos (\theta )\left(\mathbf{v}-{\mathbf{v}}_{\Vert }\right)+\sin (\theta )\mathbf{k}\times \mathbf{v}\\ & =\cos (\theta )\mathbf{v}+(1-\cos \theta ){\mathbf{v}}_{\Vert }+\sin (\theta )\mathbf{k}\times \mathbf{v}\\ & =\cos (\theta )\mathbf{v}+(1-\cos \theta )(\mathbf{k}\cdot \mathbf{v})\mathbf{k}+\sin (\theta )\mathbf{k}\times \mathbf{v}\end{array}$$
我们将叉乘部分按照矩阵分量形式表示出来 [ ( k × v ) x ( k × v ) y ( k × v ) z ] = [ k y v z − k z v y k z v x − k x v z k x v y − k y v x ] = [ 0 − k z k y k z 0 − k x − k y k x 0 ] [ v x v y v z ] \left[\begin{array}{c} (\mathbf{k} \times \mathbf{v})_{x} \\ (\mathbf{k} \times \mathbf{v})_{y} \\ (\mathbf{k} \times \mathbf{v})_{z} \end{array}\right]=\left[\begin{array}{l} k_{y} v_{z}-k_{z} v_{y} \\ k_{z} v_{x}-k_{x} v_{z} \\ k_{x} v_{y}-k_{y} v_{x} \end{array}\right]=\left[\begin{array}{ccc} 0 & -k_{z} & k_{y} \\ k_{z} & 0 & -k_{x} \\ -k_{y} & k_{x} & 0 \end{array}\right]\left[\begin{array}{l} v_{x} \\ v_{y} \\ v_{z} \end{array}\right] ​(k×v)x​(k×v)y​(k×v)z​​ ​= ​ky​vz​−kz​vy​kz​vx​−kx​vz​kx​vy​−ky​vx​​ ​= ​0kz​−ky​​−kz​0kx​​ky​−kx​0​ ​ ​vx​vy​vz​​ ​
为了表达方便，我们定义一个叉乘矩阵 $\mathbf{K}$
K = [ 0 − k z k y k z 0 − k x − k y k x 0 ] \mathbf{K}=\left[\begin{array}{ccc} 0 & -k_{z} & k_{y} \\ k_{z} & 0 & -k_{x} \\ -k_{y} & k_{x} & 0 \end{array}\right] K= ​0kz​−ky​​−kz​0kx​​ky​−kx​0​ ​
将叉乘简化为
$$\mathbf{Kv}=\mathbf{k}\times \mathbf{v}$$
由于 $k$ 为单位向量，因此有单位 2-范数
$$\Vert K{\Vert }_2=1$$
我们回到之前的 $v_{rot}$ 表达式上
$$\begin{array}{rl}{\mathbf{v}}_{\text{rot }}& =\mathbf{v}\cos \theta +(\mathbf{k}\times \mathbf{v})\sin \theta +\mathbf{k}(\mathbf{k}\cdot \mathbf{v})(1-\cos \theta )\\ & =\mathbf{v}\cos \theta +(\mathbf{k}\times \mathbf{v})\sin \theta +\left(\mathbf{v}-{\mathbf{v}}_{\perp }\right)(1-\cos \theta )\\ & =\mathbf{v}\cos \theta +(\mathbf{k}\times \mathbf{v})\sin \theta +(\mathbf{v}+\mathbf{k}\times (\mathbf{k}\times \mathbf{v}))(1-\cos \theta )\\ & =\mathbf{v}\cos \theta +(\mathbf{k}\times \mathbf{v})\sin \theta +(\mathbf{v}+\mathbf{K}(\mathbf{Kv}))(1-\cos \theta )\\ & =\mathbf{v}(\cos \theta +1-\cos \theta )+(\mathbf{k}\times \mathbf{v})\sin \theta +\mathbf{K}(\mathbf{Kv})(1-\cos \theta )\end{array}$$
这里我们就得到了
$${\mathbf{v}}_{\mathrm{rot}}=\mathbf{v}+(\sin \theta )\mathbf{Kv}+(1-\cos \theta ){\mathbf{K}}^2\mathbf{v},\Vert \mathbf{K}{\Vert }_2=1$$
那么旋转矩阵就可以使用上式得到（${\mathbf{v}}_{\mathrm{rot}}=Rv=>R$）
$$\mathbf{R}=\mathbf{I}+\sin \theta \mathbf{K}+(1-\cos \theta ){\mathbf{K}}^2$$
展开之后有
R ( k , θ ) = [ cos ⁡ θ + k x 2 ( 1 − cos ⁡ θ ) − sin ⁡ θ k z + ( 1 − cos ⁡ θ ) k x k y sin ⁡ θ k y + ( 1 − cos ⁡ θ ) k x k z sin ⁡ θ k z + ( 1 − cos ⁡ θ ) k x k y cos ⁡ θ + k y 2 ( 1 − cos ⁡ θ ) − sin ⁡ θ k x + ( 1 − cos ⁡ θ ) k y k z − sin ⁡ θ k y + ( 1 − cos ⁡ θ ) k x k z sin ⁡ θ k x + ( 1 − cos ⁡ θ ) k y k x cos ⁡ θ + k z 2 ( 1 − cos ⁡ θ ) ] \mathbf{R}(k, \theta)=\left[\begin{array}{ccc} \cos \theta+k_{x}^{2}(1-\cos \theta) & -\sin \theta k_{z}+(1-\cos \theta) k_{x} k_{y} & \sin \theta k_{y}+(1-\cos \theta) k_{x} k_{z} \\ \sin \theta k_{z}+(1-\cos \theta) k_{x} k_{y} & \cos \theta+k_{y}^{2}(1-\cos \theta) & -\sin \theta k_{x}+(1-\cos \theta) k_{y} k_{z} \\ -\sin \theta k_{y}+(1-\cos \theta) k_{x} k_{z} & \sin \theta k_{x}+(1-\cos \theta) k_{y} k_{x} & \cos \theta+k_{z}^{2}(1-\cos \theta) \end{array}\right] R(k,θ)= ​cosθ+kx2​(1−cosθ)sinθkz​+(1−cosθ)kx​ky​−sinθky​+(1−cosθ)kx​kz​​−sinθkz​+(1−cosθ)kx​ky​cosθ+ky2​(1−cosθ)sinθkx​+(1−cosθ)ky​kx​​sinθky​+(1−cosθ)kx​kz​−sinθkx​+(1−cosθ)ky​kz​cosθ+kz2​(1−cosθ)​ ​

    Eigen::AngleAxisd rot_vec(vec.norm(), vec.normalized());
    Eigen::Matrix3d M = rot_vec.toRotationMatrix();

4. 欧拉角

Eigen::Vector3d eulerAngle(yaw,pitch,roll);

4.1. 欧拉角 -> 四元数

把旋转外参表示为绕ZYX定轴分解得到连乘形式：

$\mathbf{q}={\mathbf{q}}_z(\psi )\otimes {\mathbf{q}}_y(\theta )\otimes {\mathbf{q}}_x(\varphi )$

= [ cos ⁡ ( ψ / 2 ) 0 0 sin ⁡ ( ψ / 2 ) ] [ cos ⁡ ( θ / 2 ) 0 sin ⁡ ( θ / 2 ) 0 ] [ cos ⁡ ( ϕ / 2 ) sin ⁡ ( ϕ / 2 ) 0 0 ] \begin{aligned} =\left[\begin{array}{c}\cos (\psi / 2) \\ 0 \\ 0 \\ \sin (\psi / 2)\end{array}\right]\left[\begin{array}{c}\cos (\theta / 2) \\ 0 \\ \sin (\theta / 2) \\ 0\end{array}\right]\left[\begin{array}{c}\cos (\phi / 2) \\ \sin (\phi / 2) \\ 0 \\ 0\end{array}\right] \\ &\end{aligned} = ​cos(ψ/2)00sin(ψ/2)​ ​ ​cos(θ/2)0sin(θ/2)0​ ​ ​cos(ϕ/2)sin(ϕ/2)00​ ​​​
= [ cos ⁡ ( ϕ / 2 ) cos ⁡ ( θ / 2 ) cos ⁡ ( ψ / 2 ) + sin ⁡ ( ϕ / 2 ) sin ⁡ ( θ / 2 ) sin ⁡ ( ψ / 2 ) sin ⁡ ( ϕ / 2 ) cos ⁡ ( θ / 2 ) cos ⁡ ( ψ / 2 ) − cos ⁡ ( ϕ / 2 ) sin ⁡ ( θ / 2 ) sin ⁡ ( ψ / 2 ) cos ⁡ ( ϕ / 2 ) sin ⁡ ( θ / 2 ) cos ⁡ ( ψ / 2 ) + sin ⁡ ( ϕ / 2 ) cos ⁡ ( θ / 2 ) sin ⁡ ( ψ / 2 ) cos ⁡ ( ϕ / 2 ) cos ⁡ ( θ / 2 ) sin ⁡ ( ψ / 2 ) − sin ⁡ ( ϕ / 2 ) sin ⁡ ( θ / 2 ) cos ⁡ ( ψ / 2 ) ] \begin{aligned}=\left[\begin{array}{l}\cos (\phi / 2) \cos (\theta / 2) \cos (\psi / 2)+\sin (\phi / 2) \sin (\theta / 2) \sin (\psi / 2) \\ \sin (\phi / 2) \cos (\theta / 2) \cos (\psi / 2)-\cos (\phi / 2) \sin (\theta / 2) \sin (\psi / 2) \\ \cos (\phi / 2) \sin (\theta / 2) \cos (\psi / 2)+\sin (\phi / 2) \cos (\theta / 2) \sin (\psi / 2) \\ \cos (\phi / 2) \cos (\theta / 2) \sin (\psi / 2)-\sin (\phi / 2) \sin (\theta / 2) \cos (\psi / 2)\end{array}\right] \end{aligned} = ​cos(ϕ/2)cos(θ/2)cos(ψ/2)+sin(ϕ/2)sin(θ/2)sin(ψ/2)sin(ϕ/2)cos(θ/2)cos(ψ/2)−cos(ϕ/2)sin(θ/2)sin(ψ/2)cos(ϕ/2)sin(θ/2)cos(ψ/2)+sin(ϕ/2)cos(θ/2)sin(ψ/2)cos(ϕ/2)cos(θ/2)sin(ψ/2)−sin(ϕ/2)sin(θ/2)cos(ψ/2)​ ​​

	Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ());
	Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());
	Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX());
	Eigen::Quaternion<double> q = yawAngle * pitchAngle * rollAngle;

4.2. 欧拉角 -> 旋转矩阵

R x ( θ ) = [ 1 0 0 0 cos ⁡ θ − sin ⁡ θ 0 sin ⁡ θ cos ⁡ θ ] R_{x}(\theta)=\left[\begin{array}{ccc}1 & 0 & 0 \\ 0 & \cos \theta & -\sin \theta \\ 0 & \sin \theta & \cos \theta\end{array}\right] Rx​(θ)= ​100​0cosθsinθ​0−sinθcosθ​ ​

R y ( θ ) = [ cos ⁡ θ 0 sin ⁡ θ 0 1 0 − sin ⁡ θ 0 cos ⁡ θ ] R_{y}(\theta)=\left[\begin{array}{ccc}\cos \theta & 0 & \sin \theta \\ 0 & 1 & 0 \\ -\sin \theta & 0 & \cos \theta\end{array}\right] Ry​(θ)= ​cosθ0−sinθ​010​sinθ0cosθ​ ​

R z ( θ ) = [ cos ⁡ θ − sin ⁡ θ 0 sin ⁡ θ cos ⁡ θ 0 0 0 1 ] R_{z}(\theta)=\left[\begin{array}{ccc}\cos \theta & -\sin \theta & 0 \\ \sin \theta & \cos \theta & 0 \\ 0 & 0 & 1 \end{array}\right] Rz​(θ)= ​cosθsinθ0​−sinθcosθ0​001​ ​

所以，欧拉角转旋转矩阵如下：

$R=R_z(\varphi )R_y(\theta )R_x(\psi )=$

[ cos ⁡ θ cos ⁡ ϕ sin ⁡ ψ sin ⁡ θ cos ⁡ ϕ − cos ⁡ ψ sin ⁡ ϕ cos ⁡ ψ sin ⁡ θ cos ⁡ ϕ + sin ⁡ ψ sin ⁡ ϕ cos ⁡ θ sin ⁡ ϕ sin ⁡ ψ sin ⁡ θ sin ⁡ ϕ + cos ⁡ ψ cos ⁡ ϕ cos ⁡ ψ sin ⁡ θ sin ⁡ ϕ − sin ⁡ ψ cos ⁡ ϕ − sin ⁡ θ sin ⁡ ψ cos ⁡ θ cos ⁡ ψ cos ⁡ θ ] \left[\begin{array}{ccc}\cos \theta \cos \phi & \sin \psi \sin \theta \cos \phi-\cos \psi \sin \phi & \cos \psi \sin \theta \cos \phi+\sin \psi \sin \phi \\ \cos \theta \sin \phi & \sin \psi \sin \theta \sin \phi+\cos \psi \cos \phi & \cos \psi \sin \theta \sin \phi-\sin \psi \cos \phi \\ -\sin \theta & \sin \psi \cos \theta & \cos \psi \cos \theta\end{array}\right] ​cosθcosϕcosθsinϕ−sinθ​sinψsinθcosϕ−cosψsinϕsinψsinθsinϕ+cosψcosϕsinψcosθ​cosψsinθcosϕ+sinψsinϕcosψsinθsinϕ−sinψcosϕcosψcosθ​ ​

附录： 其他转换顺序的欧拉角转旋转矩阵

	Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ());
	Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());
	Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX());
	Eigen::Quaternion<double> q = yawAngle * pitchAngle * rollAngle;
	Eigen::Matrix3d rotationMatrix = q.matrix();

也可以

	Eigen::Matrix3d Rx_angle(double angle)
	{
	   Eigen::Matrix3d R;
	   R << 1,0,0,0, cos(angle),-sin(angle),0,sin(angle),cos(angle);
	   return R;
	}
	
	Eigen::Matrix3d Ry_angle(double angle)
	{
	   Eigen::Matrix3d R;
	   R << cos(angle),0,sin(angle),0,1,0,-sin(angle),0,cos(angle);
	   return R;
	}
	
	Eigen::Matrix3d Rz_angle(double angle)
	{
	   Eigen::Matrix3d R;
	   R << cos(angle),-sin(angle),0,sin(angle),cos(angle),0,0,0,1;
	   return R;
	}
	Eigen::Matrix3d R_angle_ = Rz_angle(angle[i][0]/ 180 *M_PI) * Ry_angle(angle[i][1]/ 180 *M_PI) * Rx_angle(angle[i][2]/ 180 *M_PI);

• 验证$R_x(\theta ),R_y(\theta ),R_z(\theta )$
  我们首先假定坐标系为常用的右手坐标系：
  
  那么$R_x(\theta )$就表示为从y轴旋转到z轴（明确逆时针为正，判断顺时针还是逆时针需要把轴朝向自己后再判断）

对应到下图，x轴相当于y轴，y轴相当与z轴。

我们可以建立公式：
$$\begin{array}{l}{x}^{\prime }=rcos(\theta +\varphi )=r\cos (\varphi )\cos (\theta )-r\sin (\varphi )\sin (\theta )=x\cos (\theta )-y\sin (\theta )\\ y^{\prime }=rsin(\theta +\varphi )=r\sin (\varphi )\cos (\theta )+r\cos (\varphi )\sin (\theta )=x\sin (\theta )+y\cos (\theta )\end{array}$$
矩阵形式为:
$$R_x(\theta )=\left[\begin{array}{l}{x}^{\prime }\\ y^{\prime }\end{array}\right]=\left[\begin{array}{cc}\cos (\theta )& -\sin (\theta )\\ \sin (\theta )& \cos (\theta )\end{array}\right]\left[\begin{array}{l}x\\ y\end{array}\right]$$

R x ( θ ) = [ 1 0 0 0 cos ⁡ ( θ ) − sin ⁡ ( θ ) 0 sin ⁡ ( θ ) cos ⁡ ( θ ) ] [ x y z ] R_{x}(\theta)=\left[\begin{array}{cc} 1&0&0\\ 0&\cos (\theta) & -\sin (\theta) \\ 0&\sin (\theta) & \cos (\theta) \end{array}\right]\left[\begin{array}{l} x \\ y \\ z \end{array}\right] Rx​(θ)= ​100​0cos(θ)sin(θ)​0−sin(θ)cos(θ)​ ​ ​xyz​ ​

$R_y(\theta )$就表示为从z轴旋转到x轴

对应到下图，x轴相当于z轴，y轴相当与x轴。

矩阵形式为:
$$R_x(\theta )=\left[\begin{array}{l}{x}^{\prime }\\ y^{\prime }\end{array}\right]=\left[\begin{array}{cc}\cos (\theta )& -\sin (\theta )\\ \sin (\theta )& \cos (\theta )\end{array}\right]\left[\begin{array}{l}x\\ y\end{array}\right]$$
明确：二维的x对应三维的z，二维的y对应三维的x

R y ( θ ) = [ cos ⁡ ( θ ) 0 sin ⁡ ( θ ) 0 1 0 − sin ⁡ ( θ ) 0 cos ⁡ ( θ ) ] [ x y z ] R_{y}(\theta)=\left[\begin{array}{cc} \cos (\theta)&0&\sin (\theta)\\ 0& 1& 0 \\ -\sin (\theta) &0& \cos (\theta) \end{array}\right]\left[\begin{array}{l} x \\ y \\ z \end{array}\right] Ry​(θ)= ​cos(θ)0−sin(θ)​010​sin(θ)0cos(θ)​ ​ ​xyz​ ​
$R_z(\theta )$同理

4.3. 欧拉角 -> 轴角

 Eigen::AngleAxisd angel_axisd =
      Eigen::AngleAxisd(yaw角（弧度制） , Eigen::Vector3d::UnitZ()) *
      Eigen::AngleAxisd(pitch角（弧度制）], Eigen::Vector3d::UnitY()) *
      Eigen::AngleAxisd(roll角（弧度制）], Eigen::Vector3d::UnitX());

5. 四元数

5.1. 单位四元数 -> 旋转矩阵

假设有单位四元数$\mathbf{q}=q_0+q_1\mathbf{i}+q_2\mathbf{j}+q_3\mathbf{k}=s+\mathbf{v}$,三维点$\mathbf{p}$经过旋转之后变为${\mathbf{p}}^{\prime }$,如果使用矩阵描述，那么有${\mathbf{p}}^{\prime }=\mathbf{Rp}$ 。 如果用四元数描述旋转，则表示为${\mathbf{p}}^{\prime }=\mathbf{qp}{\mathbf{q}}^{-1}$。

$${\mathbf{p}}^{\prime }=\mathbf{qp}{\mathbf{q}}^{-1}=\mathcal{L}(\mathbf{q})\mathcal{R}({\mathbf{q}}^{-1})\mathbf{p}=\mathbf{Rp}$$

${\mathbf{q}}^{-1}={\mathbf{q}}^{\ast }/||\mathbf{q}|{|}^2$，所以单位四元数的逆即为${\mathbf{q}}^{\ast }$

$$\begin{gathered} \mathcal{L}(\mathbf{q})\mathcal{R}({\mathbf{q}}^{\ast })=\left[\begin{array}{cc}s& -{\mathbf{v}}^T\\ \mathbf{v}& s{\mathbf{I}}_{3\times 3}+[\mathbf{v}\times ]\end{array}\right]\left[\begin{array}{cc}s& {\mathbf{v}}^T\\ -\mathbf{v}& s{\mathbf{I}}_{3\times 3}+[\mathbf{v}\times ]\end{array}\right] \\ =\left[\begin{array}{cc}a& b\\ c& \mathbf{v}{\mathbf{v}}^T+s^2\mathbf{I}+2s[\mathbf{v}\times ]+(\mathbf{v}\times {)}^2\end{array}\right] \end{gathered}$$
因为$\mathbf{p}$和${\mathbf{p}}^{\prime }$都是虚四元数，所以该矩阵的右下角即为从四元数到旋转矩阵的变换关系：
R = v v T + s 2 I + 2 s [ v × ] + ( v × ) 2 = [ 1 − 2 q 2 2 − 2 q 3 2 2 q 1 q 2 − 2 q 3 q 0 2 q 1 q 3 + 2 q 2 q 0 2 q 1 q 2 + 2 q 3 q 0 1 − 2 q 1 2 − 2 q 3 2 2 q 2 q 3 − 2 q 1 q 0 2 q 1 q 3 − 2 q 2 q 0 2 q 2 q 3 + 2 q 1 q 0 1 − 2 q 1 2 − 2 q 2 2 ] R= \mathbf v \mathbf v^T + s^2 \mathbf I +2s [\mathbf v\times] +(\mathbf v\times)^2 \\= \left[\begin{array}{ccc} 1-2 q_{2}^{2}-2 q_{3}^{2} && 2 q_{1} q_{2}-2 q_{3} q_{0} && 2 q_{1} q_{3}+2 q_{2} q_{0} \\ 2 q_{1} q_{2}+2 q_{3} q_{0} && 1-2 q_{1}^{2}-2 q_{3}^{2} && 2 q_{2} q_{3}-2 q_{1} q_{0} \\ 2 q_{1} q_{3}-2 q_{2} q_{0} && 2 q_{2} q_{3}+2 q_{1} q_{0} && 1-2 q_{1}^{2}-2 q_{2}^{2} \end{array}\right] R=vvT+s2I+2s[v×]+(v×)2= ​1−2q22​−2q32​2q1​q2​+2q3​q0​2q1​q3​−2q2​q0​​​2q1​q2​−2q3​q0​1−2q12​−2q32​2q2​q3​+2q1​q0​​​2q1​q3​+2q2​q0​2q2​q3​−2q1​q0​1−2q12​−2q22​​ ​
其中$\mathbf{q}=q_0+q_1\mathbf{i}+q_2\mathbf{j}+q_3\mathbf{k}$

	Eigen::Matrix3d rotation_matrix;
	rotation_matrix=quaternion.matrix();

5.2. 四元数 -> 欧拉角

$\mathbf{q}={\mathbf{q}}_z(\psi )\otimes {\mathbf{q}}_y(\theta )\otimes {\mathbf{q}}_x(\varphi )$

= [ cos ⁡ ( ψ / 2 ) 0 0 sin ⁡ ( ψ / 2 ) ] [ cos ⁡ ( θ / 2 ) 0 sin ⁡ ( θ / 2 ) 0 ] [ cos ⁡ ( ϕ / 2 ) sin ⁡ ( ϕ / 2 ) 0 0 ] \begin{aligned} =\left[\begin{array}{c}\cos (\psi / 2) \\ 0 \\ 0 \\ \sin (\psi / 2)\end{array}\right]\left[\begin{array}{c}\cos (\theta / 2) \\ 0 \\ \sin (\theta / 2) \\ 0\end{array}\right]\left[\begin{array}{c}\cos (\phi / 2) \\ \sin (\phi / 2) \\ 0 \\ 0\end{array}\right] \\ &\end{aligned} = ​cos(ψ/2)00sin(ψ/2)​ ​ ​cos(θ/2)0sin(θ/2)0​ ​ ​cos(ϕ/2)sin(ϕ/2)00​ ​​​
= [ cos ⁡ ( ϕ / 2 ) cos ⁡ ( θ / 2 ) cos ⁡ ( ψ / 2 ) + sin ⁡ ( ϕ / 2 ) sin ⁡ ( θ / 2 ) sin ⁡ ( ψ / 2 ) sin ⁡ ( ϕ / 2 ) cos ⁡ ( θ / 2 ) cos ⁡ ( ψ / 2 ) − cos ⁡ ( ϕ / 2 ) sin ⁡ ( θ / 2 ) sin ⁡ ( ψ / 2 ) cos ⁡ ( ϕ / 2 ) sin ⁡ ( θ / 2 ) cos ⁡ ( ψ / 2 ) + sin ⁡ ( ϕ / 2 ) cos ⁡ ( θ / 2 ) sin ⁡ ( ψ / 2 ) cos ⁡ ( ϕ / 2 ) cos ⁡ ( θ / 2 ) sin ⁡ ( ψ / 2 ) − sin ⁡ ( ϕ / 2 ) sin ⁡ ( θ / 2 ) cos ⁡ ( ψ / 2 ) ] \begin{aligned}=\left[\begin{array}{l}\cos (\phi / 2) \cos (\theta / 2) \cos (\psi / 2)+\sin (\phi / 2) \sin (\theta / 2) \sin (\psi / 2) \\ \sin (\phi / 2) \cos (\theta / 2) \cos (\psi / 2)-\cos (\phi / 2) \sin (\theta / 2) \sin (\psi / 2) \\ \cos (\phi / 2) \sin (\theta / 2) \cos (\psi / 2)+\sin (\phi / 2) \cos (\theta / 2) \sin (\psi / 2) \\ \cos (\phi / 2) \cos (\theta / 2) \sin (\psi / 2)-\sin (\phi / 2) \sin (\theta / 2) \cos (\psi / 2)\end{array}\right] \end{aligned} = ​cos(ϕ/2)cos(θ/2)cos(ψ/2)+sin(ϕ/2)sin(θ/2)sin(ψ/2)sin(ϕ/2)cos(θ/2)cos(ψ/2)−cos(ϕ/2)sin(θ/2)sin(ψ/2)cos(ϕ/2)sin(θ/2)cos(ψ/2)+sin(ϕ/2)cos(θ/2)sin(ψ/2)cos(ϕ/2)cos(θ/2)sin(ψ/2)−sin(ϕ/2)sin(θ/2)cos(ψ/2)​ ​​

则有
[ ϕ θ ψ ] = [ arctan ⁡ 2 ( q 0 q 1 + q 2 q 3 ) 1 − 2 ( q 1 2 + q 2 2 ) arcsin ⁡ ( 2 ( q 0 q 2 − q 3 q 1 ) ) arctan ⁡ 2 ( q 0 q 3 + q 1 q 2 ) 1 − 2 ( q 2 2 + q 3 2 ) ] \left[\begin{array}{c}\phi \\ \theta \\ \psi\end{array}\right]=\left[\begin{array}{c}\arctan \frac{2\left(q_{0} q_{1}+q_{2} q_{3}\right)}{1-2\left(q_{1}^{2}+q_{2}^{2}\right)} \\ \arcsin \left(2\left(q_{0} q_{2}-q_{3} q_{1}\right)\right) \\ \arctan \frac{2\left(q_{0} q_{3}+q_{1} q_{2}\right)}{1-2\left(q_{2}^{2}+q_{3}^{2}\right)}\end{array}\right] ​ϕθψ​ ​= ​arctan1−2(q12​+q22​)2(q0​q1​+q2​q3​)​arcsin(2(q0​q2​−q3​q1​))arctan1−2(q22​+q32​)2(q0​q3​+q1​q2​)​​ ​

arctan 和arcsin的结果是是 $\left[-\frac{\pi }{2},\frac{\pi }{2}\right]$, 这并不能覆盖所有朝向,因此需要用atan2来代替arctan。

注:

• arctan(y / x)求出的取值范围是[-PI/2, PI/2]。

• atan2(y , x) 求出的θ取值范围是[-PI, PI]。

[ ϕ θ ψ ] = [ atan ⁡ 2 ( 2 ( q 0 q 1 + q 2 q 3 ) , 1 − 2 ( q 1 2 + q 2 2 ) ) asin ⁡ ( 2 ( q 0 q 2 − q 3 q 1 ) ) atan ⁡ 2 ( 2 ( q 0 q 3 + q 1 q 2 ) , 1 − 2 ( q 2 2 + q 3 2 ) ) ] \left[\begin{array}{c}\phi \\ \theta \\ \psi\end{array}\right]=\left[\begin{array}{c}\operatorname{atan} 2\left(2\left(q_{0} q_{1}+q_{2} q_{3}\right), 1-2\left(q_{1}^{2}+q_{2}^{2}\right)\right) \\ \operatorname{asin}\left(2\left(q_{0} q_{2}-q_{3} q_{1}\right)\right) \\ \operatorname{atan} 2\left(2\left(q_{0} q_{3}+q_{1} q_{2}\right), 1-2\left(q_{2}^{2}+q_{3}^{2}\right)\right)\end{array}\right] ​ϕθψ​ ​= ​atan2(2(q0​q1​+q2​q3​),1−2(q12​+q22​))asin(2(q0​q2​−q3​q1​))atan2(2(q0​q3​+q1​q2​),1−2(q22​+q32​))​ ​

// 方法1：
	double roll = atan2(2 * (quaternion.w()* quaternion.x() + quaternion.y() * quaternion.z()) , 1 - 2 * (pow( quaternion.x() + quaternion.y(),2 )));
    double pitch = asin(2 * (quaternion.w()* quaternion.y() -quaternion.z() * quaternion.x()) );
    double yaw = atan2(2 * (quaternion.w()* quaternion.z() + quaternion.x() * quaternion.y()) , 1 - 2 * (pow( quaternion.y() + quaternion.z(),2 )));
// 方法2：
    Eigen::Vector3d eulerAngle=quaternion.matrix().eulerAngles(2,1,0);

5.3. 四元数 -> 轴角

假设有单位四元数$\mathbf{q}=q_0+q_1\mathbf{i}+q_2\mathbf{j}+q_3\mathbf{k}=s+\mathbf{v}$,三维点$\mathbf{p}$经过旋转之后变为${\mathbf{p}}^{\prime }$,如果使用矩阵描述，那么有${\mathbf{p}}^{\prime }=\mathbf{Rp}$ 。 如果用四元数描述旋转，则表示为${\mathbf{p}}^{\prime }=\mathbf{qp}{\mathbf{q}}^{-1}$。

$${\mathbf{p}}^{\prime }=\mathbf{qp}{\mathbf{q}}^{-1}=\mathcal{L}(\mathbf{q})\mathcal{R}({\mathbf{q}}^{-1})\mathbf{p}=\mathbf{Rp}$$

${\mathbf{q}}^{-1}={\mathbf{q}}^{\ast }/||\mathbf{q}|{|}^2$，所以单位四元数的逆即为${\mathbf{q}}^{\ast }$

L ( q ) R ( q ∗ ) = [ s − v T v s I 3 × 3 + [ v × ] ] [ s v T − v s I 3 × 3 + [ v × ] ] = [ a b c v v T + s 2 I + 2 s [ v × ] + (