Eigen库学习笔记

定义

Eigen 中所有向量和矩阵都是Eigen::Matrix，它是一个模板类，它的前三个参数为：数据类型，行，列

Matrix<float, 2, 3> matrix_23; // 声明一个2*3的float矩阵

同时，Eigen 通过 typedef 提供了许多内置类型，不过底层仍是Eigen::Matrix。
例如，Vector3d 实质上是 Eigen::Matrix<double, 3, 1>，即三维向量；Matrix3d 实质上是 Eigen::Matrix<double, 3, 3>
类型名的最后一个字符为d表示双精度类型,换成f表示单精度类型，两种类型不能混用，必须显示转换

Vector3d v_3d;                  //声明一个3*1的double矩阵
Matrix<double, 3, 1> v_3d;	    //与上一行是等价的
Matrix3d matrix_3d;             //声明一个3*3的double矩阵
Matrix<double, 3, 3> matrix_3d;	//与上一行是等价的

如果不确定矩阵大小，可以使用动态大小的矩阵

Matrix<double, Dynamic, Dynamic> matrix_dynamic;
MatrixXd matrix_dynamic;       //与上一行是等价的

定义矩阵

Matrix3d temp1 = MatrixXd::Identity(3, 3);   //定义3*3的单位阵，或Matrix3d::Identity()
Matrix3d temp2 = MatrixXd::Zero(3, 3);       //定义3*3的零矩阵，或Matrix3d::Zero()
Matrix3d temp3 = MatrixXd::Ones(3, 3);       //定义3*3的全1矩阵，或Matrix3d::Ones()
Matrix3d temp4 = MatrixXd::Random(3, 3);     //定义3*3的随机矩阵，或Matrix3d::Random();
Vector3d temp5 = VectorXd::Zero(3, 1);       //定义3*1的零向量，或Vector3d::Zero()
Vector3d temp6 = VectorXd::Ones(3, 1);       //定义3*1的全1向量，或Vector3d::Ones()

基本运算

转置、逆、行列式

MatrixXd R = MatrixXd::Identity(3, 3);
R << 1, 2, 3,
	5, 6, 7,
	9, 0, 0;
cout << "行数:" << R.rows() << endl;
cout << "列数:" << R.cols() << endl;
cout << "转置:" << endl << R.transpose() << endl;
cout << "逆:" << endl << R.inverse() << endl;
cout << "行列式:" << R.determinant() << endl;

用()访问矩阵中的元素

Matrix3d matrix= MatrixXd::Identity(3, 3);
for (int i = 0; i < matrix.rows(); i++) 
{
    for (int j = 0; j < matrix.cols(); j++)
    {
    	cout << matrix(i, j) << "\t";
    }
    cout << endl;
}

几何运算

Eigen/Geometry 模块提供了各种旋转和平移的表示
旋转向量、旋转矩阵和四元数在Eigen中转换关系的总结：

//下面三个变量作为下面演示的中间变量
AngleAxisd rotation_vector(M_PI / 4, Vector3d(0, 0, 1)); //旋转向量
Matrix3d rotation_matrix = rotation_vector.matrix();     //旋转矩阵
Quaterniond quaternion(rotation_vector);                 //四元数

旋转向量

旋转向量的赋值方法

1：旋转向量的直接赋值

AngleAxisd V1(M_PI / 4, Vector3d(0, 0, 1)); //以z轴为旋转轴，旋转45度
cout << "Rotation_vector1" << endl << V1.matrix() << endl;

2：旋转矩阵转换为旋转向量

//2.1：使用旋转向量的fromRotationMatrix()函数来对旋转向量赋值（注意此方法为旋转向量独有,四元数没有）
AngleAxisd V2;
V2.fromRotationMatrix(rotation_matrix);
cout << "Rotation_vector2" << endl << V2.matrix() << endl;

//2.2： 直接使用旋转矩阵来对旋转向量赋值
AngleAxisd V3;
V3 = rotation_matrix;
cout << "Rotation_vector3" << endl << V3.matrix() << endl;

