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

Public Functions

inline virtual const Vec2i &get_image_size() const final

Gets the sensor’s size.

inline virtual T get_distance_to_image_plane() const final

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

inline virtual std::unique_ptr<CameraGeometryBase<T>> clone() const final

Creates a deep copy of this instance.

Returns

The new instance

inline virtual void get_undist_norm_to_undist_img_transform(Mat3RMRef m) const final

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

m – [out] The transform

inline virtual void get_undist_norm_to_undist_img_transform(Mat3CMRef m) const final

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

m – [out] The transform

inline virtual void camera_to_img(Vec3RefConst pt_c, Vec2Ref pt_dist_img) const final

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

Parameters

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

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

inline virtual void camera_to_undist_img(Vec3RefConst pt_c, Vec2Ref pt_dist_img) const final

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

Parameters

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

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

inline virtual void undist_norm_to_undist_img(Vec2RefConst pt_undist_norm, Vec2Ref pt_undist_img) const final

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

Parameters

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

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

inline virtual void undist_norm_to_dist_norm(Vec2RefConst pt_undist_norm, Vec2Ref pt_dist_norm) const final

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

Parameters

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

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

inline virtual void undist_img_to_undist_norm(Vec2RefConst pt_undist_img, Vec2Ref pt_undist_norm) const final

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

Parameters

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

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

inline virtual void undist_norm_to_img(Vec2RefConst pt_undist_norm, Vec2Ref pt_dist_img) const final

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

Parameters

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

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

inline virtual void img_to_undist_norm(Vec2RefConst pt_dist_img, Vec2Ref pt_undist_norm) const final

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

Parameters

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

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

inline virtual void vector_undist_norm_to_img(Vec2RefConst ctr_undist_norm, Vec2RefConst vec_undist_norm, Vec2Ref ctr_dist_img, Vec2Ref vec_dist_img) const final

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

Note

The output vector is normalized

Parameters

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

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

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

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

inline virtual void vector_img_to_undist_norm(Vec2RefConst ctr_dist_img, Vec2RefConst vec_dist_img, Vec2Ref ctr_undist_norm, Vec2Ref vec_undist_norm) const final

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

Note

The output vector is normalized

Parameters

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

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

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

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

inline virtual void get_undist_norm_to_img_jacobian(Vec2RefConst pt_undist_norm, Vec2Ref pt_dist_img, Mat2RMRef J) const final

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

Parameters

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

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

  • J – [out] The computed jacobian

inline virtual void get_undist_norm_to_img_jacobian(Vec2RefConst pt_undist_norm, Vec2Ref pt_dist_img, Mat2CMRef J) const final

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

Parameters

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

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

  • J – [out] The computed jacobian

inline virtual void get_img_to_undist_norm_jacobian(Vec2RefConst pt_dist_img, Vec2Ref pt_undist_norm, Mat2RMRef J) const final

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

Parameters

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

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

  • J – [out] The computed jacobian

inline virtual void get_img_to_undist_norm_jacobian(Vec2RefConst pt_dist_img, Vec2Ref pt_undist_norm, Mat2CMRef J) const final

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

Parameters

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

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

  • J – [out] The computed jacobian

inline virtual void accept(CameraGeometryVisitor<T> &visitor) const final

Accepts a camera geometry visitor for implementation-specific processing.

Parameters

visitor – The visitor to accept

template<typename T>
class 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

virtual ~CameraGeometryBase() = default

Default destructor.

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

Creates a deep copy of this instance.

Returns

The new instance

virtual const Vec2i &get_image_size() const = 0

Gets the sensor’s size.

virtual T get_distance_to_image_plane() const = 0

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

virtual 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

m – [out] The transform

virtual 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

m – [out] The transform

virtual 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

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

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

virtual 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

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

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

virtual 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

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

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

virtual 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

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

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

virtual 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

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

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

virtual 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

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

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

virtual 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

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

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

virtual 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

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

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

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

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

virtual 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

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

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

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

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

virtual 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

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

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

  • J – [out] The computed jacobian

