在 Matplotlib 中使用位置和四元数绘制 3D 矢量

Question

我有以下信息：

• 位置点 (x,y,z)
• 四元数方向（w、i、j、k）

我想在 Matplotlib 中将其绘制为 3D 向量。我看到很多关于在 Matplotlib 中绘制 3D 矢量的问题(example question)。例如，我知道我可以在 Matplotlib 中绘制一个向量给定两个点，一个起点 (x, y, z) 和一个终点 (x, y, z)，如下所示。

start = [2 3 5];
end = [4 5 5]; 
quiver3(start(1), start(2), start(3), end(1), end(2), end(3));

我的问题是：如何将我的位置和四元数转换为起点和终点，以便将其绘制为向量，或者如何直接绘制我的位置和四元数？

注意：我有在 3D 中绘制我的位置的代码，我只是不确定如何获得方向：

ax.set_title("Pos: (" + str(x) + ", " + str(y) + ", " + str(z) + ")")
ax.set_xlim([-5, 5])
ax.set_ylim([-5, 5])
ax.set_zlim([0, 5])
ax.set_xlabel("X Axis")
ax.set_ylabel("Y Axis")
ax.set_zlabel("Z Axis")
ax.scatter(x, y, z)

Answer 1

我找到了使用ROS 的部分解决方案。在ROS 中有一种方法可以使用以下方法将四元数转换为欧拉角：

from tf.transformations import euler_from_quaternion
quaternion = (w,i,j,k)
euler = euler_from_quaternion(quaternion) 

使用它，我可以使用以下方法绘制偏航：

yaw = euler[2]
new_x = sin(yaw)
new_y = cos(yaw)
self.ax.quiver(x, y, z, new_x, new_y, 0, color='b')

我认为应该有一种方法可以在纯 Python 中执行此操作，并且应该有一种方法可以获取所有角度，但是我决定将其作为部分解决方案，以防有人遇到同样的问题而没有其他人答案在这里。