刚体的旋转_eigen::isometry3d t(r)

Eigen中的旋转操作

旋转矩阵

• 构造

Eigen::Matrix3d rotation_matrix;
rotation_matrix<< 0.707107,  -0.707107,  0,
         		  0.707107,   0.707107,  0,
         		  0,          0,         1;
1
2
3
4

---

四元数

• 构造

//直接构造
Eigen::Quaterniond(1,0,0,0);

// 从旋转矩阵
Eigen::Matrix3d R; 
      R<< 0.707107,  -0.707107,  0,
          0.707107,   0.707107,  0,
          0,          0,         1;

Eigen::Quaterniond q(R);


// 从角轴
Eigen::AngleAxisd rotation_vector(M_PI/4, Eigen::Vector3d(0,0,1)); //绕z轴旋转45度
Eigen::Quaterniond q(rotation_vector);

//从数组, 数组的顺序应该是[x y z w]
double array[4]= {1,2,3,4};
Eigen::Quaterniond q1(array); //相当于直接构造　q1(4,1,2,3);


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

• 读取系数

// 1. 
q.coeffs()  // x, y, z, w 的顺序
q.vec()　　　// x, y, z  虚数部分
q.x();
q.y();
q.z();
q.w();
1
2
3
4
5
6
7

• 其他常用操作

1. 归一化　normalize() :直接改原始的q，normalized() const ：返回一个归一化的q，原始不改变
2. 返回旋转矩阵toRotationMatrix
3. 取模 norm()
4. 模长的平方squaredNorm()
5. 取共轭conjugate()
6. 取逆 inverse()
7. 点乘dot

---

欧拉角

• 注意事项：

1. 常用的一种欧拉角顺序 yaw-pitch-roll, 所对应的坐标轴：Z, Y, X
2. 我们如果定义欧拉角：Eigen::Vector3d　euler_angle(X, Y, Z) ,　euler_angle(0) : roll …

// 1. 可以从旋转矩阵得到欧拉角
Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); // 旋转顺序z,y,x; 得到欧拉角(yaw, pitch roll) 
1
2

---

角轴

• 注意事项：

1. 角轴的表示的轴必须是单位向量, 角是弧度制
2. 角轴的表示遵守右手定则，大拇指朝上，四指的方向是正
3. 角轴１(-M_PI/4, 0, 0, 1) 和 角轴2(M_PI/4, 0, 0, -1) 表示的是同一个旋转，绕z轴旋转-45度

• 构造

// 1. 从角轴
Eigen::AngleAxisd rotation_vector(M_PI/4, Eigen::Vector3d(0,0,1)); //绕z轴旋转45度, 注意，角度制：弧度，轴：必须是单位的

// 2. 从四元数
Eigen::Quaterniond q1(1,1,1,1);
Eigen::AngleAxisd rotation_vector(q1);　// q1可以不是归一化的，自动取q1的归一化之后的q取构造

// 3.从旋转矩阵
Eigen::Matrix3d R;
     R<< 0.707107,  -0.707107,  0,
         0.707107,   0.707107,  0,
         0,          0,         1;
Eigen::AngleAxisd rotation_vector(R);
1
2
3
4
5
6
7
8
9
10
11
12
13

• 其他常用操作

1. 转到旋转矩阵 toRotationMatrix() const
2. 取角度(弧度制)angle()
3. 取轴(单位向量)axis() 自动单位化输出
4. 相反的旋转inverse() const

变换矩阵

• 构造

// 1. 可以从角轴和平移
    Eigen::AngleAxisd n(M_PI/4, Eigen::Vector3d(0,0,1));
    Eigen::Vector3d t(1,1,1);
    


    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
    T.rotate(n);
    T.pretranslate(t);




// 2. 可以从四元数和平移
    Eigen::AngleAxisd n(M_PI/4, Eigen::Vector3d(0,0,1));
    Eigen::Quaterniond q(n);
    Eigen::Vector3d t(1,1,1);
    


    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
    T.rotate(q);
    T.pretranslate(t);




// 3. 可以从旋转矩阵和平移
    Eigen::AngleAxisd n(M_PI/4, Eigen::Vector3d(0,0,1));
    Eigen::Matrix3d R = n.matrix();
    Eigen::Vector3d t(1,1,1);
    


    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
    T.rotate(R);
    T.pretranslate(t);




// 4. 除了创建好对象再传递　旋转　和平移之外，还可以以旋转创建，再传递平移,以下以旋转矩阵为例，其他两种一样可以
    Eigen::Isometry3d T(R);
    T.pretranslate(t);


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46

• 常用操作

