eigen学习(5)——Geometry

eigen

---

1 Space transformations介绍

geometry模块提供的变换有两类：

• 2D and 3D rotations and projective
• affine transformations

Note:
如果你使用OpenGL 4x4矩阵，那么Affine3f和Affine3d就是你想要的。由于Eigen默认为列主存储，所以可以直接使用Transform::data()方法将转换矩阵传递给OpenGL。

• 旋转矩阵(3 × 3):Eigen::Matrix3d。
• 旋转向量(3 × 1):Eigen::AngleAxisd。
• 欧拉角(3 × 1):Eigen::Vector3d。
• 四元数(4 × 1):Eigen::Quaterniond。
• 欧氏变换矩阵(4 × 4):Eigen::Isometry3d。
• 仿射变换(4 × 4):Eigen::Affine3d。
• 射影变换(4 × 4):Eigen::Projective3d

2 欧式变换

// 欧氏变换矩阵使用 Eigen::Isometry
Eigen::Isometry3d T=Eigen::Isometry3d::Identity();                // 虽然称为3d，实质上是4＊4的矩阵

• 2d旋转

Rotation2D<float> rot2(angle_in_radian);

• 3d旋转

//角轴
AngleAxis<float> aa(angle_in_radian, Vector3f(ax,ay,az));
//旋转矩阵
Matrix3f m(aa);
//四元数
Quaternion<float> q;
q = AngleAxis<float>(angle_in_radian, axis);
//欧拉角
Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles ( 2,1,0 ); // ZYX顺序，即roll pitch yaw顺序
Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles ( 2,1,2 ); // ZYZ顺序

• prerotate和rotate区别

Eigen::Isometry3d trans = Eigen::Isometry3d::Identity();//欧式变换矩阵4x4
trans.prerotate(R);//T左乘一个R
trans.pretranslate(t);//T左乘一个t
trans.rotate(R)；//右乘一个R
trans.translate(t);//右乘一个t
//可以通过如下方法初始化一个trans
    Eigen::Isometry3d trans = Eigen::Isometry3d::Identity();//欧式变换矩阵4x4
    trans.prerotate(R);//一定要先设置旋转，在设置平移，不然结果不对
    // trans.rotate(R);也可以
    trans.pretranslate(t);
    cout << "rotation matrix: \n" << R << endl;
    cout << "transfrom matrix:\n" << trans.matrix() << endl;

3 旋转矩阵和欧拉角的理解和代码实践

3.1 一些设定

• 坐标系设定：x向前(roll)，y向左(pitch)，z向上(yaw)的右手系。
• 世界坐标系W（也成惯性系I或者target frame或者reference frame）
• 机体坐标系B（也称source frame）

3.2 从I系到B系的坐标变换

此时我们使用ZYX欧拉角表示方法

$$R_{BI}=R_x(roll)*R_y(pitch)*R_z(yaw)$$
其中，
$R_z(yaw) \space \space = \left[\begin{array}{ccc}{\cos(y)} & {\sin (y)} & {0} \\ {-\sin (y)} & {\cos (y)} & {0} \\ {0} & {0} & {1}\end{array}\right]$

$R_y(pitch)=\left[\begin{array}{ccc}{\cos (p)} & {0} & {-\sin (p)} \\ {0} & {1} & {0} \\ {\sin(p)} & {0} & {\cos (p)}\end{array}\right]$

$R_x(roll) \space \space = \left[\begin{array}{ccc}{1} & {0} & {0} \\ {0} & {\cos (r)} & {\sin (r)} \\ {0} & {-\sin (r)} & {\cos (r)}\end{array}\right]$

3.3 从B系到I系的坐标变换(B在I中的位姿)

注意这里我们定义的这些旋转矩阵是从I系到B系的，也就是认为B系是target frame的。
source在target中的位姿，这个最常用。

$$R_{IB}=R_z(yaw)^T*R_y(pitch)^T*R_x(roll)^T$$

在Eigen中的实践

//使用角轴表示的旋转对应着我们这里的R_z(yaw)^T，也就是认为I系是target frame的
Eigen::AngleAxisd init_rotation_z(0.3, Eigen::Vector3d::UnitZ());

所以表示从B系到I系（B中的点变换到I中的点）是

    Eigen::Translation3d init_translation(1, 2, 3);
    Eigen::AngleAxisd init_rotation_x(0.1, Eigen::Vector3d::UnitX());
    Eigen::AngleAxisd init_rotation_y(0.2, Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd init_rotation_z(0.3, Eigen::Vector3d::UnitZ());
    Eigen::Matrix4d init_guess1 = (init_translation*init_rotation_z*init_rotation_y*init_rotation_x).matrix();