"""
===============
Pose Trajectory
===============
Plotting pose trajectories with pytransform3d is easy.
"""
print(__doc__)
import numpy as np
import matplotlib.pyplot as plt
from pytransform3d.rotations import q_id, quaternion_slerp
from pytransform3d.trajectories import plot_trajectory
n_steps = 100
P = np.empty((n_steps, 7))
P[:, 0] = np.cos(np.linspace(-2 * np.pi, 2 * np.pi, n_steps))
P[:, 1] = np.sin(np.linspace(-2 * np.pi, 2 * np.pi, n_steps))
P[:, 2] = np.linspace(-1, 1, n_steps)
q_end = np.array([0.0, 0.0, np.sqrt(0.5), np.sqrt(0.5)])
P[:, 3:] = np.vstack(
[quaternion_slerp(q_id, q_end, t) for t in np.linspace(0, 1, n_steps)])
ax = plot_trajectory(P=P, s=0.3, lw=2, c="k")
plt.show()
"""
===============
Pose Trajectory
===============
Plotting pose trajectories with pytransform3d is easy.
"""
print(__doc__)
import numpy as np
import matplotlib.pyplot as plt
from pytransform3d.rotations import q_id, quaternion_slerp
from pytransform3d.trajectories import plot_trajectory
n_steps = 100
P = np.empty((n_steps, 7))
P[:, 0] = np.linspace(-0.8, 0.8, n_steps)**2
P[:, 1] = np.linspace(-0.8, 0.8, n_steps)**2
P[:, 2] = np.linspace(-0.8, 0.8, n_steps)
q_end = np.array([0.0, 0.0, np.sqrt(0.5), np.sqrt(0.5)])
P[:, 3:] = np.vstack(
[quaternion_slerp(q_id, q_end, t) for t in np.linspace(0, 1, n_steps)])
plot_trajectory(P=P, s=0.1, lw=1, c="k")
plt.show()
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,
c=c)
pos_min = np.min(P[:, :3], axis=0)
pos_max = np.max(P[:, :3], axis=0)
q_start = quaternion_from_matrix(R1.dot(R2_start))
q_end = quaternion_from_matrix(R1.dot(R2_end))
Y = np.zeros((len(T), 7))
Y[:, 0] = radius * np.cos(np.deg2rad(90) - end_angle * sigmoid)
Y[:, 2] = radius * np.sin(np.deg2rad(90) - end_angle * sigmoid)
if end_angle > np.pi:
q_end *= -1.0
Y[:, 3:] = (1.0 - T)[:, np.newaxis] * q_start + T[:, np.newaxis] * q_end
Y_slerp = np.zeros((len(T), 7))
Y_slerp[:, 0] = 0.7 * radius * np.cos(np.deg2rad(90) - end_angle * sigmoid)
Y_slerp[:, 2] = 0.7 * radius * np.sin(np.deg2rad(90) - end_angle * sigmoid)
for i, t in enumerate(T):
Y_slerp[i, 3:] = quaternion_slerp(q_start, q_end, t)
ax = plot_trajectory(P=Y[:, :7],
show_direction=False,
n_frames=40,
s=0.05,
ax_s=0.7)
ax = plot_trajectory(P=Y_slerp[:, :7],
show_direction=False,
n_frames=40,
s=0.05,
ax=ax)
ax.text(0.1, 0, 0, "SLERP")
ax.text(0.4, 0, 0.6, "Naive linear interpolation")
ax.view_init(elev=10, azim=90)
plt.show()
"""
===============
Pose Trajectory
===============
Plotting pose trajectories with pytransform3d is easy.
"""
print(__doc__)
import numpy as np
import matplotlib.pyplot as plt
from pytransform3d.batch_rotations import quaternion_slerp_batch
from pytransform3d.rotations import q_id
from pytransform3d.trajectories import plot_trajectory
n_steps = 100000
P = np.empty((n_steps, 7))
P[:, 0] = np.cos(np.linspace(-2 * np.pi, 2 * np.pi, n_steps))
P[:, 1] = np.sin(np.linspace(-2 * np.pi, 2 * np.pi, n_steps))
P[:, 2] = np.linspace(-1, 1, n_steps)
q_end = np.array([0.0, 0.0, np.sqrt(0.5), np.sqrt(0.5)])
P[:, 3:] = quaternion_slerp_batch(q_id, q_end, np.linspace(0, 1, n_steps))
ax = plot_trajectory(P=P,
s=0.3,
n_frames=100,
normalize_quaternions=False,
lw=2,
c="k")
plt.show()