"""
Abstract base class and hierarchy for camera models (perspective, fisheye, catadioptric, etc.).
"""
from __future__ import annotations
import copy
from abc import ABC, abstractmethod
from collections import namedtuple
from math import cos, pi, sin, sqrt, tan
from typing import TYPE_CHECKING, Any, Callable
import cv2
import matplotlib.pyplot as plt
import numpy as np
import scipy
import spatialmath.base as smb
from matplotlib.axes import Axes
from matplotlib.figure import Figure
# from mpl_toolkits.mplot3d import Axes3D, art3d
from mpl_toolkits.mplot3d.art3d import Poly3DCollection
from spatialmath import SE3, Line3
from machinevisiontoolbox import Image
from machinevisiontoolbox.base import idisp
from machinevisiontoolbox.base.imageio import _isnotebook
from machinevisiontoolbox.ImagePointFeatures import FeatureMatch
from machinevisiontoolbox.mvtb_types import ArrayLike
# from machinevisiontoolbox.classes import Image
# import CameraVisualizer as CamVis
class CameraBase(ABC):
_name: str
_camtype: str
_imagesize: np.ndarray | None
_pp: np.ndarray | None
_rhou: float
_rhov: float
_image: np.ndarray | None
_ax: Axes | None
def __init__(
self,
name: str | None = None,
camtype: str = "central",
rho: ArrayLike = 1,
imagesize: ArrayLike | None = None,
sensorsize: ArrayLike | None = None,
pp: ArrayLike | None = None,
noise: float | None = None,
pose: SE3 | None = None,
limits: ArrayLike | None = None,
labels: tuple[str, str] | None = None,
seed: int | None = None,
) -> None:
"""Abstract camera base class
:param name: camera instance name, defaults to ``None``
:type name: str, optional
:param camtype: camera projection type, defaults to 'central'
:type camtype: str, optional
:param rho: pixel size, defaults to 1
:type rho: scalar or array_like(2), optional
:param imagesize: image dimension (width, height) in pixels, defaults to ``None``
:type imagesize: int or array_like(2), optional
:param sensorsize: image sensor size (width, height), defaults to ``None``
:type sensorsize: array_like(2), optional
:param pp: principal point position, defaults to ``None``
:type pp: array_like(2), optional
:param noise: standard deviation of image plane projection noise, defaults to ``None``
:type noise: float, optional
:param pose: camera pose, defaults to ``None``
:type pose: :class:`~spatialmath.pose3d.SE3`, optional
:param limits: bounds of virtual image plane [umin, umax, vmin, vmax], defaults to ``None``
:type limits: array_like(4), optional
:param labels: axis labels for virtual image plane, defaults to ``('u', 'v')``
:type labels: 2-tuple of str, optional
:param seed: random number seed for projection noise, defaults to ``None``
:type seed: int, optional
:raises TypeError: name must be a string
:raises TypeError: camtype must be a string
:raises ValueError: rho must be a 1- or 2-element vector
This abstract class is the base for all camera projection model
classes. All baseclass constructors support these options.
"""
# initialise nullable attributes before any setter that checks them
self._imagesize = None
self._pp = None
if name is None:
self._name = camtype
else:
if not isinstance(name, str):
raise TypeError(name, "name must be a string")
self._name = name
if not isinstance(camtype, str):
raise TypeError(camtype, "camtype must be a string")
self._camtype = camtype
if imagesize is None:
if pp is None:
self.pp = (0, 0)
else:
self.pp = pp
else:
self.imagesize = imagesize
if pp is None:
self.pp = [x / 2 for x in self.imagesize]
else:
self.pp = pp
if sensorsize is not None:
assert (
self._imagesize is not None
), "imagesize must be set when sensorsize is given"
self._rhou = sensorsize[0] / self._imagesize[0]
self._rhov = sensorsize[1] / self._imagesize[1]
else:
rho = smb.getvector(rho)
if len(rho) == 1:
self._rhou = rho[0]
self._rhov = rho[0]
elif len(rho) == 2:
self._rhou = rho[0]
self._rhov = rho[1]
else:
raise ValueError(rho, "rho must be a 1- or 2-element vector")
if noise is not None:
self._noise = noise
self._random = np.random.default_rng(seed)
if pose is None:
self._pose = SE3()
else:
self._pose = SE3(pose)
self.pose0 = self.pose
self._noise = noise
self._image = None
self._ax = None
self._distortion = None
self.labels = labels
self.limits = limits
def reset(self) -> None:
"""
Reset camera pose (base method)
Restore camera to a copy of the pose given to the constructor. The copy
means that the camera pose can be modified freely, without destroying
the initial pose value.
"""
self.pose = self.pose0.copy()
def __str__(self) -> str:
"""
String representation of camera parameters (base method)
:return: string representation
:rtype: str
Multi-line string representation of camera intrinsic and extrinsic
parameters.
"""
# TODO, imagesize should be integers
s = ""
self.fmt = "{:>15s}: {}\n"
s += self.fmt.format("Name", self.name + " [" + self.__class__.__name__ + "]")
s += self.fmt.format("pixel size", " x ".join([str(x) for x in self.rho]))
if self.imagesize is not None:
s += self.fmt.format(
"image size", " x ".join([str(x) for x in self.imagesize])
)
s += self.fmt.format("pose", self.pose.strline(fmt="{:.3g}", orient="rpy/yxz"))
return s
def __repr__(self) -> str:
"""
Readable representation of camera parameters (base method)
:return: string representation
:rtype: str
Single-line string representation including camera name and pixel size.
"""
def fmt(x: tuple[float, float], spec: str = ".3g") -> str:
return f"({x[0]:{spec}}, {x[1]:{spec}})"
s = f"{self.__class__.__name__}(name='{self.name}'"
s += f", pixel_size={fmt(self.rho)}"
if self.imagesize is not None:
s += f", image_size={fmt(self._imagesize, spec='d')}"
s += f", pose=[{self.pose.strline()}]"
return s + ")"
@abstractmethod
def project_point(self, P, **kwargs: Any) -> np.ndarray:
pass
def project_line(self, *args: Any, **kwargs: Any) -> np.ndarray:
raise NotImplementedError("not implemented for this camera model")
def project_conic(self, *args: Any, **kwargs: Any) -> np.ndarray:
raise NotImplementedError("not implemented for this camera model")
@property
def name(self) -> str:
"""
Set/get camera name (base method)
A camera has a string-valued name that can be read and written.
Example:
.. runblock:: pycon
>>> from machinevisiontoolbox import CentralCamera
>>> camera = CentralCamera();
>>> camera.name
>>> camera.name = "foo"
>>> camera.name
"""
return self._name
@name.setter
def name(self, newname: str) -> None:
"""
Set camera name
:param newname: camera name
:type newname: str
"""
if isinstance(newname, str):
self._name = newname
else:
raise TypeError(newname, "name must be a string")
@property
def camtype(self) -> str:
"""
Set/get camera type (base method)
A camera has a string-valued type that can be read and written. This
is unique to the camera subclass and projection model.
Example:
.. runblock:: pycon
>>> from machinevisiontoolbox import CentralCamera
>>> camera = CentralCamera();
>>> camera.camtype
>>> camera.camtype = "foo"
>>> camera.camtype
"""
return self._camtype
@camtype.setter
def camtype(self, newcamtype: str) -> None:
"""
Set camera type
:param newcamtype: camera projection type
:type newcamtype: str
"""
if isinstance(newcamtype, str):
self._camtype = newcamtype
else:
raise TypeError(newcamtype, "camtype must be a string")
@property
def imagesize(self) -> np.ndarray | None:
"""
Set/get size of virtual image plane (base method)
The dimensions of the virtual image plane is a 2-tuple (width, height) that can
be read or written. For writing the size must be an iterable of length 2.
Example:
.. runblock:: pycon
>>> from machinevisiontoolbox import CentralCamera
>>> camera = CentralCamera.Default();
>>> camera.imagesize
>>> camera.imagesize = (500, 500)
>>> camera.imagesize
.. note:: If the principal point is not set, then setting imagesize
sets the principal point to the centre of the image plane.
:seealso: :meth:`width` :meth:`height` :meth:`nu` :meth:`nv`
"""
return self._imagesize
@imagesize.setter
def imagesize(self, npix: ArrayLike) -> None:
"""
Set image plane size
:param npix: [description]
:type npix: array_like(2)
:raises ValueError: bad value
Sets the size of the virtual image plane.
.. note:: If the principle point is not set, then it
is set to the centre of the image plane.
:seealso: :meth:`width` :meth:`height` :meth:`nu` :meth:`nv`
"""
npix = smb.getvector(npix, dtype="int")
if len(npix) == 1:
self._imagesize = np.r_[npix[0], npix[0]]
elif len(npix) in (2, 3):
# ignore color dimension in case it is given
self._imagesize = npix[:2]
else:
raise ValueError(npix, "imagesize must be a 1- or 2-element vector")
if self._pp is None:
assert self._imagesize is not None
self._pp = self._imagesize / 2
@property
def nu(self) -> int:
"""
Get image plane width (base method)
:return: width (width)
Example:
.. runblock:: pycon
>>> from machinevisiontoolbox import CentralCamera
>>> camera = CentralCamera.Default();
>>> camera.nu
:seealso: :meth:`nv` :meth:`width` :meth:`imagesize`
"""
assert self._imagesize is not None, "imagesize not set"
return self._imagesize[0]
@property
def nv(self) -> int:
"""
Get image plane height (base method)
:return: height (height)
Example:
.. runblock:: pycon
>>> from machinevisiontoolbox import CentralCamera
>>> camera = CentralCamera.Default();
>>> camera.nv
:seealso: :meth:`nu` :meth:`height` :meth:`imagesize`
"""
assert self._imagesize is not None, "imagesize not set"
return self._imagesize[1]
@property
def width(self) -> int:
"""
Get image plane width (base method)
:return: width
Example:
.. runblock:: pycon
>>> from machinevisiontoolbox import CentralCamera
>>> camera = CentralCamera.Default();
>>> camera.width
:seealso: :meth:`nu` :meth:`height`
"""
assert self._imagesize is not None, "imagesize not set"
return self._imagesize[0]
@property
def height(self) -> int:
"""
Get image plane height (base method)
:return: height
Example:
.. runblock:: pycon
>>> from machinevisiontoolbox import CentralCamera
>>> camera = CentralCamera.Default();
>>> camera.height
:seealso: :meth:`nv` :meth:`width`
"""
assert self._imagesize is not None, "imagesize not set"
return self._imagesize[1]
@property
def pp(self) -> np.ndarray | None:
"""
Set/get principal point coordinate (base method)
The principal point is the coordinate of the point where
the optical axis pierces the image plane. It is a 2-tuple which can
be read or written. For writing the size must be an
iterable of length 2.
Example:
.. runblock:: pycon
>>> from machinevisiontoolbox import CentralCamera
>>> camera = CentralCamera.Default();
>>> camera.pp
:seealso: :meth:`u0` :meth:`v0`
"""
return self._pp
@pp.setter
def pp(self, pp: ArrayLike) -> None:
"""
Set principal point coordinate
:param pp: principal point
:type pp: array_like(2)
:seealso: :meth:`pp` :meth:`u0` :meth:`v0`
"""
pp = smb.getvector(pp)
if len(pp) == 1:
self._pp = np.r_[pp[0], pp[0]]
elif len(pp) == 2:
self._pp = pp
else:
raise ValueError(pp, "pp must be a 1- or 2-element vector")
@property
def u0(self) -> float:
"""
Get principal point: horizontal coordinate (base method)
:return: horizontal component of principal point
:rtype: float
Example:
.. runblock:: pycon
>>> from machinevisiontoolbox import CentralCamera
>>> camera = CentralCamera();
>>> camera.u0
:seealso: :meth:`v0` :meth:`pp`
"""
assert self._pp is not None
return self._pp[0]
@property
def v0(self) -> float:
"""
Get principal point: vertical coordinate (base method)
:return: vertical component of principal point
:rtype: float
Example:
.. runblock:: pycon
>>> from machinevisiontoolbox import CentralCamera
>>> camera = CentralCamera();
>>> camera.v0
:seealso: :meth:`u0` :meth:`pp`
"""
assert self._pp is not None
return self._pp[1]
@property
def rhou(self) -> float:
"""
Get pixel width (base method)
:return: horizontal pixel size
:rtype: float
Example:
.. runblock:: pycon
>>> from machinevisiontoolbox import CentralCamera
>>> camera = CentralCamera.Default();
>>> camera.rhou
:seealso: :meth:`rhov` :meth:`rho`
"""
return self._rhou
# this is generally the centre of the image, has special meaning for
# perspective camera
@property
def rhov(self) -> float:
"""
Get pixel width (base method)
:return: vertical pixel size
:rtype: float
Example:
.. runblock:: pycon
>>> from machinevisiontoolbox import CentralCamera
>>> camera = CentralCamera.Default();
>>> camera.rhov
:seealso: :meth:`rhov` :meth:`rho`
"""
return self._rhov
@property
def rho(self) -> np.ndarray:
"""
Get pixel dimensions (base method)
:return: horizontal pixel size
:rtype: ndarray(2)
Example:
.. runblock:: pycon
>>> from machinevisiontoolbox import CentralCamera
>>> camera = CentralCamera();
>>> camera.rhov
:seealso: :meth:`rhou` :meth:`rhov`
"""
return np.array([self._rhou, self._rhov])
# @property
# def image(self):
# return self._image
# @image.setter
# def image(self, newimage):
# """
# :param newimage: [description]
# :type newimage: [type]
# """
# self._image = Image(newimage)
@property
def pose(self) -> SE3:
"""
Set/get camera pose (base method)
The camera pose with respect to the global frame can be read or written
as an :class:`~spatialmath..pose3d.SE3` instance.
Example:
.. runblock:: pycon
>>> from machinevisiontoolbox import CentralCamera
>>> from spatialmath import SE3
>>> camera = CentralCamera();
>>> camera.pose
>>> camera.pose = SE3.Trans(1, 2, 3)
>>> camera
.. note:: Changes the pose of the current camera instance, whereas
:meth:`move` clones the camera instance with a new pose.
:seealso: :meth:`move`
"""
return self._pose
@pose.setter
def pose(self, newpose: SE3) -> None:
"""
Set camera pose
:param newpose: pose of camera frame
:type newpose: :class:`~spatialmath..pose3d.SE3` or ndarray(4,4)
:seealso: :meth:`move`
"""
self._pose = SE3(newpose)
@property
def noise(self) -> float | None:
"""
Set/Get projection noise (base method)
:return: standard deviation of noise added to projected image plane points
:rtype: float
The noise parameter is set by the object constructor.
Example:
.. runblock:: pycon
>>> from machinevisiontoolbox import CentralCamera
>>> camera = CentralCamera.Default();
>>> camera.project_point([0, 0, 3])
>>> camera.noise = 2
>>> camera.project_point([0, 0, 2])
>>> camera.project_point([0, 0, 2])
:seealso: :meth:`project_point`
"""
return self._noise
@noise.setter
def noise(self, noise: float | None) -> None:
self._noise = noise
@property
def distortion(self) -> np.ndarray | None:
r"""
Set/Get distortion model (base method)
:return: distortion model
:rtype: array_like()
The distortion model is a vector of various lengths that can be read or written.
It is passed directly to OpenCV functions which define how it is used:
- 4-element vector, :math:`[k_1, k_2, p_1, p_2]`
- 5-element vector, :math:`[k_1, k_2, p_1, p_2, k_3]`
- 8-element vector, :math:`[k_1, k_2, p_1, p_2, k_3, k_4, k_5, k_6]`
- 12-element vector, :math:`[k_1, k_2, p_1, p_2, k_3, k_4, k_5, k_6, s_1, s_2, s_3, s_4]`,
- 14-element vector, :math:`[k_1, k_2, p_1, p_2, k_3, k_4, k_5, k_6, s_1, s_2, s_3, s_4, \tau_x, \tau_y]`
The elements :math:`k_i` are radial distortion coefficients, :math:`p_i` are
tangential distortion coefficients, :math:`s_i` are thin prism
distortion coefficients, and :math:`\tau_i` apply when the camera's image plane
is tilted with respect to the lens.
If the vector is ``None``, then zero distortion (all coefficients are zero) is
assumed.
:seealso: :meth:`~CentralCamera.project_point` `OpenCV distortion model <https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html>`_
"""
return self._distortion
@distortion.setter
def distortion(self, distortion: np.ndarray | None) -> None:
self._distortion = distortion
def move(
self, T: SE3, name: str | None = None, relative: bool = False
) -> CameraBase:
"""
Move camera (base method)
:param T: pose of camera frame
:type T: :class:`~spatialmath..pose3d.SE3`
:param relative: move relative to pose of original camera, defaults to ``False``
:type relative: bool, optional
:return: new camera object
:rtype: :class:`CameraBase` subclass
Returns a copy of the camera object with pose set to ``T``.
Example:
.. runblock:: pycon
>>> from machinevisiontoolbox import CentralCamera
>>> from spatialmath import SE3
>>> camera = CentralCamera();
>>> camera.move(SE3.Trans(0.1, 0.2, 0.3))
>>> camera
.. note:: The ``plot`` method of this cloned camera will create a new
window.
:seealso: :meth:`pose`
"""
newcamera = copy.copy(self)
if name is not None:
newcamera.name = name
else:
newcamera.name = self.name + "-moved"
newcamera._ax = None
if relative:
newcamera.pose = self.pose * T
else:
newcamera.pose = T
return newcamera
# ----------------------- plotting ----------------------------------- #
def _new_imageplane(
self, fig: Figure | None = None, ax: Axes | None = None
) -> bool:
"""
Create a new virtual image plane if required
:param fig: Matplotlib figure number, defaults to ``None``
:type fig: int, optional
:param ax: Matplotlob axes, defaults to ``None``
:type ax: :class:`matplotlib.axes`, optional
:return: whether virtual image plane exists
:rtype: bool
If this camera already has a virtual image plane, return ``True``.
Otherwise, create an axes, and optionally a figure, and return ``False``.
"""
if _isnotebook():
ax = plt.gca() # return current or create new
if self._ax is None or self._ax is not ax:
self._ax = ax
self._fig = plt.gcf()
return False
else:
return True
else:
# if not Jupyter, reuse an existing imageplane plot
if self._ax is not None:
return True
if (fig is None) and (ax is None):
# create our own handle for the figure/plot
# print('creating new figure and axes for camera')
fig, ax = plt.subplots() # TODO *args, **kwargs?
# TODO elif ax is a plot handle, else raise ValueError
self._ax = ax
self._fig = fig
return False
def _init_imageplane(
self, fig: Figure | None = None, ax: Axes | None = None
) -> Axes:
"""
Create plot window for camera image plane
:param fig: figure to plot into, defaults to ``None``
:type fig: figure handle, optional
:param ax: axis to plot into, defaults to ``None``
:type ax: 2D axis handle, optional
:return: figure and axis
:rtype: (fig, axis)
Creates a 2D axis that represents the image plane of the virtual
camera.
:seealso: :meth:`plot` :meth:`mesh`
"""
if self._new_imageplane(fig, ax):
assert self._ax is not None
return self._ax
ax = self._ax
assert ax is not None
if self._image is not None:
# if camera has an image, display said image
idisp(self._image, fig=fig, ax=ax, title=self._name, drawonly=True)
else:
if self.limits is None:
ax.set_xlim(0, self.nu)
ax.set_ylim(0, self.nv)
else:
ax.set_xlim(self.limits[0], self.limits[1])
ax.set_ylim(self.limits[2], self.limits[3])
ax.autoscale(False)
ax.set_aspect("equal")
ax.invert_yaxis()
ax.grid(True)
if self.labels is None:
ax.set_xlabel("u (pixels)")
ax.set_ylabel("v (pixels)")
else:
ax.set_xlabel(self.labels[0])
ax.set_ylabel(self.labels[1])
ax.set_title(self.name)
ax.set_facecolor("lightyellow")
try:
ax.figure.canvas.manager.set_window_title( # type: ignore[union-attr]
"Machine Vision Toolbox for Python"
)
except AttributeError:
# can happen during unit test without GUI
pass
# TODO figure out axes ticks, etc
return ax # likely this return is not necessary
def clf(self) -> None:
"""
Clear the virtual image plane (base method)
Every camera object has a virtual image plane drawn using Matplotlib.
Remove all points and lines from the image plane.
:seealso: :meth:`plot_point` :meth:`plot_line` :meth:`disp`
"""
if self._ax is not None:
for artist in self._ax.get_children():
try:
artist.remove()
except:
pass
def plot_point(
self,
P: np.ndarray | ArrayLike,
*fmt,
return_artist: bool = False,
objpose: SE3 | None = None,
pose: SE3 | None = None,
ax: Axes | None = None,
**kwargs,
) -> np.ndarray:
"""
Plot points on virtual image plane (base method)
:param P: 3D world point or points, or 2D image plane point or points
:type P: ndarray(3,), ndarray(3,N), or ndarray(2,), ndarray(2,N)
:param objpose: transformation for the 3D points, defaults to ``None``
:type objpose: :class:`~spatialmath..pose3d.SE3`, optional
:param pose: pose of the camera, defaults to ``None``
:type pose: :class:`~spatialmath..pose3d.SE3`, optional
:param ax: axes to plot into
:type ax: :class:`matplotlib.axes`
:param kwargs: additional arguments passed to :obj:`~matplotlib.pyplot.plot`
:return: Matplotlib line objects
:rtype: list of :class:`~matplotlib.lines.Line2d`
3D world points are first projected to the image plane and then
plotted on the camera's virtual image plane.
Points are organized as columns of the arrays.
Example:
.. code-block:: python
from machinevisiontoolbox import CentralCamera
from spatialmath import SE3
camera = CentralCamera.Default()
camera.plot_point([0.2, 0.3, 2])
camera.plot_point([0.2, 0.3, 2], 'r*')
camera.plot_point([0.2, 0.3, 2], pose=SE3(0.1, 0, 0))
.. plot::
from machinevisiontoolbox import CentralCamera
from spatialmath import SE3
camera = CentralCamera.Default()
camera.plot_point([0.2, 0.3, 2])
camera.plot_point([0.2, 0.3, 2], 'r*')
camera.plot_point([0.2, 0.3, 2], pose=SE3(0.1, 0, 0))
.. note::
- Successive calls add items to the virtual image plane.
- This method is common to all ``CameraBase`` subclasses, but it
invokes a camera-specific projection method.
:seealso: :meth:`plot_line2` :meth:`plot_line3` :meth:`plot_wireframe` :meth:`clf`
"""
self._init_imageplane(ax)
if not isinstance(P, np.ndarray):
P = smb.getvector(P)
if P.shape[0] == 3:
# plot world points
p = self.project_point(P, pose=pose, objpose=objpose)
else:
# plot image plane points
p = P
assert isinstance(p, np.ndarray)
if p.shape[0] != 2:
raise ValueError("p must have be (2,), (3,), (2,n), (3,n)")
defaults = dict(markersize=6, color="k")
if len(fmt) == 0:
fmt = ["o"]
kwargs = {**defaults, **kwargs}
assert self._ax is not None
artist = self._ax.plot(p[0, :], p[1, :], *fmt, **kwargs)
if plt.isinteractive():
plt.show(block=False)
if return_artist:
return p, artist[0]
else:
return p
def plot_line2(self, l: ArrayLike, *args: Any, **kwargs: Any) -> None:
r"""
Plot 2D line on virtual image plane (base method)
:param l: homogeneous line
:type l: array_like(3)
:param kwargs: arguments passed to ``plot``
Plot the homogeneous line on the camera's virtual image plane. The line
is expressed in the form
.. math:: \ell_0 u + \ell_1 v + \ell_2 = 0
Example:
.. runblock:: pycon
>>> from machinevisiontoolbox import CentralCamera
>>> camera = CentralCamera.Default()
>>> camera.plot_line2([1, 0.2, -500])
.. note::
- Successive calls add items to the virtual image plane.
- This method is common to all ``CameraBase`` subclasses, but it
invokes a camera-specific projection method.
:seealso: :meth:`plot_point` :meth:`plot_line3` :meth:`clf`
"""
# get handle for this camera image plane
self._init_imageplane()
plt.autoscale(False)
assert self._ax is not None
smb.plot_homline(l, *args, ax=self._ax, **kwargs)
# def plot_line3(self, L, nsteps=21, **kwargs):
# """
# Plot 3D line on virtual image plane (base method)
# :param L: 3D line or lines in Plucker coordinates
# :type L: :class:`~spatialmath..geom3d.Line3`
# :param kwargs: arguments passed to ``plot``
# The Plucker lines are projected to the camera's virtual image plane and
# plotted. Each line is approximated by ``nsteps`` points, each of which
# is projected, allowing straight lines to appear curved after projection.
# Example:
# .. runblock:: pycon
# >>> from machinevisiontoolbox import CentralCamera, mkcube
# >>> from spatialmath import Line3
# >>> camera = CentralCamera()
# >>> line = Line3.Join((-1, -2, -3), (4, 5, 6))
# >>> camera.plot_line3(line, 'k--')
# .. note::
# - Successive calls add items to the virtual image plane.
# - This method is common to all ``CameraBase`` subclasses, but it
# invokes a camera-specific projection method.
# :seealso: :meth:`plot_point` :meth:`plot_line2` :meth:`plot_wireframe` :meth:`clf`
# """
# # draw 3D line segments
# s = np.linspace(0, 1, nsteps)
# this is actually pretty tricky
# - how to determine which part of the 3D line is visible
# - if highly curved it may be in two or more segments
# for line in L:
# l = self.project_line(line)
# # straight world lines are not straight, plot them piecewise
# P = (1 - s) * P0[:, np.newaxis] + s * P2[:, np.newaxis]
# uv = self.project_point(P, pose=pose)
def plot_wireframe(
self,
X: np.ndarray,
Y: np.ndarray,
Z: np.ndarray,
*fmt: Any,
objpose: SE3 | None = None,
pose: SE3 | None = None,
nsteps: int = 21,
**kwargs: Any,
) -> None:
"""
Plot 3D wireframe in virtual image plane (base method)
:param X: world X coordinates
:type X: ndarray(N,M)
:param Y: world Y coordinates
:type Y: ndarray(N,M)
:param Z: world Z coordinates
:type Z: ndarray(N,M)
:param objpose: transformation for the wireframe points, defaults to ``None``
:type objpose: :class:`~spatialmath..pose3d.SE3`, optional
:param pose: pose of the camera, defaults to ``None``
:type pose: :class:`~spatialmath..pose3d.SE3`, optional
:param nsteps: number of points for each wireframe segment, defaults to 21
:type nsteps: int, optional
:param kwargs: arguments passed to ``plot``
The 3D wireframe is projected to the camera's virtual image plane. Each
wire link in the wireframe is approximated by ``nsteps`` points, each of
which is projected, allowing straight edges to appear curved.
Example:
.. code-block:: python
from machinevisiontoolbox import CentralCamera, mkcube
from spatialmath import SE3
camera = CentralCamera.Default()
X, Y, Z = mkcube(0.2, pose=SE3(0, 0, 1), edge=True)
camera.plot_wireframe(X, Y, Z, 'k--')
.. plot::
from machinevisiontoolbox import CentralCamera, mkcube
from spatialmath import SE3
camera = CentralCamera.Default()
X, Y, Z = mkcube(0.2, pose=SE3(0, 0, 1), edge=True)
camera.plot_wireframe(X, Y, Z, 'k--')
:seealso: :func:`mkcube` :obj:`spatialmath.base.cylinder` :obj:`spatialmath.base.sphere` :obj:`spatialmath.base.cuboid`
"""
# self._ax.plot_surface(X, Y, Z)
# plt.show()
# check that mesh matrices conform
if X.shape != Y.shape or X.shape != Z.shape:
raise ValueError("matrices must be the same shape")
if pose is None:
pose = self.pose
if objpose is not None:
pose = objpose.inv() * pose
# get handle for this camera image plane
self._init_imageplane()
plt.autoscale(False)
assert self._ax is not None
ax = self._ax
# draw 3D line segments
s = np.linspace(0, 1, nsteps)
# c.clf
# holdon = c.hold(1);
for i in range(X.shape[0] - 1): # i=1:numrows(X)-1
for j in range(X.shape[1] - 1): # j=1:numcols(X)-1
P0 = np.r_[X[i, j], Y[i, j], Z[i, j]]
P1 = np.r_[X[i + 1, j], Y[i + 1, j], Z[i + 1, j]]
P2 = np.r_[X[i, j + 1], Y[i, j + 1], Z[i, j + 1]]
if self.camtype == "perspective":
# straight world lines are straight on the image plane
uv = self.project_point(np.c_[P0, P1], pose=pose)
else:
# straight world lines are not straight, plot them piecewise
P = (1 - s) * P0[:, np.newaxis] + s * P1[:, np.newaxis]
uv = self.project_point(P, pose=pose)
ax.plot(uv[0, :], uv[1, :], *fmt, **kwargs)
if self.camtype == "perspective":
# straight world lines are straight on the image plane
uv = self.project_point(np.c_[P0, P2], pose=pose)
else:
# straight world lines are not straight, plot them piecewise
P = (1 - s) * P0[:, np.newaxis] + s * P2[:, np.newaxis]
uv = self.project_point(P, pose=pose)
ax.plot(uv[0, :], uv[1, :], *fmt, **kwargs)
for j in range(X.shape[1] - 1): # j=1:numcols(X)-1
P0 = [X[-1, j], Y[-1, j], Z[-1, j]]
P1 = [X[-1, j + 1], Y[-1, j + 1], Z[-1, j + 1]]
# if c.perspective
# straight world lines are straight on the image plane
uv = self.project_point(np.c_[P0, P1], pose=pose)
# else
# # straight world lines are not straight, plot them piecewise
# P = bsxfun(@times, (1-s), P0) + bsxfun(@times, s, P1);
# uv = c.project(P, 'setopt', opt);
ax.plot(uv[0, :], uv[1, :], *fmt, **kwargs)
# c.hold(holdon); # turn hold off if it was initially off
plt.draw()
def disp(self, im: Image, **kwargs: Any) -> None:
"""
Display image on virtual image plane (base method)
:param im: image to display
:type im: :class:`Image` instance
:param kwargs: options to :func:`~machinevisiontoolbox.base.imageio.idisp()`
An image is displayed on camera's the virtual image plane.
.. note: The dimensions of the image plane should match the dimensions of the image.
:seealso: :func:`machinevisiontoolbox.base.idisp()`
"""
self.imagesize = (im.shape[1], im.shape[0])
self._init_imageplane()
im.disp(ax=self._ax, title=False, **kwargs)
plt.autoscale(False)
def plot(
self=None,
pose: SE3 | None = None,
scale: float = 1,
shape: str = "camera",
label: bool = True,
alpha: float = 1,
solid: bool = False,
color: str = "r",
projection: str = "ortho",
frame: bool = False,
ax: Axes | None = None,
) -> Axes:
"""
Plot 3D camera icon in world view (base method)
:param pose: camera pose
:type pose: :class:`~spatialmath..pose3d.SE3`
:param scale: scale factor, defaults to 1
:type scale: float
:param shape: icon shape: 'frustum' [default], 'camera'
:type shape: str, optional
:param label: show camera name, defaults to ``True``
:type label: bool, optional
:param alpha: transparency of icon, defaults to 1
:type alpha: float, optional
:param solid: icon comprises solid faces, defaults to ``False``
:type solid: bool, optional
:param color: icon color, defaults to 'r'
:type color: str, optional
:param projection: projection model for new axes, defaults to 'ortho'
:type projection: str, optional
:param ax: axes to draw in, defaults to current 3D axes
:type ax: :class:`~matplotlib.Axes3D`, optional
: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.
"""
# if (fig is None) and (ax is None):
# # create our own handle for the figure/plot
# print('creating new figure and axes for camera')
# fig = plt.figure()
# ax = fig.gca(projection='3d')
# # ax.set_aspect('equal')
"""[summary]
face order -x, +y, +x, -y
"""
# get axes to draw in
ax = smb.axes_logic(ax, 3, projection=projection)
if pose is None:
pose = self.pose
# Matplotlib >=3.10 can raise while autoscaling ragged 3D line
# collections (eg. from cuboid/cylinder helper primitives). Disable
# autoscale only for these collection additions.
_orig_add_collection3d = ax.add_collection3d
def _add_collection3d_compat(collection, *args, **kwargs):
kwargs.setdefault("autolim", False)
try:
return _orig_add_collection3d(collection, *args, **kwargs)
except TypeError:
# Older Matplotlib versions may not accept autolim.
kwargs.pop("autolim", None)
return _orig_add_collection3d(collection, *args, **kwargs)
ax.add_collection3d = _add_collection3d_compat
try:
# draw camera-like object:
if shape == "frustum":
# TODO make this kwargs or optional inputs
# side colors:
# +x red
# -y red
# +y green
# -y yellow
length = scale
widthb = scale / 10
widtht = scale
widthb /= 2
widtht /= 2
b0 = np.array([-widthb, -widthb, 0, 1])
b1 = np.array([-widthb, widthb, 0, 1])
b2 = np.array([widthb, widthb, 0, 1])
b3 = np.array([widthb, -widthb, 0, 1])
t0 = np.array([-widtht, -widtht, length, 1])
t1 = np.array([-widtht, widtht, length, 1])
t2 = np.array([widtht, widtht, length, 1])
t3 = np.array([widtht, -widtht, length, 1])
# bottom/narrow end
T = pose.A
b0 = (T @ b0)[:-1]
b1 = (T @ b1)[:-1]
b2 = (T @ b2)[:-1]
b3 = (T @ b3)[:-1]
# wide/top end
t0 = (T @ t0)[:-1]
t1 = (T @ t1)[:-1]
t2 = (T @ t2)[:-1]
t3 = (T @ t3)[:-1]
points = [
np.array([b0, b1, t1, t0]), # -x face
np.array([b1, b2, t2, t1]), # +y face
np.array([b2, b3, t3, t2]), # +x face
np.array([b3, b0, t0, t3]), # -y face
]
poly = Poly3DCollection(
points, facecolors=["r", "g", "r", "y"], alpha=alpha
)
ax.add_collection3d(poly)
elif shape == "camera":
# the box is centred at the origin and its centerline parallel to the
# z-axis. Its z-extent is -bh/2 to bh/2.
W = 0.5 # width & height of the box
L = 1.2 # length of the box
cr = 0.2 # cylinder radius
ch = 0.4 # cylinder height
cn = 12 # number of facets of cylinder
a = 3 # length of axis line segments
# draw the box part of the camera
smb.plot_cuboid(
sides=np.r_[W, W, L] * scale,
pose=pose, # type: ignore[arg-type]
filled=solid,
color=color,
alpha=0.5 * alpha if solid else alpha,
ax=ax,
)
# draw the lens
smb.plot_cylinder(
radius=cr * scale,
height=np.r_[L / 2, L / 2 + ch] * scale,
resolution=cn,
pose=pose, # type: ignore[arg-type]
filled=solid,
color=color,
alpha=0.5 * alpha,
ax=ax,
)
if label:
ax.set_xlabel("X")
ax.set_ylabel("Y")
ax.set_zlabel("Z")
if frame is True:
self.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)
finally:
ax.add_collection3d = _orig_add_collection3d
return ax
def _add_noise_distortion(self, uv: np.ndarray) -> np.ndarray:
"""
Add noise to pixel coordinates
:param uv: image plane point coordinates
:type uv: ndarray(2,N)
:return: point coordinates with additive noise
:rtype: ndarray(2,N)
Model noise in the image process by adding zero-mean Gaussian noise
to the coordinates of projected world points. The noise has a
standard deviation specified by the camera constructor.
:seealso: :meth:`noise`
"""
# distort the pixels
# add Gaussian noise with specified standard deviation
if self.noise is not None:
uv += self._random.normal(0.0, self.noise, size=uv.shape)
return uv
[docs]
class CentralCamera(CameraBase):
"""
Create central camera projection model
.. inheritance-diagram:: machinevisiontoolbox.Camera.CentralCamera
:top-classes: machinevisiontoolbox.Camera.Camera
:parts: 1
:param f: focal length, defaults to 8mm
:type f: float, optional
:param distortion: camera distortion parameters, defaults to ``None``
:type distortion: array_like(5), optional
:param kwargs: arguments passed to :class:`CameraBase` constructor
A camera object contains methods for projecting 3D points and lines
to the image plane, as well as supporting a virtual image plane onto
which 3D points and lines can be drawn.
:references:
- |RVC3|, Section 13.1.
:seealso: :class:`CameraBase` :class:`FishEyeCamera` :class:`SphericalCamera`
"""
_fu: float
_fv: float
_distortion: np.ndarray | None
def __init__(
self, f: ArrayLike = 1, distortion: np.ndarray | None = None, **kwargs
) -> None:
super().__init__(camtype="perspective", **kwargs)
# TODO some of this logic to f and pp setters
self.f = f
self._distortion = distortion
[docs]
@classmethod
def Default(cls, **kwargs: Any) -> CentralCamera:
r"""
Set default central camera parameters
:return: central camera model
:rtype: :class:`CentralCamera` instance
Initialize a central camera with: focal length of 8mm, :math:`10\mu\mbox{m}` pixels,
image size of :math:`1000 \times 1000` with principal point at (500, 500).
Example:
.. runblock:: pycon
>>> from machinevisiontoolbox import CentralCamera
>>> camera = CentralCamera.Default(name='camera1')
>>> camera
:references:
- |RVC3|, Section 13.1.
:seealso: :class:`CentralCamera`
"""
default = {
"f": 0.008,
"rho": 10e-6,
"imagesize": 1000,
"pp": (500, 500),
"name": "default perspective camera",
}
return CentralCamera(**{**default, **kwargs})
def __str__(self) -> str:
"""
String representation of central camera parameters
:return: string representation
:rtype: str
Multi-line string representation of camera intrinsic and extrinsic
parameters.
"""
s = super().__str__()
s += self.fmt.format("principal pt", self.pp)
s += self.fmt.format("focal length", self.f)
return s
[docs]
def project_point(
self,
P: np.ndarray | ArrayLike,
pose: SE3 | None = None,
objpose: SE3 | None = None,
behind: bool = True,
visibility: bool = False,
retinal: bool = False,
**kwargs,
) -> np.ndarray:
r"""
Project 3D points to image plane
:param P: 3D world point or points in Euclidean or homogeneous form
:type P: array_like(3), array_like(3,n), array_like(4), array_like(4,n)
:param pose: camera pose with respect to the world frame, defaults to
camera's ``pose`` attribute
:type pose: :class:`~spatialmath..pose3d.SE3`, optional
:param objpose: 3D point reference frame, defaults to world frame
:type objpose: :class:`~spatialmath..pose3d.SE3`, optional
:param behind: points behind the camera indicated by NaN, defaults to ``True``
:type behind: bool, optional
:param visibility: return visibility array, defaults to ``False``
:type visibility: bool
:param retinal: transform to retinal coordinates, defaults to ``False``
:type retinal: bool, optional
:return: image plane points, optional visibility vector
:rtype: ndarray(2,n), ndarray(n)
Project a 3D point to the image plane
.. math::
\hvec{p} = \mat{C} \hvec{P}
where :math:`\mat{C}` is the camera calibration matrix and
:math:`\hvec{p}` and :math:`\hvec{P}` are the image plane and world
frame coordinates respectively, in homogeneous form.
World points are given as a 1D array or the columns of a 2D array of
Euclidean or homogeneous coordinates. The computed image plane
coordinates are Euclidean or homogeneous and given as a 1D array or the
corresponding columns of a 2D array.
If ``pose`` is specified it is used for the camera frame pose, otherwise
the attribute ``pose`` is used. The object's ``pose`` attribute is not
updated if ``pose`` is specified.
A single point can be specified as a 3-vector, multiple points as an
array with three rows and each column is the 3D point coordinate (X, Y,
Z).
The points ``P`` are by default with respect to the world frame, but
they can be transformed by specifying ``objpose``.
If world points are behind the camera and ``behind`` is ``True`` then the
image plane coordinates are set to NaN.
If ``visibility`` is ``True`` then each projected point is checked to ensure
it lies in the bounds of the image plane. In this case there are two
return values: the image plane coordinates and an array of booleans
indicating if the corresponding point is visible.
If ``retinal`` is ``True`` then project points in retinal coordinates,
in units of metres with respect to the principal point.
Example:
.. runblock:: pycon
>>> from machinevisiontoolbox import CentralCamera
>>> camera = CentralCamera.Default()
>>> camera.project_point((0.3, 0.4, 2))
:references:
- |RVC3|, Section 13.1.
:seealso: :meth:`C` :meth:`project_point` :meth:`project_line` :meth:`project_quadric`
"""
if pose is None:
pose = self.pose
C = self.C(pose, retinal=retinal)
if isinstance(P, np.ndarray):
if P.ndim == 1:
P = P.reshape((-1, 1)) # make it a column
else:
P = smb.getvector(P, out="col")
if P.shape[0] == 3:
P = smb.e2h(P) # make it homogeneous
euclidean = True
else:
euclidean = False
# project 3D points
if objpose is not None:
P = objpose.A @ P
x = C @ P
if behind:
x[2, x[2, :] < 0] = np.nan # points behind the camera are set to NaN
if euclidean:
# Euclidean points given, return Euclidean points
x = smb.h2e(x)
# add Gaussian noise and distortion
if self._distortion:
x = self.distort(x)
x = self._add_noise_distortion(x)
# do visibility check if required
if visibility:
visible = (
~np.isnan(x[0, :])
& (x[0, :] >= 0)
& (x[1, :] >= 0)
& (x[0, :] < self.nu)
& (x[1, :] < self.nv)
)
return x, visible
else:
return x
else:
# homogeneous points given, return homogeneous points
return x
[docs]
def project_line(self, lines: Line3) -> np.ndarray:
r"""
Project 3D lines to image plane
:param lines: Plucker line or lines
:type lines: :class:`~spatialmath..geom3d.Line3` instance with N values
:return: 2D homogeneous lines, one per column
:rtype: ndarray(3,N)
The :class:`~spatialmath..geom3d.Line3` object can contain multiple lines. The result array has one
column per line, and each column is a vector describing the image plane
line in homogeneous form :math:`\ell_0 u + \ell_1 v + \ell_2 = 0`.
The projection is
.. math::
\ell = \vex{\mat{C} \sk{\vec{L}} \mat{C}^{\top}}
where :math:`\mat{C}` is the camera calibration matrix and :math:`\sk{\vec{L}}`
is the skew matrix representation of the Plucker line.
Example:
.. runblock:: pycon
>>> from machinevisiontoolbox import CentralCamera
>>> from spatialmath import Line3
>>> line = Line3.Join((-3, -4, 5), (5, 2, 6))
>>> line
>>> camera = CentralCamera()
>>> camera.project_line(line)
:references:
- |RVC3|, Section 13.7.1.
:seealso: :meth:`C` :class:`~spatialmath..geom3d.Line3` :meth:`project_point` :meth:`project_quadric`
"""
if not isinstance(lines, Line3):
raise ValueError("expecting Line3 lines")
# project Plucker lines
lines2d = []
C = self.C()
for line in lines:
l = smb.vex(C @ line.skew() @ C.T)
x = l / np.max(np.abs(l)) # normalize by largest element
lines2d.append(x)
return np.column_stack(lines2d)
[docs]
def project_quadric(self, Q: np.ndarray) -> np.ndarray:
r"""
Project 3D quadric to image plane
:param Q: quadric matrix
:type Q: ndarray(4,4)
:return: image plane conic
:rtype: ndarray(3,3)
Quadrics, short for quadratic surfaces, are a rich family of
3-dimensional surfaces. There are 17 standard types including spheres,
ellipsoids, hyper- boloids, paraboloids, cylinders and cones all
described by points :math:`\vec{x} \in \mathbb{P}^3` such that
.. math::
\hvec{x}^{\top} \mat{Q} \hvec{x} = 0
The outline of the quadric is projected to a conic section on the image
plane
.. math::
c^* = \mat{C} \mat{Q}^* \mat{C}^{\top}
where :math:`(\mat{X})^* = det(\mat{X}) \mat{X}^{-1}` is the adjugate
operator.
Example:
.. runblock:: pycon
>>> from machinevisiontoolbox import CentralCamera
>>> from spatialmath import SE3
>>> T_cam = SE3.Trans(0.2, 0.1, -5) * SE3.Rx(0.2)
>>> Q = np.diag([1, 1, 1, -1]) # unit sphere at origin
>>> camera = CentralCamera.Default(f=0.015, pose=T_cam);
>>> camera.project_quadric(Q)
:references:
- |RVC3|, Section 13.7.1.
:seealso: :meth:`C` :meth:`project_point` :meth:`project_line`
"""
if not smb.ismatrix(Q, (4, 4)):
raise ValueError("expecting 4x4 conic matrix")
return self.C() @ Q @ self.C().T
[docs]
def epiline(self, p: np.ndarray, camera2: CentralCamera) -> np.ndarray:
r"""
Compute epipolar line
:param p: image plane point or points
:type p: array_like(2) or ndarray(2,N)
:param camera2: second camera
:type camera2: :class:`CentralCamera` instance
:return: epipolar line or lines in homogeneous form
:rtype: ndarray(3), ndarray(3,N)
Compute the epipolar line in ``camera2`` induced by the image plane
points ``p`` in the current camera. Each line is given by
.. math::
\ell = \mat{F} {}^1 \hvec{p}
which is in homogeneous form :math:`\ell_0 u + \ell_1 v + \ell_2 = 0`
and the conjugate point :math:`{}^2 \vec{p}` lies on this line.
Example:
.. runblock:: pycon
>>> from machinevisiontoolbox import CentralCamera
>>> from spatialmath import SE3
>>> camera1 = CentralCamera.Default(name='camera1')
>>> camera2 = CentralCamera.Default(pose=SE3(0.1, 0.05, 0), name='camera2')
>>> P = [-0.2, 0.3, 5] # world point
>>> p1 = camera1.project_point(P) # project to first camera
>>> camera1.epiline(p1, camera2) # epipolar line in second camera
:references:
- |RVC3|, Section 14.2.1.
:seealso: :meth:`plot_epiline` :meth:`CentralCamera.F`
"""
# p is 3 x N, result is 3 x N
return self.F(camera2) @ smb.e2h(p)
[docs]
def plot_epiline(
self, F: np.ndarray, p: np.ndarray, *fmt: Any, **kwargs: Any
) -> None:
r"""
Plot epipolar line
:param F: fundamental matrix
:type F: ndarray(3,3)
:param p: image plane point or points
:type p: array_like(2) or ndarray(2,N)
:param fmt: line style argument passed to ``plot``
:param kwargs: additional line style arguments passed to ``plot``
Plot the epipolar line induced by the image plane points ``p`` in the
camera's virtual image plane. Each line is given by
.. math::
\ell = \mat{F} {}^1 \hvec{p}
which is in homogeneous form :math:`\ell_0 u + \ell_1 v + \ell_2 = 0`
and the conjugate point :math:`{}^2 \vec{p}` lies on this line.
Example:
.. runblock:: pycon
>>> from machinevisiontoolbox import CentralCamera
>>> from spatialmath import SE3
>>> camera1 = CentralCamera.Default(name='camera1')
>>> camera2 = CentralCamera.Default(pose=SE3(0.1, 0.05, 0), name='camera2')
>>> P = [-0.2, 0.3, 5] # world point
>>> p1 = camera1.project_point(P) # project to first camera
>>> camera2.plot_point(P, 'kd') # project and display in second camera
>>> camera2.plot_epiline(camera1.F(camera2), p1) # plot epipolar line in second camera
:references:
- |RVC3|, Section 14.2.1.
:seealso: :meth:`plot_point` :meth:`epiline` :meth:`CentralCamera.F`
"""
# p is 3 x N, result is 3 x N
self.plot_line2(F @ smb.e2h(p), *fmt, **kwargs)
[docs]
def plot_line3(self, L: Line3, **kwargs: Any) -> None:
"""
Plot 3D line on virtual image plane (base method)
:param L: 3D line in Plucker coordinates
:type L: :class:`~spatialmath..geom3d.Line3`
:param kwargs: arguments passed to ``plot``
The Plucker line is projected to the camera's virtual image plane and
plotted.
.. note::
- Successive calls add items to the virtual image plane.
- This method is common to all ``CameraBase`` subclasses, but it
invokes a camera-specific projection method.
:seealso: :meth:`plot_point` :meth:`plot_line2` :meth:`plot_wireframe` :meth:`clf`
"""
l = self.project_line(L)
for hl in l.T:
self.plot_line2(hl, **kwargs)
[docs]
def ray(self, points: np.ndarray | ArrayLike, pose: SE3 | None = None) -> Line3:
"""
Project image plane points to a ray
:param points: image plane points
:type points: ndarray(2,N)
:param pose: camera pose, defaults to ``None``
:type pose: :class:`~spatialmath..pose3d.SE3`, optional
:return: corresponding Plucker lines
:rtype: :class:`~spatialmath..geom3d.Line3`
For each image plane point compute the equation of a Plucker line
that represents the 3D ray from the camera origin through the image
plane point.
Example:
.. runblock:: pycon
>>> from machinevisiontoolbox import CentralCamera
>>> camera = CentralCamera.Default()
>>> line = camera.ray((100, 200))
>>> line
:references:
- "Multiview Geometry", Hartley & Zisserman, p.162
- |RVC3|, Section 14.3.
:seealso: :class:`~spatialmath..geom3d.Line3`
"""
# define Plucker line in terms of point (centre of camera) and direction
C = self.C(pose=pose)
Mi = np.linalg.inv(C[:3, :3])
v = C[:, 3]
lines = []
for point in smb.getmatrix(points, (2, None)).T:
lines.append(Line3.PointDir(-Mi @ v, Mi @ smb.e2h(point)))
return Line3(lines)
@property
def centre(self) -> np.ndarray:
"""
Position of camera frame
:return: Euclidean coordinate of the camera frame's origin
:rtype: ndarray(3)
Example:
.. runblock:: pycon
>>> from machinevisiontoolbox import CentralCamera
>>> from spatialmath import SE3
>>> camera1 = CentralCamera.Default(name='camera1', pose=SE3.Trans(1,2,3))
>>> camera1
>>> camera1.centre
"""
return self.pose.t
@property
def center(self) -> np.ndarray:
"""
Position of camera frame
:return: Euclidean coordinate of the camera frame's origin
:rtype: ndarray(3)
Example:
.. runblock:: pycon
>>> from machinevisiontoolbox import CentralCamera
>>> from spatialmath import SE3
>>> camera1 = CentralCamera.Default(name='camera1', pose=SE3.Trans(1,2,3))
>>> camera1
>>> camera1.center
"""
return self.pose.t
[docs]
def fov(self) -> np.ndarray:
"""
Camera field-of-view angles
:return: field of view angles in radians
:rtype: ndarray(2)
Computes the field of view angles (2x1) in radians for the camera
horizontal and vertical directions.
Example:
.. runblock:: pycon
>>> from machinevisiontoolbox import CentralCamera
>>> camera1 = CentralCamera.Default(name='camera1')
>>> camera1.fov()
:references:
- |RVC3|, Section 13.1.4.
"""
try:
return 2 * np.arctan(np.r_[self.imagesize] / 2 * np.r_[self.rho] / self.f)
except:
raise ValueError("imagesize or rho properties not set")
[docs]
def distort(self, points: np.ndarray) -> np.ndarray:
"""
Compute distorted coordinate
:param points: image plane points
:type points: ndarray(2,n)
:returns: distorted image plane coordinates
:rtype: ndarray(2,n)
Compute the image plane coordinates after lens distortion has been
applied. The lens distortion model is initialized at constructor time.
"""
if self._distortion is None:
return points
# convert to normalized image coordinates
X = np.linalg.inv(self.K) * smb.e2h(points)
# unpack coordinates
u = X[0, :]
v = X[1, :]
# unpack distortion vector
k = self._distortion[:3]
p = self._distortion[3:]
r = np.sqrt(u**2 + v**2) # distance from principal point
# compute the shift due to distortion
delta_u = (
u * (k[0] * r**2 + k[1] * r**4 + k[2] * r**6)
+ 2 * p[0] * u * v
+ p[1] * (r**2 + 2 * u**2)
)
delta_v = (
v * (k[0] * r**2 + k[1] * r**4 + k[2] * r**6)
+ p[0] * (r**2 + 2 * v**2)
+ 2 * p[1] * u * v
)
# distorted coordinates
ud = u + delta_u
vd = v + delta_v
return self.K * smb.e2h(np.r_[ud, vd]) # convert to pixel coords
@property
def fu(self) -> float:
"""
Get focal length in horizontal direction
:return: focal length in horizontal direction
:rtype: float
Example:
.. runblock:: pycon
>>> from machinevisiontoolbox import CentralCamera
>>> camera = CentralCamera.Default(name='camera1')
>>> camera.fu
:seealso: :meth:`fv` :meth:`f`
"""
return self._fu
@property
def fv(self) -> float:
"""
Get focal length in vertical direction
:return: focal length in vertical direction
:rtype: float
Example:
.. runblock:: pycon
>>> from machinevisiontoolbox import CentralCamera
>>> camera = CentralCamera.Default(name='camera1')
>>> camera.fv
:seealso: :meth:`fu` :meth:`f`
"""
return self._fv
@property
def f(self) -> np.ndarray:
"""
Set/get focal length
:return: focal length in horizontal and vertical directions
:rtype: ndarray(2)
Return focal length in horizontal and vertical directions.
Example:
.. runblock:: pycon
>>> from machinevisiontoolbox import CentralCamera
>>> camera = CentralCamera.Default(name='camera1')
>>> camera.f
>>> camera.f = 0.015
>>> camera.f
>>> camera.f = [0.015, 0.020]
>>> camera.f
.. note:: These are normally identical but will differ if the sensor
has non-square pixels or the frame grabber is changing the aspect
ratio of the image.
:seealso: :meth:`fu` :meth:`fv`
"""
return np.r_[self._fu, self._fv]
@f.setter
def f(self, f: ArrayLike) -> None:
"""[summary]
:param f: focal length
:type f: scalar or array_like(2)
:raises ValueError: incorrect length of ``f``
"""
f = smb.getvector(f)
if len(f) == 1:
self._fu = f[0]
self._fv = f[0]
elif len(f) == 2:
self._fu = f[0]
self._fv = f[1]
else:
raise ValueError(f, "f must be a 1- or 2-element vector")
@property
def fpix(self) -> np.ndarray:
"""
Get focal length in pixels
:return: focal length in horizontal and vertical directions in pixels
:rtype: ndarray(2)
Example:
.. runblock:: pycon
>>> from machinevisiontoolbox import CentralCamera
>>> camera = CentralCamera.Default(name='camera1')
>>> camera.fpix
:seealso: :meth:`f`
"""
return np.r_[self._fu / self._rhou, self._fv / self._rhov]
@property
def K(self) -> np.ndarray:
"""
Intrinsic matrix of camera
:return: intrinsic matrix
:rtype: ndarray(3,3)
Return the camera intrinsic matrix.
Example:
.. runblock:: pycon
>>> from machinevisiontoolbox import CentralCamera
>>> camera = CentralCamera.Default(name='camera1')
>>> camera.K
:references:
- |RVC3|, Section 13.1.
:seealso: :meth:`C` :meth:`pp` :meth:`rho`
"""
# fmt: off
K = np.array([[self.fu / self.rhou, 0, self.u0],
[ 0, self.fv / self.rhov, self.v0],
[ 0, 0, 1]
], dtype=np.float64)
# fmt: on
return K
# =================== camera calibration =============================== #
[docs]
def C(self, pose: SE3 | None = None, retinal: bool = False) -> np.ndarray:
"""
Camera projection matrix
:param T: camera pose with respect to world frame, defaults to pose from camera object
:type T: :class:`~spatialmath..pose3d.SE3`, optional
:param retinal: transform to retinal coordinates, default False
:type retinal: bool, optional
:return: camera projection/calibration matrix
:rtype: ndarray(3,4)
Return the camera matrix which projects 3D points to the image plane.
It is a function of the camera's intrinsic and extrinsic parameters.
If ``retinal`` is ``True`` then project points in retinal coordinates,
in units of metres with respect to the principal point.
Example:
.. runblock:: pycon
>>> from machinevisiontoolbox import CentralCamera
>>> camera = CentralCamera.Default(name='camera1')
>>> camera.C()
>>> camera.C(SE3.Trans(0.1, 0, 0))
>>> camera.move(SE3(0.1, 0, 0)).C()
:references:
- |RVC3|, Section 13.1.
:seealso: :meth:`project_point` :meth:`K` :meth:`decomposeC`
"""
P0 = np.eye(3, 4)
if pose is None:
pose = self.pose
if retinal:
K = np.diag([self.fu, self.fv, 1])
else:
K = self.K
return K @ P0 @ pose.inv().A
[docs]
@staticmethod
def points2C(P: np.ndarray, p: np.ndarray) -> tuple[np.ndarray, float]:
r"""
Estimate camera matrix from data points
:param P: calibration points in world coordinate frame
:type P: ndarray(3,N)
:param p: calibration points in image plane
:type p: ndarray(2,N)
:return: camera calibration matrix and residual
:rtype: ndarray(3,4), float
Estimate the camera matrix :math:`\mat{C}` determined by least
squares from corresponding world ``P`` and image-plane ``p`` points.
Corresponding points are represented by corresponding columns of ``P``
and ``p``. Also returns the residual which is:
.. math::
\max | \mat{C}\mat{P} - \mat{p} |
Example:
.. runblock:: pycon
>>> from machinevisiontoolbox import CentralCamera, mkcube
>>> P = mkcube(0.2)
>>> camera_unknown = CentralCamera(f=0.015, rho=10e-6, imagesize=[1280, 1024], noise=0.05, seed=0)
>>> T_unknown = SE3.Trans(0.1, 0.2, 1.5) * SE3.RPY(0.1, 0.2, 0.3)
>>> p = camera_unknown.project_point(P, objpose=T_unknown)
>>> C, resid = CentralCamera.points2C(P, p)
>>> C
>>> camera_unknown.C()
>>> resid
.. note:: This method assumes no lens distortion affecting the image plane
coordinates.
:references:
- |RVC3|, Section 13.2.1.
:seealso: :meth:`C` :meth:`images2C` :meth:`decomposeC`
"""
z4 = np.zeros((4,))
A = np.empty(shape=(0, 11))
b = np.empty(shape=(0,))
for uv, X in zip(p.T, P.T):
u, v = uv
# fmt: off
row = np.array([
np.r_[ X, 1, z4, -u * X],
np.r_[z4, X, 1, -v * X]
])
# fmt: on
A = np.vstack((A, row))
b = np.r_[b, uv]
# solve Ax = b where c is 11 elements of camera matrix
c, *_ = scipy.linalg.lstsq(A, b)
# compute and print the residual
r = np.max(np.abs((A @ c - b)))
c = np.r_[c, 1] # append a 1
C = c.reshape((3, 4)) # make a 3x4 matrix
return C, r
[docs]
@classmethod
def images2C(
cls, images, gridshape: tuple[int, int] = (7, 6), squaresize: float = 0.025
) -> tuple[np.ndarray, np.ndarray, list] | None:
"""
Calibrate camera from checkerboard images
:param images: an iterator that returns :class:`~machinevisiontoolbox.ImageCore.Image` objects
:type images: :class:`~machinevisiontoolbox.Sources.ImageSource`
:param gridshape: number of grid squares in each dimension, defaults to (7,6)
:type gridshape: tuple, optional
:param squaresize: size of the grid squares in units of length, defaults to 0.025
:type squaresize: float, optional
:return: camera calibration matrix, distortion parameters, image frames
:rtype: ndarray(3,4), ndarray(5), list of named tuples
The distortion coefficients are in the order :math:`(k_1, k_2, p_1, p_2, k_3)`
where :math:`k_i` are radial distortion coefficients and :math:`p_i` are
tangential distortion coefficients.
Image frames that were successfully processed are returned as a list of
named tuples ``CalibrationFrame`` with elements:
======= ===================== ===========================================================
element type description
======= ===================== ===========================================================
image :class:`Image` calibration image with overlaid annotation
pose :class:`SE3` instance pose of the camera with respect to the origin of this image
id int sequence number of this image in ``images``
======= ===================== ===========================================================
.. note:: The units used for ``squaresize`` must match the units used
for defining 3D points in space.
:references:
- |RVC3|, Section 13.7.
:seealso: :meth:`C` :meth:`points2C` :meth:`decomposeC` :class:`~spatialmath..pose3d.SE3`
"""
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
# create set of feature points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
# these all have Z=0 since they are relative to the calibration target frame
objp = np.zeros((gridshape[0] * gridshape[1], 3), np.float32)
objp[:, :2] = (
np.mgrid[0 : gridshape[0], 0 : gridshape[1]].T.reshape(-1, 2) * squaresize
)
# lists to store object points and image points from all the images
objpoints = [] # 3d point in real world space
imgpoints = [] # 2d points in image plane.
corner_images = []
valid = []
for i, image in enumerate(images):
gray = image.mono()._A
# Find the chess board corners
ret, corners = cv2.findChessboardCorners(
image=gray, patternSize=gridshape, corners=None
)
# If found, add object points, image points (after refining them)
if ret:
objpoints.append(objp)
corners2 = cv2.cornerSubPix(
image=gray,
corners=corners,
winSize=(11, 11),
zeroZone=(-1, -1),
criteria=criteria,
)
imgpoints.append(corners)
# Draw the corners
image = Image(image, copy=True)
if not image.iscolor:
image = image.colorize()
corner_images.append(
cv2.drawChessboardCorners(
image=image._A,
patternSize=gridshape,
corners=corners2,
patternWasFound=ret,
)
)
valid.append(i)
ret, C, distortion, rvecs, tvecs = cv2.calibrateCamera( # type: ignore[call-overload]
objectPoints=objpoints,
imagePoints=imgpoints,
imageSize=gray.shape[::-1],
cameraMatrix=None, # type: ignore[arg-type]
distCoeffs=None, # type: ignore[arg-type]
)
CalibrationFrame = namedtuple("CalibrationFrame", "image pose id")
if ret:
frames = []
for rvec, tvec, corner_image, id in zip(rvecs, tvecs, corner_images, valid):
frame = CalibrationFrame(
Image(corner_image, colororder="BGR"),
SE3(SE3(tvec) * SE3.EulerVec(rvec.flatten())).inv(), # type: ignore[arg-type]
id,
)
frames.append(frame)
return C, distortion[0], frames
else:
return None
[docs]
@classmethod
def decomposeC(cls, C: np.ndarray) -> CentralCamera:
r"""
Decompose camera calibration matrix
:param C: camera calibration matrix
:type C: ndarray(3,4)
:return: camera model parameters
:rtype: :class:`CentralCamera`
Decompose a :math:`3\times 4` camera calibration matrix ``C`` to
determine feasible intrinsic and extrinsic parameters. The result is a
``CentralCamera`` instance with the following parameters set:
================ ====================================
Parameter Meaning
================ ====================================
``f`` focal length in pixels
``sx``, ``sy`` pixel size where ``sx`` =1
(``u0``, ``v0``) principal point
``pose`` pose of the camera frame wrt world
================ ====================================
Example:
.. runblock:: pycon
>>> from machinevisiontoolbox import CentralCamera
>>> from spatialmath import SE3
>>> camera = CentralCamera(name='camera1')
>>> C = camera.C(SE3(0.1, 0, 0))
>>> CentralCamera.decomposeC(C)
.. note:: Since only :math:`f s_x` and :math:`f s_y` can be estimated we
set :math:`s_x = 1`.
:references:
- Multiple View Geometry, Hartley&Zisserman, p 163-164
- |RVC3|, Section 13.2.3.
:seealso: :meth:`C` :meth:`points2C`
"""
def rq(S: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
# from vgg_rq.m
# [R,Q] = vgg_rq(S) Just like qr but the other way around.
# If [R,Q] = vgg_rq(X), then R is upper-triangular, Q is orthogonal, and X==R*Q.
# Moreover, if S is a real matrix, then det(Q)>0.
# By awf
S = S.T
Q, U = np.linalg.qr(S[::-1, ::-1])
Q = Q.T
Q = Q[::-1, ::-1]
U = U.T
U = U[::-1, ::-1]
if np.linalg.det(Q) < 0:
U[:, 0] = -U[:, 0]
Q[0, :] = -Q[0, :]
return U, Q
if not C.shape == (3, 4):
raise ValueError("argument is not a 3x4 matrix")
u, s, v = np.linalg.svd(C)
v = v.T
# determine camera position
t = v[:, 3] # last column
t = t / t[3]
t = t[:3]
# determine camera orientation
M = C[:3, :3]
# K, R = rq(M)
K, R = scipy.linalg.rq(M)
# deal with K having negative elements on the diagonal
# make a matrix to fix this, K*C has positive diagonal
C = np.diag(np.sign(np.diag(K)))
# now K*R = (K*C) * (inv(C)*R), so we need to check C is a proper rotation
# matrix. If isn't then the situation is unfixable
if not np.isclose(np.linalg.det(C), 1):
raise RuntimeError("cannot correct signs in the intrinsic matrix")
# all good, let's fix it
K = K @ C
R = C.T @ R
# normalize K so that lower left is 1
K = K / K[2, 2]
# pull out focal length and scale factors
f = K[0, 0]
s = np.r_[1, K[1, 1] / K[0, 0]]
# build an equivalent camera model
return cls(name="invC", f=f, pp=K[:2, 2], rho=s, pose=SE3.Rt(R.T, t))
# https://docs.opencv.org/3.1.0/d9/d0c/group__calib3d.html#gaaae5a7899faa1ffdf268cd9088940248
# ======================= homography =============================== #
[docs]
def H(self, T: SE3, n: ArrayLike, d: float) -> np.ndarray:
"""
Compute homography from plane and camera pose
:param T: relative camera motion
:type T: :class:`~spatialmath..pose3d.SE3`
:param n: plane normal with respect to world frame
:type n: array_like(3)
:param d: plane offset from world frame origin
:type d: float
:return: homography matrix
:rtype: ndarray(3,3)
Computes the homography matrix for the camera observing points on a
plane from two viewpoints. The first view is from the current camera
pose (``self.pose``), and the second is after a relative motion
represented by the rigid-body motion ``T``. The plane has normal ``n``
and at distance ``d`` with respect to the world frame.
Example:
.. runblock:: pycon
>>> from machinevisiontoolbox import CentralCamera
>>> from spatialmath import SE3
>>> camera = CentralCamera.Default(name='camera1') # looking along z-axis
>>> plane = [0, 1, 1]
>>> H = camera.H(SE3.Tx(0.2), plane, 5)
>>> H
:seealso: :meth:`points2H` :meth:`decomposeH`
"""
if d < 0:
raise ValueError(d, "plane distance d must be > 0")
n = smb.getvector(n)
if n[2] < 0:
raise ValueError(n, "normal must be away from camera (n[2] >= 0)")
# T transform view 1 to view 2
T = SE3(T).inv()
HH = T.R + 1.0 / d * T.t @ n # need to ensure column then row = 3x3
# apply camera intrinsics
HH = self.K @ HH @ np.linalg.inv(self.K)
return HH / HH[2, 2] # normalised
[docs]
@staticmethod
def points2H(
p1: np.ndarray,
p2: np.ndarray,
method: str = "leastsquares",
seed: int | None = None,
**kwargs,
) -> tuple[np.ndarray, float] | tuple[np.ndarray, float, np.ndarray]:
"""
Estimate homography from corresponding points
:param p1: image plane points from first camera
:type p1: ndarray(2,N)
:param p2: image plane points from second camera
:type p2: ndarray(2,N)
:param method: algorithm: 'leastsquares' [default], 'ransac', 'lmeds', 'prosac'
:type method: str
:param kwargs: optional arguments as required for ransac' and 'lmeds'
methods
:return: homography, residual and optional inliers
:rtype: ndarray(3,3), float, ndarray(N,bool)
Compute a homography from two sets of corresponding image plane points
whose world points lie on a plane.
Example:
.. runblock:: pycon
>>> from machinevisiontoolbox import CentralCamera, mkgrid
>>> from spatialmath import SE3
>>> camera1 = CentralCamera(name="camera 1", f=0.002, imagesize=1000, rho=10e-6, pose=SE3.Tx(-0.1)*SE3.Ry(0.4))
>>> camera2 = CentralCamera(name="camera 2", f=0.002, imagesize=1000, rho=10e-6, pose=SE3.Tx(0.1)*SE3.Ry(-0.4))
>>> T_grid = SE3.Tz(1) * SE3.Rx(0.1) * SE3.Ry(0.2)
>>> P = mkgrid(3, 1.0, pose=T_grid)
>>> p1 = camera1.project_point(P)
>>> p2 = camera2.project_point(P);
>>> H, resid = CentralCamera.points2H(p1, p2)
>>> H
>>> resid
.. note:: If the method is 'ransac' or 'lmeds' then a boolean array
of inliers is also returned, True means the corresponding input
point pair is an inlier.
:references:
- |RVC3|, Section 14.2.4.
:seealso: :meth:`H` :meth:`decomposeH`
`opencv.findHomography <https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html#ga4abc2ece9fab9398f2e560d53c8c9780>`_
"""
points2H_dict = {
"leastsquares": 0,
"ransac": cv2.RANSAC,
"lmeds": cv2.LMEDS,
"prosac": cv2.RHO,
}
if seed is not None:
cv2.setRNGSeed(seed)
H, mask = cv2.findHomography(
srcPoints=p1.T, dstPoints=p2.T, method=points2H_dict[method], **kwargs
)
mask = mask.ravel().astype(bool)
e = smb.homtrans(H, p1[:, mask]) - p2[:, mask]
resid = np.linalg.norm(e)
if method in ("ransac", "lmeds"):
return H, resid, mask
else:
return H, resid
# https://docs.opencv.org/3.1.0/d9/d0c/group__calib3d.html#ga7f60bdff78833d1e3fd6d9d0fd538d92
[docs]
def decomposeH(
self, H: np.ndarray, K: np.ndarray | None = None
) -> tuple[SE3, list[np.ndarray]]:
"""
Decompose homography matrix
:param H: homography matrix
:type H: ndarray(3,3)
:param K: camera intrinsics, defaults to parameters from object
:type K: ndarray(3,3), optional
:return: camera poses, plane normals
:rtype: :class:`~spatialmath..pose3d.SE3`, list of ndarray(3,1)
Decomposes the homography matrix into the camera motion and the normal
to the plane. In practice, there are multiple solutions. The translation
not to scale.
Example:
.. runblock:: pycon
>>> from machinevisiontoolbox import CentralCamera, mkgrid
>>> from spatialmath import SE3
>>> camera1 = CentralCamera(name="camera 1", f=0.002, imagesize=1000, rho=10e-6, pose=SE3.Tx(-0.1)*SE3.Ry(0.4))
>>> camera2 = CentralCamera(name="camera 2", f=0.002, imagesize=1000, rho=10e-6, pose=SE3.Tx(0.1)*SE3.Ry(-0.4))
>>> T_grid = SE3.Tz(1) * SE3.Rx(0.1) * SE3.Ry(0.2)
>>> P = mkgrid(3, 1.0, pose=T_grid)
>>> p1 = camera1.project_point(P)
>>> p2 = camera2.project_point(P);
>>> H, resid = CentralCamera.points2H(p1, p2)
>>> T, normals = camera1.decomposeH(H)
>>> T.printline(orient="camera")
>>> normals
:references:
- |RVC3|, Section 14.2.4.
:seealso: :meth:`points2H` :meth:`H`
`opencv.decomposeHomographyMat <https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html#ga7f60bdff78833d1e3fd6d9d0fd538d92>`_
"""
retval, rotations, translations, normals = cv2.decomposeHomographyMat(
H=H, K=self.K
)
T = SE3.Empty()
for R, t in zip(rotations, translations):
# we normalize the rotation matrix, those returned by openCV can
# not quite proper SO(3) values
pose = SE3.Rt(smb.trnorm(R), t).inv()
T.append(pose)
return T, normals
# if K is None:
# K = np.identity(3)
# # also have K = self.K
# H = np.linalg.inv(K) @ H @ K
# # normalise so that the second singular value is one
# U, S, V = np.linalg.svd(H, compute_uv=True)
# H = H / S[1]
# # compute the SVD of the symmetric matrix H'*H = VSV'
# U, S, V = np.linalg.svd(np.transpose(H) @ H)
# # ensure V is right-handed
# if np.linalg.det(V) < 0:
# print('det(V) was < 0')
# V = -V
# # get squared singular values
# s0 = S[0]
# s2 = S[2]
# # v0 = V[0:, 0]
# # v1 = V[0:, 1]
# # v2 = V[0:, 2]
# # pure rotation - where all singular values == 1
# if np.abs(s0 - s2) < (100 * np.spacing(1)):
# print('Warning: Homography due to pure rotation')
# if np.linalg.det(H) < 0:
# H = -H
# # sol = namedtuple('T', T, ''
# # TODO finish from invhomog.m
# print('Unfinished')
# return False
# =================== fundamental matrix =============================== #
[docs]
def F(self, other: CentralCamera | SE3) -> np.ndarray:
"""
Fundamental matrix
:param other: second camera view
:type other: :class:`CentralCamera`, :class:`~spatialmath..pose3d.SE3`
:return: fundamental matrix
:rtype: numpy(3,3)
Compute the fundamental matrix relating two camera views. The
first view is defined by the instance. The second
is defined by:
* another :class:`CentralCamera` instance
* an SE3 pose describing the pose of the second view with respect to
the first, assuming the same camera intrinsic parameters.
Example:
.. runblock:: pycon
>>> from machinevisiontoolbox import CentralCamera
>>> from spatialmath import SE3
>>> camera1 = CentralCamera(name="camera 1", f=0.002, imagesize=1000, rho=10e-6, pose=SE3.Tx(-0.1)*SE3.Ry(0.4))
>>> camera2 = CentralCamera(name="camera 2", f=0.002, imagesize=1000, rho=10e-6, pose=SE3.Tx(0.1)*SE3.Ry(-0.4))
>>> F = camera1.F(camera2)
>>> F
>>> F = camera1.F(SE3.Tx(0.2))
>>> F
:references:
- Y.Ma, J.Kosecka, S.Soatto, S.Sastry, "An invitation to 3D",
Springer, 2003. p.177
- |RVC3|, Section 14.2.1.
:seealso: :meth:`points2F` :meth:`E`
"""
if isinstance(other, SE3):
E = self.E(other)
K = self.K
return np.linalg.inv(K).T @ E @ np.linalg.inv(K)
elif isinstance(other, CentralCamera):
# use relative pose and camera parameters of
E = self.E(other)
K1 = self.K
K2 = other.K
return np.linalg.inv(K2).T @ E @ np.linalg.inv(K1)
else:
raise ValueError("bad type")
[docs]
@staticmethod
def points2F(
p1: np.ndarray,
p2: np.ndarray,
method: str = "8p",
residual: bool = True,
seed: int | None = None,
**kwargs,
) -> list:
"""
Estimate fundamental matrix from corresponding points
:param p1: image plane points from first camera
:type p1: ndarray(2,N)
:param p2: image plane points from second camera
:type p2: ndarray(2,N)
:param method: algorithm '7p', '8p' [default], 'ransac', 'lmeds'
:type method: str, optional
:param kwargs: optional arguments as required for ransac', 'lmeds'
methods
:return: fundamental matrix and residual
:rtype: ndarray(3,3), float
Computes the fundamental matrix from two sets of corresponding
image-plane points. Corresponding points are given by corresponding
columns of ``p1`` and ``p2``.
Example:
.. runblock:: pycon
>>> from machinevisiontoolbox import CentralCamera, mkgrid
>>> from spatialmath import SE3
>>> camera1 = CentralCamera(name="camera 1", f=0.002, imagesize=1000, rho=10e-6, pose=SE3.Tx(-0.1)*SE3.Ry(0.4))
>>> camera2 = CentralCamera(name="camera 2", f=0.002, imagesize=1000, rho=10e-6, pose=SE3.Tx(0.1)*SE3.Ry(-0.4))
>>> T_grid = SE3.Tz(1) * SE3.Rx(0.1) * SE3.Ry(0.2)
>>> P = mkgrid(3, 1.0, pose=T_grid)
>>> p1 = camera1.project_point(P)
>>> p2 = camera2.project_point(P);
>>> F, resid = CentralCamera.points2F(p1, p2)
>>> F
>>> resid
:seealso: :meth:`F` :meth:`E`
`opencv.findFundamentalMat <https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html#ga59b0d57f46f8677fb5904294a23d404a>`_
"""
points2F_dict = {
"7p": cv2.FM_7POINT,
"8p": cv2.FM_8POINT,
"ransac": cv2.FM_RANSAC,
"lmeds": cv2.FM_LMEDS,
}
if seed is not None:
cv2.setRNGSeed(seed)
F, mask = cv2.findFundamentalMat(
points1=p1.T, points2=p2.T, method=points2F_dict[method], **kwargs
)
mask = mask.ravel().astype(bool)
# add various return values
retval = [F]
if residual:
e = smb.e2h(p2[:, mask]).T @ F @ smb.e2h(p1[:, mask])
resid = np.linalg.norm(np.diagonal(e))
retval.append(resid)
if method in ("ransac", "lmeds"):
retval.append(mask)
return retval
# elines = smb.e2h(p2).T @ F # homog lines, one per row
# p1h = smb.e2h(p1)
# residuals = []
# for i, line in enumerate(elines):
# d = line @ p1h[:, i] / np.sqrt(line[0] ** 2 + line[1] ** 2)
# residuals.append(d)
# resid = np.array(residuals).mean()
[docs]
@staticmethod
def epidist(F: np.ndarray, p1: np.ndarray, p2: np.ndarray) -> np.ndarray:
"""
Epipolar distance
:param F: fundamental matrix
:type F: ndarray(3,3)
:param p1: image plane point or points from first camera
:type p1: ndarray(2) or ndarray(2,N)
:param p2: image plane point or points from second camera
:type p2: ndarray(2) or ndarray(2,M)
:return: distance matrix
:rtype: ndarray(N,M)
Computes the distance of the points ``p2`` from the
epipolar lines induced by points ``p1``. Element [i,j] of the return
value is the distance of point j in camera 2 from the epipolar line
induced by point i in camera 1.
"""
if p1.ndim == 1:
p1 = np.c_[p1]
if p2.ndim == 1:
p2 = np.c_[p2]
D = np.empty((p1.shape[1], p2.shape[1]))
# compute epipolar lines corresponding to p1
l = F @ smb.e2h(p1)
for i in range(p1.shape[1]):
for j in range(p2.shape[1]):
D[i, j] = np.abs(
l[0, i] * p2[0, j] + l[1, i] * p2[1, j] + l[2, i]
) / np.sqrt(l[0, i] ** 2 + l[1, i] ** 2)
return D
# ===================== essential matrix =============================== #
[docs]
def E(self, other: CentralCamera | SE3 | np.ndarray) -> np.ndarray:
"""
Essential matrix from two camera views
:param other: second camera view, camera pose or fundamental matrix
:type other: :class:`CentralCamera`, :class:`~spatialmath..pose3d.SE3`, ndarray(3,3)
:return: essential matrix
:rtype: ndarray(3,3)
Compute the essential matrix relating two camera views. The first view
is defined by the instance, and second view is specified by:
* a camera instance represented by a :class:`CentralCamera`. Assumes the
cameras have the same intrinsics.
* a relative motion represented by a :class:`~spatialmath..pose3d.SE3`
* a fundamental matrix
:references:
- Y.Ma, J.Kosecka, S.Soatto, S.Sastry, "An invitation to 3D",
Springer, 2003. p.177
:seealso: :meth:`F` :meth:`decomposeE` :meth:`points2E`
"""
if isinstance(other, np.ndarray) and other.shape == (3, 3):
# essential matrix from F matrix and intrinsics
return self.K.T @ other @ self.K
elif isinstance(other, CentralCamera):
# camera relative pose
T21 = SE3(other.pose.inv() * self.pose) # type: ignore[arg-type]
elif isinstance(other, SE3):
# relative pose given explicitly
T21 = SE3(other.inv()) # type: ignore[arg-type]
else:
raise ValueError("bad type")
return smb.skew(T21.t) @ T21.R
[docs]
def points2E(
self,
p1: np.ndarray,
p2: np.ndarray,
method: str | None = None,
K: np.ndarray | None = None,
**kwargs,
) -> np.ndarray | tuple[np.ndarray, np.ndarray]:
"""
Essential matrix from points
:param P1: image plane points
:type P1: ndarray(2,N)
:param P2: image plane points
:type P2: ndarray(2,N)
:param method: method, can be 'ransac' or 'lmeds'
:type method: str
:param K: camera intrinsic matrix, defaults to that of camera object
:type K: ndarray(3,3), optional
:param kwargs: additional arguments required for 'ransac' or 'lmeds'
options
:return: essential matrix and optional inlier vevtor
:rtype: ndarray(3,3), ndarray(N, bool)
Compute the essential matrix from two sets of corresponding points.
Each set of points is represented by the columns of the array ``p1``
or ``p2``.
Example:
.. runblock:: pycon
>>> from machinevisiontoolbox import CentralCamera, mkgrid
>>> from spatialmath import SE3
>>> camera1 = CentralCamera(name="camera 1", f=0.002, imagesize=1000, rho=10e-6, pose=SE3.Tx(-0.1)*SE3.Ry(0.4))
>>> camera2 = CentralCamera(name="camera 2", f=0.002, imagesize=1000, rho=10e-6, pose=SE3.Tx(0.1)*SE3.Ry(-0.4))
>>> T_grid = SE3.Tz(1) * SE3.Rx(0.1) * SE3.Ry(0.2)
>>> P = mkgrid(3, 1.0, pose=T_grid)
>>> p1 = camera1.project_point(P)
>>> p2 = camera2.project_point(P);
>>> E, inliers = camera1.points2E(p1, p2)
>>> E
>>> inliers
.. note:: If the method is 'ransac' or 'lmeds' then a boolean array
of inliers is also returned, True means the corresponding input
point pair is an inlier.
:seealso: :meth:`E` :meth:`decomposeE`
`opencv.findEssentialMat <https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html#gad245d60e64d0c1270dbfd0520847bb87>`_
"""
if K is None:
K = self.K
_points2E_dict = {"ransac": cv2.RANSAC, "lmeds": cv2.LMEDS}
if method is not None:
method_int = _points2E_dict[method]
else:
method_int = None
E, mask = cv2.findEssentialMat(
points1=p1.T, points2=p2.T, cameraMatrix=K, method=method_int, **kwargs # type: ignore[arg-type]
)
if mask is not None:
mask = mask.flatten().astype(bool)
return E, mask
else:
return E
[docs]
def decomposeE(
self, E: np.ndarray, P: np.ndarray | FeatureMatch | None = None
) -> SE3:
"""
Decompose essential matrix
:param E: essential matrix
:type E: ndarray(3,3)
:param P: world point or feature match object to resolve ambiguity
:type P: array_like(3), :class:`~machinevisiontoolbox.ImagePointFeatures.FeatureMatch`
:return: camera relative pose
:rtype: :class:`~spatialmath..pose3d.SE3`
Determines relative pose from essential matrix. This operation has
multiple solutions which is resolved by passing in:
- a single 3D world point in front of the camera
- a :class:`~machinevisiontoolbox.ImagePointFeatures.FeatureMatch` object
:references:
- |RVC3|, Section 14.2.2.
:seealso: :meth:`E` :meth:`points2E`
:class:`~machinevisiontoolbox.ImagePointFeatures.FeatureMatch`
`opencv.decomposeEssentialMat <https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html#ga54a2f5b3f8aeaf6c76d4a31dece85d5d>`_
`opencv.recoverPose <https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html#gadb7d2dfcc184c1d2f496d8639f4371c0>`_
"""
if isinstance(P, FeatureMatch):
# passed a Match object
match = P
retval, R, t, mask = cv2.recoverPose(
E=E,
points1=match.p1.T,
points2=match.p2.T,
cameraMatrix=self.C()[:3, :3],
)
# not explicitly stated, but seems that this returns (R, t) from
# camera to world
return SE3.Rt(R, t).inv()
else:
R1, R2, t = cv2.decomposeEssentialMat(E=E)
# not explicitly stated, but seems that this returns (R, t) from
# camera to world
possibles = [(R1, t), (R1, -t), (R2, t), (R2, -t)]
if smb.isvector(P, 3):
for Rt in possibles:
pose = SE3.Rt(Rt[0], Rt[1]).inv()
p = self.project_point(P, pose=pose, behind=True)
# check if point is projected behind the camera, indicated
# by nan values
if not np.isnan(p[0]):
# return the first good one
return pose
else:
T = SE3.Empty()
for Rt in possibles:
pose = SE3.Rt(Rt[0], Rt[1]).inv()
T.append(pose)
return T
# ===================== image plane motion ============================= #
[docs]
def visjac_p(
self, uv: np.ndarray | ArrayLike, depth: float | np.ndarray
) -> np.ndarray:
r"""
Visual Jacobian for point features
:param p: image plane point or points
:type p: array_like(2), ndarray(2,N)
:param depth: point depth
:type depth: float, array_like(N)
:return: visual Jacobian matrix
:rtype: ndarray(2,6), ndarray(2N,6)
Compute the image Jacobian :math:`\mat{J}` which maps
.. math::
\dvec{p} = \mat{J}(\vec{p}, z) \vec{\nu}
camera spatial velocity :math:`\vec{\nu}` to the image plane velocity
:math:`\dvec{p}` of the point.
If ``p`` describes multiple points then return a stack of these
:math:`2\times 6` matrices, one per point.
Depth is the z-component of the point's coordinate in the camera frame.
If ``depth`` is a scalar then it is the depth for all points.
Example:
.. runblock:: pycon
>>> from machinevisiontoolbox import CentralCamera
>>> from spatialmath import SE3
>>> camera = CentralCamera.Default()
>>> camera.visjac_p((200, 300), 2)
:references:
- A tutorial on Visual Servo Control, Hutchinson, Hager & Corke,
IEEE Trans. R&A, Vol 12(5), Oct, 1996, pp 651-670.
- |RVC3|, Section 15.2.1.
:seealso: :meth:`flowfield` :meth:`visjac_p_polar` :meth:`visjac_l` :meth:`visjac_e`
"""
uv = smb.getmatrix(uv, (2, None))
Z = depth
Z = smb.getvector(Z)
if len(Z) == 1:
Z = np.repeat(Z, uv.shape[1])
elif len(Z) != uv.shape[1]:
raise ValueError("Z must be a scalar or have same number of columns as uv")
L = np.empty((0, 6)) # empty matrix
K = self.K
Kinv = np.linalg.inv(K)
for z, p in zip(Z, uv.T): # iterate over each column (point)
# convert to normalized image-plane coordinates
xy = Kinv @ smb.e2h(p)
x = xy[0, 0]
y = xy[1, 0]
# 2x6 Jacobian for this point
# fmt: off
Lp = K[:2,:2] @ np.array(
[ [-1/z, 0, x/z, x * y, -(1 + x**2), y],
[ 0, -1/z, y/z, (1 + y**2), -x*y, -x] ])
# fmt: on
# stack them vertically
L = np.vstack([L, Lp])
return L
[docs]
def visjac_p_polar(
self, p: np.ndarray | ArrayLike, Z: float | np.ndarray
) -> np.ndarray:
r"""
Visual Jacobian for point features in polar coordinates
:param p: image plane point or points
:type p: array_like(2), ndarray(2,N)
:param depth: point depth
:type depth: float, array_like(N)
:return: visual Jacobian matrix in polar coordinates
:rtype: ndarray(2,6), ndarray(2N,6)
Compute the image Jacobian :math:`\mat{J}` which maps
.. math::
\begin{pmatrix} \dot{\phi} \\ \dot{r} \end{pmatrix} = \mat{J}(\vec{p}, z) \vec{\nu}
camera spatial velocity :math:`\vec{\nu}` to the image plane velocity
of the point expressed in polar coordinate form :math:`(\phi, r)`.
If ``p`` describes multiple points then return a stack of these
:math:`2\times 6` matrices, one per point.
Depth is the z-component of the point's coordinate in the camera frame.
If ``depth`` is a scalar then it is the depth for all points.
Example:
.. runblock:: pycon
>>> from machinevisiontoolbox import CentralCamera
>>> from spatialmath import SE3
>>> camera = CentralCamera.Default()
>>> camera.visjac_p_polar((200, 300), 2)
:references:
- Combining Cartesian and polar coordinates in IBVS.
Corke PI, Spindler F, Chaumette F
IROS 2009, pp 5962–5967
- |RVC3|, Section 16.2.
:seealso: :meth:`visjac_p` :meth:`visjac_l` :meth:`visjac_e`
"""
J = []
p = smb.getmatrix(p, (2, None))
f = self.f[0]
if smb.isscalar(Z):
Z = [Z] * p.shape[1]
for (phi, r), Zk in zip(p.T, Z):
# k = (f**2 + r**2) / f
# k2 = f / (r * Zk)
c = np.cos(phi)
s = np.sin(phi)
r = max(r, 0.05)
Jk = np.array(
[
[-s / r / Zk, c / r / Zk, 0, -c / r, -s / r, 1],
[c / Zk, s / Zk, -r / Zk, -(1 + r**2) * s, (1 + r**2) * c, 0],
]
)
# Jk = np.array([
# [ k2 * sth, -k2 * cth, 0, f / r * cth, f / r * sth , -1],
# [-f / Zk * cth, -f / Zk * sth, r / Zk, k * sth, -k * cth, 0],
# ])
# Jk = np.array([
# [cth/Zk , sth / Zk, -r / Zk, -(1+r**2) * sth, -k * cth, 0],
# [ k2 * sth, -k2 * cth, 0, f / r * cth, f / r * sth , -1]])
J.append(Jk)
return np.vstack(J)
# if 0
# r = rt(1); theta = rt(2);
# % compute the mapping from uv-dot to r-theta dot
# M = 1/r * [r*cos(theta) r*sin(theta); -sin(theta) cos(theta)];
# % convert r-theta form to uv form
# u = r * cos(theta); v = r * sin(theta);
# % compute the Jacobian
# J = M * cam.visjac_p([u; v], Z);
[docs]
def visjac_l(self, lines: np.ndarray | ArrayLike, plane: ArrayLike) -> np.ndarray:
r"""
Visual Jacobian for line features
:param lines: image plane line parameters
:type p: array_like(2), ndarray(2,N)
:param plane: plane containing the line
:type plane: array_like(4)
:return: visual Jacobian matrix for line feature
:rtype: ndarray(2,6), ndarray(2N,6)
Compute the Jacobian which gives the rates of change of the line
parameters in terms of camera spatial velocity.
For image planes lines
.. math:: u \cos \theta + v \sin \theta = \rho
the image Jacobian :math:`\mat{J}` maps
.. math::
\begin{pmatrix} \dot{\theta} \\ \dot{\rho} \end{pmatrix} = \mat{J}(\vec{p}, z) \vec{\nu}
camera spatial velocity :math:`\vec{\nu}` to the image plane velocity
of the line parameters :math:`(\theta, \rho)`.
The world plane containing the line is also required, and is provided
as a vector :math:`(a,b,c,d)` such that
.. math:: aX + bY + cZ + d = 0
If ``lines`` describes multiple points then return a stack of these
:math:`2\times 6` matrices, one per point.
Depth is the z-component of the point's coordinate in the camera frame.
If ``depth`` is a scalar then it is the depth for all points.
Example:
.. runblock:: pycon
>>> from machinevisiontoolbox import CentralCamera
>>> from spatialmath import SE3
>>> camera = CentralCamera.Default()
>>> camera.visjac_l((0.2, 500), (0, 0, 1, -3))
:references:
- A New Approach to Visual Servoing in Robotics,
B. Espiau, F. Chaumette, and P. Rives,
IEEE Transactions on Robotics and Automation,
vol. 8, pp. 313-326, June 1992.
- Visual servo control 2: Advanced approaches
Chaumette F, Hutchinson S,
IEEE Robot Autom Mag 14(1):109–118 (2007)
- |RVC3|, Section 15.3.1.
:seealso: :meth:`visjac_p` :meth:`visjac_p_polar` :meth:`visjac_e`
"""
a, b, c, d = plane
lines = smb.getmatrix(lines, (2, None))
jac = []
for theta, rho in lines.T:
sth = np.sin(theta)
cth = np.cos(theta)
lam_th = (a * sth - b * cth) / d
lam_rho = (a * rho * cth + b * rho * sth + c) / d
L = np.array(
[
[
lam_th * cth,
lam_th * sth,
-lam_th * rho,
-rho * cth,
-rho * sth,
-1,
],
[
lam_rho * cth,
lam_rho * sth,
-lam_rho * rho,
(1 + rho**2) * sth,
-(1 + rho**2) * cth,
0,
],
]
)
jac.append(L)
return np.vstack(jac)
[docs]
def visjac_e(self, E: ArrayLike, plane: ArrayLike) -> np.ndarray:
r"""
Visual Jacobian for ellipse features
:param E: image plane ellipse parameters
:type E: array_like(5), ndarray(5,N)
:param plane: plane containing the ellipse
:type plane: array_like(4)
:return: visual Jacobian matrix for ellipse feature
:rtype: ndarray(2,6), ndarray(2N,6)
Compute the Jacobian gives the rates of change of the ellipse parameters
in terms of camera spatial velocity.
For image plane ellipses
.. math:: u^2 + E_0 v^2 -2 E_1 u v + 2 E_2 u + 2 E_3 v + E_4 = 0
the image Jacobian :math:`\mat{J}` maps
.. math::
\begin{pmatrix} \dot{E_0} \\ \vdots \\ \dot{E_4} \end{pmatrix} = \mat{J}(\vec{p}, z) \vec{\nu}
camera spatial velocity :math:`\vec{\nu}` to the velocity
of the ellipse parameters :math:`(E_0 \ldots E_4)`.
The world plane containing the ellipse is also required, and is provided
as a vector :math:`(a,b,c,d)` such that
.. math:: aX + bY + cZ + d = 0
Example:
.. runblock:: pycon
>>> from machinevisiontoolbox import CentralCamera
>>> from spatialmath import SE3
>>> camera = CentralCamera.Default()
>>> camera.visjac_e(((0.5, 0, -1000, -500, 374900)), (0, 0, 1, -1)
:references:
- A New Approach to Visual Servoing in Robotics,
B. Espiau, F. Chaumette, and P. Rives,
IEEE Transactions on Robotics and Automation,
vol. 8, pp. 313-326, June 1992.
- Visual servo control 2: Advanced approaches
Chaumette F, Hutchinson S,
IEEE Robot Autom Mag 14(1):109–118 (2007)
- |RVC3|, Section 15.3.2.
:seealso: :meth:`visjac_p` :meth:`visjac_p_polar` :meth:`visjac_l`
"""
a = -plane[0] / plane[3]
b = -plane[1] / plane[3]
c = -plane[2] / plane[3]
L = np.array(
[
[
2 * b * E[1] - 2 * a * E[0],
2 * E[0] * (b - a * E[1]),
2 * b * E[3] - 2 * a * E[0] * E[2],
2 * E[3],
2 * E[0] * E[2],
-2 * E[1] * (E[0] + 1),
],
[
b - a * E[1],
b * E[1] - a * (2 * E[1] ** 2 - E[0]),
a * (E[3] - 2 * E[1] * E[2]) + b * E[2],
-E[2],
-(2 * E[1] * E[2] - E[3]),
E[0] - 2 * E[1] ** 2 - 1,
],
[
c - a * E[2],
a * (E[3] - 2 * E[1] * E[2]) + c * E[1],
c * E[2] - a * (2 * E[2] ** 2 - E[4]),
-E[1],
1 + 2 * E[2] ** 2 - E[4],
E[3] - 2 * E[1] * E[2],
],
[
E[2] * b + E[1] * c - 2 * a * E[3],
E[3] * b + E[0] * c - 2 * a * E[1] * E[3],
b * E[4] + c * E[3] - 2 * a * E[2] * E[3],
E[4] - E[0],
2 * E[2] * E[3] + E[1],
-2 * E[1] * E[3] - E[2],
],
[
2 * c * E[2] - 2 * a * E[4],
2 * c * E[3] - 2 * a * E[1] * E[4],
2 * c * E[4] - 2 * a * E[2] * E[4],
-2 * E[3],
2 * E[2] * E[4] + 2 * E[2],
-2 * E[1] * E[4],
],
]
)
L = L @ np.diag([0.5, 0.5, 0.5, 1, 1, 1]) # not sure why...
return L
[docs]
def flowfield(self, vel: ArrayLike, Z: float = 2) -> None:
"""
Display optical flow field
:param vel: camera spatial velocity
:type vel: array_like(6)
:param Z: _description_, defaults to 2
:type Z: scalar, optional
Display the optical flow field using Matplotlib, for a grid of points at
distance ``Z`` for a camera velocity of ``vel``.
Example:
.. runblock:: pycon
>>> from machinevisiontoolbox import CentralCamera
>>> camera = CentralCamera.Default()
>>> camera.flowfield([0, 0, 0, 0, 1, 0])
:seealso: :meth:`visjac_p`
"""
vel = smb.getvector(vel, 6)
u = np.arange(0, self.nu, 50)
v = np.arange(0, self.nv, 50)
[U, V] = np.meshgrid(u, v, indexing="ij")
du = np.empty(shape=U.shape)
dv = np.empty(shape=U.shape)
for r in range(U.shape[0]):
for c in range(U.shape[1]):
J = self.visjac_p((U[r, c], V[r, c]), Z)
ud, vd = J @ vel
du[r, c] = ud
dv[r, c] = -vd
self.clf()
ax = self._init_imageplane()
ax.quiver(U, V, du, dv, 0.4, zorder=20)
[docs]
def derivatives(
self, x: ArrayLike, P: ArrayLike
) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
r"""
Compute projection and derivatives for bundle adjustment
:param x: camera pose as translation and quaternion vector part
:type x: array_like(6)
:param P: 3D world point
:type P: array_like(3)
:return: p, A, B
:rtype: ndarray(2), ndarray(2,6), ndarray(2,3)
For a world point :math:`\vec{x}` compute the image plane projection and the
sensitivity to camera and point change
.. math:: \mat{A} = \frac{\partial \vec{f}(\vec{x})}{\partial \pose}, \mat{B} = \frac{\partial \vec{f}(\vec{x})}{\partial \vec{P}}
where :math:`\vec{f}(\vec{x})` is the perspective projection function.
:seealso: :meth:`project_point`
"""
# compute Jacobians and projection
from machinevisiontoolbox.camera_derivatives import cameraModel
Kp = [self.f[0], self.rhou, self.rhov, self.u0, self.v0]
return cameraModel(*x, *P, *Kp)
[docs]
def estpose(
self,
P: np.ndarray,
p: np.ndarray,
method: str = "iterative",
frame: str = "world",
) -> SE3 | None:
"""
Estimate object pose
:param P: A set of 3D points defining the object with respect to its own frame
:type P: ndarray(3, N)
:param p: Image plane projection of the object points
:type p: ndarray(2, N)
:param method: pose estimation algorithm, see OpenCV solvePnP, defaults to 'iterative'
:type method: str, optional
:param frame: estimate pose with respect to frame "world" [default] or "camera"
:type frame: str, optional
:return: pose of target frame relative to the world frame
:rtype: :class:`~spatialmath..pose3d.SE3`
Using a set of points defining some object with respect to its own frame {B}, and
a set of image-plane projections, estimate the pose of {B} with respect to the world
or camera frame.
To estimate the camera's pose with respect to the world frame the camera's pose
``self.pose`` is used.
.. note::
* All of the OpenCV estimation algorithms are supported.
* Algorithm ``"ippe-square"`` requires exactly four points at the corners of a
square and in the order: (-x, y), (x, y), (x, -y), (-x, -y).
:seealso: :meth:`project_point`
`opencv.solvePnP <https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html#ga549c2075fac14829ff4a58bc931c033d>`_
"""
method_dict = {
"iterative": cv2.SOLVEPNP_ITERATIVE,
"epnp": cv2.SOLVEPNP_EPNP,
"p3p": cv2.SOLVEPNP_P3P,
"ap3p": cv2.SOLVEPNP_AP3P,
"ippe": cv2.SOLVEPNP_IPPE,
"ippe-square": cv2.SOLVEPNP_IPPE_SQUARE,
}
# as per the Note on solvePnP page
# we need to ensure that the image point data is contiguous nx1x2 array
n = p.shape[1]
p = np.ascontiguousarray(p[:2, :].T).reshape((n, 1, 2))
# do the pose estimation
sol = cv2.solvePnP(
objectPoints=P.T,
imagePoints=p,
cameraMatrix=self.K,
distCoeffs=self._distortion,
flags=method_dict[method],
)
if sol[0]:
# pose of target with respect to camera
pose_C_T = SE3(sol[2]) * SE3.EulerVec(sol[1])
# pose with respect to world frame
if frame == "camera":
return pose_C_T
elif frame == "world":
return self.pose * pose_C_T
else:
raise ValueError(f"bad frame value {frame}")
else:
return None
# ------------------------------------------------------------------------ #
[docs]
class FishEyeCamera(CameraBase):
r"""
Create fisheye camera projection model
.. inheritance-diagram:: machinevisiontoolbox.Camera.FishEyeCamera
:top-classes: machinevisiontoolbox.Camera.Camera
:parts: 1
:param k: scale factor
:type k: float, optional
:param projection: projection model: ``'equiangular'`` [default], ``'sine'``, ``'equisolid'`` or ``'stereographic'``
:type projection: str, optional
:param kwargs: arguments passed to :class:`CameraBase` constructor
A fisheye camera contains a wide angle lens, and the angle of the
incoming ray is mapped to a radius with respect to the principal point.
The mapping from elevation angle :math:`\theta` to image plane radius is
given by:
============= =======================================
Projection :math:`r(\theta)`
============= =======================================
equiangular :math:`r = k \theta`
sine :math:`r = k \sin \theta`
equisolid :math:`r = k \sin \frac{\theta}{2}`
stereographic :math:`r = k \tan \frac{\theta}{2}`
============= =======================================
.. note::
- If ``K`` is not specified it is computed such that the circular
imaging region maximally fills the square image plane.
- This camera model assumes central projection, that is, the focal point
is at z=0 and the image plane is at z=f. The image is not inverted.
:references:
- |RVC3|, Section 13.3.1.
:seealso: :class:`CameraBase` :class:`CentralCamera` :class:`CatadioptricCamera`
:class:`SphericalCamera`
"""
k: float
rfunc: Callable
projection: str
def __init__(
self, k: float | None = None, projection: str = "equiangular", **kwargs
) -> None:
super().__init__(camtype="fisheye", **kwargs)
self.projection = projection
if k is None:
assert self._imagesize is not None and self._pp is not None
r = np.min((self._imagesize - self._pp) * self.rho)
if self.projection == "equiangular":
if k is None:
k = r / (pi / 2)
rfunc = lambda theta: k * theta
elif self.projection == "sine":
if k is None:
k = r
rfunc = lambda theta: k * np.sin(theta)
elif self.projection == "equisolid":
if k is None:
k = r / sin(pi / 4)
rfunc = lambda theta: k * np.sin(theta / 2)
elif self.projection == "stereographic":
if k is None:
k = r / tan(pi / 4)
rfunc = lambda theta: k * np.tan(theta / 2)
else:
raise ValueError("unknown projection model")
self.k = k
self.rfunc = rfunc
def __str__(self) -> str:
s = super().__str__()
s += self.fmt.format("model", self.projection, fmt="{}")
s += self.fmt.format("k", self.k, fmt="{:.4g}")
return s
[docs]
def project_point(
self,
P: np.ndarray | ArrayLike,
pose: SE3 | None = None,
objpose: SE3 | None = None,
) -> np.ndarray:
r"""
Project 3D points to image plane
:param P: 3D world point or points
:type P: array_like(3), array_like(3,n)
:param pose: camera pose with respect to the world frame, defaults to
camera's ``pose`` attribute
:type pose: :class:`~spatialmath.pose3d.SE3`, optional
:param objpose: 3D point reference frame, defaults to world frame
:type objpose: :class:`~spatialmath..pose3d.SE3`, optional
:param visibility: test if points are visible, default False
:type visibility: bool
:raises ValueError: [description]
:return: image plane points
:rtype: ndarray(2,n)
Project world points to the fisheye camera image plane.
The elevation angle range is from :math:`-pi/2` (below the mirror) to
maxangle above the horizontal plane. The mapping from elevation angle
:math:`\theta` to image plane radius is given by:
============= =======================================
Projection :math:`r(\theta)`
============= =======================================
equiangular :math:`r = k \theta`
sine :math:`r = k \sin \theta`
equisolid :math:`r = k \sin \frac{\theta}{2}`
stereographic :math:`r = k \tan \frac{\theta}{2}`
============= =======================================
World points are given as a 1D array or the columns of a 2D array of
Euclidean coordinates. The computed image plane coordinates are
Euclidean and given as a 1D array or the corresponding columns of a 2D
array.
If ``pose`` is specified it is used for the camera pose instead of the
attribute ``pose``. The object's attribute is not updated.
The points ``P`` are by default with respect to the world frame, but
they can be transformed by specifying ``objpose``.
:seealso: :meth:`plot_point`
"""
P = smb.getmatrix(P, (3, None))
if pose is not None:
T = self.pose.inv()
else:
T = SE3()
if objpose is not None:
T *= objpose
R = np.sqrt(np.sum(P**2, axis=0))
phi = np.arctan2(-P[1, :], P[0, :])
theta = np.arccos(P[2, :] / R)
r = self.rfunc(theta)
x = r * np.cos(phi)
y = r * np.sin(phi)
uv = np.array([x / self.rhou + self.u0, y / self.rhov + self.v0])
return self._add_noise_distortion(uv)
# ------------------------------------------------------------------------ #
[docs]
class CatadioptricCamera(CameraBase):
r"""
Create catadioptric camera projection model
.. inheritance-diagram:: machinevisiontoolbox.Camera.CatadioptricCamera
:top-classes: machinevisiontoolbox.Camera.Camera
:parts: 1
:param k: scale factor
:type k: float, optional
:param projection: projection model: ``'equiangular'`` [default], ``'sine'``, ``'equisolid'`` or ``'stereographic'``
:type projection: str, optional
:param kwargs: arguments passed to :class:`CameraBase` constructor
A catadioptric camera comprises a perspective camera pointed at a
convex mirror, typically paraboloidal or conical.
The elevation angle range is from :math:`-\pi/2` (below the mirror) to
maxangle above the horizontal plane. The mapping from elevation angle
:math:`\theta` to image plane radius is given by:
============= =======================================
Projection :math:`r(\theta)`
============= =======================================
equiangular :math:`r = k \theta`
sine :math:`r = k \sin \theta`
equisolid :math:`r = k \sin \frac{\theta}{2}`
stereographic :math:`r = k \tan \frac{\theta}{2}`
============= =======================================
.. note::
- If ``K`` is not specified it is computed such that the circular
imaging region maximally fills the image plane.
- This camera model assumes central projection, that is, the focal point
is at :math:`z=0` and the image plane is at :math:`z=f`. The image is not inverted.
:references:
- |RVC3|, Section 13.3.2.
:seealso: :class:`CameraBase` :class:`CentralCamera` :class:`FishEyeCamera`
:class:`SphericalCamera`
"""
k: float
rfunc: Callable
projection: str
def __init__(
self,
k: float | None = None,
projection: str = "equiangular",
maxangle: float | None = None,
**kwargs,
) -> None:
super().__init__(camtype="catadioptric", **kwargs)
self.projection = projection
if k is None:
assert self._imagesize is not None and self._pp is not None
r = np.min((self._imagesize - self._pp) * self.rho)
# compute k if not specified, so that hemisphere fits into
# image plane, requires maxangle being set
if self.projection == "equiangular":
if k is None:
if maxangle is not None:
k = r / (pi / 2 + maxangle)
self.maxangle = maxangle
else:
raise ValueError("must specify either k or maxangle")
rfunc = lambda theta: k * theta
elif self.projection == "sine":
if k is None:
k = r
rfunc = lambda theta: k * np.sin(theta)
elif self.projection == "equisolid":
if k is None:
k = r / sin(pi / 4)
rfunc = lambda theta: k * np.sin(theta / 2)
elif self.projection == "stereographic":
if k is None:
k = r / tan(pi / 4)
rfunc = lambda theta: k * np.tan(theta / 2)
else:
raise ValueError("unknown projection model")
self.k = k
self.rfunc = rfunc
def __str__(self) -> str:
s = super().__str__()
s += self.fmt.format("model", self.projection, fmt="{}")
s += self.fmt.format("k", self.k, fmt="{:.4g}")
return s
[docs]
def project_point(
self,
P: np.ndarray | ArrayLike,
pose: SE3 | None = None,
objpose: SE3 | None = None,
) -> np.ndarray:
"""
Project 3D points to image plane
:param P: 3D world point or points
:type P: array_like(3), array_like(3,n)
:param pose: camera pose with respect to the world frame, defaults to
camera's ``pose`` attribute
:type pose: :class:`~spatialmath..pose3d.SE3`, optional
:param objpose: 3D point reference frame, defaults to world frame
:type objpose: :class:`~spatialmath..pose3d.SE3`, optional
:param visibility: test if points are visible, default False
:type visibility: bool
:raises ValueError: [description]
:return: image plane points
:rtype: ndarray(2,n)
Project world points to the catadioptric camera image plane.
World points are given as a 1D array or the columns of a 2D array of
Euclidean coordinates. The computed image plane coordinates are
Euclidean and given as a 1D array or the corresponding columns of a 2D
array.
If ``pose`` is specified it is used for the camera pose instead of the
attribute ``pose``. The object's attribute is not updated.
The points ``P`` are by default with respect to the world frame, but
they can be transformed by specifying ``objpose``.
:seealso: :meth:`plot_point`
"""
P = smb.getmatrix(P, (3, None))
if pose is not None:
T = self.pose.inv()
else:
T = SE3()
if objpose is not None:
T *= objpose
P = T * P # transform points to camera frame
P_arr: np.ndarray = np.asarray(P)
# project to the image plane
phi = np.arctan2(P_arr[1, :], P_arr[0, :])
Rxy = np.sqrt(np.sum(P_arr[:2, :] ** 2, axis=0)) # sqrt(X^2 + Y^2)
theta = np.arctan(P_arr[2, :] / Rxy)
r = self.rfunc(theta) # depends on projection model
x = r * np.cos(phi)
y = r * np.sin(phi)
uv = np.array([x / self.rhou + self.u0, y / self.rhov + self.v0])
return self._add_noise_distortion(uv)
# ------------------------------------------------------------------------ #
[docs]
class SphericalCamera(CameraBase):
"""
Create spherical camera projection model
.. inheritance-diagram:: machinevisiontoolbox.Camera.SphericalCamera
:top-classes: machinevisiontoolbox.Camera.Camera
:parts: 1
:param kwargs: arguments passed to :class:`CameraBase` constructor
The spherical camera is an idealization with a complete field of view
that can be used to generalize all camera projection models.
:references:
- |RVC3|, Section 13.3.3.
:seealso: :class:`CameraBase` :class:`CentralCamera` :class:`CatadioptricCamera`
:class:`FishEyeCamera`
"""
def __init__(self, **kwargs: Any) -> None:
# invoke the superclass constructor
super().__init__(
camtype="spherical",
limits=[-pi, pi, 0, pi],
labels=["Longitude φ (rad)", "Colatitude θ (rad)"],
**kwargs,
)
# return field-of-view angle for x and y direction (rad)
[docs]
def fov(self) -> list[float]:
"""
Camera field-of-view angles
:return: field of view angles in radians
:rtype: ndarray(2)
Computes the field of view angles (2x1) in radians for the camera
horizontal and vertical directions.
"""
return [2 * pi, 2 * pi]
[docs]
def project_point(
self,
P: np.ndarray | ArrayLike,
pose: SE3 | None = None,
objpose: SE3 | None = None,
) -> np.ndarray:
r"""
Project 3D points to image plane
:param P: 3D world point or points
:type P: array_like(3), array_like(3,n)
:param pose: camera pose with respect to the world frame, defaults to
camera's ``pose`` attribute
:type pose: :class:`~spatialmath..pose3d.SE3`, optional
:param objpose: 3D point reference frame, defaults to world frame
:type objpose: :class:`~spatialmath..pose3d.SE3`, optional
:return: image plane points
:rtype: ndarray(2,n)
Project world points to the spherical camera image plane.
World points are given as a 1D array or the columns of a 2D array of
Euclidean coordinates. The computed image plane coordinates are
in polar form :math:`(\phi, \theta)` (longitude, colatitude),
and given as a 1D array or the corresponding columns of a 2D
array.
If ``pose`` is specified it is used for the camera pose instead of the
attribute ``pose``. The object's attribute is not updated.
The points ``P`` are by default with respect to the world frame, but
they can be transformed by specifying ``objpose``.
:seealso: :meth:`plot_point`
"""
P = smb.getmatrix(P, (3, None))
if pose is None:
pose = self.pose
pose = pose.inv()
if objpose is not None:
pose *= objpose
P = pose * P # transform points to camera frame
P_arr: np.ndarray = np.asarray(P)
R = np.linalg.norm(P_arr, axis=0)
x = P_arr[0, :] / R
y = P_arr[1, :] / R
z = P_arr[2, :] / R
phi = np.arctan2(y, x)
r = np.sqrt(x**2 + y**2)
theta = np.arcsin(r)
return np.array([phi, theta])
[docs]
def visjac_p(
self, p: np.ndarray | ArrayLike, depth: float | np.ndarray | None = None
) -> np.ndarray:
r"""
Visual Jacobian for point features
:param p: image plane points
:type p: array_like(2) or ndarray(2,N)
:param depth: point depth, defaults to ``None``
:type depth: float or array_like(N), optional
:return: visual Jacobian
:rtype: ndarray(2,6) or ndarray(2N,6)
Compute the image Jacobian :math:`\mat{J}` which maps
.. math::
\dvec{p} = \mat{J}(\vec{p}, z) \vec{\nu}
camera spatial velocity :math:`\vec{\nu}` to the image plane velocity
:math:`\dvec{p}` of the point where :math:`\vec{p}=(\phi, \theta)`
If ``p`` describes multiple points then return a stack of these
:math:`2\times 6` matrices, one per point.
Depth is the z-component of the point's coordinate in the camera frame.
If ``depth`` is a scalar then it is the depth for all points.
"""
J = []
if smb.isscalar(depth):
depth = [depth] * p.shape[1]
assert depth is not None
for (phi, theta), R in zip(p.T, depth):
sp = np.sin(phi)
cp = np.cos(phi)
st = np.sin(theta)
ct = np.cos(theta)
Jk = np.array(
[
[sp / R / st, -cp / R / st, 0, cp * ct / st, sp * ct / st, -1],
[-cp * ct / R, -sp * ct / R, st / R, sp, -cp, 0],
]
)
J.append(Jk)
return np.vstack(J)
[docs]
def plot(self, frame: bool = False, **kwargs: Any) -> None:
smb.plot_sphere(
radius=1,
filled=True,
color="lightyellow",
alpha=0.3,
resolution=20,
centre=self.pose.t,
)
self.pose.plot(style="arrow", axislabel=True, length=1.4)
# class CentralCamera_polar(Camera):
# """
# Central projection camera class
# """
# def __init__(self,
# f=1,
# distortion=None,
# **kwargs):
# """
# Create central camera projection model in polar coordinates
# :param f: focal length, defaults to 8*1e-3
# :type f: float, optional
# :param distortion: camera distortion parameters, defaults to None
# :type distortion: array_like(5), optional
# :seealso: :meth:`distort`
# """
# super().__init__(type='perspective', **kwargs)
# # TODO some of this logic to f and pp setters
# self.f = f
# self._distortion = distortion
# @classmethod
# def Default(cls, **kwargs):
# default = {
# 'f': 0.008,
# 'rho': 10e-6,
# 'imagesize': 1000,
# 'pp': (500,500),
# 'name': 'default perspective camera'
# }
# return CentralCamera_polar(**{**default, **kwargs})
# def __str__(self):
# s = super().__str__()
# s += self.fmt.format('principal pt', self.pp)
# s += self.fmt.format('focal length', self.f)
# return s
# def project_point(self, P, pose=None, objpose=None, **kwargs):
# r"""
# Project 3D points to image plane
# :param P: 3D points to project into camera image plane
# :type P: array_like(3), array_like(3,n)
# :param pose: camera pose with respect to the world frame, defaults to
# camera's ``pose`` attribute
# :type pose: SE3, optional
# :param objpose: 3D point reference frame, defaults to world frame
# :type objpose: SE3, optional
# :param visibility: test if points are visible, default False
# :type visibility: bool
# :param retinal: transform to retinal coordinates, default False
# :type retinal: bool, optional
# :return: image plane points
# :rtype: ndarray(2,n)
# Project a 3D point to the image plane
# .. math::
# \hvec{p} = \mat{C} \hvec{P}
# where :math:`\mat{C}` is the camera calibration matrix and :math:`\hvec{p}` and :math:`\hvec{P}`
# are the image plane and world frame coordinates respectively.
# Example:
# .. runblock:: pycon
# >>> from machinevisiontoolbox import CentralCamera
# >>> camera = CentralCamera()
# >>> camera.project_point((0.3, 0.4, 2))
# If ``pose`` is specified it is used for the camera frame pose, otherwise
# the attribute ``pose``. The object's ``pose`` attribute is not updated
# if ``pose`` is specified.
# A single point can be specified as a 3-vector, multiple points as an
# array with three rows and one column (x, y, z) per point.
# The points ``P`` are by default with respect to the world frame, but
# they can be transformed by specifying ``objpose``.
# If world points are behind the camera, the image plane points are set to
# NaN.
# if ``visibility`` is True then each projected point is checked to ensure
# it lies in the bounds of the image plane. In this case there are two
# return values: the image plane coordinates and an array of booleans
# indicating if the corresponding point is visible.
# """
# if pose is None:
# pose = self.pose
# C = self.C(pose, retinal=retinal)
# if isinstance(P, np.ndarray):
# if P.ndim == 1:
# P = P.reshape((-1, 1)) # make it a column
# else:
# P = smb.getvector(P, out='col')
# # make it homogeneous if not already
# if P.shape[0] == 3:
# P = smb.e2h(P)
# # project 3D points
# if objpose is not None:
# P = objpose.A @ P
# x = C @ P
# if behind:
# x[2, x[2, :] < 0] = np.nan # points behind the camera are set to NaN
# x = smb.h2e(x)
# # add Gaussian noise and distortion
# x = self.add_noise_distortion(x)
# # do visibility check if required
# if visibility:
# visible = ~np.isnan(x[0,:]) \
# & (x[0, :] >= 0) \
# & (x[1, :] >= 0) \
# & (x[0, :] < self.nu) \
# & (x[1, :] < self.nv)
# return x, visible
# else:
# return x
# def plot_point(self, Pp
# ax = _newplot(self, fig, ax)
# if self._image is not None:
# # if camera has an image, display said image
# idisp(self._image,
# fig=fig,
# ax=ax,
# title=self._name,
# drawonly=True)
# else:
# if self.limits is None:
# ax.set_xlim(0, self.nu)
# ax.set_ylim(0, self.nv)
# else:
# ax.set_xlim(self.limits[0], self.limits[1])
# ax.set_ylim(self.limits[2], self.limits[3])
# ax.autoscale(False)
# ax.set_aspect('equal')
# ax.invert_yaxis()
# ax.grid(True)
# if self.labels is None:
# ax.set_xlabel('u (pixels)')
# ax.set_ylabel('v (pixels)')
# else:
# ax.set_xlabel(self.labels[0])
# ax.set_ylabel(self.labels[1])
# ax.set_title(self.name)
# ax.set_facecolor('lightyellow')
# ax.figure.canvas.set_window_title('Machine Vision Toolbox for Python')
# # TODO figure out axes ticks, etc
# return ax # likely this return is not necessary
if __name__ == "__main__":
from pathlib import Path
import pytest
pytest.main(
[str(Path(__file__).parent.parent.parent / "tests" / "test_camera.py"), "-v"]
)
