SDK CV Utils

template<typename Projection>
class CameraGeometry : public Metavision::CameraGeometryBase<Projection::underlying_type>

Camera geometry’s implementation.

Template Parameters
  • Projection: The projection model used to map points

template<typename T>
class Metavision::CameraGeometryBase

Base class for camera geometries.

A camera geometry is a mathematical model allowing to map points from world to image plane and vice versa. As this class uses virtual methods, it was not possible to make them templated. Therefore, eigen has been used for the vector and matrices types because it allows to map other types without copies (i.e. by using a combination of Eigen::Map and Eigen::Ref).

Template Parameters
  • T: Either float or double

Public Functions

~CameraGeometryBase() = default

Default destructor.

std::unique_ptr<CameraGeometryBase<T>> clone() const = 0

Creates a deep copy of this instance.

Return

The new instance

const Vec2i &get_image_size() const = 0

Gets the sensor’s size.

T get_distance_to_image_plane() const = 0

Gets the distance between the camera’s optical center and the undistorted image plane.

void get_undist_norm_to_undist_img_transform(Mat3RMRef m) const = 0

Gets the transform that maps a point from the undistorted normalized image plane (i.e. Z = 1) into the undistorted image plane (row major mode matrix)

Parameters
  • [out] m: The transform

void get_undist_norm_to_undist_img_transform(Mat3CMRef m) const = 0

Gets the transform that maps a point from the undistorted normalized image plane (i.e. Z = 1) into the undistorted image plane (col major mode matrix)

Parameters
  • [out] m: The transform

void camera_to_img(Vec3RefConst pt_c, Vec2Ref pt_dist_img) const = 0

Maps a point from the camera’s coordinates system into the distorted image plane.

Parameters
  • [in] pt_c: The 3D point in the camera’s coordinates system

  • [out] pt_dist_img: The mapped point in the distorted image plane

void camera_to_undist_img(Vec3RefConst pt_c, Vec2Ref pt_undist_img) const = 0

Maps a point from the camera’s coordinates system into the undistorted image plane.

Parameters
  • [in] pt_c: The 3D point in the camera’s coordinates system

  • [out] pt_undist_img: The mapped point in the undistorted image plane

void undist_norm_to_undist_img(Vec2RefConst pt_undist_norm, Vec2Ref pt_undist_img) const = 0

Maps a point from the undistorted normalized image plane into the normalized image plane.

Parameters
  • [in] pt_undist_norm: The point in the undistorted normalized image plane

  • [out] pt_undist_img: The mapped point in the undistorted image plane

void undist_img_to_undist_norm(Vec2RefConst pt_undist_img, Vec2Ref pt_undist_norm) const = 0

Maps a point from the undistorted image plane into the undistorted normalized image plane.

Parameters
  • [in] pt_undist_img: The point in the undistorted image plane

  • [out] pt_undist_norm: The mapped point in the undistorted normalized image plane

void undist_norm_to_img(Vec2RefConst pt_undist_norm, Vec2Ref pt_dist_img) const = 0

Maps a point from the undistorted normalized image plane into the distorted image plane.

Parameters
  • [in] pt_undist_norm: The point in the undistorted normalized image plane

  • [out] pt_dist_img: The mapped point in the distorted image plane

void img_to_undist_norm(Vec2RefConst pt_dist_img, Vec2Ref pt_undist_norm) const = 0

Maps a point from the distorted image plane into the undistorted normalized image plane.

Parameters
  • [in] pt_dist_img: The point in the distorted image plane

  • [out] pt_undist_norm: The mapped point in the undistorted normalized image plane

void undist_norm_to_dist_norm(Vec2RefConst pt_undist_norm, Vec2Ref pt_dist_norm) const = 0

Maps a point from the undistorted normalized image plane into the distorted normalized image plane.

Parameters
  • [in] pt_undist_norm: The mapped point in the undistorted normalized image plane

  • [out] pt_dist_norm: The mapped point in the distorted normalized image plane

void vector_undist_norm_to_img(Vec2RefConst ctr_undist_norm, Vec2RefConst vec_undist_norm, Vec2Ref ctr_dist_img, Vec2Ref vec_dist_img) const = 0

Maps a vector from the undistorted normalized image plane into the distorted image plane.

Note

The output vector is normalized

Parameters
  • [in] ctr_undist_norm: The vector’s starting point in the undistorted normalized image plane

  • [in] vec_undist_norm: The vector in the undistorted normalized image plane (the vector must be normalized)

  • [out] ctr_dist_img: The vector’s starting point in the distorted image plane

  • [out] vec_dist_img: The mapped vector in the distorted image plane

void vector_img_to_undist_norm(Vec2RefConst ctr_dist_img, Vec2RefConst vec_dist_img, Vec2Ref ctr_undist_norm, Vec2Ref vec_undist_norm) const = 0

Maps a vector from the distorted image plane into the undistorted normalized image plane.

Note

The output vector is normalized