virtual 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

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

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

  • J – [out] The computed jacobian

virtual 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

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

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

  • J – [out] The computed jacobian

virtual 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

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

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

  • J – [out] The computed jacobian

virtual void accept(CameraGeometryVisitor<T> &visitor) const = 0

Accepts a camera geometry visitor for implementation-specific processing.

Parameters

visitor – The visitor to accept

template<typename FloatType>
class 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

  • img_size – [in] Sensor’s size

  • poly – [in] Polynomial used to undistort coordinates

  • inv_poly – [in] Polynomial used to distort coordinates

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

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

  • zoom_factor – [in] 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

m – [out] The transform

template<typename V1, typename V2>
inline 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

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

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

template<typename V1, typename V2>
inline 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

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

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

template<typename V1, typename V2>
inline 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

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

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

template<typename V1, typename V2>
inline 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

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

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

template<typename V1, typename V2>
inline 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

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

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

template<typename V1, typename V2>
inline 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

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

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

template<typename V1, typename V2>
inline 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

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

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

template<typename V1, typename V2, typename M>
inline 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

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

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

  • J – [out] The computed jacobian

template<typename V1, typename V2, typename M>
inline 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

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

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

  • J – [out] The computed jacobian

inline const VecX &P() const

Gets the polynomial used for undistortion.

Returns

The polynomial used for undistortion

inline const VecX &i_P() const

Gets the polynomial used for distortion.

Returns

The polynomial used for distortion

inline const Mat2CM &A() const

Gets the affine transform that maps from the ideal sensor plane to the image plane.

Returns

The affine transform

inline const Vec2 &center() const

Gets the projection of the camera’s optical center in the image plane.

Returns

The projection of the camera’s optical center in the image plane

inline FloatType zoom_factor() const

Gets the undistorted scale factor.

Returns

The undistorted scale factor

template<typename FloatType>
class StereoCameraGeometry

Class representing the stereo camera geometry.

Note

The stereo camera geometry implementation only supports pinhole camera models

Template Parameters

FloatType – The floating point type used for the computations

Public Functions

StereoCameraGeometry(const CamGeometry &model_master, const CamGeometry &model_slave, Mat4RefConst T_s_m, bool initialize_rectified_geometry = false)

Constructor.

Parameters

  • model_master – The pinhole camera geometry of the master camera

  • model_slave – The pinhole camera geometry of the slave camera

  • T_s_m – The 4x4 matrix mapping 3d points from the master camera coordinates to the slave camera coordinates

  • initialize_rectified_geometry – Whether to automatically initialize the rectified geometry or not

StereoCameraGeometry(const CamGeometry &model_master, const CamGeometry &model_slave, Mat4RefConst T_s_m, Mat3RefConst R_rect_master, Mat3RefConst K_rect_master, Mat3RefConst R_rect_slave, Mat3RefConst K_rect_slave)

Constructor.

Parameters

  • model_master – The pinhole camera geometry of the master camera

  • model_slave – The pinhole camera geometry of the slave camera

  • T_s_m – The 4x4 matrix mapping 3d points from the master camera coordinates to the slave camera coordinates

  • R_rect_master – The 3x3 rotation matrix mapping from regular camera coordinates to rectified camera coordinates for the master camera

  • K_rect_master – The 3x3 rotation matrix mapping from rectified normalized coordinates to rectified image coordinates for the master camera

  • R_rect_slave – The 3x3 rotation matrix mapping from regular camera coordinates to rectified camera coordinates for the slave camera

  • K_rect_slave – The 3x3 rotation matrix mapping from rectified normalized coordinates to rectified image coordinates for the slave camera

void initialize_rectified_geometry()

Initializes the stereo rectified geometry.

Note

This function does nothing if the rectified geometry is already initialized.

void initialize_rectified_geometry(Mat3RefConst R_rect_master, Mat3RefConst K_rect_master, Mat3RefConst R_rect_slave, Mat3RefConst K_rect_slave)

Reinitializes the stereo rectified geometry from the given matrices.

Note

