[基础知识点]Eigen几何模块-旋转变换

刚体运动中的旋转通常可以由旋转矩阵，旋转向量和四元数等多种方式表示，在Eigen库中也有其对应的实现。本文主要介绍刚体运动时旋转矩阵，旋转向量和四元数的初始化以及相互转换在Eigen中的实现方式。

1. Eigen库中各种形式的表示

旋转矩阵（3X3）:Eigen::Matrix3d
旋转向量（3X1):Eigen::AngleAxisd
四元数（4X1）:Eigen::Quaterniond
平移向量（3X1）:Eigen::Vector3d
变换矩阵（4X4）:Eigen::Isometry3d

四元数笔记参考
Eigen官网-四元数部分
补充学习资料
注意：
🕐程序代码与数学表示有细微差别，例如通过运算符重载，四元数和三维向量可以直接计算乘法。
🕑只有单位四元数才表示旋转矩阵，所以要先对四元数做单位化

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

相关代码：

#include <iostream>
#include <cmath>
using namespace std;

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

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

int main(int argc, char **argv){
	// Eigen/Geometry 模块提供了各种旋转和平移的表示
	
	// 1.1 旋转矩阵初始化
	// 3D旋转矩阵直接使用Matrix3d和Matrix3f表示 3x3维
	Matrix3d rotation_matrix = Matrix3d::Identity();
	
	// 1.2 旋转向量初始化
	// 旋转向量使用AngleAxisd,它的底层不是Matrix,但运算可以当作矩阵（因为重载了运算符）
	AngleAxisd rotation_vector(M_PI / 4, Vector3d(0, 0, 1)); //沿z轴旋转45度
	cout.precision(3); // 输出保留小数点后三位
	
	// 1.3 （向量）转换成矩阵形式
	cout << "rotation matrix = \n" << rotation_vector.matrix() << endl; // 用matrix()转换成矩阵
	//阵也可以直接赋值
	rotation_matrix = rotation_vector.toRotationMatrix();
	
	// 2.1 用AngleAxis可以进行坐标变换
	Vector3d v(1, 0, 0);
	Vector3d v_rotated = rotation_vector * v;
	cout << "(1, 0, 0) after rotation (by angle axis) = " << v_rotated.transpose() << endl;
	// 2.2 用旋转矩阵进行坐标变换
	v_rotated = rotation_matrix * v;
	cout << "(1, 0, 0) after rotation (by matrix) = " << v_rotated.transpose() << endl;
	
	// 3. 欧拉角： 可以将旋转矩阵直接转换成欧拉角
	// 欧拉角直接用Vector3d表示
	Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0);  // ZYX顺序，即roll, pitch, yaw顺序
	cout << " yaw pitch roll = " << euler_angles.transpose() << endl;
	
	// 4. 欧式变换矩阵使用Eigen::Isometry
	Isometry3d T = Isometry3d::Identity(); // 虽然成为3d,但实际上是4x4矩阵
	T.rotate(rotation_vector); // 按照ratation_vector进行旋转
	T.pretranslate(Vector3d(1, 3, 4)); // 把平移向量设置成（1， 3， 4）
	cout << "Transform matrix = \n" << T.matrix() << endl;
	
	// 5. 用变换矩阵进行坐标变换
	Vector3d v_transformed = T * v; // 相当于R*V+t
	cout << "v transformed =" << v_transformed.transpose() << endl;
	
	// 6. 对于仿射变换和影射变换，使用Eigen::Affine3d和Eigen::Projective3d
	
	// 7. 四元数 Eigen::Quaterniond（4X1）
	// 7.1 可以直接把AngleAxis赋值给四元数，反之亦然
	Quaterniond q = Quaterniond(rotation_vector); // 赋值
	//coeffs输出系数函数：的顺序是（x,y,z,w）,w是实部，前三项是虚部
	cout << " quaternion from rotation vector = " << q.coeffs().transpose() << endl; 
	// 7.2 也可以把旋转矩阵赋给它
	q = Quaterniond(rotation_matrix);
	cout << "quaternion from rotation matrix = " << q.coeffs().transpose() << endl;
	// 输出旋转矩阵,只有单位四元数才表示旋转矩阵，所以要先对四元数做单位化
	q.normalized();	//important
    Matrix3d R=q.toRotationMatrix();
    cout << R << endl;
	// 7.3 使用四元数旋转一个向量，使用重载的乘法即可
	v_rotated = q * v; // 注意数学上是qvq^{-1}
	cout << "(1，0，0) after rotation = " << v_rotated.transpose() << endl;
	// 用常规向量乘法表示，则计算如下
	cout << "should be equal to " << (q * Quaterniond(0, 1, 0, 0) * q.inverse()).coeffs().transpose() << endl;
	
	return 0;	
	
}

输出结果：

rotation matrix = 
 0.707 -0.707      0
 0.707  0.707      0
     0      0      1
(1, 0, 0) after rotation (by angle axis) = 0.707 0.707     0
(1, 0, 0) after rotation (by matrix) = 0.707 0.707     0
 yaw pitch roll = 0.785    -0     0
Transform matrix = 
 0.707 -0.707      0      1
 0.707  0.707      0      3
     0      0      1      4
     0      0      0      1
v transformed =1.71 3.71    4
 quaternion from rotation vector =     0     0 0.383 0.924
quaternion from rotation matrix =     0     0 0.383 0.924
 0.707 -0.707      0
 0.707  0.707      0
     0      0      1
(1，0，0) after rotation = 0.707 0.707     0
should be equal to 0.707 0.707     0     0