//2.3： 使用旋转矩阵来对旋转向量进行初始化
AngleAxisd V4(rotation_matrix);
cout << "Rotation_vector4" << endl << V4.matrix() << endl;

3：四元数转换为旋转向量

//3.1： 直接使用四元数来对旋转向量赋值
AngleAxisd V5;
V5 = quaternion;
cout << "Rotation_vector5" << endl << V5.matrix() << endl;

//3.2：使用四元数来对旋转向量进行初始化
AngleAxisd V6(quaternion);
cout << "Rotation_vector6" << endl << V6.matrix() << endl;

旋转矩阵

旋转矩阵的赋值方法

1：使用旋转矩阵的函数来初始化旋转矩阵

Matrix3d R1 = Matrix3d::Identity();
cout << "Rotation_matrix1" << endl << R1 << endl;

2：旋转向量转换位旋转矩阵

//2.1： 使用旋转向量的成员函数matrix()来对旋转矩阵赋值
Matrix3d R2;
R2 = rotation_vector.matrix();
cout << "Rotation_matrix2" << endl << R2 << endl;

//2.2： 使用旋转向量的成员函数toRotationMatrix()来对旋转矩阵赋值
Matrix3d R3;
R3 = rotation_vector.toRotationMatrix();//由罗德里格公式进行转换
cout << "Rotation_matrix3" << endl << R3 << endl;

3：四元数转换位旋转矩阵

//3.1 使用四元数的成员函数matrix()来对旋转矩阵赋值
Matrix3d R4;
R4 = quaternion.matrix();
cout << "Rotation_matrix4" << endl << R4 << endl;

//3.2 使用四元数的成员函数toRotationMatrix()来对旋转矩阵赋值
Matrix3d R5;
R5 = quaternion.toRotationMatrix();
cout << "Rotation_matrix5" << endl << R5 << endl;

四元数

四元数由4个数 [x y z w] 构成，RotationAxis 为旋转轴，RotationAngle 为旋转的角度。下图的旋转用四元数可表示为：

// RotationAngle is in radians
x = RotationAxis.x * sin(RotationAngle / 2)
y = RotationAxis.y * sin(RotationAngle / 2)
z = RotationAxis.z * sin(RotationAngle / 2)
w = cos(RotationAngle / 2)

四元数的赋值方法

1：四元数的直接赋值

使用旋转的角度和旋转轴向量（此向量为单位向量）来初始化四元数,即使用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));
//第一种输出四元数的方式:
cout << "Quaternion1" << endl << Q1.coeffs() << endl; //coeffs的顺序是(x,y,z,w)，w为实部，前三者为虚部
 
//第二种输出四元数的方式:
cout << Q1.x() << endl << endl;
cout << Q1.y() << endl << endl;
cout << Q1.z() << endl << endl;
cout << Q1.w() << endl << endl;

2：旋转矩阵转换为四元数

//2.1: 直接使用旋转矩阵来对旋转向量赋值
Quaterniond Q2;
Q2 = rotation_matrix;
cout << "Quaternion2" << endl << Q2.coeffs() << endl;

//2.2: 使用旋转矩阵来对四元數进行初始化
Quaterniond Q3(rotation_matrix);//或Quaterniond Q3 = Quaterniond(rotation_matrix);
cout << "Quaternion3" << endl << Q3.coeffs() << endl;

3：旋转向量转换为四元数

//3.1: 直接使用旋转向量对四元数来赋值
Quaterniond Q4;
Q4 = rotation_vector;
cout << "Quaternion4" << endl << Q4.coeffs() << endl;

//3.2: 使用旋转向量来对四元数进行初始化
Quaterniond Q5(rotation_vector);//或Quaterniond Q5 = Quaterniond(rotation_vector);
cout << "Quaternion5" << endl << Q5.coeffs() << endl;

欧拉角

旋转矩阵转换为欧拉角

Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); //ZYX顺序，即yaw pitch roll顺序
cout << "yaw pitch roll = " << euler_angles.transpose() << endl;

坐标变换

用旋转向量进行旋转变换

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 = quaternion * v; //注意数学上是qvq^{-1}
cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;

欧式变换

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;