Parameters
  • [in] ctr_dist_img: The vector’s starting point in the distorted image plane

  • [in] vec_dist_img: The vector in the distorted image plane (the vector must be normalized)

  • [out] ctr_undist_norm: The vector’s starting point in the undistorted normalized image plane

  • [out] vec_undist_norm: The vector in the undistorted normalized image plane

void get_undist_norm_to_img_jacobian(Vec2RefConst pt_undist_norm, Vec2Ref pt_dist_img, Mat2RMRef J) const = 0

Computes the distortion function’s jacobian (Row major mode matrix)

Parameters
  • [in] pt_undist_norm: The point in the undistorted normalized image plane at which the jacobian is computed

  • [out] pt_dist_img: The point in the distorted image plane

  • [out] J: The computed jacobian

void get_undist_norm_to_img_jacobian(Vec2RefConst pt_undist_norm, Vec2Ref pt_dist_img, Mat2CMRef J) const = 0

Computes the distortion function’s jacobian (Col major mode matrix)

Parameters
  • [in] pt_undist_norm: The point in the undistorted normalized image plane at which the jacobian is computed

  • [out] pt_dist_img: The point in the distorted image plane

  • [out] J: The computed jacobian

void get_img_to_undist_norm_jacobian(Vec2RefConst pt_dist_img, Vec2Ref pt_undist_norm, Mat2RMRef J) const = 0

Computes the undistortion function’s jacobian (Row major mode matrix)

Parameters
  • [in] pt_dist_img: The point in the distorted image plane at which the jacobian is computed

  • [out] pt_undist_norm: The point in the undistorted normalized image plane

  • [out] J: The computed jacobian

void get_img_to_undist_norm_jacobian(Vec2RefConst pt_dist_img, Vec2Ref pt_undist_norm, Mat2CMRef J) const = 0

Computes the undistortion function’s jacobian (Col major mode matrix)

Parameters
  • [in] pt_dist_img: The point in the distorted image plane at which the jacobian is computed

  • [out] pt_undist_norm: The point in the undistorted normalized image plane

  • [out] J: The computed jacobian

template<typename FloatType>
class Metavision::OcamModel

Class implementing the Scaramuzza’s fisheye camera model.

(“A Toolbox for Easily Calibrating Omnidirectional Cameras”). The projection and distortion are done simultaneously.

Template Parameters
  • FloatType: Either float or double.

Public Functions

EIGEN_MAKE_ALIGNED_OPERATOR_NEW OcamModel(const Vec2i &img_size, const VecX &poly, const VecX &inv_poly, const Vec2 &center, const Mat2CM &affine_transform, FloatType zoom_factor = FloatType(1))

Builds a new OcamModel.

The intrinsics must correspond to a camera having:

  • its X axis pointing to the right

  • its Y axis pointing down, and

  • its Z axis pointing toward

    Parameters
    • [in] img_size: Sensor’s size

    • [in] poly: Polynomial used to undistort coordinates

    • [in] inv_poly: Polynomial used to distort coordinates

    • [in] center: Projection of the camera’s optical center in the image plane

    • [in] affine_transform: Transform that maps from the ideal sensor plane to the image plane

    • [in] zoom_factor: Scale factor indicating how the image appears once undistorted. The bigger is the factor, the smaller is the undistorted image (i.e. the more black areas in the image).

const Vec2i &get_image_size() const

Gets the sensor’s size.

FloatType get_distance_to_image_plane() const

Gets the distance between the camera’s optical center and the undistorted image plane.

template<typename M>
void get_undist_norm_to_undist_img_transform(M &m) const

Gets the transform that maps a point from the undistorted normalized image plane (i.e. Z = 1) into the undistorted image plane.

Template Parameters
  • M: The 3x3 matrix’s type used

Parameters
  • [out] m: The transform

template<typename V1, typename V2>
void camera_to_img(const V1 &pt_c, V2 &pt_dist_img) const

Maps a point from the camera’s coordinates system into the distorted image plane.

Template Parameters
  • V1: The 3D point’s type used

  • V2: The 2D point’s type used

Parameters
  • [in] pt_c: The 3D point in the camera’s coordinates system

  • [out] pt_dist_img: The mapped point in the distorted image plane

template<typename V1, typename V2>
void undist_norm_to_img(const V1 &pt_undist_norm, V2 &pt_dist_img) const

Maps a point from the undistorted normalized image plane into the distorted image plane.

Template Parameters
  • V1: The input 2D point’s type used

  • V2: The output 2D point’s type used

Parameters
  • [in] pt_undist_norm: The point in the undistorted normalized image plane

  • [out] pt_dist_img: The mapped point in the distorted image plane

template<typename V1, typename V2>
void camera_to_undist_img(const V1 &pt_c, V2 &pt_undist_img) const

Maps a point from the camera’s coordinates system into the undistorted image plane.

Template Parameters
  • V1: The 3D point’s type used

  • V2: The 2D point’s type used

Parameters
  • [in] pt_c: The 3D point in the camera’s coordinates system

  • [out] pt_undist_img: The mapped point in the undistorted image plane