this function resets and reinitializes the rectified geometry even if it was already initialized.

Parameters

  • R_rect_master – The 3x3 rotation matrix mapping from regular camera coordinates to rectified camera coordinates for the master camera

  • K_rect_master – The 3x3 rotation matrix mapping from rectified normalized coordinates to rectified image coordinates for the master camera

  • R_rect_slave – The 3x3 rotation matrix mapping from regular camera coordinates to rectified camera coordinates for the slave camera

  • K_rect_slave – The 3x3 rotation matrix mapping from rectified normalized coordinates to rectified image coordinates for the slave camera

void reset_rectified_geometry()

Resets the stereo rectified geometry.

StereoCameraGeometry clone() const

Clones the stereo camera geometry.

const CamGeometry &proj_master() const

Returns the pinhole camera geometry of the master camera.

const CamGeometry &proj_slave() const

Returns the pinhole camera geometry of the slave camera.

const RectifiedStereoGeometry &rect()

Returns the stereo rectified geometry.

Note

This function initializes the rectified geometry if not previously done.

const RectifiedStereoGeometry &rect() const

Returns the stereo rectified geometry.

Throws

std::runtime_error – if the rectified geometry has not been initialized

const RectifiedCameraGeometry &rect_master()

Returns the rectified stereo geometry of the master camera.

Note

This function initializes the rectified geometry if not previously done.

const RectifiedCameraGeometry &rect_master() const

Returns the rectified stereo geometry of the master camera.

Throws

std::runtime_error – if the rectified geometry has not been initialized

const RectifiedCameraGeometry &rect_slave()

Returns the rectified stereo geometry of the slave camera.

Note

This function initializes the rectified geometry if not previously done.

const RectifiedCameraGeometry &rect_slave() const

Returns the rectified stereo geometry of the slave camera.

Throws

std::runtime_error – if the rectified geometry has not been initialized

Mat3 R_s_m() const

Returns the 3x3 rotation matrix mapping 3d points from the master camera coordinates to the slave camera.

Vec3 t_s_m() const

Returns the 3x1 translation vector mapping 3d points from the master camera coordinates to the slave camera.

Vec3 pos_s_m() const

Returns the 3d position of the optical center of the slave camera expressed in the master camera coordinates.

Mat3 R_m_s() const

Returns the 3x3 rotation matrix mapping 3d points from the slave camera coordinates to the master camera.

Vec3 t_m_s() const

Returns the 3x1 translation vector mapping 3d points from the slave camera coordinates to the master camera.

Vec3 pos_m_s() const

Returns the 3d position of the optical center of the master camera expressed in the slave camera coordinates.

const Mat4 &T_s_m() const

Returns the 4x4 transformation matrix mapping 3d points from the master camera coordinates to the slave camera coordinates.

Mat4 T_m_s() const

Returns the 4x4 transformation matrix mapping 3d points from the slave camera coordinates to the master camera coordinates.

void camera_master_to_camera_slave(Vec3RefConst pt3_master, Vec3Ref pt3_slave) const

Maps a 3d point from the master camera coordinates to the slave camera coordinates.

void camera_slave_to_camera_master(Vec3RefConst pt3_slave, Vec3Ref pt3_master) const

Maps a 3d point from the slave camera coordinates to the master camera coordinates.

struct RectifiedCameraGeometry

Class representing the rectified geometry for a given camera.

Public Functions

explicit EIGEN_MAKE_ALIGNED_OPERATOR_NEW RectifiedCameraGeometry(const CamModel &proj_model)

Constructor.

Parameters

proj_model – The pinhole camera model

RectifiedCameraGeometry(const CamModel &proj_model, Mat3RefConst R_rect_undist_norm, Mat3RefConst K_rect, const cv::Mat &undist_rect_map1, const cv::Mat &undist_rect_map2)

Constructor.

