Transform Concatenation ======================= In this example, we have a point p that is defined in a frame C, we know the transform C2B and B2A. We can construct a transform C2A to extract the position of p in frame A. """ print(__doc__) import numpy as np import matplotlib.pyplot as plt import pytransform3d.rotations as pyrot import pytransform3d.transformations as pytr p = np.array([0.0, 0.0, -0.5]) a = np.array([0.0, 0.0, 1.0, np.pi]) B2A = pytr.transform_from(pyrot.matrix_from_axis_angle(a), p) p = np.array([0.3, 0.4, 0.5]) a = np.array([0.0, 0.0, 1.0, -np.pi / 2.0]) C2B = pytr.transform_from(pyrot.matrix_from_axis_angle(a), p) C2A = pytr.concat(C2B, B2A) p = pytr.transform(C2A, np.ones(4)) ax = pytr.plot_transform(A2B=B2A) pytr.plot_transform(ax, A2B=C2A) ax.scatter(p[0], p[1], p[2]) plt.show()
from pytransform3d.transformations import (
plot_transform, plot_screw, screw_axis_from_screw_parameters,
transform_from_exponential_coordinates, concat, transform_from)
# Screw parameters
q = np.array([-0.2, -0.1, -0.5])
s_axis = np.array([0, 0, 1])
h = 0.05
theta = 5.5 * np.pi
Stheta = screw_axis_from_screw_parameters(q, s_axis, h) * theta
A2B = transform_from_exponential_coordinates(Stheta)
origin = transform_from(
active_matrix_from_extrinsic_roll_pitch_yaw([0.5, -0.3, 0.2]),
np.array([0.0, 0.1, 0.1]))
ax = plot_transform(A2B=origin, s=0.4)
plot_transform(ax=ax, A2B=concat(A2B, origin), s=0.2)
plot_screw(ax=ax,
q=q,
s_axis=s_axis,
h=h,
theta=theta,
A2B=origin,
s=1.5,
alpha=0.6)
ax.view_init(elev=40, azim=170)
plt.subplots_adjust(0, 0, 1, 1)
plt.show()
sensor_size = (0.2, 0.15)
image_size = (640, 480)
plt.figure(figsize=(12, 5))
ax = plt.subplot(121, projection="3d", aspect="equal")
ax.set_title("Grid in 3D camera coordinate system")
ax.set_xlim((-1, 1))
ax.set_ylim((-1, 1))
ax.set_zlim((0, 2))
ax.set_xlabel("X")
ax.set_ylabel("Y")
ax.set_zlabel("Z")
cam_grid = make_world_grid(n_points_per_line=11) - np.array([0, 0, -2, 0])
img_grid = cam_grid * focal_length
c = np.arange(len(cam_grid))
ax.scatter(cam_grid[:, 0], cam_grid[:, 1], cam_grid[:, 2], c=c)
ax.scatter(img_grid[:, 0], img_grid[:, 1], img_grid[:, 2], c=c)
plot_transform(ax)
sensor_grid = cam2sensor(cam_grid, focal_length)
img_grid = sensor2img(sensor_grid, sensor_size, image_size)
ax = plt.subplot(122, aspect="equal")
ax.set_title("Grid in 2D image coordinate system")
ax.scatter(img_grid[:, 0], img_grid[:, 1], c=c)
ax.set_xlim((0, image_size[0]))
ax.set_ylim((0, image_size[1]))
plt.show()
""" ========= Transform ========= We can display transformations by plotting the basis vectors of the corresponding coordinate frame. """ print(__doc__) import matplotlib.pyplot as plt from pytransform3d.transformations import plot_transform plot_transform() plt.show()
"""
===========
Plot Sphere
===========
"""
print(__doc__)
import numpy as np
import matplotlib.pyplot as plt
from pytransform3d.transformations import plot_transform
from pytransform3d.plot_utils import plot_sphere, remove_frame
random_state = np.random.RandomState(42)
ax = plot_sphere(
radius=0.5, wireframe=False, alpha=0.1, color="k", n_steps=20, ax_s=0.5)
plot_sphere(ax=ax, radius=0.5, wireframe=True)
plot_transform(ax=ax, A2B=np.eye(4), s=0.3, lw=3)
remove_frame(ax)
plt.show()
def inertia_of_cylinder(mass, length, radius):
I_xx = I_yy = 0.25 * mass * radius ** 2 + 1.0 / 12.0 * mass * length ** 2
I_zz = 0.5 * mass * radius ** 2
return np.eye(3) * np.array([I_xx, I_yy, I_zz])
random_state = np.random.RandomState(42)
A2B = np.eye(4)
length = 1.0
radius = 0.1
mass = 1.0
dt = 0.2
I = inertia_of_cylinder(mass, length, radius)
tau = np.array([0.05, 0.05, 0.0])
angular_velocity = np.zeros(3)
orientation = np.zeros(3)
ax = None
for p_xy in np.linspace(-2, 2, 21):
A2B = transform_from(R=matrix_from_compact_axis_angle(orientation),
p=np.array([p_xy, p_xy, 0.0]))
ax = plot_cylinder(length=length, radius=radius, A2B=A2B, wireframe=False,
alpha=0.2, ax_s=2.0, ax=ax)
plot_transform(ax=ax, A2B=A2B, s=radius, lw=3)
angular_acceleration = np.linalg.inv(I).dot(tau)
angular_velocity += dt * angular_acceleration
orientation += dt * angular_velocity
ax.view_init(elev=30, azim=70)
plt.show()
plt.figure(figsize=(12, 5))
try:
ax = plt.subplot(121, projection="3d", aspect="equal")
except NotImplementedError:
# HACK: workaround for bug in new matplotlib versions (ca. 3.02):
# "It is not currently possible to manually set the aspect"
ax = plt.subplot(121, projection="3d")
ax.view_init(elev=30, azim=-70)
ax.set_xlim((-1, 1))
ax.set_ylim((-1, 1))
ax.set_zlim((-1, 1))
ax.set_xlabel("X")
ax.set_ylabel("Y")
ax.set_zlabel("Z")
plot_transform(ax)
plot_transform(ax, A2B=cam2world)
ax.set_title("Camera and world frames")
ax.scatter(world_grid[:, 0], world_grid[:, 1], world_grid[:, 2])
ax.scatter(world_grid[-1, 0], world_grid[-1, 1], world_grid[-1, 2], color="r")
for p in world_grid[::10]:
ax.plot([p[0], cam2world[0, 3]],
[p[1], cam2world[1, 3]],
[p[2], cam2world[2, 3]], c="k", alpha=0.2, lw=2)
ax = plt.subplot(122, aspect="equal")
ax.set_title("Camera image")
ax.set_xlim(0, image_size[0])
ax.set_ylim(0, image_size[1])
ax.scatter(image_grid[:, 0], -(image_grid[:, 1] - image_size[1]))
ax.scatter(image_grid[-1, 0], -(image_grid[-1, 1] - image_size[1]), color="r")
import pytransform3d.plot_utils as ppu
random_state = np.random.RandomState(21)
pose1 = pt.random_transform(random_state)
pose2 = pt.random_transform(random_state)
dq1 = pt.dual_quaternion_from_transform(pose1)
dq2 = -pt.dual_quaternion_from_transform(pose2)
Stheta1 = pt.exponential_coordinates_from_transform(pose1)
Stheta2 = pt.exponential_coordinates_from_transform(pose2)
n_steps = 100
interpolated_dqs = (np.linspace(1, 0, n_steps)[:, np.newaxis] * dq1 +
np.linspace(0, 1, n_steps)[:, np.newaxis] * dq2)
# renormalization (not required here because it will be done with conversion)
interpolated_dqs /= np.linalg.norm(interpolated_dqs[:, :4], axis=1)[:,
np.newaxis]
interpolated_poses_from_dqs = np.array(
[pt.transform_from_dual_quaternion(dq) for dq in interpolated_dqs])
interpolated_ecs = (np.linspace(1, 0, n_steps)[:, np.newaxis] * Stheta1 +
np.linspace(0, 1, n_steps)[:, np.newaxis] * Stheta2)
interpolates_poses_from_ecs = ptr.transforms_from_exponential_coordinates(
interpolated_ecs)
ax = pt.plot_transform(A2B=pose1, s=0.3, ax_s=2)
pt.plot_transform(A2B=pose2, s=0.3, ax=ax)
traj_from_dqs = ppu.Trajectory(interpolated_poses_from_dqs, s=0.1, c="g")
traj_from_dqs.add_trajectory(ax)
traj_from_ecs = ppu.Trajectory(interpolates_poses_from_ecs, s=0.1, c="r")
traj_from_ecs.add_trajectory(ax)
plt.show()
intrinsic_camera_matrix = np.array([[focal_length, 0, sensor_size[0] / 2],
[0, focal_length, sensor_size[1] / 2],
[0, 0, 1]])
world_grid = make_world_grid(n_points_per_line=101)
image_grid = world2image(world_grid,
cam2world,
sensor_size,
image_size,
focal_length,
kappa=0.4)
plt.figure(figsize=(12, 5))
ax = make_3d_axis(1, 121, unit="m")
ax.view_init(elev=30, azim=-70)
plot_transform(ax)
plot_transform(ax, A2B=cam2world, s=0.3, name="Camera")
plot_camera(ax,
intrinsic_camera_matrix,
cam2world,
sensor_size=sensor_size,
virtual_image_distance=0.5)
ax.set_title("Camera and world frames")
ax.scatter(world_grid[:, 0],
world_grid[:, 1],
world_grid[:, 2],
s=1,
alpha=0.2)
ax.scatter(world_grid[-1, 0], world_grid[-1, 1], world_grid[-1, 2], color="r")
ax.view_init(elev=25, azim=-130)
"""
===========================
Camera Representation in 3D
===========================
This visualization is inspired by Blender's camera visualization. It will
show the camera center, a virtual image plane at a desired distance to the
camera center, and the top direction of the virtual image plane.
"""
print(__doc__)
import numpy as np
import matplotlib.pyplot as plt
import pytransform3d.camera as pc
import pytransform3d.transformations as pt
cam2world = pt.transform_from_pq([0, 0, 0, np.sqrt(0.5), -np.sqrt(0.5), 0, 0])
# default parameters of a camera in Blender
sensor_size = np.array([0.036, 0.024])
intrinsic_matrix = np.array([[0.05, 0, sensor_size[0] / 2.0],
[0, 0.05, sensor_size[1] / 2.0], [0, 0, 1]])
virtual_image_distance = 1
ax = pt.plot_transform(A2B=cam2world, s=0.2)
pc.plot_camera(ax,
cam2world=cam2world,
M=intrinsic_matrix,
sensor_size=sensor_size,
virtual_image_distance=virtual_image_distance)
plt.show()
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,
c=c)
pos_min = np.min(P[:, :3], axis=0)