template<typename V1, typename V2>
void undist_norm_to_undist_img(const V1 &pt_undist_norm, V2 &pt_undist_img) const

Maps a point from the undistorted normalized image plane into the undistorted image plane.

Template Parameters
  • V1: The input 2D point’s type used

  • V2: The output 2D point’s type used

Parameters
  • [in] pt_undist_norm: The point in the undistorted normalized image plane

  • [out] pt_undist_img: The mapped point in the undistorted image plane

template<typename V1, typename V2>
void undist_img_to_undist_norm(const V1 &pt_undist_img, V2 &pt_undist_norm) const

Maps a point from the undistorted image plane into the undistorted normalized image plane.

Template Parameters
  • V1: The input 2D point’s type used

  • V2: The output 2D point’s type used

Parameters
  • [in] pt_undist_img: The point in the undistorted image plane

  • [out] pt_undist_norm: The mapped point in the undistorted normalized image plane

template<typename V1, typename V2>
void undist_norm_to_dist_norm(const V1 &pt_undist_norm, V2 &pt_dist_norm) const

Maps a point from the undistorted normalized image plane into the distorted normalized image plane.

Template Parameters
  • V1: The input 2D point’s type used

  • V2: The output 2D point’s type used

Parameters
  • [in] pt_undist_norm: The mapped point in the undistorted normalized image plane

  • [out] pt_dist_norm: The mapped point in the distorted normalized image plane

template<typename V1, typename V2>
void img_to_undist_norm(const V1 &pt_dist_img, V2 &pt_undist_norm) const

Maps a point from the distorted image plane into the undistorted normalized image plane.

Template Parameters
  • V1: The input 2D point’s type used

  • V2: The output 2D point’s type used

Parameters
  • [in] pt_dist_img: The point in the distorted image plane

  • [out] pt_undist_norm: The mapped point in the undistorted normalized image plane

template<typename V1, typename V2, typename M>
void get_undist_norm_to_img_jacobian(const V1 &pt_undist_norm, V2 &pt_dist_img, M &J) const

Computes the distortion function’s jacobian.

Template Parameters
  • V1: The input 2D point’s type used

  • V2: The output 2D point’s type used

  • V3: The output 2x2 matrix’s type used

Parameters
  • [in] pt_undist_norm: The point in the undistorted normalized image plane at which the jacobian is computed

  • [out] pt_dist_img: The point in the distorted image plane

  • [out] J: The computed jacobian

template<typename V1, typename V2, typename M>
void get_img_to_undist_norm_jacobian(const V1 &pt_dist_img, V2 &pt_undist_norm, M &J) const

Computes the undistortion function’s jacobian.

Template Parameters
  • V1: The input 2D point’s type used

  • V2: The output 2D point’s type used

  • V3: The output 2x2 matrix’s type used

Parameters
  • [in] pt_dist_img: The point in the distorted image plane at which the jacobian is computed

  • [out] pt_undist_norm: The point in the undistorted normalized image plane

  • [out] J: The computed jacobian

template<typename FloatType>
class Metavision::PinholeCameraModel

Class implementing the Pinhole camera model with radial and tangential distortion.

Template Parameters
  • FloatType: Either float or double

Public Functions

EIGEN_MAKE_ALIGNED_OPERATOR_NEW PinholeCameraModel(int width, int height, const std::vector<FloatType> &K, const std::vector<FloatType> &D)

Builds a new PinholeCameraModel.

Parameters
  • [in] width: Sensor’s width

  • [in] height: Sensor’s height

  • [in] K: Camera’s intrinsics 3x3 matrix

  • [in] D: Camera’s distortion coefficients

const Vec2i &get_image_size() const

Gets the sensor’s size.

const Mat3 &K() const

Gets the camera’s intrinsics.

const Mat3 &Kinv() const

Gets the camera’s inverse intrinsics.

const Vec5 &D() const

Gets the camera’s distortion coefficients.

FloatType get_distance_to_image_plane() const

Gets the distance between the camera’s optical center and the undistorted image plane.

template<typename M>
void get_undist_norm_to_undist_img_transform(M &m) const

Gets the transform that maps a point from the undistorted normalized image plane (i.e. Z = 1) into the undistorted image plane.

Template Parameters
  • M: The 3x3 matrix’s type used

Parameters
  • [out] m: The transform

template<typename V1, typename V2>
void camera_to_img(const V1 &pt_c, V2 &pt_dist_img) const

Maps a point from the camera’s coordinates system into the distorted image plane.

Template Parameters
  • V1: The 3D point’s type used

  • V2: The 2D point’s type used

Parameters
  • [in] pt_c: The 3D point in the camera’s coordinates system

  • [out] pt_dist_img: The mapped point in the distorted image plane

template<typename V1, typename V2>
void undist_norm_to_img(const V1 &pt_undist_norm, V2 &pt_dist_img) const

Maps a point from the undistorted normalized image plane into the distorted image plane.

