三维空间刚体运动（未完）

三维空间刚体运动

1. 旋转矩阵与旋转向量

1.1 旋转向量

旋转向量只需要四位就能够指定：

• 旋转角度：1位
• 旋转轴：3位（x、y、z）

Eigen表示：

// theta为旋转角，x为x轴，y为y轴，z为z轴
Eigen::AngleAxisd rotation_vector (theta, Eigen::Vector3d(x, y, z))

1.2 旋转向量

旋转向量的确定方法为：

对于x轴旋转$\theta$，对于y轴旋转$\alpha$，对于z轴旋转$\beta$。这三个角度分别为roll，pitch，YAW

而旋转矩阵分别为：

$$\begin{gathered} R_x= \\ [100 \\ 0cos\theta -sin\theta \\ 0sin\theta cos\theta ] \end{gathered}$$

$$\begin{gathered} R_y= \\ [cos\alpha 0sin\alpha \\ 010 \\ -sin\alpha 0cos\alpha ] \end{gathered}$$

$$\begin{gathered} R_z= \\ [cos\beta -sin\beta 0 \\ sin\beta cos\beta 0 \\ 001] \end{gathered}$$

由以上矩阵可以得到最终的旋转矩阵：

$R=R_x\ast R_y\ast R_z$

1.3 旋转向量与旋转矩阵的转化

方法：Rodrigue旋转向量公式

// 由旋转向量转过来
Eigen::Matrix3d rotation_matrix = rotation_vector.toRotationMatrix();
// 当然，你也可以转过去
rotarion_vector.fromRotationMatrix(rotation_matrix);

1.4 旋转向量和旋转矩阵的缺点

• 不直观：真正到使用的时候，难以看懂其表示的转向如何。
• 旋转向量具有冗余性：9个量描述3DOF的旋转。

基于这种缺点，我们提出了一个概念——欧拉角！

2. 欧拉角

2.1 欧拉角的优点

上次说道，旋转矩阵&旋转向量的缺点是不直观。

那么，我们就使用了更加直观的表达方式——欧拉角！它将旋转分离为三个转角。也就是说，一个旋转分解为了三次绕不同轴的旋转。

2.2 欧拉角的旋转方式

欧拉角有ZYZ、ZYX的旋转方式。其中，著名的yaw-pitch-roll方式**(rpy)**就是ZYX这种方式。

2.3 表达方式

// 从旋转矩阵到欧拉角
Eigen::Vector3d euler_angles = rotationVector.matrix().eulerAngles(0,1,2); //ZYX方式
cout<<"eulerAngle roll pitch yaw\n"<< euler_angles <<endl;

2.4 欧拉角的缺点

万向锁问题，俯仰角（pitch）为±90°时，出现错误！

3. 四元数

上面讲到欧拉角由奇异性，而四元数恰恰解决了这个问题。

3.1

四元数的变换：

Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector ); //从旋转矩阵到四元数

4. 变换矩阵

上面提到这么多，都是关于旋转而不存在平移的。

变换矩阵：

$$\begin{gathered} [R_{3x3}{T}_{3x1} \\ {0}_{1x3}{1}_{1x1}{]}_{4X4} \end{gathered}$$

其中的$R_{3x3}$就是旋转矩阵，T是平移矩阵

Eigen::Isometry3d T=Eigen::Isometry3d::Identity();
T.rotate(rotation_vector);	//加入旋转矩阵
Eigen::Vector transform = Eigen::Vector(1,3,4);
T.pretranslate(transform); //加入平移矩阵

5. 对某向量进行旋转or变换

假设我有一个向量（1,0,0）

Eigen::Vector3d v ( 1,0,0 );

现在我要对其进行坐标变换

Eigen::Vector3d v_rotated = rotation_vector*v;  // 旋转向量坐标转化
v_rotated = rotation_matrix * v;	// 旋转矩阵坐标转化
Eigen::Vector3d v_transformed = T*v;  // 变换矩阵坐标转化

6. 总结

/*
旋转矩阵（3X3）:Eigen::Matrix3d
旋转向量（3X1）:Eigen::AngleAxisd
四元数（4X1）:Eigen::Quaterniond
平移向量（3X1）:Eigen::Vector3d
变换矩阵（4X4）:Eigen::Isometry3d
*/

其中，欧拉角用来直观感受旋转方式，旋转矩阵与旋转向量进行旋转，变换矩阵增加了平移的方式。