Trait CameraModel 

pub trait CameraModel:
    Send
    + Sync
    + Clone
    + Debug
    + 'static {
    type IntrinsicJacobian: Clone + Debug + Default + Index<(usize, usize), Output = f64>;
    type PointJacobian: Clone + Debug + Default + Mul<Matrix<f64, Const<3>, Const<6>, ArrayStorage<f64, 3, 6>>, Output = Matrix<f64, Const<2>, Const<6>, ArrayStorage<f64, 2, 6>>, Output = Matrix<f64, Const<2>, Const<3>, ArrayStorage<f64, 2, 3>>> + Mul<Matrix<f64, Const<3>, Const<3>, ArrayStorage<f64, 3, 3>>> + Index<(usize, usize), Output = f64>;

    const INTRINSIC_DIM: usize;

    // Required methods
    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>;
    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>;
    fn jacobian_point(
        &self,
        p_cam: &Matrix<f64, Const<3>, Const<1>, ArrayStorage<f64, 3, 1>>,
    ) -> Self::PointJacobian;
    fn jacobian_intrinsics(
        &self,
        p_cam: &Matrix<f64, Const<3>, Const<1>, ArrayStorage<f64, 3, 1>>,
    ) -> Self::IntrinsicJacobian;
    fn validate_params(&self) -> Result<(), CameraModelError>;
    fn get_pinhole_params(&self) -> PinholeParams;
    fn get_distortion(&self) -> DistortionModel;
    fn get_model_name(&self) -> &'static str;

    // Provided methods
    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>>) { ... }
    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>> { ... }
}

Trait for camera projection models.

Defines the interface for camera models used in bundle adjustment and SfM.

Type Parameters

• INTRINSIC_DIM: Number of intrinsic parameters
• IntrinsicJacobian: Jacobian type for intrinsics (2 × INTRINSIC_DIM)
• PointJacobian: Jacobian type for 3D point (2 × 3)

Required Associated Constants

const INTRINSIC_DIM: usize

Number of intrinsic parameters (compile-time constant).

Required Associated Types

type IntrinsicJacobian: Clone + Debug + Default + Index<(usize, usize), Output = f64>

Jacobian type for intrinsics: 2 × INTRINSIC_DIM.

type PointJacobian: Clone + Debug + Default + Mul<Matrix<f64, Const<3>, Const<6>, ArrayStorage<f64, 3, 6>>, Output = Matrix<f64, Const<2>, Const<6>, ArrayStorage<f64, 2, 6>>, Output = Matrix<f64, Const<2>, Const<3>, ArrayStorage<f64, 2, 3>>> + Mul<Matrix<f64, Const<3>, Const<3>, ArrayStorage<f64, 3, 3>>> + Index<(usize, usize), Output = f64>

Jacobian type for 3D point: 2 × 3.

Required Methods

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 in camera coordinates to 2D image coordinates.

The projection pipeline is: 3D point → normalized coordinates → distortion → pixel coordinates.

Mathematical Formula

For a 3D point p = (x, y, z) in camera frame:

(u, v) = K · distort(x/z, y/z)

where K is the intrinsic matrix [fx 0 cx; 0 fy cy; 0 0 1].

Arguments

• p_cam - 3D point in camera coordinate frame (x, y, z) in meters

Returns

• Ok(Vector2) - 2D image coordinates (u, v) in pixels if projection is valid
• Err(CameraModelError) - If point is behind camera, at center, or causes numerical issues

Errors

• PointBehindCamera - If z ≤ GEOMETRIC_PRECISION (point behind or too close)
• PointAtCameraCenter - If point is too close to optical axis
• DenominatorTooSmall - If projection causes numerical instability
• ProjectionOutOfBounds - If projection falls outside valid image region

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 normalized 3D ray in camera frame.

The inverse of projection: 2D pixel → undistortion → normalized 3D ray. Some models use iterative methods (e.g., Newton-Raphson) for undistortion.

Mathematical Formula

For a 2D point (u, v) in image coordinates:

(mx, my) = ((u - cx)/fx, (v - cy)/fy)
ray = normalize(undistort(mx, my))

Arguments

• point_2d - 2D point in image coordinates (u, v) in pixels

Returns

• Ok(Vector3) - Normalized 3D ray direction (unit vector)
• Err(CameraModelError) - If point is outside valid image region or numerical issues occur

Errors

• PointOutsideImage - If 2D point is outside valid unprojection region
• NumericalError - If iterative solver fails to converge

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

Jacobian of projection with respect to 3D point coordinates.

Returns the 2×3 matrix J where J[i,j] = ∂(u,v)[i] / ∂(x,y,z)[j].

Mathematical Formula

J = ∂(u,v)/∂(x,y,z) = [ ∂u/∂x  ∂u/∂y  ∂u/∂z ]
                      [ ∂v/∂x  ∂v/∂y  ∂v/∂z ]

This Jacobian is used for:

• Structure optimization (adjusting 3D landmark positions)
• Triangulation refinement
• Bundle adjustment with landmark optimization

Arguments

• p_cam - 3D point in camera coordinate frame (x, y, z) in meters

Returns

2×3 Jacobian matrix of projection w.r.t. point coordinates

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

Jacobian of projection with respect to intrinsic parameters.

