Eigen笔记

四元数

构造函数

常用的四元数格式有Quaternionf(float)和Quaterniond(double)，模板类中的Scalar决定数据类型。
官方介绍有七种构造方式，不过常用的一般有以下几种：

• 直接赋值

Eigen::Quaternion< _Scalar, _Options >::Quaternion( const Scalar & w,const Scalar & x,const Scalar &y,const Scalar & z ) 	
//for example
Quaterniond q(1.0, 0.0, 0.0, 0.0);

要注意Eigen中四元数赋值的顺序，实数w在首；但是实际上它的内部存储顺序是[x y z w]。实际上后面输出系数的时候也是按内部存储顺序输出

• 从旋转矩阵或向量构造

Eigen::Quaternion< _Scalar, _Options >::Quaternion(const MatrixBase< Derived > & other)	
//for example
Matrix3d mat;
Quaterniond q(mat);
VectorXd vq(4);
vq<<1.0, 0, 0, 0;
Quaterniond qv(vq);

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

Eigen::Quaternion< _Scalar, _Options >::Quaternion	(const Scalar * data)	

常用函数

• 输出系数

q.coeffs();     //[x y z w]

• 输出虚部

q.vec();    //[x y z]

以上两种输出都是以Eigen中的向量Vector形式输出

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

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

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

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

• 遍历元素

cout<<q.w()<<"  "<<q.x()<<"  "<<q.y()<<"  "<<q.z()<<endl;

变换矩阵

1.Eigen::Isometry3d构造变换矩阵

1.1.对各个元素赋值

Eigen::Isometry3d T1=Eigen::Isometry3d::Identity();
T1(0,0) = 1.000000e+00, T1(0,1) = 1.197624e-11, T1(0,2) = 1.704639e-10, T1(0,3) = 3.214096e-14;
T1(1,0) = 1.197625e-11, T1(1,1) = 1.197625e-11, T1(1,2) = 3.562503e-10, T1(1,3) = -1.998401e-15;
T1(2,0) = 1.704639e-10, T1(2,1) = 3.562503e-10, T1(2,2) = 1.000000e+00, T1(2,3) = -4.041212e-14;
T1(3,0) =            0, T1(3,1) =            0, T1(3,2) =            0, T1(3,3) =             1;

1.2.通过旋转矩阵和平移向量

Eigen::Matrix3d rotation_matrix1 = Eigen::Matrix3d::Identity();
rotation_matrix1 << 1.000000e+00, 1.197624e-11, 1.704639e-10,
                    1.197625e-11, 1.000000e+00, 3.562503e-10,
                    1.704639e-10, 3.562503e-10, 1.000000e+00;
Eigen::Vector3d t1;
t1 <<  3.214096e-14, -1.998401e-15, -4.041212e-14;
    
T1=Eigen::Isometry3d::Identity();
T1.rotate ( rotation_matrix1 );
T1.pretranslate ( t1 );

注意不能直接变换矩阵赋值,像这样子会报错

T1<< 1.000000e+00, 1.197624e-11, 1.704639e-10, 3.214096e-14,
     1.197625e-11, 1.197625e-11, 3.562503e-10, -1.998401e-15,
     1.704639e-10, 3.562503e-10, 1.000000e+00, -4.041212e-14,
                0,            0,            0,              1;

2.Eigen::Matrix4d构造变换矩阵

对每一个元素赋值的方法是可行的的,我这里采用的是按矩阵块赋值

// ----3.eigen::matrix4d----
Eigen::Matrix4d T2;
T2.setIdentity();
T2.block<3,3>(0,0) = rotation_matrix1;
T2.topRightCorner(3, 1) = t1;
//T2.topRightCorner<3, 1>() = t1;

创建矩阵类型vector容器

std::vector<Eigen::Vector3d,Eigen::aligned_allocator<Eigen::Vector3d>>point;   
Eigen::Vector3d v(x,y,z);
points.push_back(v);

#include 
#include 
using namespace std;

#include 
// Eigen 几何模块
#include 

/****************************
* 本程序演示了 Eigen 几何模块的使用方法
****************************/

int main ( int argc, char** argv )
{
    // Eigen/Geometry 模块提供了各种旋转和平移的表示
    // 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f
    Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
    // 旋转向量使用 AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）
    Eigen::AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d ( 0,0,1 ) );     //沿 Z 轴旋转 45 度
    cout .precision(3);
    cout<<"rotation matrix =\n"<<rotation_vector.matrix() <<endl;                //用matrix()转换成矩阵
    // 也可以直接赋值
    rotation_matrix = rotation_vector.toRotationMatrix();
    // 用 AngleAxis 可以进行坐标变换
    Eigen::Vector3d v ( 1,0,0 );
    Eigen::Vector3d v_rotated = rotation_vector * v;
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;
    // 或者用旋转矩阵
    v_rotated = rotation_matrix * v;
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;

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

    // 欧氏变换矩阵使用 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;

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

    // 对于仿射和射影变换，使用 Eigen::Affine3d 和 Eigen::Projective3d 即可，略

    // 四元数
    // 可以直接把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;

    return 0;
}