1. p = T*v T内部重载了符号，虽然T是四维的, v和p都是3维的，但是相当于p = Rv+t的操作
2. translation() 取平移部分
3. rotation()　取旋转矩阵

---

各种旋转之间的转化

四元数和角轴

单位四元数：$(q_0,q_1,q_2,q_3)$   角轴：$n\theta$    

• 四元数到角轴，得到轴是单位的

$$\theta =2arc\cos q_0$$

$$n=[n_x,n_y,n_z{]}^T=[q_1,q_2,q_3{]}^T/sin\frac{\theta }{2}$$

• 角轴到四元数轴，$n=(n_x,n_y,n_z)$是单位的，得到四元数是单位的

$$q=[q_0,q_1,q_2,q_3{]}^T=[\cos \frac{\theta }{2},\text{n}sin\frac{\theta }{2},{]}^T=[\cos \frac{\theta }{2},n_{x}sin\frac{\theta }{2},n_{y}sin\frac{\theta }{2}{n}_{z}sin\frac{\theta }{2}{]}^T$$

---

四元数和旋转矩阵

单位四元数：$(q_0,q_1,q_2,q_3)$   旋转矩阵R：

• 四元数到旋转矩阵

R = [ 1 − 2 q 2 2 − 2 q 3 2 2 q 1 q 2 − 2 q 0 q 3 2 q 1 q 3 + 2 q 0 q 2 2 q 1 q 2 + 2 q 0 q 3 1 − 2 q 1 2 − 2 q 3 2 2 q 2 q 3 − 2 q 0 q 1 2 q 1 q 3 − 2 q 0 q 2 2 q 2 q 3 + 2 q 0 q 1 1 − 2 q 1 2 − 2 q 2 2 ] R= \left[

$$\begin{matrix} 1-2q_2^2-2q_3^2 & 2q_1q_2-2q_0q_3 & 2q_1q_3+2q_0q_2 \\ 2q_1q_2+2q_0q_3 & 1-2q_1^2-2q_3^2 & 2q_2q_3-2q_0q_1 \\ 2q_1q_3-2q_0q_2 & 2q_2q_3+2q_0q_1 & 1-2q_1^2-2q_2^2 \end{matrix}$$ \right] R=⎣⎡​1−2q22​−2q32​2q1​q2​+2q0​q3​2q1​q3​−2q0​q2​​2q1​q2​−2q0​q3​1−2q12​−2q32​2q2​q3​+2q0​q1​​2q1​q3​+2q0​q2​2q2​q3​−2q0​q1​1−2q12​−2q22​​⎦⎤​

• 旋转矩阵到四元数
  假设$R=m_{ij},i,j\in [1,2,3]$

$$q_0=\frac{\sqrt{tr(R)+1}}{2}tr(R)代表迹$$

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

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

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

注意：当$q_0\to 0$的时候，也就是R的迹趋近－１，其他三个分量很大，导致解不稳定，则应用下面的公式转化

1. 如果$max(m_{11},m_{22},m_{33})=m_{11}$

$$t=\sqrt{1+m_{11}-m_{22}-m_{33}}$$

$$q_0=\frac{m_{32}-m_{23}}{t}$$

$$q_1=\frac{t}{4}$$

$$q_2=\frac{m_{31}+m_{13}}{t}$$

$$q_3=\frac{m_{12}+m_{21}}{t}$$

2. 如果$max(m_{11},m_{22},m_{33})=m_{22}$

$$t=\sqrt{1-m_{11}+m_{22}-m_{33}}$$

$$q_0=\frac{m_{13}-m_{31}}{t}$$

$$q_1=\frac{m_{12}+m_{21}}{t}$$

$$q_2=\frac{t}{4}$$

$$q_3=\frac{m_{32}+m_{23}}{t}$$

3. 如果$max(m_{11},m_{22},m_{33})=m_{33}$

$$t=\sqrt{1-m_{11}-m_{22}+m_{33}}$$

$$q_0=\frac{m_{21}-m_{12}}{t}$$

$$q_1=\frac{m_{13}+m_{31}}{t}$$

$$q_2=\frac{m_{23}-m_{32}}{t}$$

$$q_3=\frac{t}{4}$$

---

四元数和欧拉角

单位四元数：$(q_0,q_1,q_2,q_3)$   　欧拉角：$(yaw,pitch,roll)$, 分别为绕Z轴、Y轴、X轴的旋转角度，记作$\psi ,\theta ,\varphi$

• 四元数到欧拉角

$$roll=\varphi =\arctan (\frac{2(q_0{q}_1+q_2{q}_3)}{1-2(q_1^2+q_2^2)})$$

$$pitch=\theta =\arcsin (2(q_0{q}_2-q_3{q}_1))$$

