四元数，旋转矩阵，轴角，欧拉角的相互转换（原理+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 }$

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 × ] + ( v × ) 2 ] \mathcal L(\mathbf q) \mathcal R(\mathbf q^{*})=

$$\begin{bmatrix} s &-\mathbf v^T\\ \mathbf v & s\mathbf I_{3\times3} +[\mathbf v\times]\end{bmatrix}$$ $$\begin{bmatrix} s &\mathbf v^T\\ -\mathbf v & s\mathbf I_{3\times3} +[\mathbf v\times]\end{bmatrix}$$ \\{}\\= $$\begin{bmatrix} a &b\\c & \mathbf v \mathbf v^T+s^2 \mathbf I+2s [\mathbf v\times]+(\mathbf v\times)^2\end{bmatrix}$$ L(q)R(q∗)=[sv​−vTsI3×3​+[v×]​][s−v​vTsI3×3​+[v×]​]=[ac​bvvT+s2I+2s[v×]+(v×)2​]
因为$\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);
1

或者

	Eigen::Quaterniond quaternion;
	quaternion = rotation_matrix;
1
2

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​​ ​=
$\left[\begin{array}{ccc}\cos \theta \cos \varphi & \sin \psi \sin \theta \cos \varphi -\cos \psi \sin \varphi & \cos \psi \sin \theta \cos \varphi +\sin \psi \sin \varphi \\ \cos \theta \sin \varphi & \sin \psi \sin \theta \sin \varphi +\cos \psi \cos \varphi & \cos \psi \sin \theta \sin \varphi -\sin \psi \cos \varphi \\ -\sin \theta & \sin \psi \cos \theta & \cos \psi \cos \theta \end{array}\right]$

则对于绕ZYX定轴得到的旋转矩阵可以如下表示欧拉角：
θ x = atan ⁡ 2 ( r 32 , r 33 ) θ y = atan ⁡ 2 ( − r 31 , r 32 2 + r 33 2 ) θ z = atan ⁡ 2 ( r 21 , r 11 )

$$\begin{array}{c} \theta_{x}=\operatorname{atan} 2\left(r_{32}, r_{33}\right) \\ \theta_{y}=\operatorname{atan} 2\left(-r_{31}, \sqrt{r_{32}^{2}+r_{33}^{2}}\right) \\ \theta_{z}=\operatorname{atan} 2\left(r_{21}, r_{11}\right) \end{array}$$ θx​=atan2(r32​,r33​)θy​=atan2(−r31​,r322​+r332​ ​)θz​=atan2(r21​,r11​)​

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

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​​ ​=
$$\left[\begin{array}{ccc}{r}_x^2(1-\cos \theta )+\cos \theta & r_x{r}_y(1-\cos \theta )-r_{z}sin\theta & r_x{r}_z(1-\cos \theta )+r_{y}sin\theta \\ r_x{r}_y(1-\cos \theta )+r_{z}sin\theta & r_y^2(1-\cos \theta )+\cos \theta & r_y{r}_z(1-\cos \theta )-r_{x}sin\theta \\ r_x{r}_z(1-\cos \theta )-r_{y}sin\theta & r_y{r}_z(1-\cos \theta )+r_{x}sin\theta & r_z^2(1-\cos \theta )+cos\theta \end{array}\right]$$

$$\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);
1

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());
1
2
3
4
5

或

    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());
1
2
3
4

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

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

或

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

2.3. 旋转向量->欧拉角

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

2.4. 旋转向量->四元数

	Eigen::Quaterniond quaternion(rotation_vector);
1

3. 轴角

3.1. 轴角->旋转向量

轴角和旋转向量本质上是一个东西, 轴角用四个元素表达旋转, 其中的三个元素用来描述旋转轴, 另外一个元素描述旋转的角度,如 下所示：
$$r=[x,y,z,\theta ]$$
其中单位向量 $\vec{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 }$

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 × ] + ( v × ) 2 ] \mathcal L(\mathbf q) \mathcal R(\mathbf q^{*})=

$$\begin{bmatrix} s &-\mathbf v^T\\ \mathbf v & s\mathbf I_{3\times3} +[\mathbf v\times]\end{bmatrix}$$ $$\begin{bmatrix} s &\mathbf v^T\\ -\mathbf v & s\mathbf I_{3\times3} +[\mathbf v\times]\end{bmatrix}$$ \\{}\\= $$\begin{bmatrix} a &b\\c & \mathbf v \mathbf v^T+s^2 \mathbf I+2s [\mathbf v\times]+(\mathbf v\times)^2\end{bmatrix}$$ L(q)R(q∗)=[sv​−vTsI3×3​+[v×]​][s−v​vTsI3×3​+[v×]​]=[ac​bvvT+s2I+2s[v×]+(v×)2​]
因为$\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​​ ​
$(\mathbf{v}\times {)}^2=\left[\begin{array}{ccc}0& -q_3& q_2\\ q_3& 0& -q_1\\ -q_2& q_1& 0\end{array}\right]\left[\begin{array}{ccc}0& -q_3& q_2\\ q_3& 0& -q_1\\ -q_2& q_1& 0\end{array}\right]=\left[\begin{array}{ccc}-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\end{array}\right]$

