diff --git a/examples/animations/animate_camera.py b/examples/animations/animate_camera.py index 4083d1bc1..2efc42cd0 100644 --- a/examples/animations/animate_camera.py +++ b/examples/animations/animate_camera.py @@ -1,11 +1,10 @@ """ -================ +============== Animate Camera -================ +============== Animate a camera moving along a circular trajectory while looking at a target. """ - import matplotlib.pyplot as plt import matplotlib.animation as animation import numpy as np @@ -16,12 +15,13 @@ def update_camera(step, n_frames, camera): - phi = 2 * step / n_frames * np.pi + phi = 2 * np.pi * step / n_frames tf = transform_from( - matrix_from_euler([-1 / 2 * np.pi, phi, 0], 0, 1, 2, False), + matrix_from_euler([-0.5 * np.pi, phi, 0], 0, 1, 2, False), -10 * np.array([np.sin(phi), np.cos(phi), 0]), ) camera.set_data(tf) + return camera if __name__ == "__main__": diff --git a/pytransform3d/camera.py b/pytransform3d/camera.py index 0d47af3a4..6bb63b3b0 100644 --- a/pytransform3d/camera.py +++ b/pytransform3d/camera.py @@ -266,9 +266,8 @@ def plot_camera(ax=None, M=None, cam2world=None, virtual_image_distance=1.0, cam2world = check_transform(cam2world, strict_check=strict_check) - camera_artist = Camera( - M, cam2world, virtual_image_distance, sensor_size, **kwargs - ) - camera_artist.add_camera(ax) + camera = Camera( + M, cam2world, virtual_image_distance, sensor_size, **kwargs) + camera.add_camera(ax) return ax diff --git a/pytransform3d/plot_utils/_artists.py b/pytransform3d/plot_utils/_artists.py index cf9472255..5625be8ca 100644 --- a/pytransform3d/plot_utils/_artists.py +++ b/pytransform3d/plot_utils/_artists.py @@ -317,7 +317,8 @@ class Camera(artist.Artist): same as for the sensor size. cam2world : array-like, shape (4, 4) - Transform from frame A to frame B + Transformation matrix of camera in world frame. We assume that the + position is given in meters. virtual_image_distance : float, optional (default: 1) Distance from pinhole to virtual image plane that will be displayed. @@ -349,10 +350,10 @@ def __init__( else: color = "k" - self.sensor_corners = self._calculate_sensor_corners_in_camera( + self.sensor_corners = _calculate_sensor_corners_in_camera( M, virtual_image_distance, sensor_size ) - self.top_corners = self._calculate_top_corners_in_camera( + self.top_corners = _calculate_top_corners_in_camera( self.sensor_corners ) @@ -363,41 +364,6 @@ def __init__( self.set_data(cam2world) - def _calculate_sensor_corners_in_camera( - self, M, virtual_image_distance, sensor_size - ): - """Calculate the corners of the sensor frame in camera coordinates.""" - focal_length = np.mean((M[0, 0], M[1, 1])) - sensor_corners = np.array( - [ - [0, 0, focal_length], - [0, sensor_size[1], focal_length], - [sensor_size[0], sensor_size[1], focal_length], - [sensor_size[0], 0, focal_length], - ] - ) - sensor_corners[:, 0] -= M[0, 2] - sensor_corners[:, 1] -= M[1, 2] - return virtual_image_distance / focal_length * sensor_corners - - def _calculate_top_corners_in_camera(self, sensor_corners): - """Calculate the corners of the top triangle in camera coordinates. - - Parameters - ---------- - sensor_corners : array-like, shape (4, 3) - Corners of the sensor in camera coordinates - """ - up = sensor_corners[0] - sensor_corners[1] - return np.array( - [ - sensor_corners[0] + 0.1 * up, - 0.5 * (sensor_corners[0] + sensor_corners[3]) + 0.5 * up, - sensor_corners[3] + 0.1 * up, - sensor_corners[0] + 0.1 * up, - ] - ) - def set_data(self, cam2world): """Set the transformation data. @@ -406,9 +372,10 @@ def set_data(self, cam2world): cam2world : array-like, shape (4, 4) Transform from frame A to frame B """ + cam2world = np.asarray(cam2world) sensor_in_world = np.dot( - cam2world, np.vstack((self.sensor_corners.T, np.ones(4))) - ) + cam2world, np.vstack((self.sensor_corners.T, + np.ones(len(self.sensor_corners))))) for i in range(4): xs, ys, zs = [ [ @@ -420,7 +387,9 @@ def set_data(self, cam2world): ] self.lines_sensor[i].set_data_3d(xs, ys, zs) - top_in_world = np.dot(cam2world, np.vstack((self.top_corners.T, np.ones(4)))) + top_in_world = np.dot( + cam2world, np.vstack((self.top_corners.T, + np.ones(len(self.top_corners))))) xs, ys, zs, _ = top_in_world self.line_top.set_data_3d(xs, ys, zs) @@ -437,3 +406,33 @@ def add_camera(self, axis): for b in self.lines_sensor: axis.add_line(b) axis.add_line(self.line_top) + + +def _calculate_sensor_corners_in_camera( + M, virtual_image_distance, sensor_size): + """Calculate the corners of the sensor frame in camera coordinates.""" + focal_length = np.mean((M[0, 0], M[1, 1])) + sensor_corners = np.array( + [ + [0, 0, focal_length], + [0, sensor_size[1], focal_length], + [sensor_size[0], sensor_size[1], focal_length], + [sensor_size[0], 0, focal_length], + ] + ) + sensor_corners[:, 0] -= M[0, 2] + sensor_corners[:, 1] -= M[1, 2] + return virtual_image_distance / focal_length * sensor_corners + + +def _calculate_top_corners_in_camera(sensor_corners): + """Calculate the corners of the top triangle in camera coordinates.""" + up = sensor_corners[0] - sensor_corners[1] + return np.array( + [ + sensor_corners[0] + 0.1 * up, + 0.5 * (sensor_corners[0] + sensor_corners[3]) + 0.5 * up, + sensor_corners[3] + 0.1 * up, + sensor_corners[0] + 0.1 * up, + ] + ) diff --git a/pytransform3d/plot_utils/_artists.pyi b/pytransform3d/plot_utils/_artists.pyi index 8dc369500..b0d17c6ad 100644 --- a/pytransform3d/plot_utils/_artists.pyi +++ b/pytransform3d/plot_utils/_artists.pyi @@ -5,7 +5,7 @@ from mpl_toolkits.mplot3d.art3d import Line3D from matplotlib import artist from matplotlib.patches import FancyArrowPatch from matplotlib.backend_bases import RendererBase -from typing import Union +from typing import Union, List class Frame(artist.Artist): @@ -54,18 +54,17 @@ class Arrow3D(FancyArrowPatch): def draw(self, renderer: RendererBase): ... -class Camera(artist.Artist): - def __init__( - self, M: npt.ArrayLike, cam2world: npt.ArrayLike, - virtual_image_distance: float = ..., - sensor_size: npt.ArrayLike = ..., **kwargs): ... - def _calculate_sensor_corners_in_camera( - self, M: npt.ArrayLike, virtual_image_distance: float, - sensor_size: npt.ArrayLike) -> npt.ArrayLike: ... +class Camera(artist.Artist): + sensor_corners: np.ndarray + top_corners: np.ndarray + lines_sensor: List[Line3D] + line_top: Line3D - def _calculate_top_corners_in_camera( - self, sensor_corners: npt.ArrayLike) -> npt.ArrayLike: ... + def __init__( + self, M: npt.ArrayLike, cam2world: npt.ArrayLike, + virtual_image_distance: float = ..., + sensor_size: npt.ArrayLike = ..., **kwargs): ... def set_data(self, cam2world: npt.ArrayLike): ... @@ -73,3 +72,12 @@ class Camera(artist.Artist): def draw(self, renderer: RendererBase, *args, **kwargs): ... def add_camera(self, axis: Axes3D): ... + + +def _calculate_sensor_corners_in_camera( + M: npt.ArrayLike, virtual_image_distance: float, + sensor_size: npt.ArrayLike) -> npt.ArrayLike: ... + + +def _calculate_top_corners_in_camera( + sensor_corners: npt.ArrayLike) -> npt.ArrayLike: ... diff --git a/pytransform3d/test/test_plot_utils.py b/pytransform3d/test/test_plot_utils.py index c372d709a..7a2eab2ca 100644 --- a/pytransform3d/test/test_plot_utils.py +++ b/pytransform3d/test/test_plot_utils.py @@ -105,7 +105,6 @@ def test_trajectory(): def test_camera(): ax = make_3d_axis(1.0) try: - fl = 3000 # [pixels] w, h = 1920, 1080 # [pixels] M = np.array(((100, 0, 100), (0, 100, 100), (0, 0, 1))) camera = Camera(