$$yaw=\psi =\arctan (\frac{2(q_0{q}_3+q_1{q}_2)}{1-2(q_2^2+q_3^2)})$$

• 欧拉角到四元数

q = [ q 0 q 1 q 2 q 3 ] = [ w x y z ] = [ cos ⁡ ψ 2 cos ⁡ θ 2 cos ⁡ ϕ 2 + sin ⁡ ψ 2 sin ⁡ θ 2 sin ⁡ ϕ 2 cos ⁡ ψ 2 cos ⁡ θ 2 sin ⁡ ϕ 2 − sin ⁡ ψ 2 sin ⁡ θ 2 cos ⁡ ϕ 2 cos ⁡ ψ 2 sin ⁡ θ 2 cos ⁡ ϕ 2 + sin ⁡ ψ 2 cos ⁡ θ 2 sin ⁡ ϕ 2 sin ⁡ ψ 2 cos ⁡ θ 2 cos ⁡ ϕ 2 − cos ⁡ ψ 2 sin ⁡ θ 2 sin ⁡ ϕ 2 ] q=\left[

$$\begin{matrix} q_0 \\ q_1 \\ q_2\\ q_3 \end{matrix}$$ \right]=\left[ $$\begin{matrix} w \\ x \\ y\\ z \end{matrix}$$ \right]=\left[ $$\begin{matrix} \cos\frac{\psi}{2} \cos\frac{\theta}{2} \cos\frac{\phi}{2} +\sin\frac{\psi}{2}\sin\frac{\theta}{2}\sin\frac{\phi}{2} \\ \cos\frac{\psi}{2}\cos\frac{\theta}{2}\sin\frac{\phi}{2}-\sin\frac{\psi}{2}\sin\frac{\theta}{2} \cos\frac{\phi}{2} \\ \cos\frac{\psi}{2} \sin\frac{\theta}{2} \cos\frac{\phi}{2} +\sin\frac{\psi}{2}\cos\frac{\theta}{2}\sin\frac{\phi}{2}\\ \sin\frac{\psi}{2} \cos\frac{\theta}{2} \cos\frac{\phi}{2}- \cos\frac{\psi}{2} \sin\frac{\theta}{2} \sin\frac{\phi}{2} \end{matrix}$$ \right] q=⎣⎢⎢⎡​q0​q1​q2​q3​​⎦⎥⎥⎤​=⎣⎢⎢⎡​wxyz​⎦⎥⎥⎤​=⎣⎢⎢⎡​cos2ψ​cos2θ​cos2ϕ​+sin2ψ​sin2θ​sin2ϕ​cos2ψ​cos2θ​sin2ϕ​−sin2ψ​sin2θ​cos2ϕ​cos2ψ​sin2θ​cos2ϕ​+sin2ψ​cos2θ​sin2ϕ​sin2ψ​cos2θ​cos2ϕ​−cos2ψ​sin2θ​sin2ϕ​​⎦⎥⎥⎤​

---

角轴和旋转矩阵

旋转角$\theta$和单位方向轴$n=(x,y,z)$

• 角轴到旋转矩阵

R = [ cos ⁡ θ + x 2 ( 1 − cos ⁡ θ ) − z sin ⁡ θ + x y ( 1 − cos ⁡ θ ) y sin ⁡ θ + x z ( 1 − cos ⁡ θ ) z sin ⁡ θ + x y ( 1 − cos ⁡ θ ) cos ⁡ θ + y 2 ( 1 − cos ⁡ θ ) − x sin ⁡ θ + y z ( 1 − cos ⁡ θ ) − y sin ⁡ θ + x z ( 1 − cos ⁡ θ ) x sin ⁡ θ + y z ( 1 − cos ⁡ θ ) cos ⁡ θ + z 2 ( 1 − cos ⁡ θ ) ] R=\left[

$$\begin{matrix} \cos\theta+x^2(1-\cos\theta) & -z\sin\theta+xy(1-\cos\theta) & y\sin\theta+xz(1-\cos\theta)\\ z\sin\theta+xy(1-\cos\theta) & \cos\theta+y^2(1-\cos\theta) & -x\sin\theta+yz(1-\cos\theta) \\ -y\sin\theta+xz(1-\cos\theta) & x\sin\theta+yz(1-\cos\theta) & \cos\theta +z^2(1-\cos\theta) \end{matrix}$$ \right] R=⎣⎡​cosθ+x2(1−cosθ)zsinθ+xy(1−cosθ)−ysinθ+xz(1−cosθ)​−zsinθ+xy(1−cosθ)cosθ+y2(1−cosθ)xsinθ+yz(1−cosθ)​ysinθ+xz(1−cosθ)−xsinθ+yz(1−cosθ)cosθ+z2(1−cosθ)​⎦⎤​

