Struct EucmCamera 

pub struct EucmCamera {
    pub pinhole: PinholeParams,
    pub distortion: DistortionModel,
}

Extended Unified Camera Model with 6 parameters.

Fields

pinhole: PinholeParams

distortion: DistortionModel

Implementations

impl EucmCamera

pub fn new( pinhole: PinholeParams, distortion: DistortionModel, ) -> Result<EucmCamera, CameraModelError>

Create a new Extended Unified Camera Model (EUCM) camera.

Arguments

• pinhole - Pinhole parameters (fx, fy, cx, cy).
• distortion - MUST be DistortionModel::EUCM with alpha and beta.

Returns

Returns a new EucmCamera instance if the distortion model matches.

Errors

Returns CameraModelError::InvalidParams if distortion is not DistortionModel::EUCM.

pub fn linear_estimation( &mut self, points_3d: &Matrix<f64, Const<3>, Dyn, VecStorage<f64, Const<3>, Dyn>>, points_2d: &Matrix<f64, Const<2>, Dyn, VecStorage<f64, Const<2>, Dyn>>, ) -> Result<(), CameraModelError>

Performs linear estimation to initialize distortion parameters from point correspondences.

This method estimates the alpha parameter using a linear least squares approach given 3D-2D point correspondences. The beta parameter is fixed to 1.0. It assumes the intrinsic parameters (fx, fy, cx, cy) are already set.

Arguments

• points_3d: Matrix3xX - 3D points in camera coordinates (each column is a point)
• points_2d: Matrix2xX - Corresponding 2D points in image coordinates

Returns

Returns Ok(()) on success or a CameraModelError if the estimation fails.

Trait Implementations

impl CameraModel for EucmCamera

fn project( &self, p_cam: &Matrix<f64, Const<3>, Const<1>, ArrayStorage<f64, 3, 1>>, ) -> Result<Matrix<f64, Const<2>, Const<1>, ArrayStorage<f64, 2, 1>>, CameraModelError>

Projects a 3D point to 2D image coordinates.

Mathematical Formula

d = √(β(x² + y²) + z²)
denom = α·d + (1-α)·z
u = fx · (x/denom) + cx
v = fy · (y/denom) + cy

Arguments

• p_cam - 3D point in camera coordinate frame.

Returns

• Ok(uv) - 2D image coordinates if valid.

Errors

Returns CameraModelError::InvalidParams if the geometric projection condition fails or the denominator is too small.

fn unproject( &self, point_2d: &Matrix<f64, Const<2>, Const<1>, ArrayStorage<f64, 2, 1>>, ) -> Result<Matrix<f64, Const<3>, Const<1>, ArrayStorage<f64, 3, 1>>, CameraModelError>

Unprojects a 2D image point to a 3D ray.

Algorithm

Algebraic solution using EUCM inverse equations with α and β parameters.

Arguments

• point_2d - 2D point in image coordinates.

Returns

• Ok(ray) - Normalized 3D ray direction.

Errors

Returns CameraModelError::PointOutsideImage if the unprojection condition fails. Returns CameraModelError::NumericalError if a division by zero occurs during calculation.

fn jacobian_point( &self, p_cam: &Matrix<f64, Const<3>, Const<1>, ArrayStorage<f64, 3, 1>>, ) -> <EucmCamera as CameraModel>::PointJacobian

Jacobian of projection w.r.t. 3D point coordinates (2×3).

Computes ∂π/∂p where π is the projection function and p = (x, y, z) is the 3D point.

Mathematical Derivation

For the EUCM camera model, projection is defined as:

r² = x² + y²
d = √(β·r² + z²)
denom = α·d + (1-α)·z
u = fx · (x/denom) + cx
v = fy · (y/denom) + cy

Jacobian Structure

Computing ∂u/∂p and ∂v/∂p where p = (x, y, z):

J_point = [ ∂u/∂x  ∂u/∂y  ∂u/∂z ]
          [ ∂v/∂x  ∂v/∂y  ∂v/∂z ]

Chain rule application for u = fx · (x/denom) + cx and v = fy · (y/denom) + cy:

∂(x/denom)/∂x = (denom - x·∂denom/∂x) / denom²
∂(x/denom)/∂y = -x·∂denom/∂y / denom²
∂(x/denom)/∂z = -x·∂denom/∂z / denom²

∂(y/denom)/∂x = -y·∂denom/∂x / denom²
∂(y/denom)/∂y = (denom - y·∂denom/∂y) / denom²
∂(y/denom)/∂z = -y·∂denom/∂z / denom²

Computing ∂d/∂p where d = √(β·r² + z²):