又有了罗德里格斯公式可以得到旋转向量到旋转矩阵的转换关系：
$$\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 }$$
垂直于轴的分量只会改变方向，但不会改变大小
∣ v ⊥ r o t ∣ = ∣ v ⊥ ∣ v ⊥ = cos ⁡ θ v ⊥ + sin ⁡ θ k × v ⊥

$$\begin{gathered} \left|v_{\perp r o t}\right|=\left|v_{\perp}\right| \\ v_{\perp}=\cos \theta v_{\perp}+\sin \theta k \times v_{\perp} \end{gathered}$$ ∣v⊥rot​∣=∣v⊥​∣v⊥​=cosθv⊥​+sinθk×v⊥​​
代入分解式
$$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}$$
我们将叉乘部分按照矩阵分量形式表示出来$$\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]$$
为了表达方便，我们定义一个叉乘矩阵 $\mathbf{K}$
$$\mathbf{K}=\left[\begin{array}{ccc}0& -k_z& k_y\\ k_z& 0& -k_x\\ -k_y& k_x& 0\end{array}\right]$$
将叉乘简化为
$$\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$$
展开之后有
$$\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]$$

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

4. 欧拉角

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

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​ ​​​
$\begin{array}{r}=\left[\begin{array}{l}\cos (\varphi /2)\cos (\theta /2)\cos (\psi /2)+\sin (\varphi /2)\sin (\theta /2)\sin (\psi /2)\\ \sin (\varphi /2)\cos (\theta /2)\cos (\psi /2)-\cos (\varphi /2)\sin (\theta /2)\sin (\psi /2)\\ \cos (\varphi /2)\sin (\theta /2)\cos (\psi /2)+\sin (\varphi /2)\cos (\theta /2)\sin (\psi /2)\\ \cos (\varphi /2)\cos (\theta /2)\sin (\psi /2)-\sin (\varphi /2)\sin (\theta /2)\cos (\psi /2)\end{array}\right]\end{array}$

	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;
1
2
3
4

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();
1
2
3
4
5

也可以

	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);
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

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

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

我们可以建立公式：
x ′ = r c o s ( θ + ϕ ) = r cos ⁡ ( ϕ ) cos ⁡ ( θ ) − r sin ⁡ ( ϕ ) sin ⁡ ( θ ) = x cos ⁡ ( θ ) − y sin ⁡ ( θ ) y ′ = r s i n ( θ + ϕ ) = r sin ⁡ ( ϕ ) cos ⁡ ( θ ) + r cos ⁡ ( ϕ ) sin ⁡ ( θ ) = x sin ⁡ ( θ ) + y cos ⁡ ( θ )

$$\begin{array}{l} x^{\prime}=rcos(\theta + \phi )=r \cos (\phi) \cos (\theta)-r \sin (\phi) \sin (\theta)=x \cos (\theta)-y \sin (\theta) \\ y^{\prime}=rsin(\theta + \phi )=r \sin (\phi) \cos (\theta)+r \cos (\phi) \sin (\theta)=x \sin (\theta)+y \cos (\theta) \end{array}$$ x′=rcos(θ+ϕ)=rcos(ϕ)cos(θ)−rsin(ϕ)sin(θ)=xcos(θ)−ysin(θ)y′=rsin(θ+ϕ)=rsin(ϕ)cos(θ)+rcos(ϕ)sin(θ)=xsin(θ)+ycos(θ)​
矩阵形式为:
$$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 ( θ ) = [ x ′ y ′ ] = [ cos ⁡ ( θ ) − sin ⁡ ( θ ) sin ⁡ ( θ ) cos ⁡ ( θ ) ] [ x y ] 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] Rx​(θ)=[x′y′​]=[cos(θ)sin(θ)​−sin(θ)cos(θ)​][xy​]
明确：二维的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());
1
2
3
4

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 }$

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 × ] + ( v × ) 2 ] \mathcal L(\mathbf q) \mathcal R(\mathbf q^{*})=

