矩阵的几何变换(Eigen/Geometry类)

#include<iostream>
#include<cmath>
#include<eigen3/Eigen/Core>
// 几何模块
#include<eigen3/Eigen/Geometry>

using namespace std;

int main(){
	// eigen/geometry模块提供了各种旋转和平移的表示
	// 3D旋转矩阵直接使用matrix3d或matrix3f
	Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();	// 对角线为1，其余为0
	// 旋转向量使用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顺序，即yaw pitch roll顺序
	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;
	cout<< T(1,2) <<endl; 

	// 用变换矩阵进行坐标变换
	Eigen::Vector3d v_transformed = T*v;	// 相当于 R×v+t
	cout<<"v transformed = "<<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;
}