Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
37 changes: 28 additions & 9 deletions src/machinevisiontoolbox/Camera.py
Original file line number Diff line number Diff line change
Expand Up @@ -1152,7 +1152,7 @@ def plot(
"""
Plot 3D camera icon in world view (base method)

:param pose: camera pose
:param pose: camera pose, required when called unbound (see below)
:type pose: :class:`~spatialmath.pose3d.SE3`
:param scale: scale factor, defaults to 1
:type scale: float
Expand All @@ -1170,15 +1170,28 @@ def plot(
:type projection: str, optional
:param ax: axes to draw in, defaults to current 3D axes
:type ax: :class:`~matplotlib.Axes3D`, optional
:raises ValueError: called unbound (eg. ``CentralCamera.plot(...)``)
without a ``pose``
:return: axes drawn into
:rtype: :class:`~matplotlib.Axes3D`

Plot a 3D icon representing the pose of a camera into a 3D Matplotlib
plot. Two icons are supported: the traditional frustum, and a
simplistic camera comprising a box and cylinder.

.. note:: If ``pose`` is not given it defaults to the pose of the
instance.
This method can be called two ways:

- **Bound**, ``camera.plot(...)`` -- draws an icon for ``camera``.
``pose`` defaults to the instance's own pose if not given.
- **Unbound**, ``CentralCamera.plot(pose=..., ...)`` -- called on
the class itself, with no camera instance involved (``self`` is
``None``). Useful for drawing icons at a set of poses that don't
correspond to any single camera instance -- e.g. plotting the
calibration frames returned by :meth:`images2C`. ``pose`` is
required in this form.

.. note:: If ``pose`` is not given, it defaults to the pose of
``self`` -- this only applies to the bound form.
"""

# if (fig is None) and (ax is None):
Expand All @@ -1195,6 +1208,11 @@ def plot(
ax = smb.axes_logic(ax, 3, projection=projection)

if pose is None:
if self is None:
raise ValueError(
"pose is required when plot() is called unbound, eg. "
"CentralCamera.plot(pose=...)"
)
pose = self.pose

# Matplotlib >=3.10 can raise while autoscaling ragged 3D line
Expand All @@ -1216,26 +1234,27 @@ def _add_collection3d_compat(collection, *args, **kwargs):
try:
# draw camera-like object:
if shape == "frustum":
self._plot_frustum(ax, pose, scale, alpha)
CameraBase._plot_frustum(ax, pose, scale, alpha)

elif shape == "camera":
self._plot_camera_icon(ax, pose, scale, solid, color, alpha, label)
CameraBase._plot_camera_icon(ax, pose, scale, solid, color, alpha, label)

if frame is True:
self.pose.plot(
pose.plot(
length=scale * 1.5,
style="line",
color=color,
flo=(0.07, 0, -0.01),
)
elif frame is not False:
self.pose.plot(**frame)
pose.plot(**frame)
finally:
ax.add_collection3d = _orig_add_collection3d

return ax

def _plot_frustum(self, ax: Axes, pose: SE3, scale: float, alpha: float) -> None:
@staticmethod
def _plot_frustum(ax: Axes, pose: SE3, scale: float, alpha: float) -> None:
"""Draw the ``shape="frustum"`` camera icon into ``ax``."""
# TODO make this kwargs or optional inputs
# side colors:
Expand Down Expand Up @@ -1279,8 +1298,8 @@ def _plot_frustum(self, ax: Axes, pose: SE3, scale: float, alpha: float) -> None
poly = Poly3DCollection(points, facecolors=["r", "g", "r", "y"], alpha=alpha)
ax.add_collection3d(poly)

@staticmethod
def _plot_camera_icon(
self,
ax: Axes,
pose: SE3,
scale: float,
Expand Down
12 changes: 12 additions & 0 deletions tests/test_camera.py
Original file line number Diff line number Diff line change
Expand Up @@ -429,6 +429,18 @@ def test_plot_frame(self):
self.assertEqual(len(ax.collections), 4)
self.assertEqual(len(ax.lines), 3)

def test_plot_unbound_with_pose(self):
# eg. CentralCamera.plot(pose=frame.pose, ...) as used to plot a
# set of calibration frames with no single camera instance
ax = CentralCamera.plot(pose=SE3.Tz(1), shape="camera")
self.assertIsNotNone(ax)
self.assertEqual(len(ax.collections), 3)
self.assertEqual(len(ax.lines), 0)

def test_plot_unbound_without_pose_raises(self):
with self.assertRaises(ValueError):
CentralCamera.plot(shape="camera")


# ----------------------------------------------------------------------- #
if __name__ == "__main__":
Expand Down