Returns the 2×N matrix where N = INTRINSIC_DIM (model-dependent).

Mathematical Formula

J = ∂(u,v)/∂(params) = [ ∂u/∂fx  ∂u/∂fy  ∂u/∂cx  ∂u/∂cy  ∂u/∂k1  ... ]
                      [ ∂v/∂fx  ∂v/∂fy  ∂v/∂cx  ∂v/∂cy  ∂v/∂k1  ... ]

Intrinsic parameters vary by model:

• Pinhole: [fx, fy, cx, cy] (4 params)
• RadTan: [fx, fy, cx, cy, k1, k2, p1, p2, k3] (9 params)
• Kannala-Brandt: [fx, fy, cx, cy, k1, k2, k3, k4] (8 params)

Arguments

• p_cam - 3D point in camera coordinate frame (x, y, z) in meters

Returns

2×N Jacobian matrix of projection w.r.t. intrinsic parameters

Usage

Used in camera calibration and self-calibration bundle adjustment.

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

Validates camera intrinsic and distortion parameters.

Performs comprehensive validation including:

• Focal lengths: must be positive and finite
• Principal point: must be finite
• Distortion coefficients: must be finite
• Model-specific constraints (e.g., UCM α ∈ [0,1], Double Sphere ξ ∈ [-1,1])

Validation Rules

Common validations across all models:

• fx > 0, fy > 0 (focal lengths must be positive)
• fx, fy finite (no NaN or Inf)
• cx, cy finite (principal point must be valid)

Model-specific validations:

• UCM: α ∈ [0, 1]
• EUCM: α ∈ [0, 1], β > 0
• Double Sphere: ξ ∈ [-1, 1], α ∈ (0, 1]
• FOV: w ∈ (0, π]

Returns

• Ok(()) - All parameters satisfy validation rules
• Err(CameraModelError) - Specific error indicating which parameter is invalid

fn get_pinhole_params(&self) -> PinholeParams

Returns the pinhole parameters (fx, fy, cx, cy).

Returns

PinholeParams struct containing focal lengths and principal point.

fn get_distortion(&self) -> DistortionModel

Returns the distortion model and parameters.

Returns

DistortionModel enum variant with model-specific parameters. Returns DistortionModel::None for pinhole cameras without distortion.

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

Returns the camera model name identifier.

Returns

Static string identifier for the camera model type:

• "pinhole", "rad_tan", "kannala_brandt", etc.

Provided Methods

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

Mathematical Derivation

The pose is treated as a world-to-camera transform T_wc so that:

p_cam = T_wc · p_world = R · p_world + t

Perturbation Model (Right Jacobian)

We perturb the pose with a right perturbation:

T'(δξ) = T · Exp(δξ),  δξ = [δρ; δθ] ∈ ℝ⁶

The perturbed camera-frame point becomes:

p_cam' = R·(I + [δθ]×)·p_world + (t + R·δρ)
       = p_cam + R·δρ - R·[p_world]×·δθ

Taking derivatives:

∂p_cam/∂δρ = R
∂p_cam/∂δθ = -R · [p_world]×

Return Value

Returns a tuple (J_pixel_point, J_point_pose):

• J_pixel_point: 2×3 Jacobian ∂uv/∂p_cam (from jacobian_point)
• J_point_pose: 3×6 Jacobian ∂p_cam/∂δξ = [R | -R·[p_world]×]

The caller multiplies these to get the full 2×6 Jacobian ∂uv/∂δξ.

SE(3) Conventions

• Pose meaning: world-to-camera T_wc (p_cam = R·p_world + t)
• Parameterization: δξ = [δρ_x, δρ_y, δρ_z, δθ_x, δθ_y, δθ_z]
• Perturbation: Right perturbation T’ = T · Exp(δξ)
• Result: ∂p_cam/∂δρ = R, ∂p_cam/∂δθ = -R·[p_world]×

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.

Projects N 3D points efficiently. Invalid projections are marked with a sentinel value (1e6, 1e6) rather than returning an error.

Arguments

• points_cam - 3×N matrix where each column is a 3D point (x, y, z) in camera frame

Returns

2×N matrix where each column is the projected 2D point (u, v) in pixels. Invalid projections are set to (1e6, 1e6).

Performance

Default implementation iterates over points. Camera models may override with vectorized implementations for better performance.

Dyn Compatibility

This trait is not dyn compatible.

In older versions of Rust, dyn compatibility was called "object safety", so this trait is not object safe.

Implementors

impl CameraModel for BALPinholeCameraStrict

const INTRINSIC_DIM: usize = 3

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

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

impl CameraModel for DoubleSphereCamera

const INTRINSIC_DIM: usize = 6

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

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

impl CameraModel for EucmCamera

const INTRINSIC_DIM: usize = 6

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

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

impl CameraModel for FovCamera

const INTRINSIC_DIM: usize = 5

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

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

impl CameraModel for KannalaBrandtCamera

const INTRINSIC_DIM: usize = 8

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

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

impl CameraModel for PinholeCamera

const INTRINSIC_DIM: usize = 4

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

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

impl CameraModel for RadTanCamera

const INTRINSIC_DIM: usize = 9

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

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

impl CameraModel for UcmCamera

const INTRINSIC_DIM: usize = 5

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

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