Eigen 教程进阶篇（三）

void Test2DRotation()
{
  Eigen::Rotation2D<double> rot(M_PI/2);

  Eigen::Vector2d v0;
  v0 << 1.4, -3.5;

  auto v1 = rot * v0;
  ROS_INFO_STREAM("after rotation, v1:" << std::endl << v1);
}

3d 旋转

右手坐标系和左手坐标系

不管是右手坐标系，还是左手坐标系，从大拇指到中指的顺序都是 X 轴，Y 轴，Z 轴的正方向！

内旋，外旋

绕着自身外的固定轴旋转，称为外旋。

绕着自身旋转，称为内旋。

旋转方向：

右手大拇指朝向旋转轴的正方向，其余 4 个手指握住，其他手指的朝向为旋转角度的正方向。

下图中以右手坐标系的 Z 轴为例，从 x 轴正方向转向 y 轴正方向的角度为正旋转角度。

欧拉角

欧拉角不是描述一个静态的姿态角，而是描述一种旋转的过程！

欧拉角描述把坐标系（一般是右手坐标系）从静态，按照一定顺序把每个轴（一般是 Z，Y，X 轴，也就是 yaw 角，pitch角，roll角）旋转一次。最后一次得到新的姿态。

Urdf 文件中的 <r,p,y> 参数，就是以 Z，Y，X 轴的顺序旋转，描述坐标系的姿态。

假设坐标系 link1 位于坐标系 base_link 上方 1 米位置（ base_link 是 link1 的父坐标系）：

<robot name="simple_robot">
  <link name="base_link"/>
  <link name="link1"/>
  
  <joint name="joint1" type="fixed">
    <parent link="base_link"/>
    <child link="link1"/>
    <origin xyz="0 0 1" rpy="0 0 0"/>
  </joint>
</robot>

坐标系关系如下，其中红色是 x 轴正方向，绿色是 y 轴正方向，蓝色是 z 轴正方向。

首先，令 link1 的欧拉角 yaw = 1.570796（90°），link1 绕 z 轴（蓝色）逆时针旋转了 90°。
接着令 link1 的欧拉角 pitch = 0.785398（45°），link1 绕 y 轴（绿色）逆时针旋转了 45°
最后令 link1 的欧拉角 roll = -0.523598（-30°），link1 绕 x 轴（红色）顺时针旋转 30°：

最后的 urdf 文件如下：

<robot name="simple_robot">
  <link name="base_link"/>
  <link name="link1"/>
  
  <joint name="joint1" type="fixed">
    <parent link="base_link"/>
    <child link="link1"/>
    <origin xyz="0 0 1" rpy="-0.523598 0.785398 1.570796"/>
  </joint>
</robot>

欧拉角的万向锁

万向锁发生在内旋过程中，外旋并不会有这个问题。其次，万向锁影响的是用欧拉角表示连续的旋转过程，所以用欧拉角表示单次旋转后的姿态（静态相对位姿，urdf 描述了静态 tf ）是没问题的。

这里搬运一个欧拉角万象锁的例子，个人觉得比较生动。

一个简单直观的例子是炮塔模型。假设地面上的一个炮塔有两个旋转轴：Y垂直于地面，使炮塔可以平行地面360度旋转（正北设为0度）；X平行于地面，使炮口可以绕着它上下90度旋转（平行地面使设为0度）。现在，天空中的任意一点就可以使用两个坐标的度数来表示了！

这时，一架敌机从正东面飞来，我们转动炮塔对准它，目前的坐标是（10,90）。因为飞机飞行方向不变，所以Y固定为90，而X由于飞机距离的接近而增大。当飞机恰好飞到炮塔顶端时，即X的角度也达到90度时，飞机忽然向南飞行！我们必须立即改变炮塔朝向，否则即将都丢失目标。但是我们发现，无论是转动X轴还是Y轴，我们都无法让炮塔转向南方了，即炮塔在此时丢失了一个自由度！

欧拉角优缺点

优点：简单易懂

缺点：xyz 轴的旋转顺序影响结果；存在万向锁，不适合描述连续旋转；

轴角 Axis-Angle

顾名思义，用一个旋转轴和旋转角度来描述三维物体围绕轴体旋转的过程。采用右手坐标系，大拇指方向为旋转轴正方向，四指握住的方向为旋转的正方向。

下图中，空间中点 A 和 B 确定了一个三维向量作为旋转轴，轴体正方向定义为 A 指向 B，那么从 A 向 B 的方向观察，顺时针为旋转的正方向。