Template Parameters
  • V1: The input 2D point’s type used

  • V2: The output 2D point’s type used

Parameters
  • [in] pt_undist_norm: The point in the undistorted normalized image plane

  • [out] pt_dist_img: The mapped point in the distorted image plane

template<typename V1, typename V2>
void camera_to_undist_img(const V1 &pt_c, V2 &pt_undist_img) const

Maps a point from the camera’s coordinates system into the undistorted image plane.

Template Parameters
  • V1: The 3D point’s type used

  • V2: The 2D point’s type used

Parameters
  • [in] pt_c: The 3D point in the camera’s coordinates system

  • [out] pt_undist_img: The mapped point in the undistorted image plane

template<typename V1, typename V2>
void undist_norm_to_undist_img(const V1 &pt_undist_norm, V2 &pt_undist_img) const

Maps a point from the undistorted normalized image plane into the undistorted image plane.

Template Parameters
  • V1: The input 2D point’s type used

  • V2: The output 2D point’s type used

Parameters
  • [in] pt_undist_norm: The point in the undistorted normalized image plane

  • [out] pt_undist_img: The mapped point in the undistorted image plane

template<typename V1, typename V2>
void undist_img_to_undist_norm(const V1 &pt_undist_img, V2 &pt_undist_norm) const

Maps a point from the undistorted image plane into the undistorted normalized image plane.

Template Parameters
  • V1: The input 2D point’s type used

  • V2: The output 2D point’s type used

Parameters
  • [in] pt_undist_img: The point in the undistorted image plane

  • [out] pt_undist_norm: The mapped point in the undistorted normalized image plane

template<typename V1, typename V2>
void undist_norm_to_dist_norm(const V1 &pt_undist_norm, V2 &pt_dist_norm) const

Maps a point from the undistorted normalized image plane into the distorted normalized image plane.

Template Parameters
  • V1: The input 2D point’s type used

  • V2: The output 2D point’s type used

Parameters
  • [in] pt_undist_norm: The mapped point in the undistorted normalized image plane

  • [out] pt_dist_norm: The mapped point in the distorted normalized image plane

template<typename V1, typename V2>
void img_to_undist_norm(const V1 &pt_dist_img, V2 &pt_undist_norm) const

Maps a point from the distorted image plane into the undistorted normalized image plane.

Template Parameters
  • V1: The input 2D point’s type used

  • V2: The output 2D point’s type used

Parameters
  • [in] pt_dist_img: The point in the distorted image plane

  • [out] pt_undist_norm: The mapped point in the undistorted normalized image plane

template<typename V1, typename V2, typename M>
void get_undist_norm_to_img_jacobian(const V1 &pt_undist_norm, V2 &pt_dist_img, M &J) const

Computes the distortion function’s jacobian.

Template Parameters
  • V1: The input 2D point’s type used

  • V2: The output 2D point’s type used

  • V3: The output 2x2 matrix’s type used

Parameters
  • [in] pt_undist_norm: The point in the undistorted normalized image plane at which the jacobian is computed

  • [out] pt_dist_img: The point in the distorted image plane

  • [out] J: The computed jacobian

template<typename V1, typename V2, typename M>
void get_img_to_undist_norm_jacobian(const V1 &pt_dist_img, V2 &pt_undist_norm, M &J) const

Computes the undistortion function’s jacobian.

Template Parameters
  • V1: The input 2D point’s type used

  • V2: The output 2D point’s type used

  • V3: The output 2x2 matrix’s type used

Parameters
  • [in] pt_dist_img: The point in the distorted image plane at which the jacobian is computed

  • [out] pt_undist_norm: The point in the undistorted normalized image plane

  • [out] J: The computed jacobian

template<typename T, typename Ti, typename To>
void Metavision::undist_norm_to_camera(const Ti &pt_norm, To &pt_c, T depth)

Maps a point from the undistorted normalized image plane into the camera’s coordinates system.

Template Parameters
  • T: The floating point type used

  • Ti: The input 2D point type used

  • To: The output 3D point type used

Parameters
  • [in] pt_norm: The point in the undistorted normalized image plane

  • [out] pt_c: The point in the camera’s coordinates system

  • [in] depth: The depth of the point in the camera’s coordinates system

template<typename Ti, typename To>
void Metavision::camera_to_undist_norm(const Ti &pt3_cam, To &pt2_norm)

Maps a 3D point from the camera’s coordinates system into the undistorted normalized image plane.

Template Parameters
  • Ti: The input 3D point type used

  • To: The output 2D point type used

Parameters
  • [in] pt3_cam: The 3D point expressed in the camera’s coordinate system

  • [out] pt2_norm: The point in the undistorted normalized image plane

template<typename Ti, typename To>
void Metavision::vector_camera_to_undist_norm(const Ti &v3_cam, const Ti &pt3_cam, To &v2_undist_norm, To &pt2_undist_norm, bool normalize = true)

Maps a 3D vector from the camera’s coordinates system into the undistorted normalized image plane.

