四元数旋转相乘顺序（eigen和TF）

       四元数可以避免欧拉角的奇异性，但是不直观。旋转涉及到对全局坐标系还是对局部坐标系？对四元数旋转和对坐标旋转一样吗？左乘还是右乘？

        坐标转换相对比较容易理解，四元数的旋转困扰了我很久，思考如下，如有错误请大佬指正。

        规定ZYX顺序，对应Yaw，Pitch和Roll。

        若以局部坐标系旋转，也就是先按照Z轴旋转，再绕当前坐标系Y轴旋转，再绕当前坐标系X轴旋转。四元数和常见的姿态表示均用这种方式。用公式表示

        若以固定坐标系旋转，也就是先按照Z轴旋转，再绕固定坐标系Y轴旋转，再绕固定坐标系X轴旋转。用公式表示为：

---

       

        如果用四元数连续相乘，我觉得比较好理解的方式是全都考虑成局部坐标系中。

        Eigen中使用欧拉角、四元数、旋转矩阵和角轴不在这里提及。可参考：(12条消息) 使用Eigen实现四元数、欧拉角、旋转矩阵、旋转向量之间的转换_eigen 四元数转欧拉角_一抹烟霞的博客-CSDN博客https://blog.csdn.net/qq_34213260/article/details/113651597        下面说一下Eigen中四元数和欧拉角需要注意的地方。

欧拉角转四元数：

方法一：用eigen

Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(roll,Eigen::Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(pitch,Eigen::Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(yaw,Eigen::Vector3d::UnitZ()));
Eigen::Quaterniond Q = yawAngle*pitchAngle*rollAngle;

方法二：用tf2

tf2::Quaternion tf_q;
tf_q.setRPY( roll , pitch , yaw );

这里有篇博客跟我的相反，我也不知道为什么。

(12条消息) Eigen 四元数和欧拉角相互转换_eigen 欧拉角转四元数_heroacool的博客-CSDN博客

四元数转欧拉角：

方法一：

Eigen::Vector3d euler=quaternion.matrix().eulerAngles(0,1,2);

对应的就是Roll，Pitch和Yaw，也就是XYZ。但是由于有奇异性，所以容易超过pi。

方法二：用tf2中的四元数，不会出现超过pi

Eigen::Quaterniond q;
tf2::Quaternion tf_q(q.x(),q.y(),q.z(),q.w());
tf2::Matrix3x3 eulerMatrix(tf_q);
eulerMatrix.getRPY(r,p,y);  

//发现一个新的方法
#include "tf2/impl/utils.h"
double yaw = tf2::impl::getYaw(tf_q);
//除了yaw以外还有
//void getEulerYPR(const tf2::Quaternion& q, double &yaw, double &pitch, double &roll)
//double getYaw(const tf2::Quaternion& q)
 

备注：tf2的四元数转回eigen中的四元数

tf2::Quaternion tf_q;
Eigen::Quaterniond q(tf_q.getW(),tf_q.getX(),tf_q.getY(),tf_q.getZ());

tf和eigen中四元数对坐标旋转的区别：

eigen中四元数直接左乘vector3d

Eigen::Vector3d A{x,y,z};
Eigen::Quaterniond q;
A = q*A;

tf中需要借助函数

tf2::Vector3 V{x,y,z};
tf2::Quaternion tf_q;
tf2::Vector3 V1 = tf2::quatRotate(tf_q,V);
//或者借助Transform
tf2::Transform tftrans(tf_q);
tf2::Vector3 V1 = tftrans*V;

---

 举个例子：

现在有三个坐标系，水平系、IMUA系和IMUB系，其中IMUA传感器和IMUB传感器固连，有一个固定的变换。

已知水平系下姿态和位置编号2，水平系下IMUA系的姿态编号1（原点重合），那么可得IMUA系中表示的位姿。

接着已知IMUA和IMUB之间的旋转（比如绕z轴转90度），那么可以将IMUA下的姿态，转换到IMUB下的姿态。注意这两个姿态不是同一个姿态，是同一个旋转变换。可以理解成一个无人机上有两个IMU。