$$\begin{bmatrix} s &-\mathbf v^T\\ \mathbf v & s\mathbf I_{3\times3} +[\mathbf v\times]\end{bmatrix}$$ $$\begin{bmatrix} s &\mathbf v^T\\ -\mathbf v & s\mathbf I_{3\times3} +[\mathbf v\times]\end{bmatrix}$$ \\{}\\= $$\begin{bmatrix} a &b\\c & \mathbf v \mathbf v^T+s^2 \mathbf I+2s [\mathbf v\times]+(\mathbf v\times)^2\end{bmatrix}$$ L(q)R(q∗)=[sv​−vTsI3×3​+[v×]​][s−v​vTsI3×3​+[v×]​]=[ac​bvvT+s2I+2s[v×]+(v×)2​]
因为$\mathbf{p}$和${\mathbf{p}}^{\prime }$都是虚四元数，所以该矩阵的右下角即为从四元数到旋转矩阵的变换关系：
$$\begin{gathered} R=\mathbf{v}{\mathbf{v}}^T+s^2\mathbf{I}+2s[\mathbf{v}\times ]+(\mathbf{v}\times {)}^2 \\ =\left[\begin{array}{ccccc}1-2q_2^2-2q_3^2& & 2q_1{q}_2-2q_3{q}_0& & 2q_1{q}_3+2q_2{q}_0\\ 2q_1{q}_2+2q_3{q}_0& & 1-2q_1^2-2q_3^2& & 2q_2{q}_3-2q_1{q}_0\\ 2q_1{q}_3-2q_2{q}_0& & 2q_2{q}_3+2q_1{q}_0& & 1-2q_1^2-2q_2^2\end{array}\right] \end{gathered}$$
其中$\mathbf{q}=q_0+q_1\mathbf{i}+q_2\mathbf{j}+q_3\mathbf{k}$

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

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​ ​​​
$\begin{array}{r}=\left[\begin{array}{l}\cos (\varphi /2)\cos (\theta /2)\cos (\psi /2)+\sin (\varphi /2)\sin (\theta /2)\sin (\psi /2)\\ \sin (\varphi /2)\cos (\theta /2)\cos (\psi /2)-\cos (\varphi /2)\sin (\theta /2)\sin (\psi /2)\\ \cos (\varphi /2)\sin (\theta /2)\cos (\psi /2)+\sin (\varphi /2)\cos (\theta /2)\sin (\psi /2)\\ \cos (\varphi /2)\cos (\theta /2)\sin (\psi /2)-\sin (\varphi /2)\sin (\theta /2)\cos (\psi /2)\end{array}\right]\end{array}$

则有
[ ϕ θ ψ ] = [ 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);
1
2
3
4
5
6

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 × ] + ( v × ) 2 ] \mathcal L(\mathbf q) \mathcal R(\mathbf q^{*})=

$$\begin{bmatrix} s &-\mathbf v^T\\ \mathbf v & s\mathbf I_{3\times3} +[\mathbf v\times]\end{bmatrix}$$ $$\begin{bmatrix} s &\mathbf v^T\\ -\mathbf v & s\mathbf I_{3\times3} +[\mathbf v\times]\end{bmatrix}$$ \\{}\\= $$\begin{bmatrix} a &b\\c & \mathbf v \mathbf v^T+s^2 \mathbf I+2s [\mathbf v\times]+(\mathbf v\times)^2\end{bmatrix}$$ L(q)R(q∗)=[sv​−vTsI3×3​+[v×]​][s−v​vTsI3×3​+[v×]​]=[ac​bvvT+s2I+2s[v×]+(v×)2​]
因为$\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​​ ​
$(\mathbf{v}\times {)}^2=\left[\begin{array}{ccc}0& -q_3& q_2\\ q_3& 0& -q_1\\ -q_2& q_1& 0\end{array}\right]\left[\begin{array}{ccc}0& -q_3& q_2\\ q_3& 0& -q_1\\ -q_2& q_1& 0\end{array}\right]=\left[\begin{array}{ccc}-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\end{array}\right]$

又有了罗德里格斯公式可以得到旋转向量到旋转矩阵的转换关系：
$$\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{q}(w,\mathbf{v})=\left(\cos \frac{\theta }{2},\mathbf{u}\sin \frac{\theta }{2}\right)$

	// 方法1
    double sin_theta =
            sqrt(quaternion.x() * quaternion.x() + quaternion.y() * quaternion.y() + quaternion.z() * quaternion.z());
    double cos_theta = quaternion.w();

    double rotation_angle_rad1 = ((cos_theta < 0.0) ? atan2(-sin_theta, -cos_theta) : atan2(sin_theta, cos_theta));
    std::cout << " rotation_angle_rad :" <<  2 * rotation_angle_rad1 << std::endl;
    std::cout << " rotation_angle_degree :" <<  2 * rotation_angle_rad1  * 180.0 / M_PI << std::endl;

    // 方法2
    double rotation_angle_rad2 = 2 * acos(quaternion.w());
    std::cout << " rotation_angle_rad2 :" <<  rotation_angle_rad2 << std::endl;
    Eigen::Vector3d rotation_axis = quaternion.vec() / sin(rotation_angle_rad2 / 2);

    std::cout << "roatation_axis :" << rotation_axis.transpose() << endl;

    // 方法3
    Eigen::AngleAxisd rotation_vector(quaternion);
    std::cout << "rotation_vector.angle() : " << rotation_vector.angle()  << endl;
    std::cout << "rotation_vector.axis() : " << rotation_vector.axis().transpose() << endl;
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20