Template Parameters
  • Ti: The input 3D vector type used

  • To: The output 2D vector type used

Parameters
  • [in] v3_cam: The 3D vector in the camera’s coordinates system

  • [in] pt3_cam: The vector’s starting point in the camera’s coordinates system

  • [out] v2_undist_norm: The vector in the undistorted normalized image plane

  • [out] pt2_undist_norm: The vector’s starting point in the normalized image plane

  • normalize: The output is normalized (i.e. norm = 1) if this parameter is set to true

template<typename T, typename To>
void Metavision::get_undist_norm_to_undist_img_transform(const CameraGeometryBase<T> &camera_geometry, To &m)

Gets the transform that maps a point from the undistorted normalized image plane (i.e. Z = 1) into the undistorted image plane.

Template Parameters
  • T: The floating point type used

  • To: The 3x3 matrix type used

Parameters
  • [in] camera_geometry: The CameraGeometryBase instance used to map the point

  • [out] m: The transform

template<typename T, typename T1, typename T2>
void Metavision::camera_to_img(const CameraGeometryBase<T> &camera_geometry, const T1 &pt_c, T2 &pt_dist_img)

Maps a point from the camera’s coordinates system into the distorted image plane.

Template Parameters
  • T: The floating point type used

  • T1: The 3D point type used

  • T2: The 2D point type used

Parameters
  • [in] camera_geometry: The CameraGeometryBase instance used to map the point

  • [in] pt_c: The 3D point in the camera’s coordinates system

  • [out] pt_dist_img: The mapped point in the distorted image plane

template<typename T, typename T1, typename T2>
bool Metavision::safe_camera_to_img(const CameraGeometryBase<T> &camera_geometry, const T1 &pt_c, T2 &pt_dist_img, T epsilonz = T(1e-10))

Maps a point from the camera’s coordinates system into the distorted image plane.

Return

true if the point has been successfully mapped, false otherwise

Template Parameters
  • T: The floating point type used

  • T1: The 3D point type used

  • T2: The 2D point type used

Parameters
  • [in] camera_geometry: The CameraGeometryBase instance used to map the point

  • [in] pt_c: The 3D point in the camera’s coordinates system

  • [out] pt_dist_img: The mapped point in the distorted image plane

  • [in] epsilonz: To be mapped, the point’s depth in the camera’s coordinates system must less than this value

template<typename T, typename T1, typename T2>
void Metavision::camera_to_undist_img(const CameraGeometryBase<T> &camera_geometry, const T1 &pt_c, T2 &pt_undist_img)

Maps a point from the camera’s coordinates system into the undistorted image plane.

Template Parameters
  • T: The floating point type used

  • T1: The 3D point type used

  • T2: The 2D point type used

Parameters
  • [in] camera_geometry: The CameraGeometryBase instance used to map the point

  • [in] pt_c: The 3D point in the camera’s coordinates system

  • [out] pt_undist_img: The mapped point in the undistorted image plane

template<typename T, typename T1, typename T2>
void Metavision::undist_norm_to_undist_img(const CameraGeometryBase<T> &camera_geometry, const T1 &pt_undist_norm, T2 &pt_undist_img)

Maps a point from the undistorted normalized image plane into the undistorted image plane.

Template Parameters
  • T: The floating point type used

  • T1: The input 2D point type used

  • T2: The output 2D point type used

Parameters
  • [in] camera_geometry: The CameraGeometryBase instance used to map the point

  • [in] pt_undist_norm: The point in the undistorted normalized image plane

  • [out] pt_undist_img: The mapped point in the undistorted image plane

template<typename T, typename T1, typename T2>
void Metavision::undist_img_to_undist_norm(const CameraGeometryBase<T> &camera_geometry, const T1 &pt_undist_img, T2 &pt_undist_norm)

Maps a point from the undistorted image plane into the undistorted normalized image plane.

Template Parameters
  • T: The floating point type used

  • T1: The input 2D point type used

  • T2: The output 2D point type used

Parameters
  • [in] camera_geometry: The CameraGeometryBase instance used to map the point

  • [in] pt_undist_img: The point in the undistorted image plane

  • [out] pt_undist_norm: The mapped point in the undistorted normalized image plane

template<typename T, typename T1, typename T2>
void Metavision::undist_norm_to_img(const CameraGeometryBase<T> &camera_geometry, const T1 &pt_undist_norm, T2 &pt_dist_img)

Maps a point from the undistorted normalized image plane into the distorted image plane.

Template Parameters
  • T: The floating point type used

  • T1: The input 2D point type used

  • T2: The output 2D point type used

Parameters
  • [in] camera_geometry: The CameraGeometryBase instance used to map the point

  • [in] pt_undist_norm: The point in the undistorted normalized image plane

  • [out] pt_dist_img: The mapped point in the distorted image plane

template<typename T, typename T1, typename T2>
void Metavision::img_to_undist_norm(const CameraGeometryBase<T> &camera_geometry, const T1 &pt_dist_img, T2 &pt_undist_norm)

