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

Question

我有以下信息：

• 一个位置点 (x,y,z)
• 一个四元数方向 (w, i, j, k)

我想在Matplotlib中将其绘制为三维向量。我看到了很多关于在Matplotlib中绘制三维向量的问题(示例问题)。例如，我知道可以像下面展示的那样，在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)

- Watchdog101

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的方法来实现这个，并且应该有一种方法可以获取所有角度，但是我决定将其作为部分解决方案发布，以防万一有人遇到相同的问题而没有其他答案。

- Watchdog101

---

使用欧拉角的一个问题是它可能无法正确反映设备的旋转。例如{180, 180, 0}和{0,0, 180}(滚动、俯仰、偏航)是相同的。 - jgauthier

Answer 2

这是我用来绘制箭头的代码，使用起始点(x, y, z)和四元数(a, b, c, d)：

from scipy.spatial.transform import Rotation as R
import numpy as np
import matplotlib.pyplot as plt

def rotate_vector(vector, rotation_matrix):
    return np.dot(rotation_matrix, vector)

def calculate_endpoint(start, a, b, c, d):
    rotation_matrix = R.from_quat([a, b, c, d]).as_matrix()
    unit_vector = np.array([0, 0, 1])
    endpoint = rotate_vector(unit_vector, rotation_matrix)
    return start + endpoint

start_point = np.array([[x, y, z]])
end_point = calculate_endpoint(start_point, a, b, c, d)

fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.quiver(start_point[0], start_point[1], start_point[2],
                end_point[0], end_point[1], end_point[2],
                length = 1, normalize = True)
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_zlabel('z')
plt.show()

如果您想绘制多个箭头，假设您有两个变量start_points和end_points，形状为(number_of_data, 3)，则可以使用以下代码：

ax.quiver(start_points[:, 0], start_points[:, 1], start_points[:, 2],
                end_points[:, 0], end_points[:, 1], end_points[:, 2],
                length = 0.1, normalize = True)

根据您的数据，您需要调整length变量。

- Ava