四元数,旋转矩阵,轴角,欧拉角的相互转换(原理+Eigen代码实现)

欧拉角和四元数的相互转换 qwxyzθϕψ。 阅读详情
AI摘要

该博客聚焦于旋转矩阵、旋转向量、轴角、欧拉角和四元数之间的相互转换。详细阐述了从旋转矩阵到四元数、欧拉角、轴角的转换公式,以及旋转向量与其他形式的转换关系等,为相关领域的计算和应用提供了理论依据。


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

1. 旋转矩阵

1.1. 旋转矩阵 -> 四元数

假设有单位四元数 q = q 0 + q 1 i + q 2 j + q 3 k = s + v \mathbf q= q_0 + q_1\mathbf i + q_2\mathbf j + q_3\mathbf k =s+ \mathbf v q=q0​+q1​i+q2​j+q3​k=s+v,三维点 p \mathbf p p经过旋转之后变为 p ′ \mathbf p' p′,如果使用矩阵描述,那么有 p ′ = R p \mathbf p'=\mathbf R \mathbf p p′=Rp 。 如果用四元数描述旋转,则表示为 p ′ = q p q − 1 \mathbf p'=\mathbf q \mathbf p\mathbf q^{-1} p′=qpq−1。

p ′ = q p q − 1 = L ( q ) R ( q − 1 ) p = R p \mathbf p'=\mathbf q \mathbf p\mathbf q^{-1}=\mathcal L(\mathbf q) \mathcal R(\mathbf q^{-1})\mathbf p = \mathbf R \mathbf p p′=qpq−1=L(q)R(q−1)p=Rp

q − 1 = q ∗ / ∣ ∣ q ∣ ∣ 2 \mathbf q^{-1}=\mathbf q^*/||\mathbf q||^2 q−1=q∗/∣∣q∣∣2,所以单位四元数的逆即为 q ∗ \mathbf q^* q∗

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​]
因为 p \mathbf p p和 p ′ \mathbf p' p′都是虚四元数,所以该矩阵的右下角即为从四元数到旋转矩阵的变换关系:
R = v v T + s 2 I + 2 s [ v × ] + ( v × ) 2 R= \mathbf v \mathbf v^T + s^2 \mathbf I +2s [\mathbf v\times] +(\mathbf v\times)^2 R=vvT+s2I+2s[v×]+(v×)2

代入 s = q 0 , v = q 1 i + q 2 j + q 3 k s=q_0,\mathbf v = q_1\mathbf i + q_2\mathbf j + q_3\mathbf k s=q0​,v=q1​i+q2​j+q3​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 = 1 + r 11 + r 22 + r 33 2 q_{0}=\frac{\sqrt{1+r_{11}+r_{22}+r_{33}}}{2} q0​=21+r11​+r22​+r33​ ​​

q 1 = r 32 − r 23 4 q 0 q_{1}=\frac{r_{32}-r_{23}}{4 q_{0}} q1​=4q0​r32​−r23​​

q 2 = r 13 − r 31 4 q 0 q_{2}=\frac{r_{13}-r_{31}}{4 q_{0}} q2​=4q0​r13​−r31​​

q 3 = r 21 − r 12 4 q 0 q_{3}=\frac{r_{21}-r_{12}}{4 q_{0}} q3​=4q0​r21​−r12​​

	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定轴得到的旋转矩阵可以如下表示欧拉角:
θ 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.3. 旋转矩阵->轴角

利用罗德里格旋转公式可以获得绕某过原点一轴 n = [ r x , r y , r z ] \mathbf n = \left[r_{x}, r_{y}, r_{z}\right] n=[rx​,ry​,rz​] 旋转某一角度 θ \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θ​ ​

θ = a c o s r 11 + r 22 + r 33 − 1 2 \theta = acos\frac{r_{11}+ r_{22}+r_{33}-1}{2} θ=acos2r11​+r22​+r33​−1​

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 , θ ] r=[x, y, z, \theta] r=[x,y,z,θ]
其中单位向量 n ⃗ = [ x , y , z ] \vec{n}=[x, y, z] n =[x,y,z]对应的是旋转轴 θ \theta θ 对应的是旋转角度。旋转向量与轴角相同, 只是旋转向量用三个元素来描述旋转, 它把 θ \theta θ 角 乘到了旋转轴上, 如下:
r v = [ x ∗ θ , y ∗ θ , z ∗ θ ] r_{v}=[x * \theta, y * \theta, z * \theta] rv​=[x∗θ,y∗θ,z∗θ]

3.2. 轴角 -> 四元数

假设有单位四元数 q = q 0 + q 1 i + q 2 j + q 3 k = s + v \mathbf q= q_0 + q_1\mathbf i + q_2\mathbf j + q_3\mathbf k =s+ \mathbf v q=q0​+q1​i+q2​j+q3​k=s+v,三维点 p \mathbf p p经过旋转之后变为 p ′ \mathbf p' p′,如果使用矩阵描述,那么有 p ′ = R p \mathbf p'=\mathbf R \mathbf p p′=Rp 。 如果用四元数描述旋转,则表示为 p ′ = q p q − 1 \mathbf p'=\mathbf q \mathbf p\mathbf q^{-1} p′=qpq−1。