Parameters

  • proj_model – The pinhole camera model

  • R_rect_undist_norm – The 3x3 rotation matrix mapping from regular undistorted normalized coordinates to rectified normalized coordinates.

  • K_rect – The 3x3 projection matrix mapping from rectified normalized coordinates to rectified image coordinates.

  • undist_rect_map1 – The first map to wrap regular images into rectified images.

  • undist_rect_map2 – The second map to wrap regular images into rectified images.

void initialize(Mat3RefConst R_rect_undist_norm, Mat3RefConst K_rect, const cv::Mat &undist_rect_map1, const cv::Mat &undist_rect_map2)

Initializes the rectified geometry.

Parameters

  • R_rect_undist_norm – The 3x3 rotation matrix mapping from regular undistorted normalized coordinates to rectified normalized coordinates.

  • K_rect – The 3x3 projection matrix mapping from rectified normalized coordinates to rectified image coordinates.

  • undist_rect_map1 – The first map to wrap regular images into rectified images.

  • undist_rect_map2 – The second map to wrap regular images into rectified images.

int width() const

Returns the width of the rectified image.

int height() const

Returns the height of the rectified image.

const Mat3 &R() const

Returns the 3x3 rotation matrix mapping from regular undistorted normalized coordinates to rectified normalized coordinates.

const Mat3 &K() const

Returns the 3x3 projection matrix mapping from rectified camera coordinates to rectified undistorted image coordinates.

cv::Mat_<uchar> get_mask(int border_size = 0) const

Returns the mask representing valid pixels in the rectified image, with an optional disabled border.

Parameters

border_size – The size of the border to disable in the returned rectified image mask.

Returns

The rectified image mask

void img_to_rect_undist_img(const cv::Mat &map_img, cv::Mat &map_rect_undist_img) const

Remaps a regular image to a rectified image.

Parameters

  • map_img – The regular image to remap

  • map_rect_undist_img – The remapped rectified image

void img_to_rect_undist_img(Vec2RefConst pt2_img, Vec2Ref pt2_rect_undist_img) const

Maps a 2d point from regular image coordinates to rectified image coordinates.

void rect_undist_img_to_img(Vec2RefConst pt2_rect_undist_img, Vec2Ref pt2_img) const

Maps a 2d point from rectified image coordinates to regular image coordinates.

void camera_to_rect_camera(Vec3RefConst pt3_camera, Vec3Ref pt3_rect) const

Maps a 3d point from camera coordinates to rectified camera coordinates.

void rect_camera_to_camera(Vec3RefConst pt3_rect, Vec3Ref pt3_camera) const

Maps a 3d point from rectified camera coordinates to camera coordinates.

void rect_camera_to_rect_undist_img(Vec3RefConst pt3_rect, Vec2Ref pt2_rect_undist_img) const

Projects a 3d point from camera coordinates into the corresponding 2d point in rectified image coordinates.

void rect_undist_img_to_rect_undist_norm(Vec2RefConst pt2_rect_undist_img, Vec2Ref pt2_rect_undist_norm) const

Maps a 2d point from rectified image coordinates to rectified normalized coordinates.

struct RectifiedStereoGeometry

Class representing the rectified stereo geometry for a given camera pair.

Public Functions

RectifiedStereoGeometry(const CamGeometry &proj_model_master, const CamGeometry &proj_model_slave, const Mat4 &T_s_m)

Constructor.

Parameters

  • proj_model_master – The pinhole camera geometry of the master camera

  • proj_model_slave – The pinhole camera geometry of the slave camera

  • T_s_m – The 4x4 matrix mapping 3d points from the master camera coordinates to the slave camera coordinates

RectifiedStereoGeometry(const CamGeometry &proj_model_master, const CamGeometry &proj_model_slave, const Mat4 &T_s_m, Mat3RefConst R_rect_master, Mat3RefConst K_rect_master, Mat3RefConst R_rect_slave, Mat3RefConst K_rect_slave)

Constructor.

