下面用代码编写的四元数的转换是什么？答案

【问题标题】：What is the tranformation of quaternions written below in code?下面用代码编写的四元数的转换是什么？
【发布时间】：2019-11-07 00:13:14
【问题描述】：

我已经在代码审查时问过这个问题，因为这是一个 c++ 代码，它可以工作，但他们说这里更好。所以我有一些代码可以将一个四元数（orix，oriy，oriz，oriw）旋转到另一个四元数，但是这种旋转写得不好，有人可以用更少的行来做。所以我有两个问题有人知道这个四元数是如何旋转的吗？我认为它以欧拉角围绕 y 旋转 -pi/2？其次，有人知道如何更好地写这个吗？

这是完整的代码，它进行了转换（使用导入 tf2）。

编辑：是的，tf2 是一个库。这里是链接：docs.ros.org/melodic/api/tf2/

            tf2::Quaternion quat(orix, -oriy, oriz, oriw);
            tf2::Quaternion q_rot;
            tf2::Vector3 rotation_vector(0.7071068, 0, 0.7071068);
            q_rot.setRotation(rotation_vector, M_PI);
            quat = q_rot*quat;
            quat.normalize();   
            tf2::Matrix3x3 matrix(quat);
            tf2::Matrix3x3 change_y(1,0, 0, 0, -1,0 ,0 ,0, 1);
            matrix = change_y * matrix;

            double roll, pitch, yaw;
            matrix.getRPY(roll, pitch, yaw);
            quat.setRPY(roll,pitch,yaw);`

【问题讨论】：

我猜你得到的matrix 实际上并不是一个旋转矩阵（但包含一个反射）。因此，我不希望 matrix.getRPY() 做任何有意义的事情。
你为什么要把Y轴改成-Y？

标签： c++ transformation ros quaternions euler-angles

【解决方案1】：

rotation_vector 实际上是定义旋转轴，因此您将围绕定义为[0.7 0 0.7] 的向量旋转M_PI，这类似于围绕y 轴旋转M_PI/4 - 从而生成一个新的坐标系与[x' y' z'] - 然后围绕您当前的z'轴旋转M_PI。

也许这个 ROS tutorial 可能会有所帮助。

这应该足以让您在给定所需的滚动、俯仰和偏航旋转的情况下旋转四元数。

#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

tf2::Quaternion q_orig, q_rot, q_new;

// Get the original orientation of 'commanded_pose'
tf2::convert(commanded_pose.pose.orientation , q_orig);

double r=3.14159, p=0, y=0;  // Rotate the previous pose by 180* about X
q_rot.setRPY(r, p, y);

q_new = q_rot*q_orig;  // Calculate the new orientation
q_new.normalize();

// Stuff the new rotation back into the pose. This requires conversion into a msg type
tf2::convert(q_new, commanded_pose.pose.orientation);

【讨论】：

感谢这个好答案。你也知道第一行是做什么的吗？ -y？以欧拉角围绕 y 旋转另一个 pi/4？
您可能需要使用此标头中的一些方法，因为您收到的方向可能来自其他类型，您必须对其进行转换。 “ROS 使用两种四元数数据类型：msg 和 'tf'。要在 C++ 中进行转换，请使用 tf2_geometry_msgs 的方法。"
我不明白你的其他问题-y? 和That rotates another pi/4 around y in euler angles。你什么意思？

【解决方案2】：

下面的行是编码“quat”旋转轴的XZ平面上的反射变换：

tf2::Quaternion quat(orix, -oriy, oriz, oriw);

接下来，他们只定义 pi/2 rad 绕 XZ 平面中的轴的旋转。

tf2::Vector3 rotation_vector(0.7071068, 0, 0.7071068);
q_rot.setRotation(rotation_vector, M_PI);

接下来他们进行轮换组合（顺序很重要）：

quat = q_rot*quat;
quat.normalize();

接下来他们将数学表示从四元数改为矩阵代数：

tf2::Matrix3x3 matrix(quat);

接下来他们再次定义关于 XZ 平面的反射变换：

tf2::Matrix3x3 change_y(1,0, 0, 0, -1,0 ,0 ,0, 1);

下一行是将反射合成到当前转换：

matrix = change_y * matrix;

最后他们从矩阵传回四元数：

double roll, pitch, yaw;
matrix.getRPY(roll, pitch, yaw);
quat.setRPY(roll,pitch,yaw);`

由于您可以使用纯四元数表示反射（请参阅https://www.euclideanspace.com/maths/geometry/affine/reflection/quaternion/index.htm），因此我认为以下表达式等效于上面的代码：

// pure quaternion for doing reflection
tf2::Quaternion j(0, 1, 0, 0); 
// j*X*j is the reflection of X on the plane with normal j
quat = j*q_rot*j*quat*j*j;

可以简化为：

quat = -j*q_rot*j*quat;

由于矩阵和四元数之间的变化，很难判断这些表达式是否真的等价，因此可以进行数值测试以确认我没有将符号弄乱。我还没有测试过，所以就照原样吧。

【讨论】：