Maps a point from the distorted image plane into the undistorted normalized image plane.

Template Parameters
  • T: The floating point type used

  • T1: The input 2D point type used

  • T2: The output 2D point type used

Parameters
  • [in] camera_geometry: The CameraGeometryBase instance used to map the point

  • [in] pt_dist_img: The point in the distorted image plane

  • [out] pt_undist_norm: The mapped point in the undistorted normalized image plane

template<typename T, typename T1, typename T2, typename T3, typename T4>
void Metavision::vector_undist_norm_to_img(const CameraGeometryBase<T> &camera_geometry, const T1 &ctr_undist_norm, const T2 &vec_undist_norm, T3 &ctr_dist_img, T4 &vec_dist_img)

Maps a vector from the undistorted normalized image plane into the distorted image plane.

Note

The output vector is normalized

Template Parameters
  • T: The floating point type used

  • T1: The input 2D point type used

  • T2: The input 2D vector type used

  • T3: The output 2D point type used

  • T4: The output 2D vector type used

Parameters
  • [in] camera_geometry: The CameraGeometryBase instance used to map the vector

  • [in] ctr_undist_norm: The vector’s starting point in the undistorted normalized image plane

  • [in] vec_undist_norm: The vector in the undistorted normalized image plane (the vector must be normalized)

  • [out] ctr_dist_img: The vector’s starting point in the distorted image plane

  • [out] vec_dist_img: The mapped vector in the distorted image plane

template<typename T, typename T1, typename T2, typename T3, typename T4>
void Metavision::vector_img_to_undist_norm(const CameraGeometryBase<T> &camera_geometry, const T1 &ctr_dist_img, const T2 &vec_dist_img, T3 &ctr_undist_norm, T4 &vec_undist_norm)

Maps a vector from the distorted image plane into the undistorted normalized image plane.

Note

The output vector is normalized

Template Parameters
  • T: The floating point type used

  • T1: The input 2D point type used

  • T2: The input 2D vector type used

  • T3: The output 2D point type used

  • T4: The output 2D vector type used

Parameters
  • [in] camera_geometry: The CameraGeometryBase instance used to map the vector

  • [in] ctr_dist_img: The vector’s starting point in the distorted image plane

  • [in] vec_dist_img: The vector in the distorted image plane (the vector must be normalized)

  • [out] ctr_undist_norm: The vector’s starting point in the undistorted normalized image plane

  • [out] vec_undist_norm: The vector in the undistorted normalized image plane

template<typename T, typename T1, typename T2, typename T3>
void Metavision::get_undist_norm_to_img_jacobian(const CameraGeometryBase<T> &camera_geometry, const T1 &pt_undist_norm, T2 &pt_dist_img, T3 &J)

Computes the distortion function’s jacobian.

Template Parameters
  • T: The floating point type used

  • T1: The input 2D point type used

  • T2: The output 2D point type used

  • T3: The output 2x2 matrix type used

Parameters
  • [in] camera_geometry: The CameraGeometryBase instance used to map the point

  • [in] pt_undist_norm: The point in the undistorted normalized image plane at which the jacobian is computed

  • [out] pt_dist_img: The point in the distorted image plane

  • [out] J: The computed jacobian

template<typename T, typename T1, typename T2, typename T3>
void Metavision::get_img_to_undist_norm_jacobian(const CameraGeometryBase<T> &camera_geometry, const T1 &pt_dist_img, T2 &pt_undist_norm, T3 &J)

Computes the undistortion function’s jacobian.

Template Parameters
  • T: The floating point type used

  • T1: The input 2D point type used

  • T2: The output 2D point type used

  • T3: The output 2x2 matrix type used

Parameters
  • [in] camera_geometry: The CameraGeometryBase instance used to map the point

  • [in] pt_dist_img: The point in the distorted image plane at which the jacobian is computed

  • [out] pt_undist_norm: The point in the undistorted normalized image plane

  • [out] J: The computed jacobian

template<typename T, typename T1>
void Metavision::get_distortion_maps(const CameraGeometryBase<T> &camera_geometry, T1 &mapx, T1 &mapy)

Computes the distortion maps which are LUT used to distort coordinates.

The LUT are NxM matrices where NxM is the sensor’s resolution. The LUT must be allocated before calling this function.

Warning

Such maps are used with cv::remap to undistort an image.

Template Parameters
  • T: The floating point type used

  • T1: The output matrix type used

Parameters
  • [in] camera_geometry: The CameraGeometryBase instance used to map the point

  • [out] mapx: LUT giving the distorted x coordinate of the input undistorted point (integer coordinates)

  • [out] mapy: LUT giving the distorted y coordinate of the input undistorted point (integer coordinates)

template<typename T, typename T1>
void Metavision::get_undistortion_maps(const CameraGeometryBase<T> &camera_geometry, T1 &mapx, T1 &mapy)

Computes the undistortion maps which are LUT used to undistort coordinates.

