Source code for machinevisiontoolbox.Camera

#!/usr/bin/env python
"""
Camera class
@author: Dorian Tsai
@author: Peter Corke
"""
from math import cos, pi, sqrt, sin, tan
from abc import ABC, abstractmethod
import copy
from collections import namedtuple

import numpy as np
import matplotlib.pyplot as plt
import scipy

import cv2 as cv
from spatialmath import base, Line3, SO3
from machinevisiontoolbox.ImagePointFeatures import FeatureMatch
from machinevisiontoolbox.base import idisp
from machinevisiontoolbox.base.imageio import _isnotebook


# from machinevisiontoolbox.classes import Image
# import CameraVisualizer as CamVis

# from mpl_toolkits.mplot3d import Axes3D, art3d
from mpl_toolkits.mplot3d.art3d import Poly3DCollection

# from collections import namedtuple
from spatialmath import SE3
import spatialmath.base as smbase

from machinevisiontoolbox import Image


class CameraBase(ABC):

    # list of attributes
    _name = None  # camera  name (string)
    _camtype = None  # camera type (string)

    _imagesize = None  # number of pixels (horizontal, vertical)
    _pp = None  # principal point (horizontal, vertical)
    _rhou = None  # pixel imagesize (single pixel) horizontal
    _rhov = None  # pixel imagesize (single pixel) vertical
    _image = None  # image (TODO image class?), for now, just numpy array

    _T = []  # camera pose (homogeneous transform, SE3 class)

    _ax = []  # for plotting, axes handle

    def __init__(
        self,
        name=None,
        camtype="central",
        rho=1,
        imagesize=None,
        sensorsize=None,
        pp=None,
        noise=None,
        pose=None,
        limits=None,
        labels=None,
        seed=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 in pixels, defaults to None
        :type imagesize: int or array_like(2), optional
        :param sensorsize: image sensor size, 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.
        """
        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:
            self._rhou = sensorsize[0] / self.imagesize[1]
            self._rhov = sensorsize[1] / self.imagesize[0]
        else:
            rho = base.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):
        """
        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):
        """
        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):
        """
        Readable representatio of camera parameters (base method)

        :return: string representation
        :rtype: str

        Multi-line string representation of camera intrinsic and extrinsic
        parameters.
        """
        return str(self)

    @abstractmethod
    def project_point(self, P, **kwargs):
        pass

    def project_line(self, *args, **kwargs):
        raise NotImplementedError("not implemented for this camera model")

    def project_conic(self, *args, **kwargs):
        raise NotImplementedError("not implemented for this camera model")

    @property
    def name(self):
        """
        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):
        """
        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):
        """
        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):
        """
        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):
        """
        Set/get size of virtual image plane (base method)

        The dimensions of the virtual image plane is a 2-tuple, width and
        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):
        """
        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 = base.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:
            self._pp = self._imagesize / 2

    @property
    def nu(self):
        """
        Get image plane width (base method)

        :return: width
        :rtype: int

        Number of pixels in the u-direction (width)

        Example:

        .. runblock:: pycon

            >>> from machinevisiontoolbox import CentralCamera
            >>> camera = CentralCamera.Default();
            >>> camera.nu

        :seealso: :meth:`nv` :meth:`width` :meth:`imagesize`
        """
        return self._imagesize[0]

    @property
    def nv(self):
        """
        Get image plane height (base method)

        :return: height
        :rtype: int

        Number of pixels in the v-direction (height)

        Example:

        .. runblock:: pycon

            >>> from machinevisiontoolbox import CentralCamera
            >>> camera = CentralCamera.Default();
            >>> camera.nv

        :seealso: :meth:`nu` :meth:`height`  :meth:`imagesize`
        """
        return self._imagesize[1]

    @property
    def width(self):
        """
        Get image plane width (base method)

        :return: width
        :rtype: int

        Image plane height, number of pixels in the v-direction

        Example:

        .. runblock:: pycon

            >>> from machinevisiontoolbox import CentralCamera
            >>> camera = CentralCamera.Default();
            >>> camera.width

        :seealso: :meth:`nu` :meth:`height`
        """
        return self._imagesize[0]

    @property
    def height(self):
        """
        Get image plane height (base method)

        :return: height
        :rtype: int

        Image plane width, number of pixels in the u-direction

        Example:

        .. runblock:: pycon

            >>> from machinevisiontoolbox import CentralCamera
            >>> camera = CentralCamera.Default();
            >>> camera.height

        :seealso: :meth:`nv` :meth:`width`
        """
        return self._imagesize[1]

    @property
    def pp(self):
        """
        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):
        """
        Set principal point coordinate

        :param pp: principal point
        :type pp: array_like(2)

        :seealso: :meth:`pp` :meth:`u0` :meth:`v0`
        """
        pp = base.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):
        """
        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`
        """
        return self._pp[0]

    @property
    def v0(self):
        """
        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`
        """
        return self._pp[1]

    @property
    def rhou(self):
        """
        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):
        """
        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):
        """
        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):
        """
        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):
        """
        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):
        """
        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`
        """
        return self._noise

    @noise.setter
    def noise(self, noise):
        self._noise = noise

    def move(self, T, name=None, relative=False):
        """
        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=None, ax=None):
        """
        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=None, ax=None):
        """
        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):
            return self._ax

        ax = self._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")

            try:
                ax.figure.canvas.manager.set_window_title(
                    "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):
        """
        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, *fmt, return_artist=False, objpose=None, pose=None, ax=None, **kwargs
    ):
        """
        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::

            >>> 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 = base.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

        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}

        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, *args, **kwargs):
        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)

        base.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, Y, Z, *fmt, objpose=None, pose=None, nsteps=21, **kwargs
    ):
        """
        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::

            >>> 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)

        # 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)

                self._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)

                self._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);
            self._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, **kwargs):
        """
        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=None,
        scale=1,
        shape="camera",
        label=True,
        alpha=1,
        solid=False,
        color="r",
        projection="ortho",
        frame=False,
        ax=None,
    ):
        """
        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 = smbase.axes_logic(ax, 3, projection=projection)

        if pose is None:
            pose = self.pose

        # 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]

            # Each set of four points is a single side of the Frustrum
            # points = np.array([[b0, b1, t1, t0], [b1, b2, t2, t1], [
            #                   b2, b3, t3, t2], [b3, b0, t0, t3]])
            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
            smbase.plot_cuboid(
                sides=np.r_[W, W, L] * scale,
                pose=pose,
                filled=solid,
                color=color,
                alpha=0.5 * alpha if solid else alpha,
                ax=ax,
            )

            # draw the lens
            smbase.plot_cylinder(
                radius=cr * scale,
                height=np.r_[L / 2, L / 2 + ch] * scale,
                resolution=cn,
                pose=pose,
                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)

        return ax

    def _add_noise_distortion(self, uv):
        """
        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: - Robotics, Vision & Control for Python, Section 13.1, P. Corke, Springer 2023. :seealso: :class:`CameraBase` :class:`FishEyeCamera` :class:`SphericalCamera` """
[docs] def __init__(self, f=1, distortion=None, **kwargs): 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): 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: - Robotics, Vision & Control for Python, Section 13.1, P. Corke, Springer 2023. :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): """ 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, pose=None, objpose=None, behind=True, visibility=False, retinal=False, **kwargs, ): 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: - Robotics, Vision & Control for Python, Section 13.1, P. Corke, Springer 2023. :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 = base.getvector(P, out="col") if P.shape[0] == 3: P = base.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 = base.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): 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: - Robotics, Vision & Control for Python, Section 13.7.1, P. Corke, Springer 2023. :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 = base.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): 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: - Robotics, Vision & Control for Python, Section 13.7.1, P. Corke, Springer 2023. :seealso: :meth:`C` :meth:`project_point` :meth:`project_line` """ if not smbase.ismatrix(Q, (4, 4)): raise ValueError("expecting 4x4 conic matrix") return self.C() @ Q @ self.C().T
[docs] def epiline(self, p, camera2): 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: - Robotics, Vision & Control for Python, Section 14.2.1, P. Corke, Springer 2023. :seealso: :meth:`plot_epiline` :meth:`CentralCamera.F` """ # p is 3 x N, result is 3 x N return self.F(camera2) @ base.e2h(p)
[docs] def plot_epiline(self, F, p, *fmt, **kwargs): 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: - Robotics, Vision & Control for Python, Section 14.2.1, P. Corke, Springer 2023. :seealso: :meth:`plot_point` :meth:`epiline` :meth:`CentralCamera.F` """ # p is 3 x N, result is 3 x N self.plot_line2(F @ base.e2h(p), *fmt, **kwargs)
[docs] def plot_line3(self, L, **kwargs): """ 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, pose=None): """ 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 :reference: - "Multiview Geometry", Hartley & Zisserman, p.162 - Robotics, Vision & Control for Python, Section 14.3, P. Corke, Springer 2023. :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 base.getmatrix(points, (2, None)).T: lines.append(Line3.PointDir(-Mi @ v, Mi @ smbase.e2h(point))) return Line3(lines)
@property def centre(self): """ 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): """ 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): """ 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: - Robotics, Vision & Control for Python, Section 13.1.4, P. Corke, Springer 2023. """ 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): """ 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) * smbase.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 * smbase.e2h(np.r_[ud, vd]) # convert to pixel coords
@property def fu(self): """ 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): """ 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): """ 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): """[summary] :param f: focal length :type f: scalar or array_like(2) :raises ValueError: incorrect length of ``f`` """ f = base.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): """ 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): """ 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: - Robotics, Vision & Control for Python, Section 13.1, P. Corke, Springer 2023. :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=None, retinal=False): """ 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: - Robotics, Vision & Control for Python, Section 13.1, P. Corke, Springer 2023. :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, p): 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: - Robotics, Vision & Control for Python, Section 13.2.1, P. Corke, Springer 2023. :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(self, images, gridshape=(7, 6), squaresize=0.025): """ 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: - Robotics, Vision & Control for Python, Section 13.7, P. Corke, Springer 2023. :seealso: :meth:`C` :meth:`points2C` :meth:`decomposeC` :class:`~spatialmath..pose3d.SE3` """ criteria = (cv.TERM_CRITERIA_EPS + cv.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 = cv.findChessboardCorners(gray, gridshape, None) # If found, add object points, image points (after refining them) if ret: objpoints.append(objp) corners2 = cv.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria) imgpoints.append(corners) # Draw the corners image = Image(image, copy=True) if not image.iscolor: image = image.colorize() corner_images.append( cv.drawChessboardCorners(image.A, gridshape, corners2, ret) ) valid.append(i) ret, C, distortion, rvecs, tvecs = cv.calibrateCamera( objpoints, imgpoints, gray.shape[::-1], None, None ) 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(tvec) * SE3.EulerVec(rvec.flatten())).inv(), id, ) frames.append(frame) return C, distortion[0], frames else: return None
[docs] @classmethod def decomposeC(cls, C): 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`. :reference: - Multiple View Geometry, Hartley&Zisserman, p 163-164 - Robotics, Vision & Control for Python, Section 13.2.3, P. Corke, Springer 2023. :seealso: :meth:`C` :meth:`points2C` """ def rq(S): # 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, n, d): """ 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 = base.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, p2, method="leastsquares", seed=None, **kwargs): """ 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. :reference: - Robotics, Vision & Control for Python, Section 14.2.4, P. Corke, Springer 2023. :seealso: :meth:`H` :meth:`decomposeH` `opencv.findHomography <https://docs.opencv.org/master/d9/d0c/group__calib3d.html#ga4abc2ece9fab9398f2e560d53c8c9780>`_ """ points2H_dict = { "leastsquares": 0, "ransac": cv.RANSAC, "lmeds": cv.LMEDS, "prosac": cv.RHO, } if seed is not None: cv.setRNGSeed(seed) H, mask = cv.findHomography( srcPoints=p1.T, dstPoints=p2.T, method=points2H_dict[method], **kwargs ) mask = mask.ravel().astype(bool) e = base.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, K=None): """ 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 :reference: - Robotics, Vision & Control for Python, Section 14.2.4, P. Corke, Springer 2023. :seealso: :meth:`points2H` :meth:`H` `opencv.decomposeHomographyMat <https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#ga7f60bdff78833d1e3fd6d9d0fd538d92>`_ """ retval, rotations, translations, normals = cv.decomposeHomographyMat(H, 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(smbase.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): """ 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 :reference: - Y.Ma, J.Kosecka, S.Soatto, S.Sastry, "An invitation to 3D", Springer, 2003. p.177 - Robotics, Vision & Control for Python, Section 14.2.1, P. Corke, Springer 2023. :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, p2, method="8p", residual=True, seed=None, **kwargs): """ 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/master/d9/d0c/group__calib3d.html#ga59b0d57f46f8677fb5904294a23d404a>`_ """ points2F_dict = { "7p": cv.FM_7POINT, "8p": cv.FM_8POINT, "ransac": cv.FM_RANSAC, "lmeds": cv.FM_LMEDS, } if seed is not None: cv.setRNGSeed(seed) F, mask = cv.findFundamentalMat( p1.T, p2.T, method=points2F_dict[method], **kwargs ) mask = mask.ravel().astype(bool) # add various return values retval = [F] if residual: e = base.e2h(p2[:, mask]).T @ F @ base.e2h(p1[:, mask]) resid = np.linalg.norm(np.diagonal(e)) retval.append(resid) if method in ("ransac", "lmeds"): retval.append(mask) return retval
# elines = base.e2h(p2).T @ F # homog lines, one per row # p1h = base.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, p1, p2): """ 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 istance 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 @ base.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): """ 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 :reference: - 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 = other.pose.inv() * self.pose elif isinstance(other, SE3): # relative pose given explicitly T21 = other.inv() else: raise ValueError("bad type") return base.skew(T21.t) @ T21.R
[docs] def points2E(self, p1, p2, method=None, K=None, **kwargs): """ 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/master/d9/d0c/group__calib3d.html#gad245d60e64d0c1270dbfd0520847bb87>`_ """ if K is None: K = self.K points2E_dict = {"ransac": cv.RANSAC, "lmeds": cv.LMEDS} if method is not None: method = points2E_dict[method] E, mask = cv.findEssentialMat( p1.T, p2.T, cameraMatrix=K, method=method, **kwargs ) if mask is not None: mask = mask.flatten().astype(bool) return E, mask else: return E
[docs] def decomposeE(self, E, P=None): """ 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 :reference: - Robotics, Vision & Control for Python, Section 14.2.2 P. Corke, Springer 2023. :seealso: :meth:`E` :meth:`points2E` :class:`~machinevisiontoolbox.ImagePointFeatures.FeatureMatch` `opencv.decomposeEssentialMat <https://docs.opencv.org/master/d9/d0c/group__calib3d.html#ga54a2f5b3f8aeaf6c76d4a31dece85d5d>`_ `opencv.recoverPose <https://docs.opencv.org/master/d9/d0c/group__calib3d.html#gadb7d2dfcc184c1d2f496d8639f4371c0>`_ """ if isinstance(P, FeatureMatch): # passed a Match object match = P retval, R, t, mask = cv.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 = cv.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 base.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, depth): 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. - Robotics, Vision & Control for Python, Section 15.2.1, P. Corke, Springer 2023. :seealso: :meth:`flowfield` :meth:`visjac_p_polar` :meth:`visjac_l` :meth:`visjac_e` """ uv = base.getmatrix(uv, (2, None)) Z = depth Z = base.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 @ base.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, Z): 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 - Robotics, Vision & Control for Python, Section 16.2 P. Corke, Springer 2023. :seealso: :meth:`visjac_p` :meth:`visjac_l` :meth:`visjac_e` """ J = [] p = smbase.getmatrix(p, (2, None)) f = self.f[0] if smbase.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, plane): 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) - Robotics, Vision & Control for Python, Section 15.3.1, P. Corke, Springer 2023. :seealso: :meth:`visjac_p` :meth:`visjac_p_polar` :meth:`visjac_e` """ a, b, c, d = plane lines = smbase.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, plane): 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) - Robotics, Vision & Control for Python, Section 15.3.2, P. Corke, Springer 2023. :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, Z=2): """ 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 = base.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, P): 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, p, method="iterative", frame="world"): """ 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/3.4/d9/d0c/group__calib3d.html#ga549c2075fac14829ff4a58bc931c033d>`_ """ method_dict = { "iterative": cv.SOLVEPNP_ITERATIVE, "epnp": cv.SOLVEPNP_EPNP, "p3p": cv.SOLVEPNP_P3P, "ap3p": cv.SOLVEPNP_AP3P, "ippe": cv.SOLVEPNP_IPPE, "ippe-square": cv.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 = cv.solvePnP(P.T, p, self.K, 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: - Robotics, Vision & Control for Python, Section 13.3.1, P. Corke, Springer 2023. :seealso: :class:`CameraBase` :class:`CentralCamera` :class:`CatadioptricCamera` :class:`SphericalCamera` """
[docs] def __init__(self, k=None, projection="equiangular", **kwargs): super().__init__(camtype="fisheye", **kwargs) self.projection = projection if k is 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): 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, pose=None, objpose=None): 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: SE:class:`~spatialmath..pose3d.SE3`3, 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 = base.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: - Robotics, Vision & Control for Python, Section 13.3.2, P. Corke, Springer 2023. :seealso: :class:`CameraBase` :class:`CentralCamera` :class:`FishEyeCamera` :class:`SphericalCamera` """
[docs] def __init__(self, k=None, projection="equiangular", maxangle=None, **kwargs): super().__init__(camtype="catadioptric", **kwargs) self.projection = projection if k is 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): 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, pose=None, objpose=None): """ 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 = base.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 # project to the image plane 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) # 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: - Robotics, Vision & Control for Python, Section 13.3.3, P. Corke, Springer 2023. :seealso: :class:`CameraBase` :class:`CentralCamera` :class:`CatadioptricCamera` :class:`FishEyeCamera` """
[docs] def __init__(self, **kwargs): # 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): """ 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, pose=None, objpose=None): 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 = base.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 R = np.linalg.norm(P, axis=0) x = P[0, :] / R y = P[1, :] / R z = P[2, :] / R phi = np.arctan2(y, x) theta = np.arccos(z) return np.array([phi, theta])
[docs] def visjac_p(self, p, depth=None): 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 smbase.isscalar(depth): depth = [depth] * p.shape[1] 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=False, **kwargs): smbase.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 = base.getvector(P, out='col') # # make it homogeneous if not already # if P.shape[0] == 3: # P = base.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 = base.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 spatialmath import UnitQuaternion # im1 = Image.Read("eiffel2-1.png", grey=True) # camera = CentralCamera(); # camera.disp(im1); # cam = CentralCamera(f=0.08) # print(cam) # P = [0.1, 0.2, 3] # print(cam.project_point(P)) cam = CentralCamera(f=0.08, imagesize=1000, rho=10e-6) print(cam) cam.project_point([1, 2, 3]) # P = np.array([[0, 10], [0, 10], [10, 10]]) # p, visible = cam.project_point(P, visibility=True) # visible # P = [0.1, 0.2, 3] # print(cam.project_point(P)) # T1 = SE3(-0.1, 0, 0) * SE3.Ry(0.4); # camera1 = CentralCamera(name="camera 1", f=0.002, imagesize=1000, rho=10e-6, pose=T1) # # print(camera1) # camera1.decomposeH(np.eye(3,3)) # L = Line3.TwoPoints([0, 0, 1], [1, 1, 1]) # camera = CentralCamera.Default(); # l = camera.project_line(L) # camera.plot_line3(L) # x = np.r_[cam.pose.t, UnitQuaternion(cam.pose).vec3] # print(x) # p, JA, JB = cam.derivatives(x, P) # print(p) # print(cam.project_point(P)) # print(JA) # print(JB) # smbase.plotvol3(2) # cam.plot_camera(scale=0.5, shape='camera', T=SE3.Ry(np.pi/2)) # plt.show(block=True) # print(cam) # # cam.pose = SE3([0.1, 0.2, 0.3]) # print(cam.pose) # # fig, ax = c.plot_camera(frustum=True) # # plt.show() # np.set_printoptions(linewidth=120, formatter={'float': lambda x: f"{x:8.4g}" if abs(x) > 1e-10 else f"{0:8.4g}"}) # print(cam.project([1,2,3])) # print(cam.visjac_p((300,300), 1)) # cam.flowfield([0,0,0, 0,0,1]) # # fundamental matrix # # create +8 world points (20 in this case) # nx, ny = (4, 5) # depth = 3 # x = np.linspace(-1, 1, nx) # y = np.linspace(-1, 1, ny) # X, Y = np.meshgrid(x, y) # Z = depth * np.ones(X.shape) # P = np.dstack((X, Y, Z)) # PC = np.ravel(P, order='C') # PW = np.reshape(PC, (3, nx * ny), order='F') # # create projections from pose 1: # print(c.T) # p1 = c.project(PW) # p1 wrt c's T # print(p1) # c.plot(PW) # # define pose 2: # T2 = SE3([0.4, 0.2, 0.3]) # just pure x-translation # p2 = c.project(PW, T2) # print(p2) # c.plot(p2) # # convert p1, p2 into lists of points? # p1 = np.float32(np.transpose(p1)) # p2 = np.float32(np.transpose(p2)) # F = c.FfromPoints(p1, # p2, # method='8p', # ransacThresh=3, # confidence=0.99, # maxiters=10) # # to check F: # p1h = e2h(p1.T) # p2h = e2h(p2.T) # pfp = [p2h[:, i].T @ F @ p1h[:, i] for i in range(p1h.shape[1])] # # [print(pfpi) for pfpi in pfp] # for pfpi in pfp: # print(pfpi) # # should be all close to zero, which they are! # # essential matrix from points: # E = c.EfromPoints(p1, p2, c.C) # # TODO verify E # import code # code.interact(local=dict(globals(), **locals()))