原文:使用Eigen实现四元数、欧拉角、旋转矩阵、旋转向量之间的转换
- Vector3.normalized的特点是当前向量是不改变的并且返回一个新的规范化的向量;
- Vector3.Normalize的特点是改变当前向量,也就是当前向量长度是1
一、旋转向量
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);
五、齐次欧式变换
Isometry3d T=Isometry3d::Identity(); T.rotate(rotation_vector1); T.pretranslate(t); cout<<"齐次欧式变换:\n"<<T.matrix()<<endl;
五、Eigen等距变换(Isometry,Isometry3f,Isometry3d)常用函数翻译说明
转自“Eigen等距变换(Isometry,Isometry3f,Isometry3d)常用函数翻译说明”。
- translation():无参数,返回当前变换平移部分的向量表示(可修改),可以索引[]获取各分量
- translationExt():无参数,如果当前变换是仿射的话,返回平移部分(可修改);如果是射影变换的话,返回最后一列(可修改)
- translate():有参数,用于在当前变换上应用右乘,A.translate(B)等价于A×B。注:不是把平移部分置为参数值
- pretranslate():有参数,用于在当前变换上应用左乘,A.pretranslate(B)等价于B×A。注:不是把平移部分置为参数值
- rotation():无参数,返回只读的当前变换的旋转部分,以旋转矩阵表示
- rotate():有参数,用于设置当前变换的旋转部分,输入参数可以为角轴、四元数、旋转矩阵等。A.rotate(B)等价于A×B。注:不是把旋转部分置为参数值
- prerotate():有参数,用于设置当前变换的旋转部分,输入参数可以为角轴、四元数、旋转矩阵等。A.prerotate(B)等价于B×A。注:不是把旋转部分置为参数值
- matrix():返回变换对应的矩阵(可修改)
- cols():变换对应矩阵的列数
- rows():变换对应矩阵的行数
- linear()&.linearExt():返回变换的线性部分,对于Isometry而言就是旋转对应的旋转矩阵,Eigen::Block类型
- inverse():返回当前变换的逆变换
- Identity():返回一个单位变换,一般可用于Isometry的初始化中,将变换设为一个单位阵:Isometry3d::Identity()
- setIdentity():将当前变换变成单位变换,变换对应的矩阵变成单位阵,也可用于初始化赋值
- cast():将当前类型的变换转换为其它类型的变换,如Isometry3d转Isometry3f
- data():返回一个指向变换内部矩阵的指针,矩阵按照列优先存储
- computeRotationScaling():将当前变换的线性部分分解成rotation和scaling的乘积
- computeScalingRotation():将当前变换的线性部分分解成scaling和rotation的乘积
- fromPositionOrientationScale():从一个3D对象的位置、指向以及缩放来设置一个变换
- prescale():有参数,用于在当前变换上应用左乘,A.prescale(B)等价于B×A
- scale():有参数,用于在当前变换上应用右乘,A.scale(B)等价于A×B
- preshear():只针对2D情况,沙尔变换,有参数,用于在当前变换上应用左乘,A.preshear(B)等价于B×A
- shear():只针对2D情况,沙尔变换,有参数,用于在当前变换上应用右乘,A.shear(B)等价于A×B
- affine():无参数,返回当前变换的仿射部分(可修改),对于Isometry3d而言,返回的是一个4×3的矩阵
- makeAffine():将变换对应的矩阵的最后一行设为0,0,…,1
- isApprox():判断当前Isometry是否与给定Isometry近似相等,是的话返回true。判断精度由传入的第二个参数决定。