void Test3DAngleAxisRotation()
{
  Eigen::Vector3d axis(0.5, 1.5, -1.0);
  double angle = M_PI / 6;
  Eigen::AngleAxisd angle_axis0(angle, axis);
  Eigen::Matrix3d rotationMatrix0 = angle_axis0.toRotationMatrix();

  Eigen::Vector3d p0(1.0, -0.5, 0.5);
  ROS_INFO_STREAM("after rotation, p0:" << std::endl << rotationMatrix0*p0);
}

效果如下：黄色向量 AB 是旋转轴，绿色向量 AC 是 p0 向量，紫色向量 AD 是旋转后的向量。

按照定义，向量 p0 的起点 A 和终点 C 应该分别在其垂直于旋转轴的平面内旋转 angle 角度，也就是向量 AC 和向量 AD 的起点，终点共面，且该面垂直与旋转轴。

这里旋转前后的向量起点相同，只要观察旋转后的终点即可。

过点 C 垂直于向量 AB ，相交与点 E，E 点就是垂足。

然后过点 E 垂直于向量 AB 得到垂直平面，下图中蓝灰色所示。可以看到左下角中旋转后的向量 AD 是穿过平面了。

换个角度看的更清楚。

出现这种问题，首先想到的就是旋转轴的轴体不是单位向量。下面测试单位向量的旋转轴效果。

void Test3DAngleAxisRotation()
{
  Eigen::Vector3d axis(0.5, 1.5, -1.0);
  double angle = M_PI / 6;
  Eigen::AngleAxisd angle_axis0(angle, axis);
  Eigen::Matrix3d rotationMatrix0 = angle_axis0.toRotationMatrix();

  Eigen::Vector3d p0(1.0, -0.5, 0.5);
  ROS_INFO_STREAM("angle axit without normalized:" << axis << std::endl <<
    "after rotation, p0:" << std::endl << rotationMatrix0*p0);

  Eigen::AngleAxisd angle_axis1(angle, axis.normalized());
  Eigen::Matrix3d rotationMatrix1 = angle_axis1.toRotationMatrix();
  ROS_INFO_STREAM("angle axit with normalized:" << axis.normalized() << std::endl <<
    "after rotation, p0:" << std::endl << rotationMatrix1*p0);
}

经过单位化后得到旋转轴 AF，与原来旋转轴 AB 共线，重新计算得到的旋转后向量 AH，H 和旋转前向量 AC 的终点 C 终于共面了！

所以，eigen3 中，轴角的使用必须让旋转轴轴体单位化，不然计算结果可能有误！

四元数

复数由一个实数加上一个虚数组成，其中虚数单位 $i^2=-1$ 。

四元数由一个实数加上三个虚数 i，j，k 构成，表示为实数和 i，j，k 的线性组合：

$q=w+xi+yj+zk$

其中 $i^2=j^2=k^2=ijk=-1$ 。

只有单位四元数才能够表示旋转。即 $w^2+i^2+j^2+k^2= 1$ 。

大佬 3Blue1Brown 用球极投影可视化了四元数旋转过程，相关视频适合反复观看，细细品味。

欧拉角和四元数之间的转换，四元数的旋转

void Test3DQuaternion()
{
  double roll = 0.123, pitch = 0.234, yaw = 0.345;

  Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX());
  Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());
  Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ());

  Eigen::Quaterniond q = yawAngle * pitchAngle * rollAngle; // 按 yaw，pitch，roll 顺序旋转
  ROS_INFO_STREAM("quaternion w, x, y, z: " << q.w() << " " << q.x() << " " << q.y() << " " << q.z());

  Eigen::Matrix3d rotationMatrix = q.toRotationMatrix();
  Eigen::Vector3d euler = rotationMatrix.eulerAngles(2, 1, 0);
  ROS_INFO_STREAM("q2ryp, roll:" << euler[0] << ", pitch:" << euler[1] << ", yaw:" << euler[2]);

  // 四元数旋转
  Eigen::Vector3d p (0.112, 0.223, 0.344);
  ROS_INFO_STREAM("before rotate, p:" << std::endl << p);
  Eigen::Quaterniond p_quaternion(0, p[0], p[1], p[2]); // 需要把点转为四元数格式
  Eigen::Quaterniond rotated_p_quaternion = q * p_quaternion * q.conjugate();
  Eigen::Vector3d rotated_p(rotated_p_quaternion.x(), rotated_p_quaternion.y(), rotated_p_quaternion.z());
  ROS_INFO_STREAM("after quaternion rotate, rotated_p:" << std::endl << rotated_p);
}