def test_transforms_from_pqs_0dims():
random_state = np.random.RandomState(0)
pq = np.empty(7)
pq[:3] = random_state.randn(3)
pq[3:] = random_quaternion(random_state)
A2B = transforms_from_pqs(pq, False)
assert_array_almost_equal(A2B, transform_from_pq(pq))
def test_transforms_from_pqs_4dims():
random_state = np.random.RandomState(0)
P = random_state.randn(2, 3, 4, 5, 7)
P[..., 3:] = norm_vectors(P[..., 3:])
H = transforms_from_pqs(P)
P2 = pqs_from_transforms(H)
assert_array_almost_equal(P[..., :3], H[..., :3, 3])
assert_array_almost_equal(P[..., :3], P2[..., :3])
def test_transforms_from_pqs_1dim():
P = np.empty((10, 7))
random_state = np.random.RandomState(0)
P[:, :3] = random_state.randn(len(P), 3)
P[:, 3:] = norm_vectors(random_state.randn(len(P), 4))
H = transforms_from_pqs(P)
P2 = pqs_from_transforms(H)
assert_array_almost_equal(P[:, :3], H[:, :3, 3])
assert_array_almost_equal(P[:, :3], P2[:, :3])
for t in range(len(P)):
assert_quaternion_equal(P[t, 3:], quaternion_from_matrix(H[t, :3, :3]))
assert_quaternion_equal(P[t, 3:], P2[t, 3:])
search_path = "."
while (not os.path.exists(data_dir)
and os.path.dirname(search_path) != "pytransform3d"):
search_path = os.path.join(search_path, "..")
data_dir = os.path.join(search_path, BASE_DIR)
intrinsic_matrix = np.loadtxt(os.path.join(data_dir,
"reconstruction_camera_matrix.csv"),
delimiter=",")
P = np.loadtxt(os.path.join(data_dir, "reconstruction_odometry.csv"),
delimiter=",",
skiprows=1)
for t in range(len(P)):
P[t, 3:] = pr.quaternion_wxyz_from_xyzw(P[t, 3:])
cam2world_trajectory = ptr.transforms_from_pqs(P)
plt.figure(figsize=(5, 5))
ax = pt.plot_transform(s=0.3)
ax = ptr.plot_trajectory(ax, P=P, s=0.1, n_frames=10)
image_size = np.array([1920, 1440])
key_frames_indices = np.linspace(0, len(P) - 1, 10, dtype=int)
colors = cycle("rgb")
for i, c in zip(key_frames_indices, colors):
pc.plot_camera(ax,
intrinsic_matrix,
cam2world_trajectory[i],
sensor_size=image_size,
virtual_image_distance=0.2,