p ′ = q p q − 1 = L ( q ) R ( q − 1 ) p = R p \mathbf p'=\mathbf q \mathbf p\mathbf q^{-1}=\mathcal L(\mathbf q) \mathcal R(\mathbf q^{-1})\mathbf p = \mathbf R \mathbf p p′=qpq−1=L(q)R(q−1)p=Rp

q − 1 = q ∗ / ∣ ∣ q ∣ ∣ 2 \mathbf q^{-1}=\mathbf q^*/||\mathbf q||^2 q−1=q∗/∣∣q∣∣2,所以单位四元数的逆即为 q ∗ \mathbf q^* q∗

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​]
因为 p \mathbf p p和 p ′ \mathbf p' p′都是虚四元数,所以该矩阵的右下角即为从四元数到旋转矩阵的变换关系:
R = v v T + s 2 I + 2 s [ v × ] + ( v × ) 2 R= \mathbf v \mathbf v^T + s^2 \mathbf I +2s [\mathbf v\times] +(\mathbf v\times)^2 R=vvT+s2I+2s[v×]+(v×)2

对上式两侧求迹,得到
t r ( R ) = t r ( v v T ) + s 2 t r ( I 3 × 3 ) + 2 s t r ( [ v × ] ) + t r ( ( v × ) 2 ) = q 1 2 + q 2 2 + q 3 2 + 3 s 3 − 2 q 1 2 − 2 q 2 2 − 2 q 3 2 = 3 s 3 − ( q 1 2 + q 2 2 + q 3 2 ) = 3 s 3 − ( 1 − s 2 ) = 4 s 2 − 1 tr(R)= tr(\mathbf v \mathbf v^T) + s^2 tr(\mathbf I_{3\times3}) +2s tr([\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 tr(R)=tr(vvT)+s2tr(I3×3​)+2str([v×])+tr((v×)2)=q12​+q22​+q32​+3s3−2q12​−2q22​−2q32​=3s3−(q12​+q22​+q32​)=3s3−(1−s2)=4s2−1

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​​ ​

又有了罗德里格斯公式可以得到旋转向量到旋转矩阵的转换关系:
R = c o s θ I + ( 1 − c o s θ ) n n T + s i n θ [ n × ] \mathbf R = cos\theta \mathbf I + (1-cos\theta)\mathbf n \mathbf n^T+sin\theta[\mathbf n\times] R=cosθI+(1−cosθ)nnT+sinθ[n×]
对上式子求迹可得:
t r ( R ) = c o s θ t r ( I ) + ( 1 − c o s θ ) t r ( n n T ) + s i n θ [ ( n × ] ) = 3 c o s θ + ( 1 − c o s θ ) + 0 = 1 + 2 c o s θ = > θ = a r c c o s t r ( R ) − 1 2 = a r c c o s ( 2 s 2 − 1 ) = > c o s θ = 2 c o s 2 θ 2 − 1 = 2 s 2 − 1 = > θ = 2 a r c c o s ( s ) 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 = 2cos^2\frac{\theta}{2}-1 =2s^2-1 \\ => \theta = 2 arccos(s) tr(R)=cosθtr(I)+(1−cosθ)tr(nnT)+sinθ[(n×])=3cosθ+(1−cosθ)+0=1+2cosθ=>θ=arccos2tr(R)−1​=arccos(2s2−1)=>cosθ=2cos22θ​−1=2s2−1=>θ=2arccos(s)
轴角绕单位轴 u \mathbf{u} u 旋转 θ \theta θ 的四元数是:

至于旋转轴,如果在四元数转换的时候用 q \mathbf q q的虚部代替 p \mathbf p p,易知 q \mathbf q q的虚部组成的向量在旋转时是不动的,即构成旋转轴。于是只要将它除以它的模长,即得。
u = [ n x , n y , n z ] T = [ q 1 , q 2 , q 3 ] T / s i n θ 2 \mathbf{u}=[n_x,n_y,n_z]^T=[q_1,q_2,q_3]^T/sin\frac{\theta}{2} u=[nx​,ny​,nz​]T=[q1​,q2​,q3​]T/sin2θ​

所以,轴角绕单位轴 u \mathbf{u} u 旋转 θ \theta θ 的四元数是:

q ( w , v ) = ( cos ⁡ θ 2 , u sin ⁡ θ 2 ) \mathbf{q}(w, \mathbf{v})=\left(\cos \frac{\theta}{2}, \mathbf{u} \sin \frac{\theta}{2}\right) q(w,v)=(cos2θ​,usin2θ​)

3.3. 轴角->旋转矩阵

罗德里格斯形式旋转角(轴角)使用一个单位旋转轴k和绕轴旋转的角度 θ \theta θ(正方向右手定则)来描述
在这里插入图片描述
如上图,我们可以知道,向量 v v v 在 k k k 的作用下其实只旋转了垂直于旋转轴的分量,我们将 v v v 做分解,有
v = v ∥ + v ⊥ v=v_{\|}+v_{\perp} v=v∥​+v⊥​
其中平行分量使用投影和旋转轴表示为:( v ⋅ k v \cdot k v⋅k等价于将v向量在k向量上的投影( ∣ v ∣ ∗ ∣ k ∣ ∗ c o s θ |v|*|k|*cos\theta ∣v∣∗∣k∣∗cosθ),k是一个长度为1的旋转轴)
v ∥ = ( v ⋅ k ) k v_{\|}=(v \cdot k) k v∥​=(v⋅k)k
垂直分量表示为( − - −表示方向相反)
v ⊥ = v − v ∥ = v − ( v ⋅ k ) k = − k × ( k × v ) v_{\perp}=v-v_{\|}=v-(v \cdot k) k=-k \times(k \times v) v⊥​=v−v∥​=v−(v⋅k)k=−k×(k×v)
平行于轴的分量在旋转时不会改变幅度和方向(如图所示),
v ∥ r o t = v ∥ v_{\| r o t}=v_{\|} v∥rot​=v∥​
垂直于轴的分量只会改变方向,但不会改变大小
∣ 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 × v ⊥ = k × ( v − v ∥ ) = k × v − k × v ∥ = k × v k \times v_{\perp}=k \times\left(v-v_{\|}\right)=k \times v-k \times v_{\|}=k \times v k×v⊥​=k×(v−v∥​)=k×v−k×v∥​=k×v
因此上式可以修改为
v ⊥ = cos ⁡ θ v ⊥ + sin ⁡ θ k × v v_{\perp}=\cos \theta v_{\perp}+\sin \theta k \times v v⊥​=cosθv⊥​+sinθk×v
则
v r o t = v ∥ + cos ⁡ ( θ ) v ⊥ + sin ⁡ ( θ ) k × v = v ∥ + cos ⁡ ( θ ) ( v − v ∥ ) + sin ⁡ ( θ ) k × v = cos ⁡ ( θ ) v + ( 1 − cos ⁡ θ ) v ∥ + sin ⁡ ( θ ) k × v = cos ⁡ ( θ ) v + ( 1 − cos ⁡ θ ) ( k ⋅ v ) k + sin ⁡ ( θ ) k × v \begin{aligned} \mathbf{v}_{\mathrm{rot}} &=\mathbf{v}_{\|}+\cos (\theta) \mathbf{v}_{\perp}+\sin (\theta) \mathbf{k} \times \mathbf{v} \\ &=\mathbf{v}_{\|}+\cos (\theta)\left(\mathbf{v}-\mathbf{v}_{\|}\right)+\sin (\theta) \mathbf{k} \times \mathbf{v} \\ &=\cos (\theta) \mathbf{v}+(1-\cos \theta) \mathbf{v}_{\|}+\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{aligned} vrot​​=v∥​+cos(θ)v⊥​+sin(θ)k×v=v∥​+cos(θ)(v−v∥​)+sin(θ)k×v=cos(θ)v+(1−cosθ)v∥​+sin(θ)k×v=cos(θ)v+(1−cosθ)(k⋅v)k+sin(θ)k×v​
我们将叉乘部分按照矩阵分量形式表示出来 [ ( 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​​ ​
为了表达方便,我们定义一个叉乘矩阵 K \mathbf{K} 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​ ​
将叉乘简化为
K v = k × v \mathbf{K} \mathbf{v}=\mathbf{k} \times \mathbf{v} Kv=k×v
由于 k k k 为单位向量,因此有单位 2-范数
∥ K ∥ 2 = 1 \|K\|_{2}=1 ∥K∥2​=1
我们回到之前的 v r o t v_{r o t} vrot​ 表达式上
v rot  = v cos ⁡ θ + ( k × v ) sin ⁡ θ + k ( k ⋅ v ) ( 1 − cos ⁡ θ ) = v cos ⁡ θ + ( k × v ) sin ⁡ θ + ( v − v ⊥ ) ( 1 − cos ⁡ θ ) = v cos ⁡ θ + ( k × v ) sin ⁡ θ + ( v + k × ( k × v ) ) ( 1 − cos ⁡ θ ) = v cos ⁡ θ + ( k × v ) sin ⁡ θ + ( v + K ( K v ) ) ( 1 − cos ⁡ θ ) = v ( cos ⁡ θ + 1 − cos ⁡ θ ) + ( k × v ) sin ⁡ θ + K ( K v ) ( 1 − cos ⁡ θ ) \begin{aligned} \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{K} \mathbf{v}))(1-\cos \theta) \\ &=\mathbf{v}(\cos \theta+1-\cos \theta)+(\mathbf{k} \times \mathbf{v}) \sin \theta+\mathbf{K}(\mathbf{K} \mathbf{v})(1-\cos \theta) \end{aligned} vrot ​​=vcosθ+(k×v)sinθ+k(k⋅v)(1−cosθ)=vcosθ+(k×v)sinθ+(v−v⊥​)(1−cosθ)=vcosθ+(k×v)sinθ+(v+k×(k×v))(1−cosθ)=vcosθ+(k×v)sinθ+(v+K(Kv))(1−cosθ)=v(cosθ+1−cosθ)+(k×v)sinθ+K(Kv)(1−cosθ)​
这里我们就得到了
v r o t = v + ( sin ⁡ θ ) K v + ( 1 − cos ⁡ θ ) K 2 v , ∥ K ∥ 2 = 1 \mathbf{v}_{\mathrm{rot}}=\mathbf{v}+(\sin \theta) \mathbf{K} \mathbf{v}+(1-\cos \theta) \mathbf{K}^{2} \mathbf{v}, \quad\|\mathbf{K}\|_{2}=1 vrot​=v+(sinθ)Kv+(1−cosθ)K2v,∥K∥2​=1
那么旋转矩阵就可以使用上式得到( v r o t = R v = > R \mathbf{v}_{\mathrm{rot}}=Rv=>R vrot​=Rv=>R)
R = I + sin ⁡ θ K + ( 1 − cos ⁡ θ ) K 2 \mathbf{R}=\mathbf{I}+\sin \theta \mathbf{K}+(1-\cos \theta) \mathbf{K}^{2} R=I+sinθK+(1−cosθ)K2
展开之后有
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定轴分解得到连乘形式:

q = q z ( ψ ) ⊗ q y ( θ ) ⊗ q x ( ϕ ) \mathbf{q}= \mathbf{q}_{z}(\psi) \otimes \mathbf{q}_{y}(\theta) \otimes \mathbf{q}_{x}(\phi) q=qz​(ψ)⊗qy​(θ)⊗qx​(ϕ)

= [ 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 ( ϕ ) R y ( θ ) R x ( ψ ) = R=R_{z}(\phi) R_{y}(\theta) R_{x}(\psi) = R=Rz​(ϕ)Ry​(θ)Rx​(ψ)=

[ 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 ( θ ) , R y ( θ ) , R z ( θ ) R_{x}(\theta),R_{y}(\theta),R_{z}(\theta) Rx​(θ),Ry​(θ),Rz​(θ)
    我们首先假定坐标系为常用的右手坐标系:
    在这里插入图片描述
    那么 R x ( θ ) R_{x}(\theta) Rx​(θ)就表示为从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 ( θ ) = [ 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​]

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 ( θ ) R_{y}(\theta) Ry​(θ)就表示为从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 ( θ ) R_{z}(\theta) Rz​(θ)同理

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. 单位四元数 -> 旋转矩阵

假设有单位四元数 q = q 0 + q 1 i + q 2 j + q 3 k = s + v \mathbf q= q_0 + q_1\mathbf i + q_2\mathbf j + q_3\mathbf k =s+ \mathbf v q=q0​+q1​i+q2​j+q3​k=s+v,三维点 p \mathbf p p经过旋转之后变为 p ′ \mathbf p' p′,如果使用矩阵描述,那么有 p ′ = R p \mathbf p'=\mathbf R \mathbf p p′=Rp 。 如果用四元数描述旋转,则表示为 p ′ = q p q − 1 \mathbf p'=\mathbf q \mathbf p\mathbf q^{-1} p′=qpq−1。

p ′ = q p q − 1 = L ( q ) R ( q − 1 ) p = R p \mathbf p'=\mathbf q \mathbf p\mathbf q^{-1}=\mathcal L(\mathbf q) \mathcal R(\mathbf q^{-1})\mathbf p = \mathbf R \mathbf p p′=qpq−1=L(q)R(q−1)p=Rp

q − 1 = q ∗ / ∣ ∣ q ∣ ∣ 2 \mathbf q^{-1}=\mathbf q^*/||\mathbf q||^2 q−1=q∗/∣∣q∣∣2,所以单位四元数的逆即为 q ∗ \mathbf q^* q∗

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​]
因为 p \mathbf p p和 p ′ \mathbf p' p′都是虚四元数,所以该矩阵的右下角即为从四元数到旋转矩阵的变换关系:
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​​ ​
其中 q = q 0 + q 1 i + q 2 j + q 3 k \mathbf{q}=q_0+q_1\mathbf{i}+q_2\mathbf{j}+q_3\mathbf{k} q=q0​+q1​i+q2​j+q3​k

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

5.2. 四元数 -> 欧拉角

q = q z ( ψ ) ⊗ q y ( θ ) ⊗ q x ( ϕ ) \mathbf{q}= \mathbf{q}_{z}(\psi) \otimes \mathbf{q}_{y}(\theta) \otimes \mathbf{q}_{x}(\phi) q=qz​(ψ)⊗qy​(θ)⊗qx​(ϕ)

= [ 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的结果是是 [ − π 2 , π 2 ] \left[-\frac{\pi}{2}, \frac{\pi}{2}\right] [−2π​,2π​], 这并不能覆盖所有朝向,因此需要用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. 四元数 -> 轴角

假设有单位四元数 q = q 0 + q 1 i + q 2 j + q 3 k = s + v \mathbf q= q_0 + q_1\mathbf i + q_2\mathbf j + q_3\mathbf k =s+ \mathbf v q=q0​+q1​i+q2​j+q3​k=s+v,三维点 p \mathbf p p经过旋转之后变为 p ′ \mathbf p' p′,如果使用矩阵描述,那么有 p ′ = R p \mathbf p'=\mathbf R \mathbf p p′=Rp 。 如果用四元数描述旋转,则表示为 p ′ = q p q − 1 \mathbf p'=\mathbf q \mathbf p\mathbf q^{-1} p′=qpq−1。

p ′ = q p q − 1 = L ( q ) R ( q − 1 ) p = R p \mathbf p'=\mathbf q \mathbf p\mathbf q^{-1}=\mathcal L(\mathbf q) \mathcal R(\mathbf q^{-1})\mathbf p = \mathbf R \mathbf p p′=qpq−1=L(q)R(q−1)p=Rp

q − 1 = q ∗ / ∣ ∣ q ∣ ∣ 2 \mathbf q^{-1}=\mathbf q^*/||\mathbf q||^2 q−1=q∗/∣∣q∣∣2,所以单位四元数的逆即为 q ∗ \mathbf q^* q∗

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​]
因为 p \mathbf p p和 p ′ \mathbf p' p′都是虚四元数,所以该矩阵的右下角即为从四元数到旋转矩阵的变换关系:
R = v v T + s 2 I + 2 s [ v × ] + ( v × ) 2 R= \mathbf v \mathbf v^T + s^2 \mathbf I +2s [\mathbf v\times] +(\mathbf v\times)^2 R=vvT+s2I+2s[v×]+(v×)2

对上式两侧求迹,得到
t r ( R ) = t r ( v v T ) + s 2 t r ( I 3 × 3 ) + 2 s t r ( [ v × ] ) + t r ( ( v × ) 2 ) = q 1 2 + q 2 2 + q 3 2 + 3 s 3 − 2 q 1 2 − 2 q 2 2 − 2 q 3 2 = 3 s 3 − ( q 1 2 + q 2 2 + q 3 2 ) = 3 s 3 − ( 1 − s 2 ) = 4 s 2 − 1 tr(R)= tr(\mathbf v \mathbf v^T) + s^2 tr(\mathbf I_{3\times3}) +2s tr([\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 tr(R)=tr(vvT)+s2tr(I3×3​)+2str([v×])+tr((v×)2)=q12​+q22​+q32​+3s3−2q12​−2q22​−2q32​=3s3−(q12​+q22​+q32​)=3s3−(1−s2)=4s2−1

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​​ ​

又有了罗德里格斯公式可以得到旋转向量到旋转矩阵的转换关系:
R = c o s θ I + ( 1 − c o s θ ) n n T + s i n θ [ n × ] \mathbf R = cos\theta \mathbf I + (1-cos\theta)\mathbf n \mathbf n^T+sin\theta[\mathbf n\times] R=cosθI+(1−cosθ)nnT+sinθ[n×]
对上式子求迹可得:
t r ( R ) = c o s θ t r ( I ) + ( 1 − c o s θ ) t r ( n n T ) + s i n θ [ ( n × ] ) = 3 c o s θ + ( 1 − c o s θ ) + 0 = 1 + 2 c o s θ = > θ = a r c c o s t r ( R ) − 1 2 = a r c c o s ( 2 s 2 − 1 ) = > c o s θ = 2 c o s 2 θ 2 − 1 = 2 s 2 − 1 = > θ = 2 a r c c o s ( s ) 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 = 2cos^2\frac{\theta}{2}-1 =2s^2-1 \\ => \theta = 2 arccos(s) tr(R)=cosθtr(I)+(1−cosθ)tr(nnT)+sinθ[(n×])=3cosθ+(1−cosθ)+0=1+2cosθ=>θ=arccos2tr(R)−1​=arccos(2s2−1)=>cosθ=2cos22θ​−1=2s2−1=>θ=2arccos(s)
轴角绕单位轴 u \mathbf{u} u 旋转 θ \theta θ 的四元数是:

至于旋转轴,如果在四元数转换的时候用 q \mathbf q q的虚部代替 p \mathbf p p,易知 q \mathbf q q的虚部组成的向量在旋转时是不动的,即构成旋转轴。于是只要将它除以它的模长,即得。
u = [ n x , n y , n z ] T = [ q 1 , q 2 , q 3 ] T / s i n θ 2 \mathbf{u}=[n_x,n_y,n_z]^T=[q_1,q_2,q_3]^T/sin\frac{\theta}{2} u=[nx​,ny​,nz​]T=[q1​,q2​,q3​]T/sin2θ​

q ( w , v ) = ( cos ⁡ θ 2 , u sin ⁡ θ 2 ) \mathbf{q}(w, \mathbf{v})=\left(\cos \frac{\theta}{2}, \mathbf{u} \sin \frac{\theta}{2}\right) q(w,v)=(cos2θ​,usin2θ​)

	// 方法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;
四元数、变换矩阵、欧拉角、轴角的转换关系详解 描述空间变换通常有四种描述方式,分别是四元数、变换矩阵、欧拉角、轴角,探讨这四种方法之间的关系。大致的关系如图: 轴角 轴角一种几何描述方法。轴角定义了一个转动轴和转动角,沿着转动轴方向逆时针旋转为转动正方向,具体如下图: θ=θe⃗ 其中向量e为转轴,模长为θ角。 欧拉角 欧拉角就是物体绕坐标系三个坐标轴(x,y,z轴)的旋转角度。由于涉及到坐标系,学习SLAM的应该会经常遇到全局坐标系和局部坐标系,因此,根据不同的坐标系以及分解的顺序不同,欧拉角表达具有多样性,根据坐标系的不同一般分 阅读详情

相关推荐

机器人学回炉重造(1-3):旋转矩阵转轴角、轴角转旋转矩阵

先看书了解一下等效转轴与等效转角 下面是笔记记录部分 像确定四元数一样,确定轴和角度也只能直到符号为止。即,(u,θ)和(-u,-θ)对应于相同的旋转矩阵,就像q和-q一样。此外,轴角提取还有其他困难。角度可以限制为0°至180°,但是角度在形式上是360°的倍数不明确。当角度为零时,轴是不确定的。当角度为180°时,矩阵变得对称,这对提取轴有影响。在接近180°的倍数时,需要注意避免数值问题:在提取角度时,atan2(sinθ,cosθ)等于θ的两参数反正切可避免arccos的不敏感性;在计算轴的大小以

xuuyann 4493

(11)四元数与欧拉角的转换

Table of Contents 0、简介 一、四元数的定义 二、欧拉角到四元数的转换 2.1 公式: 2.2 code: 三、四元数到欧拉角的转换 3.1 公式 3.2 code: 3.3四元素到旋转矩阵转换 四.奇点 五. 矢量旋转 证明: 六 . 其他参考 七 python 四元素欧拉角互相转换 8 Eigen transform 欧拉角到四元素 四元素得到yaw 四元素到旋转向量 旋转轴向量到四元素 0、简介 四元数与欧拉角之间的转换 百度百科四元素..

weixin_45729043的博客 3822

欧拉角与旋转矩阵(EIgen与书中公式的区别)

文章目录1 正常坐标旋转2 Eigen中的坐标旋转 在这里记录以下使用eigen进行坐标旋转时遇到的坑。eigen中的坐标旋转与我们日常进行公式推导时的坐标旋转不同。但旋转方向都是逆时针为正。 1 正常坐标旋转 正常的旋转如上图所示,在世界坐标系中有一个点P_w(1,1,0)T, θ\thetaθ表示的是从世界坐标系到相机坐标系旋转的角度,则可以得到,旋转后的P_c为(2,0,0)T(\sqrt2,0,0)^T(2​,0,0)T 轴角、旋转矩阵、四元数、θ\thetaθ表示的都是从世界坐标系到当前坐标系

zack_liu的博客 3672

四元数和欧拉角互相转换

本文介绍了四元数和欧拉角之间的转换公式推导及C语言代码实现,在实际使用中可以根据需要进行选择。四元数具有很好的性质,可以避免万向锁等问题,因此在计算机图形学、机器人控制等领域中得到了广泛应用。欧拉角虽然表示方式较为简单,但在某些情况下容易出现奇点等问题,因此需要进行相应的处理。四元数和欧拉角都是用于表示旋转的工具,它们之间可以相互转换。以下是四元数和欧拉角之间的转换公式推导,并提供了C语言代码实现。欧拉角的定义:欧拉角是用于描述物体在空间中的旋转的一种表示方法,它可以分为三个旋转,即绕。

FlyDremever 9224

轴角与旋转矩阵、欧拉角与旋转矩阵、四元数与旋转矩阵的转换

六、 旋转矩阵转换成四元数。一、轴角转换成旋转矩阵。三、旋转矩阵转换成欧拉角。四、欧拉角转换成旋转矩阵。五、四元数转换成旋转矩阵。二、旋转矩阵转换成轴角。

qq_58060770的博客 1139

矩阵运算_Eigen使用_旋转矩阵/角轴/欧拉角/四元数相互转换

文件结构 CMakeLists.txt cmake_minimum_required( VERSION 2.8 ) project( geometry ) # 添加Eigen头文件 include_directories( "/usr/include/eigen3" ) add_executable( eigenGeometry eigenGeometry.cpp ) eigenG...

惊鸿一博 2987

旋转矩阵、欧拉角、四元数及四元数插值

在三维世界中的旋转的表示的方法主要有: 旋转只是3个自由度未知量。 3*3旋转矩阵 3维坐标系经过旋转,得到新的坐标系,新坐标系三轴在旧坐标系下的表示构成的3*3矩阵,就是能够表述旋转的旋转矩阵。 但是3*3矩阵用来表述3个自由度的旋转时,变量产生冗余,因此旋转矩阵及其还要满足一系列其他的性质。且表示也不够直观 角轴 任何旋转都可以看成,围绕空间某个轴旋转一定的角度产生的,角轴对应旋转轴的方向,其模长可以用来关联旋转的角度。 欧拉角 欧拉角它使用了三个分离的转角,把一个旋转分解成三次绕不同轴的旋转。比较清

weixin_41469272的博客 1万+

【从零开始学习SLAM】坐标变换 Eigen Sophus 旋转矩阵 轴角 旋转向量 欧拉角 四元数 位姿变换

欧拉角是描述刚体姿态的三个角,欧拉角有静态和动态两种,静态的是绕静止的惯性坐标系三个轴进行旋转,而动态的在旋转过程中旋转坐标轴会发生变化,除了第一次旋转是绕惯性系的坐标轴进行之外,后续两次旋转都是动态的,并且前面旋转的角度对后面的旋转轴是有影响的,按照不同的轴顺序进行旋转得到的欧拉角也是不同的,旋转变换可以归结为若干个沿着坐标轴旋转的组合,组合个数不超过三个并且两个相邻的旋转必须沿着不同坐标轴,总共有12种旋转方式,分别是XYZ、XZY、XYX、XZX、YXZ、YZX、YXY...............

小秋 AI SLAM 入门实战教程 6503

【C++】Eigen 库中旋转向量、旋转矩阵、欧拉角、四元数的定义及互相转换

参考: https://blog.csdn.net/u011092188/article/details/77430988 https://www.cnblogs.com/lovebay/p/11215028.html Eigen: C++开源矩阵计算工具 一、Eigen中表述刚体运动的简单用法 Eigen库是一个开源的C++线性代数库,它提供了快速的有关矩阵的线性代数运算,还包括解方程等功能。Eigen是一个用纯头文件搭建起来的库,这意味这你只要能找到它的头文件,就能使用它。Eigen头文件的默认.

lemonxiaoxiao的博客 1万+

AI:Coze、Dify、n8n:三款低代码工具如何选?

Coze、Dify、n8n:三款低代码工具如何选?

Kant2048的博客 2812

CODESYS Softmotion(一)功能介绍

一、CODESYS SoftMotion RTE SL产品 1.1 CODESYS Runtime SL 产品家族介绍 目前支持的主流厂商的硬件产品,让部署在上面的成为实时设备。 对于Windows系统,根据需求选择对应的Runtime: RTE极大的提高了实时性,适合进行运动控制,同时RTE还支持了多个主从站协议: 还可以选择可选的功能(Single License): RTE对硬件和系统有要求,如果你需要硬件功能,那么网卡会有最低要求。 1.2 RTE的下载和安装 在官网http://codes

Fishfishfishfishcat 2万+

Eigen 欧拉角的说明,及四元数和旋转矩阵的变换

本文说明eulerAngles(0, 1, 2),和eulerAngles(2, 1, 0)的差异,并顺便将欧拉角、旋转矩阵、四元数一块的联系

avenger_fang的博客 7629

Android 系统低内存lowmemorykiller关于阈值修改 minfreeAbsKbytes优化

在系统rom定制化开发中,在对于运行内存ram 1G和2G的设备中,对于内存使用高峰期时,minfreeAbsKbytes优化很关键 调整合理的minfree,让内存合理的使用很关键,接下来分析相关功能的实现

安卓兼职framework和app工程师的博客 526

Eigen中几种表示三维位姿的方式以及相互转换

1. 普通4*4矩阵 Eigen::Matrix Eigen::Matrix<Type, 4, 4> transformation =Eigen::Matrix<Type, 4, 4>::Identity(). 2. 等距映射:Eigen::Isometry3d Eigen::Quaterniond quaternion; // 依次旋转yaw, pitch, roll // 即: R = R_z * R_y * R_x quaternion = Eigen::An.

u011906844的博客 1万+

R语言中的Gamma分布函数

在R语言中,我们可以使用最大似然估计(Maximum Likelihood Estimation,MLE)或最小二乘估计(Least Squares Estimation,LSE)等方法来估计Gamma分布的参数。本文介绍了在R语言中使用Gamma分布函数的基本操作,包括生成随机变量、计算概率密度函数和累积分布函数,以及参数估计和拟合。函数来生成Gamma分布的随机变量,计算概率密度函数(PDF)和累积分布函数(CDF),以及进行参数估计和拟合。要计算Gamma分布的累积分布函数(CDF),我们可以使用。

ByteWhiz的博客 1633

Python 爬虫实战:爬取 GitHub 热门项目(星数 / 语言 / 作者)

本文系统介绍了使用Python爬虫技术获取GitHub热门项目数据的完整方案。通过分析GitHub Trending页面和官方API两种数据源,详细讲解了HTML解析、JSON数据处理、反爬应对策略等关键技术。采用requests、BeautifulSoup、pandas等工具链,实现了项目信息(名称、星数、语言等)的结构化爬取,并配套数据清洗、统计分析及可视化功能。特别强调遵守GitHub API使用规范,包括Token认证、请求频率控制等合规要求。该方案为开发者提供了一种高效、合法的数据采集方法,适用于

2503_91057718的博客 2348

三维中间中旋转的表示方式--欧拉角、旋转矩阵、旋转向量、四元数

欧拉角(Euler Angle) 在上面这些表示方式中,我们最容易理解的就是欧拉角了。这种表示方式在飞行器中使用的很多。 在欧拉角的表示中,yaw、pitch、roll的顺序对旋转结果是有影响的。即给定一组欧拉角角度值,比如yaw=45度,pitch=30度,roll=60度,按照yaw-pitch-roll的顺序旋转和按照yaw-roll-pitch的顺序旋转,最终刚体的朝向是不同的!换言之,若刚体需要按照两种不同的旋转顺序旋转到相同的朝向,所需要的欧拉角角度值则是不同的! 另外需要注意的是,

sky的博客 5196

058、时域降噪(Temporal Denoise):运动补偿与递归滤波在视频降噪中的应用

本文深入探讨了时域降噪(Temporal Denoise)技术在视频处理中的应用。作者通过车载环视项目案例,揭示了空域降噪的局限性,提出利用帧间信息进行降噪的核心思路。文章详细解析了递归滤波和运动补偿两大关键技术,指出递归滤波简单但易产生拖影,而运动补偿通过计算运动矢量解决此问题。作者还分享了多帧降噪方案的优势与算力挑战,并总结了噪声模型匹配、运动矢量一致性等实战调优经验。最后强调时域降噪需要与空域降噪平衡,指出调试工具的重要性,提出"在保证运动区域无鬼影的前提下追求信噪比"的实践原则,体现了算法复杂度与效

zzfmlxs的博客 93

基于eigen实现轴角,旋转向量,欧拉角,四元数,旋转矩阵相互转换

/旋转矩阵转欧拉角,注意顺序。void AxisToRpy(double *pose)//旋转向量转欧拉角,传入位姿指针。void RpyToAxis(double *pose)//欧拉角转轴角,传入位姿指针。//四元数转旋转向量。

weixin_44928759的博客 1433

RAP BDL 里的 field ( readonly ) 到底控制了什么?——从语法到运行时的全景解析

RAP行为定义中的只读字段控制 摘要:该代码片段展示的是RAP(ABAP RESTful Application Programming)行为定义语言(BDL)中的字段控制语法,用于声明业务对象中需由框架自动维护的只读字段。field ( readonly )语句将订单ID和审计字段(CreatedBy/CreatedAt等)标记为只读,确保这些字段在更新阶段不可修改。在managed场景下,RAP框架会自动处理这些行政性数据的写入:创建时填充创建信息,更新时刷新变更记录,并利用LocalLastChang

2007 年 ~ 2025 年,深耕 SAP 技术 18 年 159

基于matlab不变矩的数字验证码识别系统

随着互联网技术的快速发展和应用,网络给人们提供了丰富的资源和极大的便利,但随之而来的是互联网系统的安全性问题,而验证码正是加强 Web 系统安全性的产物。全自动区分计算机和人类的图灵测试(Completely Automated Public Turing test to tell Computers and Humans Apart,CAPTCHA)也是验证码的一个应用程序,可以区分用户是人类还是计算机智能单击对象。

在职AI算法工程师,擅长计算机视觉,YOLO目标检测、分割等,擅长web、pyqt界面可视化,好内容持续更新中,来这里跟大家一起学习,共同进步 918

Eigen中eulerAngles()得到欧拉角的奇异问题

Eigen中得到欧拉角的奇异问题

WillWinston的博客 6487

jre9下载 jdk9

(Java SE Runtime Environment 9.0) 9.0u175 官方最新版 【x86|x64】

Eigen::关于旋转(旋转向量\旋转矩阵\四元数\欧拉角)的初始化和转换以及应用

Eigen::关于旋转(旋转向量\旋转矩阵\四元数\欧拉角)的初始化和转换以及应用 code: #include <iostream> #include <eigen3/Eigen/Core> #include <eigen3/Eigen/Geometry> using namespace std; using namespace Eigen; i...

dieju8330的博客 3048

STM32F407中文手册

STM32f407中文手册 本参考手册面向应用开发人员,提供有关使用 STM32F405xx/07xx、STM32F415xx/17xx、 STM32F42xxx 和 STM32F43xxx 微控制器存储器与外设的完整信息。STM32F405xx/07xx、STM32F415xx/17xx、STM32F42xxx 和 STM32F43xxx 构成一个微 控制器系列,各产品具有不同的存储器大小、封装和外设。有关订购信息以及器件的机械与电气特性,请参见数据手册。有关 ARM Cortex™-M4F 内核的信息,请参见《Cortex™-M4F 技术参考手册》。

pscad IEEE标准系统搭建说明

pscad IEEE标准系统搭建说明,包含典型的IEEE 9节点、14节点、30节点、39节点和118节点系统。内含标准测试系统的原始数据和潮流结果。

旋转向量与欧拉角、四元数的转换及Eigen实践

开篇介绍 在之前了解到旋转向量与旋转矩阵、变换矩阵的关系后,我们进行了Eigen库上基本矩阵的操作实践。接下来本篇将介绍的是旋转向量与欧拉角、四元数之间的转换以及向量旋转在Eigen上的表达。 一、旋转向量 在上篇有了旋转矩阵和变换矩阵描述了向量的旋转后,我们发现假如一直沿用矩阵来表达向量旋转的话,会有几个明显的缺点:①旋转矩阵具有九个量,但是一次旋转具有三个自由度,变换矩阵用了十六个量表达了六个自由度的变换,显然这是一种冗余的表达方式。②旋转矩阵必须是正交矩阵且行列式为1。变换矩阵也是在旋转矩阵的基础上得

weixin_42024702的博客 1671

汉书HE8,好用的汇编代码编写工具

汉书HE8,好用的汇编代码编写工具早期编写汇编代码所用功能强大

opencv矩阵转eigen_【姿态表示】旋转向量、旋转矩阵、欧拉角、四元数

1. 旋转矩阵与旋转向量旋转矩阵(Rotation Matrix)用 9 个量描述旋转的3个自由度,有冗余;9 个量是有约束的:必须是正交矩阵,且行列式为 1旋转向量(Rotation Vector)任意的旋转都可以用一个旋转轴和绕轴的旋转角来描述,简称“轴角”(Axis-Angle);旋转向量,是一个三维向量,其方向与旋转轴一致,长度等于旋转角;旋转向量到旋转矩阵的转换是通过罗德里格斯公式(Ro...

weixin_42508114的博客 1333
上一篇: ceres 学习笔记
下一篇: VINS-MONO原理笔记(基于计算机视觉life课程)
评论
成就一亿技术人!
拼手气红包6.0元
还能输入1000个字符
 
 条评论被折叠 查看
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付:10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值
⌂ 换网址