在博文<三维空间中刚体运动>中介绍了三维空间刚体运动和坐标转换的概念，包括坐标变换的各中表示：旋转矩阵、四元数、旋转角、旋转向量、变换矩阵等，初步尝试了使用Eigen进行坐标变换。本文在此基础上，进一步总结Eigen中各种数据类型及其基本操作，便于查阅。

 

旋转矩阵

构造函数

• 单位矩阵构造
  Eigen::Matrix3d rotation_m=Eigen::Matrix3d::Identity();

• 由旋转向量构造
  rotation_m=rotation_v.toRotationMatrix();

常用函数

• 进行旋转变换

    Eigen::Vector3d p(1,0,0);//P点
    Eigen::Vector3d p_r=m*p;
    cout<<p<<endl<<"after p rotated:"<<p_r<<endl<<endl;

---

旋转向量

构造函数

• 旋转轴和旋转角构造. 比如旋转轴向量为[0,0,1],旋转角为M_PI/4时,初始化如下:
  Eigen::AngleAxisd rotation_v(M_PI/4,Eigen::Vector3d(0,0,1));

• 由旋转矩阵构造
   

常用函数

• 转换为Matrix格式
  cout<<"rotation_v.matrix:\n"<<rotation_v.matrix()<<endl<<endl;

• 使用进行旋转

    //使用旋转向量进行旋转变换
    Eigen::Vector3d p(1,0,0);//P点
    Eigen::Vector3d p_r=rotation_v*p;
    cout<<p<<endl<<"after p rotated:"<<p_r<<endl<<endl;

---

欧拉角

构造函数

• 由旋转矩阵获得

Eigen::Vector3d euler_a=rotation_m.eulerAngles(2,1,0);
cout<<"yaw pitch roll="<<euler_a.transpose()<<endl<<endl;

---

 

四元数

构造函数

• 直接赋值,eg:
  Quaterniond q(1.0,0.0,0.0,0.0);
  赋值的顺序是先实数,再虚数:w,x,y,z,而内部存储则是先虚数,再实数:x,y,z,w

• 从旋转矩阵构造,eg:

Matrix3d mat;  
Quaterniond q(mat);

• 从旋转向量构造

VectorXd vq(4);
vq<<1.0, 0, 0, 0;
Quaterniond qv(vq);

• 使用旋转的角度和旋转轴向量（此向量为单位向量）来初始化四元数,
  即使用当旋转轴向量为N=[n_x,n_y,n_z],旋转角度为A时,四元数初始化为:
  q=[cos(A/2),n_xsin(A/2),n_ysin(A/2),n_z*sin(A/2)]

Quaterniond Q1(cos((M_PI/4)/2), 0*sin((M_PI/4)/2), 0*sin((M_PI/4)/2), 1*sin((M_PI/4)/2));//以（0,0,1）为旋转轴，旋转45度

• 从数组构造
  数组的顺序应该是[w x y z]

 

常用函数

• 输出系数
  q.coeffs(); //[x y z w]

• 输出虚部
  q.vec(); //[x y z]
  以上两种输出都是以Eigen中的向量Vector形式输出

• 输出旋转矩阵
  要注意的是，只有单位四元数才表示旋转矩阵，所以要先对四元数做单位化

q.normalized();	//important
Matrix3d R=q.toRotationMatrix();

• 输出单个元素
  cout<<q.w()<<" "<<q.x()<<" "<<q.y()<<" "<<q.z()<<endl;

• 共轭/即反向旋转
  一般不用inverse，在表示旋转的时候（范数是1），共轭即可表示相反的的旋转。

    //q.inverse();
    q.conjugate();

---

 

平移向量

构造

• 直接构造
  Eigen::Vector3d t1 = Eigen::Vector3d(0.3, 0.1, 0.1);

---

 

变换矩阵

初始化

• 初始化为单位矩阵
  Eigen::Isometry3d T1 = Eigen::Isometry3d::Identity();

• 从旋转矩阵和平移向量赋值

    T1.rotate(q1.toRotationMatrix());
    T1.pretranslate(t1);

 

常用函数

• 获得Matrix:
  cout << T1.matrix() << endl;

• 求逆:
  T1.inverse()

• 变换运算

    //欧拉矩阵求解
    p2 = T2 * T1.inverse() * p1;
    cout << p2.transpose() << endl; //转置