∂d/∂x = ∂/∂x √(β·(x²+y²) + z²)
      = (1/2) · (β·r² + z²)^(-1/2) · 2β·x
      = β·x / d

∂d/∂y = β·y / d
∂d/∂z = z / d

Computing ∂denom/∂p where denom = α·d + (1-α)·z:

∂denom/∂x = α · ∂d/∂x = α·β·x/d
∂denom/∂y = α · ∂d/∂y = α·β·y/d
∂denom/∂z = α · ∂d/∂z + (1-α) = α·z/d + (1-α)

Final Jacobian (substituting into quotient rule):

∂u/∂x = fx · (denom - x·α·β·x/d) / denom²
∂u/∂y = fx · (-x·α·β·y/d) / denom²
∂u/∂z = fx · (-x·(α·z/d + 1-α)) / denom²

∂v/∂x = fy · (-y·α·β·x/d) / denom²
∂v/∂y = fy · (denom - y·α·β·y/d) / denom²
∂v/∂z = fy · (-y·(α·z/d + 1-α)) / denom²

Arguments

• p_cam - 3D point in camera coordinate frame.

Returns

Returns the 2x3 Jacobian matrix.

References

• Khomutenko et al., “An Enhanced Unified Camera Model”, RAL 2016
• Mei & Rives, “Single View Point Omnidirectional Camera Calibration from Planar Grids”, ICRA 2007

Numerical Verification

This analytical Jacobian is verified against numerical differentiation in test_jacobian_point_numerical() with tolerance < 1e-5.

fn jacobian_intrinsics( &self, p_cam: &Matrix<f64, Const<3>, Const<1>, ArrayStorage<f64, 3, 1>>, ) -> <EucmCamera as CameraModel>::IntrinsicJacobian

Jacobian of projection w.r.t. intrinsic parameters (2×6).

Mathematical Derivation

The EUCM camera has 6 intrinsic parameters: [fx, fy, cx, cy, α, β]

Projection Equations

u = fx · (x/denom) + cx
v = fy · (y/denom) + cy

where denom = α·d + (1-α)·z and d = √(β·r² + z²).

Jacobian Structure

Intrinsic Jacobian (2×6):

J = [ ∂u/∂fx  ∂u/∂fy  ∂u/∂cx  ∂u/∂cy  ∂u/∂α  ∂u/∂β ]
    [ ∂v/∂fx  ∂v/∂fy  ∂v/∂cx  ∂v/∂cy  ∂v/∂α  ∂v/∂β ]

Linear Parameters (fx, fy, cx, cy)

These appear linearly in the projection equations:

∂u/∂fx = x/denom,   ∂u/∂fy = 0,         ∂u/∂cx = 1,   ∂u/∂cy = 0
∂v/∂fx = 0,         ∂v/∂fy = y/denom,   ∂v/∂cx = 0,   ∂v/∂cy = 1

Distortion Parameter α

The parameter α affects denom = α·d + (1-α)·z. Taking derivative:

∂denom/∂α = d - z

Using the quotient rule for u = fx·(x/denom) + cx:

∂u/∂α = fx · ∂(x/denom)/∂α
      = fx · (-x · ∂denom/∂α) / denom²
      = -fx · x · (d - z) / denom²

∂v/∂α = -fy · y · (d - z) / denom²

Distortion Parameter β

The parameter β affects d = √(β·r² + z²). Taking derivative:

∂d/∂β = ∂/∂β √(β·r² + z²)
      = (1/2) · (β·r² + z²)^(-1/2) · r²
      = r² / (2d)

Chain rule through denom = α·d + (1-α)·z:

∂denom/∂β = α · ∂d/∂β = α · r² / (2d)

Quotient rule:

∂u/∂β = fx · (-x · ∂denom/∂β) / denom²
      = -fx · x · α · r² / (2d · denom²)

∂v/∂β = -fy · y · α · r² / (2d · denom²)

Matrix Form

The complete Jacobian matrix is:

J = [ x/denom    0        1    0    ∂u/∂α    ∂u/∂β ]
    [   0     y/denom    0    1    ∂v/∂α    ∂v/∂β ]

where:

• ∂u/∂α = -fx · x · (d - z) / denom²
• ∂u/∂β = -fx · x · α · r² / (2d · denom²)
• ∂v/∂α = -fy · y · (d - z) / denom²
• ∂v/∂β = -fy · y · α · r² / (2d · denom²)

References

• Khomutenko et al., “An Enhanced Unified Camera Model”, RAL 2016
• Scaramuzza et al., “A Toolbox for Easily Calibrating Omnidirectional Cameras”, IROS 2006