Parameters

  • proj_model_master – The pinhole camera geometry of the master camera

  • proj_model_slave – The pinhole camera geometry of the slave camera

  • T_s_m – The 4x4 matrix mapping 3d points from the master camera coordinates to the slave camera coordinates

  • R_rect_master – The 3x3 rotation matrix mapping from regular camera coordinates to rectified camera coordinates for the master camera

  • K_rect_master – The 3x3 rotation matrix mapping from rectified normalized coordinates to rectified image coordinates for the master camera

  • R_rect_slave – The 3x3 rotation matrix mapping from regular camera coordinates to rectified camera coordinates for the slave camera

  • K_rect_slave – The 3x3 rotation matrix mapping from rectified normalized coordinates to rectified image coordinates for the slave camera

const RectifiedCameraGeometry &master() const

Returns the rectified geometry of the master camera.

const RectifiedCameraGeometry &slave() const

Returns the rectified geometry of the slave camera.

bool is_rectified_horizontally() const

Indicates whether the rectification is done horizontally.

FloatType disparity_sign() const

returns the sign of valid disparity values.

void rect_camera_master_to_rect_camera_slave(Vec3RefConst pt3_rectmaster, Vec3Ref pt3_rectslave) const

Maps a 3d point from master rectified camera coordinates to slave rectified camera coordinates.

void rect_camera_slave_to_rect_camera_master(Vec3RefConst pt3_rectslave, Vec3Ref pt3_rectmaster) const

Maps a 3d point from slave rectified camera coordinates to master rectified camera coordinates.

FloatType disparity_to_depth(FloatType disp_master) const

Converts a disparity value in master rectified image coordinates to a depth value in master rectified camera coordinates.

Note

Invalid disparities, recognized as std::numeric_limits<FloatType>::quiet_NaN(), are converted to a depth of -1.f.

void disparity_to_xyz(FloatType disp_master, FloatType pt2_rect_undist_img_x_master, FloatType pt2_rect_undist_img_y_master, Vec3Ref pt3_rect_master) const

Converts a disparity value and 2d point in master rectified image coordinates to a 3d point in master rectified camera coordinates.

Note

Invalid disparities, recognized as std::numeric_limits<FloatType>::quiet_NaN(), are converted to a point with depth -1.f.

void disparity_map_to_depth_map(const cv::Mat_<float> &disp_map_master, cv::Mat_<float> &depth_map_master) const

Converts a disparity map in master rectified image coordinates to a depth map in master rectified camera coordinates.

Note

Invalid disparities, recognized as std::numeric_limits<FloatType>::quiet_NaN(), are converted to a depth of -1.f.

void disparity_map_to_camera_point_cloud(const cv::Mat_<float> &disp_map_master, std::vector<Vec3> &point_cloud_master) const

Converts a disparity map in master rectified image coordinates to a 3d point cloud in master regular camera coordinates.

Note

Invalid disparities, recognized as std::numeric_limits<FloatType>::quiet_NaN(), are ignored.

template<typename FloatType>
class 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

  • width – [in] Sensor’s width

  • height – [in] Sensor’s height

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

  • D – [in] 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

m – [out] The transform

template<typename V1, typename V2>
inline 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

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

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

template<typename V1, typename V2>
inline 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

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

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

template<typename V1, typename V2>
inline 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

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

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

template<typename V1, typename V2>
inline 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

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

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

template<typename V1, typename V2>
inline 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

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

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

template<typename V1, typename V2>
inline 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

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

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

template<typename V1, typename V2>
inline 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

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

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

template<typename V1, typename V2, typename M>
inline 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

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

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

  • J – [out] The computed jacobian

template<typename V1, typename V2, typename M>
inline 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

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

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

  • J – [out] The computed jacobian

template<typename T, typename Ti, typename To>
inline 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

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

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

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

template<typename Ti, typename To>
inline 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

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

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

template<typename Ti, typename To>
inline 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.

Warning

The output v2_undist_norm could be inf if \(v3^{cam}_z + pt3^{cam}_z = 0\).

Template Parameters

  • Ti – The input 3D vector type used

  • To – The output 2D vector type used

Parameters

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

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

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

  • pt2_undist_norm – [out] 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>
inline 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

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

  • m – [out] The transform

template<typename T, typename T1, typename T2>
inline 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

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

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

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