• 旋转矩阵到角轴
  $$\theta =\arccos (\frac{tr(R)-1}{2})$$

  $$\text{n}=R\text{n}$$
  关于旋转轴$\text{n}$，由于旋转轴的向量在旋转后不会变化，即$n=Rn$，也说明n是矩阵R的特征值１对应的特征向量，求解此方程，再归一化即可。
  
  

---

角轴和欧拉角

• 角轴到欧拉角

• 欧拉角到角轴

---

欧拉角和旋转矩阵

欧拉角：$(yaw,pitch,roll)$, 分别为绕Z轴、Y轴、X轴的旋转角度，记作$\psi ,\theta ,\varphi$。Ｂ:Body系，Ｗ: word系

• 欧拉角到旋转矩阵

R b w = [ cos ⁡ ψ cos ⁡ θ sin ⁡ ψ cos ⁡ θ − sin ⁡ θ cos ⁡ ψ sin ⁡ θ sin ⁡ ϕ − sin ⁡ ψ cos ⁡ ϕ sin ⁡ ψ sin ⁡ θ sin ⁡ ϕ + cos ⁡ ψ cos ⁡ ϕ cos ⁡ θ sin ⁡ ϕ cos ⁡ ψ sin ⁡ θ cos ⁡ ϕ + sin ⁡ ψ sin ⁡ ϕ sin ⁡ ψ sin ⁡ θ cos ⁡ ϕ − cos ⁡ ψ sin ⁡ ϕ cos ⁡ θ cos ⁡ ϕ ] R_{bw}= \left[

$$\begin{matrix} \cos\psi\cos\theta & \sin\psi\cos\theta & -\sin\theta \\ \cos\psi\sin\theta\sin\phi-\sin\psi\cos\phi & \sin\psi\sin\theta\sin\phi +\cos\psi\cos\phi& \cos\theta\sin\phi\\ \cos\psi\sin\theta\cos\phi+\sin\psi\sin\phi& \sin\psi\sin\theta\cos\phi-\cos\psi\sin\phi & \cos\theta\cos\phi \end{matrix}$$ \right] Rbw​=⎣⎡​cosψcosθcosψsinθsinϕ−sinψcosϕcosψsinθcosϕ+sinψsinϕ​sinψcosθsinψsinθsinϕ+cosψcosϕsinψsinθcosϕ−cosψsinϕ​−sinθcosθsinϕcosθcosϕ​⎦⎤​

R w b = R b w T = [ cos ⁡ ψ cos ⁡ θ cos ⁡ ψ sin ⁡ θ sin ⁡ ϕ − sin ⁡ ψ cos ⁡ ϕ cos ⁡ ψ sin ⁡ θ cos ⁡ ϕ + sin ⁡ ψ sin ⁡ ϕ sin ⁡ ψ cos ⁡ ϕ sin ⁡ ψ sin ⁡ θ sin ⁡ ϕ + cos ⁡ ψ cos ⁡ ϕ sin ⁡ ψ sin ⁡ θ cos ⁡ ϕ − cos ⁡ ψ sin ⁡ ϕ − sin ⁡ θ cos ⁡ θ sin ⁡ ϕ cos ⁡ θ cos ⁡ ϕ ] R_{wb}=R_{bw}^T= \left[

$$\begin{matrix} \cos\psi\cos\theta & \cos\psi\sin\theta\sin\phi-\sin\psi\cos\phi & \cos\psi\sin\theta\cos\phi+\sin\psi\sin\phi \\ \sin\psi\cos\phi & \sin\psi\sin\theta\sin\phi+\cos\psi\cos\phi & \sin\psi\sin\theta\cos\phi-\cos\psi\sin\phi \\ -\sin\theta & \cos\theta\sin\phi & \cos\theta\cos\phi \end{matrix}$$ \right] Rwb​=RbwT​=⎣⎡​cosψcosθsinψcosϕ−sinθ​cosψsinθsinϕ−sinψcosϕsinψsinθsinϕ+cosψcosϕcosθsinϕ​cosψsinθcosϕ+sinψsinϕsinψsinθcosϕ−cosψsinϕcosθcosϕ​⎦⎤​

• 旋转矩阵到欧拉角
  假设$R=m_{ij},i,j\in [1,2,3]$

1. 正常情况：
   $$roll=\varphi =\arctan (\frac{m_{32}}{m_{33}})$$

$$pitch=\theta =\arctan (\frac{-m_{31}}{\sqrt{m_{32}^2+m_{33}^2}})$$

$$yaw=\psi =\arctan (\frac{m_{21}}{m_{11}})$$

2. 如果