参考的博客:
https://blog.csdn.net/yang__jing/article/details/82316093
https://blog.csdn.net/u011092188/article/details/77430988
https://blog.csdn.net/u011178262/article/details/84479181#_1
理论
不同的几何编程库所基于的坐标系的手性会有所不同
- Eigen: 右手系
- OpenGL: 右手系
- Unity3D: 左手系
- ROS tf: 右手系
区分 点的变换 和 坐标系本身的变换:
指的是 将某点在B坐标系中的坐标表示变换为其在A坐标系中的坐标表示,实质是同一点在不同坐标系下的不同坐标表示,即 点的变换;若将A和B坐标系假设为刚体,则B坐标系变换到A坐标系(坐标系本身的变换)的变换矩阵为:
使用传感器(Camera-IMU)标定工具(例如Kalibr)标定出的外参指的是 点的变换
ROS中 static_transform_publisher 则是 坐标系本身的变换
在分析多个坐标系的姿态变换时,要注意根据点的变换或者坐标系的变换确定矩阵左乘还是右乘:
- 点的变换:矩阵相乘 从右到左,即 矩阵左乘
- 坐标系的变换:矩阵相乘 从左到右,即 矩阵右乘
1.欧式变换:
2.单位四元数
3.欧拉角
4.坐标变换
已知坐标系0,和坐标系1,已知坐标系0的姿态R0,t0,坐标系1的姿态R1,t1。
则从坐标系0到坐标1的坐标变换t,R为:
Eigen::Vector3d t0,t1;
Eigen::Matrix3d R0,R1;
Eigen::Vector3d t = R0.transpose() * (t1 - t0);
Eigen::Matrix3d R = R0.transpose() * R1;
Eigen库变换
旋转矩阵(3X3):Eigen::Matrix3d
旋转向量(3X1):Eigen::AngleAxisd
四元数(4X1): Eigen::Quaterniond
平移向量(3X1):Eigen::Vector3d
变换矩阵(4X4):Eigen::Isometry3d
一.旋转矩阵
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.旋转矩阵转欧拉角(Z-Y-X,即RPY)
Eigen::Vector3d eulerAngle=rotation_matrix.eulerAngles(2,1,0);
3.旋转矩阵转四元数
Eigen::Quaterniond quaternion(rotation_matrix);
Eigen::Quaterniond quaternion;
quaternion=rotation_matrix;
二.欧拉角
1.初始化欧拉角(Z-Y-X,即RPY)
Eigen::Vector3d eulerAngle(yaw,pitch,roll);
2.欧拉角转旋转矩阵
Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitZ()));
Eigen::Matrix3d rotation_matrix;
rotation_matrix=yawAngle*pitchAngle*rollAngle;
3.欧拉角转四元数
Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitZ()));
Eigen::Quaterniond quaternion;
quaternion=yawAngle*pitchAngle*rollAngle;
三.四元数
- 初始化四元数
Eigen::Quaterniond quaternion(w,x,y,z);
- 四元数转旋转矩阵
Eigen::Matrix3d rotation_matrix;
rotation_matrix=quaternion.matrix();
Eigen::Matrix3d rotation_matrix;
rotation_matrix=quaternion.toRotationMatrix();
- 四元数转欧拉角(Z-Y-X,即RPY)
Eigen::Vector3d eulerAngle=quaternion.matrix().eulerAngles(2,1,0);