slam14讲—ch3—eigen库学习

表示旋转

• 旋转矩阵
• 旋转向量
• 欧拉角
• 四元数

定义和转换

• 定义矩阵

Matrix3d

Matrix<float, 2, 3> matrix_23;
//定义一个零矩阵
Matrix3d matrix_33 = Matrix3d::Zero(); 
//定义一个矩阵，初始化
Matrix3d rotation_matrix = Matrix3d::Identity();

• 定义欧拉角

Vector3d

Vector3d euler_angles;

• 定义一个旋转向量

AngleAxisd

//沿 Z 轴旋转 45 度

AngleAxisd rotation_vector(M_PI / 4, Vector3d(0, 0, 1));   

旋转向量转换成旋转矩阵

rotation_matrix = rotation_vector.toRotationMatrix();

旋转矩阵转化成欧拉角

// ZYX顺序，即yaw-pitch-roll顺序
euler_angles = rotation_matrix.eulerAngles(2, 1, 0); 

• 定义四元数

Quaterniond

Quaterniond q

旋转向量转化成四元数

q = Quaterniond(rotation_vector);

旋转矩阵转化成四元数

q = Quaterniond(rotation_matrix);

输出四元数

cout << "quaternion from rotation vector = " << q.coeffs().transpose()
       << endl;   // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部

表示旋转示例

//一个坐标
Vector3d v(1, 0, 0);
//旋转向量表示旋转
Vector3d v_rotated = rotation_vector * v;
cout << "(1,0,0) after rotation (by angle axis) = " << v_rotated.transpose() << endl;
// 旋转矩阵表示旋转
v_rotated = rotation_matrix * v;
cout << "(1,0,0) after rotation (by matrix) = " << v_rotated.transpose() << endl;
v_rotated = q * v; // 注意数学上是qvq^{-1}
cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;

表示欧式变换：旋转+平移

• 定义变换矩阵，并赋值，旋转和平移量

Isometry3d

// 欧氏变换矩阵使用 Eigen::Isometry
Isometry3d T = Isometry3d::Identity();// 虽然称为3d，实质上是4＊4的矩阵
T.rotate(rotation_vector); // 按照rotation_vector进行旋转
T.pretranslate(Vector3d(1, 3, 4));// 把平移向量设成(1,3,4)

• 输出变换矩阵

//输出变换矩阵
cout << "Transform matrix = \n" << T.matrix() << endl;

• 表示坐标系的变换

// 用变换矩阵进行坐标变换
Vector3d v_transformed = T * v;// 相当于R*v+t
cout << "v tranformed = " << v_transformed.transpose() << endl;