template<typename T, typename T1, typename T2>
inline 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.

Template Parameters

  • T – The floating point type used

  • T1 – The 3D point type used

  • T2 – The 2D point type used

Parameters

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

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

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

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

Returns

true if the point has been successfully mapped, false otherwise

template<typename T, typename T1, typename T2>
inline 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

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

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

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

template<typename T, typename T1, typename T2>
inline 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

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

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

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

template<typename T, typename T1, typename T2>
inline 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

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

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

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

template<typename T, typename T1, typename T2>
inline 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

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

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

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

template<typename T, typename T1, typename T2>
inline 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

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

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

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

template<typename T, typename T1, typename T2, typename T3, typename T4>
inline 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

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

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

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

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

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

template<typename T, typename T1, typename T2, typename T3, typename T4>
inline 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

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

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

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

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

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

template<typename T, typename T1, typename T2, typename T3>
inline 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

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

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

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

  • J – [out] The computed jacobian

template<typename T, typename T1, typename T2, typename T3>
inline 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

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

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

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

  • J – [out] The computed jacobian

template<typename T, typename T1>
inline 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.

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

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

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

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

template<typename T, typename T1>
inline 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.

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

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

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

  • mapy – [out] 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::filesystem::path &json_path)

Loads a camera geometry from a JSON file.

Template Parameters

T – Either float or double

Parameters

json_path – Path to the JSON file containing the camera geometry

Returns

The camera geometry in case of success or nullptr otherwise

template<typename T>
std::unique_ptr<StereoCameraGeometry<T>> Metavision::load_stereo_camera_geometry(const std::filesystem::path &json_path, bool initialize_rectified_geometry = false)

Loads a stereo camera geometry from a JSON file.

Template Parameters

T – Either float or double

Parameters

  • json_path – Path to the JSON file containing the stereo camera geometry

  • initialize_rectified_geometry – If true, the rectified geometry will be initialized

Returns

The stereo camera geometry in case of success, or nullptr otherwise

template<typename Scalar>
struct TerminationCriteria

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

Public Functions

TerminationCriteria() = default

Default constructor.

inline 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 Report

Structure containing information summarizing a minimization process.

Public Types

enum class 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

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

enumerator NORMAL_EQUATION_COMPUTATION_FAILED

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

enumerator PARAMETER_ACCUMULATION_FAILED

It was not possible to accumulate the parameter increment.

enumerator COST_DECREASED

The cost decreased during this iteration.

enumerator COST_INCREASED

The cost increased during this iteration.

enumerator LAST_COST_COMPUTATION_FAILED

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

enumerator LAST_COST_DECREASED

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

enumerator LAST_COST_INCREASED

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

enumerator LAST_COST_COMPUTATION

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

Public Functions

inline Report()

Constructor.

inline void reset()

Resets the report.

inline IterationReport &get_new_iter_report()

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

Returns

The newly added iteration report

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

Prints the minimization process’s report.

Parameters

stream – Stream to print the report into

Returns

The stream in which the report has been printed

Friends

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

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

Parameters

  • stream – Stream to print the report into

  • report – Report to print into the input stream

Returns

The stream in which the report has been printed

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 Scalar, int DIM, template<typename I, typename S, int D> typename LeastSquaresProblemBase>
bool Metavision::GaussNewton::solve(LeastSquaresProblemBase<Impl, Scalar, DIM> &problem, 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

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

Returns

True if the minimization process succeeded, false otherwise

template<typename Impl, typename FloatType, int D>
class 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

inline 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

Warning

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

Parameters

  • iteration – [in] Integer indicating the current iteration

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

  • cost_jacobian – [out] 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.

  • cost_hessian – [out] 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.

Returns

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

inline bool accumulate_parameter_increment(const Parameters &parameter_increment)

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

Warning

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

Parameters

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

Returns

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

inline 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.

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

parameter_increment – [in] Last accumulated parameter increment to be canceled

Returns

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

inline 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

inline 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.

inline 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.

inline 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

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

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