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)