@
- Vector3.normalized的特点是当前向量是不改变的并且返回一个新的规范化的向量；
- Vector3.Normalize的特点是改变当前向量，也就是当前向量长度是1
前言：国内复制粘贴太严重了，都不验证一下，欧拉角部分全网都是2,1,0的排序但经过自己验证0,1,2才是正确的（如有错误请指正）。
一、旋转向量
1.1 初始化旋转向量
旋转角为alpha(顺时针)，旋转轴为(x,y,z)
Eigen::AngleAxisd rotation_vector(alpha,Vector3d(x,y,z))
Eigen::AngleAxisd yawAngle(alpha,Vector3d::UnitZ());
1.2 旋转向量转旋转矩阵
Eigen::Matrix3d rotation_matrix;
rotation_matrix=rotation_vector.matrix();
Eigen::Matrix3d rotation_matrix;
rotation_matrix=rotation_vector.toRotationMatrix();
1.3 旋转向量转欧拉角(xyz，即RPY)
Eigen::Vector3d eulerAngle=rotation_vector.matrix().eulerAngles(0,1,2);
1.4 旋转向量转四元数
Eigen::Quaterniond quaternion(rotation_vector);
Eigen::Quaterniond quaternion;
Quaterniond quaternion;
Eigen::Quaterniond quaternion;
quaternion=rotation_vector;
二、旋转矩阵
2.1 初始化旋转矩阵
Eigen::Matrix3d rotation_matrix;
rotation_matrix<<x_00,x_01,x_02,x_10,x_11,x_12,x_20,x_21,x_22;
2.2 旋转矩阵转旋转向量
Eigen::AngleAxisd rotation_vector(rotation_matrix);
Eigen::AngleAxisd rotation_vector;
rotation_vector=rotation_matrix;
Eigen::AngleAxisd rotation_vector;
rotation_vector.fromRotationMatrix(rotation_matrix);
2.3 旋转矩阵转欧拉角(xyz，即RPY)
Eigen::Vector3d eulerAngle=rotation_matrix.eulerAngles(0,1,2);
2.4 旋转矩阵转四元数
Eigen::Quaterniond quaternion(rotation_matrix);
Eigen::Quaterniond quaternion;
quaternion=rotation_matrix;
三、欧拉角
3.1 初始化欧拉角(xyz，即RPY)
Eigen::Vector3d eulerAngle(roll,pitch,yaw);
3.2 欧拉角转旋转向量
Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitZ()));
Eigen::AngleAxisd rotation_vector;
rotation_vector=yawAngle*pitchAngle*rollAngle;
3.3 欧拉角转旋转矩阵
Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitZ()));
Eigen::Matrix3d rotation_matrix;
rotation_matrix=yawAngle*pitchAngle*rollAngle;
3.4 欧拉角转四元数
Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitZ()));
Eigen::Quaterniond quaternion;
quaternion=yawAngle*pitchAngle*rollAngle;
四、四元数
4.1 初始化四元数
Eigen::Quaterniond quaternion(w,x,y,z);
4.2 四元数转旋转向量
Eigen::AngleAxisd rotation_vector(quaternion);
Eigen::AngleAxisd rotation_vector;
rotation_vector=quaternion;
4.3 四元数转旋转矩阵
Eigen::Matrix3d rotation_matrix;
rotation_matrix=quaternion.matrix();
Eigen::Matrix3d rotation_matrix;
rotation_matrix=quaternion.toRotationMatrix();
4.4 四元数转欧拉角(xyz，即RPY)
Eigen::Vector3d eulerAngle=quaternion.matrix().eulerAngles(0,1,2);
五、Eigen::Affine3f和Eigen::Matrix4f的转换
Eigen::Affine3f A;
Eigen::Matrix4f M;
M = A.matrix();
A = M;
六、float 和 double类型转换
Eigen::MatrixXd matrix_d；
Eigen::MatrixXf matrix_f；
matrix_f = matrix_d.cast<float>();
绕固定坐标系转和绕当前坐标系旋转
//
// Created by qian on 2021/3/7.
//
#include "iostream"
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>
using namespace Eigen;
using namespace std;
int main(int argc, char **argv){
Vector3d t0(1,2,3);// 初始向量
Vector3d t(3,2,1); // 平移向量
const float angle_x=30.0, angle_y=20.0, angle_z=10.0;
// *************绕XYZ为当前坐标系旋转轴************
AngleAxisd rotation_X=AngleAxisd(angle_x/180.0*M_PI,Vector3d::UnitX());
//固定坐标系的 Y 轴先投影成 X 旋转后的旋转轴,再进行旋转
AngleAxisd rotation_Y=AngleAxisd(angle_y/180.0*M_PI,rotation_X.inverse()*Vector3d::UnitY());
//固定坐标系的 Z 轴先投影成 XY 旋转后的旋转轴,再进行旋转
AngleAxisd rotation_Z=AngleAxisd(angle_z/180.0*M_PI,rotation_Y.inverse()*rotation_X.inverse()*Vector3d::UnitZ());
// 旋转向量
AngleAxisd rotation_vector1;
rotation_vector1=rotation_X*rotation_Y*rotation_Z;
cout<<"旋转角："<<rotation_vector1.angle()*180.0/M_PI<<", 旋转轴"<<rotation_vector1.axis().transpose()<<endl;
// 旋转矩阵
Matrix3d rotation_matrix1(rotation_vector1);
cout<<"旋转矩阵：\n"<<rotation_matrix1<<endl;
// 四元数
Quaterniond q1(rotation_matrix1);
cout<<"四元数：\n"<<q1.coeffs()<<endl;
// 齐次欧式变换
Isometry3d T=Isometry3d::Identity();
T.rotate(rotation_vector1);
T.pretranslate(t);
cout<<"齐次欧式变换：\n"<<T.matrix()<<endl;
cout<<"旋转平移变换后的向量"<<T*t0<<endl;
// **********绕XYZ为世界固定坐标系旋转轴**********
AngleAxisd rotation_X2=AngleAxisd(angle_x/180.0*M_PI,Vector3d::UnitX());
AngleAxisd rotation_Y2=AngleAxisd(angle_y/180.0*M_PI,Vector3d::UnitY());
AngleAxisd rotation_Z2=AngleAxisd(angle_z/180.0*M_PI,Vector3d::UnitZ());
// 旋转向量
AngleAxisd rotation_vector2;
rotation_vector2=rotation_Z2*rotation_Y2*rotation_X2;
cout<<"旋转角："<<rotation_vector2.angle()*180.0/M_PI<<", 旋转轴"<<rotation_vector2.axis().transpose()<<endl;
// 旋转矩阵
Matrix3d rotation_matrix2(rotation_vector2);
cout<<"旋转矩阵：\n"<<rotation_matrix2<<endl;
// 四元数
Quaterniond q2(rotation_matrix2);
cout<<"四元数：\n"<<q2.coeffs()<<endl;
// 齐次欧式变换
Isometry3d T2=Isometry3d::Identity();
T2.rotate(rotation_vector2);
T2.pretranslate(t);
cout<<"齐次欧式变换：\n"<<T2.matrix()<<endl;
cout<<"旋转平移变换后的向量"<<q2*t0+t<<endl;
cout<<rotation_matrix1*rotation_matrix2.transpose()<<endl;
return 0;
}
