视觉SLAM学习笔记4——三维空间刚体运动

目录

一、刚体运动简介

二、eigen几何模块——Geometry编程实现

1、调用eigen库

2、旋转矩阵定义

3、使用旋转角旋转轴定义

4、也可以直接对旋转矩阵赋值

6、可以用角轴进行坐标变换

7、用旋转矩阵进行坐标变换

8、旋转矩阵转欧拉角

9、变换矩阵T

10、使用变换矩阵进行坐标变换

11、四元数

---

一、刚体运动简介

两个不同坐标系之间的的运动由一个旋转加一个平移组成，被称作刚体运动。

旋转矩阵是一个行列式为1的正交阵，反之也成立。

变换矩阵T是一个4*4的矩阵，其中包含了旋转矩阵与平移向量。

由于旋转矩阵是有9个量，用它来描述只有三个自由度的旋转是较为冗余的。

下面介绍另外的几种旋转表示形式：

（1）旋转轴+旋转角

旋转轴与旋转角二者可以用一个向量表示，其方向与旋转轴一致，模长等于旋转角。而对于变换矩阵，可以只用旋转向量+平移向量共6个参数来描述6自由度的刚体运动。

（2）欧拉角

欧拉角提供了一种直观的方式来描述旋转，它把一次旋转分解为3次绕不同轴的旋转，定义好旋转轴的顺序后（XYZ、YZX、ZXY），只用一个三维向量就可以描述任意旋转。

但会遇到万向锁问题，即第一次绕Z轴旋转，第二次绕Y轴旋转90度，第三次旋转则与第一次相同，使得系统丢失一个自由度。

（3）四元数

在二维平面，旋转可由单位复数来表示。同样，三维旋转可由单位四元数来表示。

二、eigen几何模块——Geometry编程实现

1、调用eigen库

#include <Eigen/Core>
#include <Eigen/Geometry>

2、旋转矩阵定义

 // 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f
Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
cout<<"Identity"<<endl<<rotation_matrix<<endl;

将旋转矩阵初始化为单位阵，表示无旋转：

Identity
1 0 0
0 1 0
0 0 1

3、使用旋转角旋转轴定义

// 旋转向量使用 AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）
Eigen::AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d ( 0,0,1 ) );   //沿 Z 轴旋转 45 度
cout .precision(4);         //精度
cout<<"rotation matrix =\n"<<rotation_vector.matrix() <<endl;     //用matrix()转换成矩阵

rotation matrix =
 0.7071 -0.7071       0
 0.7071  0.7071       0
      0       0       1

4、也可以直接对旋转矩阵赋值

rotation_matrix = rotation_vector.toRotationMatrix();
 cout<<"rotation matrix(toRotationMatrix) =\n"<<rotation_matrix<<endl;

rotation matrix(toRotationMatrix) =
 0.7071 -0.7071       0
 0.7071  0.7071       0
      0       0       1

6、可以用角轴进行坐标变换

// 用 AngleAxis 可以进行坐标变换
Eigen::Vector3d v ( 1,0,0 );
//cout<<"v:"<<v<<endl;
Eigen::Vector3d v_rotated = rotation_vector * v;
cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;

(1,0,0) after rotation = 0.7071 0.7071      0

7、用旋转矩阵进行坐标变换

 v_rotated = rotation_matrix * v;
cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;

(1,0,0) after rotation = 0.7071 0.7071      0

8、旋转矩阵转欧拉角

// 欧拉角: 可以将旋转矩阵直接转换成欧拉角
Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles ( 2,1,0 ); 
// ZYX顺序，即roll pitch yaw顺序
cout<<"yaw pitch roll = "<<euler_angles.transpose()<<endl;

yaw pitch roll = 0.7854     -0      0

9、变换矩阵T

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

Transform matrix = 
 0.7071 -0.7071       0       1
 0.7071  0.7071       0       3
      0       0       1       4
      0       0       0       1

10、使用变换矩阵进行坐标变换

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

v tranformed = 1.707 3.707     4

11、四元数

// 四元数
    // 可以直接把AngleAxis赋值给四元数，反之亦然
    Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector );
    cout<<"quaternion = \n"<<q.coeffs() <<endl;   // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部
    // 也可以把旋转矩阵赋给它
    q = Eigen::Quaterniond ( rotation_matrix );
    cout<<"quaternion = \n"<<q.coeffs() <<endl;
    // 使用四元数旋转一个向量，使用重载的乘法即可
    v_rotated = q*v; // 注意数学上是qvq^{-1}
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;

quaternion = 
     0
     0
0.3827
0.9239
quaternion = 
     0
     0
0.3827
0.9239
(1,0,0) after rotation = 0.7071 0.7071      0