Numerical Verification

This analytical Jacobian is verified against numerical differentiation in test_jacobian_intrinsics_numerical() with tolerance < 1e-5.

Notes

The EUCM model parameters have physical interpretation:

• α ∈ [0, 1]: Projection model parameter (α=0 is perspective, α=1 is parabolic)
• β > 0: Mirror parameter controlling field of view

fn validate_params(&self) -> Result<(), CameraModelError>

Validates camera parameters.

Validation Rules

• fx, fy must be positive.
• fx, fy must be finite.
• cx, cy must be finite.
• α must be in [0, 1].
• β must be positive (> 0).

Errors

Returns CameraModelError if any parameter violates validation rules.

fn get_pinhole_params(&self) -> PinholeParams

Returns the pinhole parameters of the camera.

Returns

A PinholeParams struct containing the focal lengths (fx, fy) and principal point (cx, cy).

fn get_distortion(&self) -> DistortionModel

Returns the distortion model and parameters of the camera.

Returns

The DistortionModel associated with this camera (typically DistortionModel::EUCM).

fn get_model_name(&self) -> &'static str

Returns the string identifier for the camera model.

Returns

The string "eucm".

const INTRINSIC_DIM: usize = 6

Number of intrinsic parameters (compile-time constant).

type IntrinsicJacobian = Matrix<f64, Const<2>, Const<6>, ArrayStorage<f64, 2, 6>>

Jacobian type for intrinsics: 2 × INTRINSIC_DIM.

type PointJacobian = Matrix<f64, Const<2>, Const<3>, ArrayStorage<f64, 2, 3>>

Jacobian type for 3D point: 2 × 3.

fn jacobian_pose( &self, p_world: &Matrix<f64, Const<3>, Const<1>, ArrayStorage<f64, 3, 1>>, pose: &SE3, ) -> (Self::PointJacobian, Matrix<f64, Const<3>, Const<6>, ArrayStorage<f64, 3, 6>>)

Jacobian of projection w.r.t. camera pose (SE3).

fn project_batch( &self, points_cam: &Matrix<f64, Const<3>, Dyn, VecStorage<f64, Const<3>, Dyn>>, ) -> Matrix<f64, Const<2>, Dyn, VecStorage<f64, Const<2>, Dyn>>

Batch projection of multiple 3D points to 2D image coordinates.

impl Clone for EucmCamera

fn clone(&self) -> EucmCamera

Returns a duplicate of the value.

fn clone_from(&mut self, source: &Self)

Performs copy-assignment from source.

impl Debug for EucmCamera

fn fmt(&self, f: &mut Formatter<'_>) -> Result<(), Error>

Formats the value using the given formatter.

impl From<&EucmCamera> for [f64; 6]

Convert camera to fixed-size array of intrinsic parameters.

Layout

The parameters are ordered as: [fx, fy, cx, cy, alpha, beta]

fn from(camera: &EucmCamera) -> [f64; 6]

Converts to this type from the input type.

impl From<&EucmCamera> for Matrix<f64, Dyn, Const<1>, VecStorage<f64, Dyn, Const<1>>>

Convert camera to dynamic vector of intrinsic parameters.

Layout

The parameters are ordered as: [fx, fy, cx, cy, alpha, beta]

fn from( camera: &EucmCamera, ) -> Matrix<f64, Dyn, Const<1>, VecStorage<f64, Dyn, Const<1>>>

Converts to this type from the input type.

impl From<[f64; 6]> for EucmCamera

Create camera from fixed-size array of intrinsic parameters.

Layout

Expected parameter order: [fx, fy, cx, cy, alpha, beta]

fn from(params: [f64; 6]) -> EucmCamera

Converts to this type from the input type.

impl PartialEq for EucmCamera

fn eq(&self, other: &EucmCamera) -> bool

Tests for self and other values to be equal, and is used by ==.

fn ne(&self, other: &Rhs) -> bool

Tests for !=. The default implementation is almost always sufficient, and should not be overridden without very good reason.

impl TryFrom<&[f64]> for EucmCamera

Create camera from slice of intrinsic parameters.

Layout

Expected parameter order: [fx, fy, cx, cy, alpha, beta]

Panics

Panics if the slice has fewer than 6 elements.

type Error = CameraModelError

The type returned in the event of a conversion error.

fn try_from( params: &[f64], ) -> Result<EucmCamera, <EucmCamera as TryFrom<&[f64]>>::Error>

Performs the conversion.

impl Copy for EucmCamera

impl StructuralPartialEq for EucmCamera