The LUT are NxM matrices where NxM is the sensor’s resolution. The LUT must be allocated before calling this function.

Warning

Such maps are used with cv::remap to distort an image.

Template Parameters
  • T: The floating point type used

  • T1: The output matrix type used

Parameters
  • [in] camera_geometry: The CameraGeometryBase instance used to map the point

  • [out] mapx: LUT giving the undistorted x coordinate of the input distorted point (integer coordinates)

  • [out] mapy: LUT giving the undistorted y coordinate of the input distorted point (integer coordinates)

template<typename T>
std::unique_ptr<CameraGeometryBase<T>> Metavision::load_camera_geometry(const std::string &json_path)

Loads a camera geometry from a JSON file.

Return

The camera geometry in case of success or nullptr otherwise

Template Parameters
  • T: Either float or double

Parameters
  • json_path: Path to the JSON file containing the camera geometry

template<typename Scalar>
struct Metavision::GaussNewton::TerminationCriteria

Structure containing the parameters for deciding when to stop the minimization process.

Public Functions

TerminationCriteria() = default

Default constructor.

TerminationCriteria(unsigned int max_iterations, Scalar min_increment_norm, Scalar min_cost_decrease_fraction, Scalar max_cost_increase_fraction)

Initialization constructor.

Parameters
  • max_iterations: Threshold on the maximum number of iterations

  • min_increment_norm: Threshold on the minimum norm of the parameter increment

  • min_cost_decrease_fraction: Threshold on the minimum relative decrease of the cost

  • max_cost_increase_fraction: Threshold on the maximum relative increase of the cost

Public Members

unsigned int max_iterations = 10

Threshold on the maximum number of iterations.

Scalar min_increment_norm = 0

Threshold on the minimum norm of the parameter increment.

Scalar min_cost_decrease_fraction = 0

Threshold on the minimum relative decrease of the cost.

Scalar max_cost_increase_fraction = 1

Threshold on the maximum relative increase of the cost.

template<typename Scalar>
struct Metavision::GaussNewton::Report

Structure containing information summarizing a minimization process.

Public Types

enum State

Enumeration of the possible states of the solver.

Values:

enumerator MIN_INCREMENT_NORM_REACHED

(i.e. the optimization converged).

The threshold on the minimum norm of the parameter increment was reached

enumerator MIN_ERROR_DECREASE_FRACTION_REACHED

reached (i.e. the optimization converged).

The threshold on the minimum relative decrease of the cost was

enumerator MAX_ITERATIONS_REACHED

The threshold on the maximum number of iterations was reached.

enumerator MAX_ERROR_INCREASE_REACHED

the algorithm was unable to reduce the cost).

The threshold on the maximum relative increase of the cost was reached (i.e.

enumerator ARITHMETIC_FAILURE

it means the hessian matrix was singular).

