c++ - 下面用代码写的四元数的变换是什么？

Question

我已经在代码审查中询问过这个问题，因为这是一个 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);`

Answer 1

rotation_vector 实际上定义了旋转轴，因此您正在围绕这个定义为 [0.7 0 0.7] 的 vector 旋转 M_PI，这类似于通过 M_PI/4 围绕 y 轴旋转 - 从而生成一个带有 [x' y' z'] 的新坐标系 - 然后旋转 M_PI 围绕您当前的 z' 轴。

也许这个 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);