An arithmetic failure happened in the course of the algorithm (most of the time,

enumerator EXTERNAL_FAILURE

happened in the course of the algorithm.

An external failure in the implementation of the AbstractLeastSquaresProblem<D,T>

enum IterationStatus

Enumerate used to define the status of a minimization’s iteration.

A minimization’s iteration’s status might consist in the combination of several of these values

Values:

enumerator JACOBIAN_VECTOR_ESTIMATION_FAILED = 0x1

It was not possible to compute the cost or the Jacobian.

enumerator NORMAL_EQUATION_COMPUTATION_FAILED = 0x2

It was not possible to solve the Gauss-Newton’s normal equation.

enumerator PARAMETER_ACCUMULATION_FAILED = 0x4

It was not possible to accumulate the parameter increment.

enumerator COST_DECREASED = 0x8

The cost decreased during this iteration.

enumerator COST_INCREASED = 0x10

The cost increased during this iteration.

enumerator LAST_COST_COMPUTATION_FAILED = 0x20

It was not possible to compute the cost at the last step of the minimization process

enumerator LAST_COST_DECREASED = 0x40

The cost decreased at the last step of the minimization process.

enumerator LAST_COST_INCREASED = 0x80

The cost increased at the last step of the minimization process.

enumerator LAST_COST_COMPUTATION = LAST_COST_COMPUTATION_FAILED | LAST_COST_DECREASED | LAST_COST_INCREASED

Tells whether the iteration corresponds to the last cost computation or not

Public Functions

Report()

Constructor.

void reset()

Resets the report.

IterationReport &get_new_iter_report()

Adds a new iteration report to the minimization process’s report.

Return

The newly added iteration report

std::ostream &print(std::ostream &stream) const

Prints the minimization process’s report.

Return

The stream in which the report has been printed

Parameters
  • stream: Stream to print the report into

Friends

friend std::ostream &operator<<(std::ostream &stream, const Report &report)

Override of the operator<< to print the minimization process’s report into streams.

Return

The stream in which the report has been printed

Parameters
  • stream: Stream to print the report into

  • report: Report to print into the input stream

struct IterationReport

Structure containing information summarizing a minimization’s iteration.

Public Members

int status = 0

The iteration’s status.

Scalar error

Cost computed during this iteration.

Scalar delta_error

Ratio (best_cost - new_cost) / best_cost.

Scalar sum_inc_norm

Infinity norm of the sum of all the parameters increments computed so far.

Scalar inc_norm

Infinity norm of the parameters increment computed during this iteration.

Scalar cost_inc_frac

Ratio (new_cost / best_cost)

bool is_result_nan

This flag is used in case of failure to determine if the parameters increment is zero (false) or NAN (true)

template<typename Impl, typename Scalar, int DIM, template<typename I, typename S, int D> typename LeastSquaresProblemBase>
void Metavision::GaussNewton::solve(LeastSquaresProblemBase<Impl, Scalar, DIM> &problem, Report<Scalar> &report, const TerminationCriteria<Scalar> &term_criteria = TerminationCriteria<Scalar>())

Solves a non linear optimization problem using the Gauss-Newton method.

Template Parameters
  • Impl: Type of the underlying least squares problem to solve

  • Scalar: Either float or double

  • DIM: Dimensions of the parameters to optimize

  • LeastSquaresProblemBase: Base least squares problem type, the CRTP design is used here

Parameters
  • problem: Least squares problem to solve

  • report: Final summary of the optimization problem

  • term_criteria: Termination criteria used for deciding when to stop the minimization process

template<typename Impl, typename FloatType, int D>
class Metavision::LeastSquaresProblemBase

Base class for least-square problems to be solved using one of the generic solver implementations.

Template Parameters
  • Impl: This class uses a Curiously Recursive Template Pattern design to avoid the cost of virtual methods. The input template parameter is expected to inherit from this class and this class should be declared as friend of the derived class.

  • FloatType: Either float or double

  • D: Dimension of the parameters vector to optimize

Public Functions

bool get_updated_innovation_vector(unsigned int iteration, Scalar &cost, CostJacobian *cost_jacobian, Hessian *cost_hessian)

Computes the updated sum of squared residuals, innovation vector and hessian matrix.

The innovation vector and hessian matrix are only required if non-NULL pointers are provided as input arguments

Return

true if the function succeeded, false if a fatal error occurred and the optimization algorithm should stop.

Warning

The behavior of this method must be implemented by implementing a get_updated_innovation_vector_impl method in the derived class.

Parameters
  • [in] iteration: Integer indicating the current iteration

  • [out] cost: Reference of the value where the cost function evaluated at the current estimate should be stored

  • [out] cost_jacobian: Pointer to the DIMx1 vector where the Jacobian of the cost function evaluated at the current estimate should be stored. May be NULL, in which case the innovation vector is not needed.

  • [out] cost_hessian: Pointer to the DxD matrix where the hessian of the cost function evaluated at the current estimate should be stored. May be NULL, in which case the hessian matrix is not needed.

bool accumulate_parameter_increment(const Parameters &parameter_increment)

Function to pass the estimated parameter increment and accumulate it in the parameter vector.

Return

true if the function succeeded, false if a fatal error occurred and the optimization algorithm should stop

Warning

The behavior of this method must be implemented by implementing an accumulate_parameter_increment_impl method in the derived class.

Parameters
  • [in] parameter_increment: Best parameter increment, estimated at the end of the current iteration, to be accumulated with the current parameter estimate

bool cancel_parameter_increment(const Parameters &parameter_increment)

Function to notify that the last parameter increment (given again as input argument) was not conclusive and should be canceled.

Return

true if the function succeeded, false if a fatal error occurred and the optimization algorithm should stop

Note

The behavior of this method can be redefined by implementing a cancel_parameter_increment_impl method in the derived class. Calls accumulate_parameter_increment with -parameter_increment by default.

Parameters
  • [in] parameter_increment: Last accumulated parameter increment to be canceled

void notify_new_best_estimate(unsigned int iteration)

Function to notify that the parameter estimate obtained after the last increment is the best one so far.

Note

The behavior of this method can be redefined by implementing a notify_new_best_estimate_impl method in the derived class. Does nothing by default.

Parameters
  • iteration: Integer indicating the current iteration

void notify_starting_minimization()

Notifies that the minimization is starting.

Note

The behavior of this method can be redefined by implementing a notify_starting_minimization_impl method in the derived class. Does nothing by default.

void notify_ending_minimization()

Notifies that the minimization is ending.

Note

The behavior of this method can be redefined by implementing a notify_ending_minimization_impl method in the derived class. Does nothing by default.

void notify_last_jacobian_and_hessian(const CostJacobian &jacobian, const Hessian &hessian)

In case of minimization success this function notifies the Jacobian and Hessian matrices corresponding to the best estimate.

Note

The behavior of this method can be redefined by implementing a notify_last_jacobian_and_hessian_impl method in the derived class. Does nothing by default.

Parameters
  • [in] jacobian: Jacobian of the cost function evaluated at the best estimate

  • [in] hessian: Hessian of the cost function evaluated at the best estimate