Struct KannalaBrandtCamera 

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

Kannala-Brandt fisheye camera model with 8 parameters.

Fields

pinhole: PinholeParams

distortion: DistortionModel

Implementations

impl KannalaBrandtCamera

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

Create a new Kannala-Brandt fisheye camera.

Arguments

• pinhole - Pinhole parameters (fx, fy, cx, cy).
• distortion - MUST be DistortionModel::KannalaBrandt with k1, k2, k3, k4.

Returns

Returns a new KannalaBrandtCamera instance if the distortion model matches.

Errors

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

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 distortion coefficients [k1, k2, k3, k4] using a linear least squares approach given 3D-2D point correspondences. 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 KannalaBrandtCamera

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

r = √(x² + y²)
θ = atan2(r, z)
θ_d = θ + k₁·θ³ + k₂·θ⁵ + k₃·θ⁷ + k₄·θ⁹
u = fx · θ_d · (x/r) + cx
v = fy · θ_d · (y/r) + cy

Arguments

• p_cam - 3D point in camera coordinate frame.

Returns

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

Errors

Returns CameraModelError::InvalidParams if point is behind camera (z <= EPSILON).

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

Newton-Raphson iteration to solve for θ from θ_d:

• f(θ) = θ + k₁·θ³ + k₂·θ⁵ + k₃·θ⁷ + k₄·θ⁹ - θ_d = 0
• f’(θ) = 1 + 3k₁·θ² + 5k₂·θ⁴ + 7k₃·θ⁶ + 9k₄·θ⁸

Arguments

• point_2d - 2D point in image coordinates.

Returns

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

Errors

Returns CameraModelError::NumericalError if Newton-Raphson fails to converge or derivative is too small.

fn jacobian_point( &self, p_cam: &Matrix<f64, Const<3>, Const<1>, ArrayStorage<f64, 3, 1>>, ) -> <KannalaBrandtCamera 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

Kannala-Brandt Projection Model Recap

r = √(x² + y²)                              // Radial distance from optical axis
θ = atan2(r, z)                             // Angle from optical axis
θ_d = θ + k₁·θ³ + k₂·θ⁵ + k₃·θ⁷ + k₄·θ⁹    // Distorted angle (polynomial)
u = fx · θ_d · (x/r) + cx                  // Pixel u-coordinate
v = fy · θ_d · (y/r) + cy                  // Pixel v-coordinate

Derivatives of intermediate quantities:

∂r/∂x = x/r,  ∂r/∂y = y/r,  ∂r/∂z = 0
∂θ/∂x = z·x / (r·(r²+z²))
∂θ/∂y = z·y / (r·(r²+z²))
∂θ/∂z = -r / (r²+z²)
∂θ_d/∂θ = 1 + 3k₁·θ² + 5k₂·θ⁴ + 7k₃·θ⁶ + 9k₄·θ⁸
∂θ_d/∂x = (∂θ_d/∂θ) · (∂θ/∂x)
∂θ_d/∂y = (∂θ_d/∂θ) · (∂θ/∂y)
∂θ_d/∂z = (∂θ_d/∂θ) · (∂θ/∂z)

Derivatives of pixel coordinates (quotient + product rule):

For u = fx · θ_d · (x/r) + cx:

∂u/∂x = fx · [∂θ_d/∂x · (x/r) + θ_d · ∂(x/r)/∂x]
      = fx · [∂θ_d/∂x · (x/r) + θ_d · (1/r - x²/r³)]

∂u/∂y = fx · [∂θ_d/∂y · (x/r) + θ_d · (-x·y/r³)]

∂u/∂z = fx · [∂θ_d/∂z · (x/r)]

Similarly for v = fy · θ_d · (y/r) + cy:

∂v/∂x = fy · [∂θ_d/∂x · (y/r) + θ_d · (-x·y/r³)]

∂v/∂y = fy · [∂θ_d/∂y · (y/r) + θ_d · (1/r - y²/r³)]

∂v/∂z = fy · [∂θ_d/∂z · (y/r)]

Near optical axis (r → 0): Use simplified Jacobian for numerical stability:

∂u/∂x ≈ fx · (∂θ_d/∂θ) / z
∂v/∂y ≈ fy · (∂θ_d/∂θ) / z
(all other terms ≈ 0)

Final Jacobian matrix (2×3):

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

Arguments

• p_cam - 3D point in camera coordinate frame.

Returns

Returns the 2×3 Jacobian matrix.

References

• Kannala & Brandt, “A Generic Camera Model and Calibration Method for Conventional, Wide-Angle, and Fish-Eye Lenses”, IEEE PAMI 2006
• Verified against numerical differentiation in test_jacobian_point_numerical()

Implementation Note

The implementation handles the optical axis singularity (r → 0) using a threshold check and falls back to a simplified Jacobian for numerical stability.

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

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

Computes ∂π/∂K where K = [fx, fy, cx, cy, k₁, k₂, k₃, k₄] are the intrinsic parameters.

Mathematical Derivation

The intrinsic parameters consist of:

1. Linear parameters: fx, fy, cx, cy (pinhole projection)
2. Distortion parameters: k₁, k₂, k₃, k₄ (Kannala-Brandt polynomial coefficients)

Kannala-Brandt Projection Model Recap

r = √(x² + y²)
θ = atan2(r, z)
θ_d = θ + k₁·θ³ + k₂·θ⁵ + k₃·θ⁷ + k₄·θ⁹
u = fx · θ_d · (x/r) + cx
v = fy · θ_d · (y/r) + cy

Linear parameters (fx, fy, cx, cy):

∂u/∂fx = θ_d · (x/r),  ∂u/∂fy = 0,  ∂u/∂cx = 1,  ∂u/∂cy = 0
∂v/∂fx = 0,  ∂v/∂fy = θ_d · (y/r),  ∂v/∂cx = 0,  ∂v/∂cy = 1

Distortion parameters (k₁, k₂, k₃, k₄):

The distortion affects θ_d through the polynomial expansion.

Derivatives of θ_d:

θ_d = θ + k₁·θ³ + k₂·θ⁵ + k₃·θ⁷ + k₄·θ⁹

∂θ_d/∂k₁ = θ³
∂θ_d/∂k₂ = θ⁵
∂θ_d/∂k₃ = θ⁷
∂θ_d/∂k₄ = θ⁹

Chain rule to pixel coordinates:

For u = fx · θ_d · (x/r) + cx:

∂u/∂k₁ = fx · (∂θ_d/∂k₁) · (x/r) = fx · θ³ · (x/r)
∂u/∂k₂ = fx · θ⁵ · (x/r)
∂u/∂k₃ = fx · θ⁷ · (x/r)
∂u/∂k₄ = fx · θ⁹ · (x/r)

Similarly for v = fy · θ_d · (y/r) + cy:

∂v/∂k₁ = fy · θ³ · (y/r)
∂v/∂k₂ = fy · θ⁵ · (y/r)
∂v/∂k₃ = fy · θ⁷ · (y/r)
∂v/∂k₄ = fy · θ⁹ · (y/r)

Final Jacobian Matrix (2×8)

J = [ ∂u/∂fx  ∂u/∂fy  ∂u/∂cx  ∂u/∂cy  ∂u/∂k₁  ∂u/∂k₂  ∂u/∂k₃  ∂u/∂k₄ ]
    [ ∂v/∂fx  ∂v/∂fy  ∂v/∂cx  ∂v/∂cy  ∂v/∂k₁  ∂v/∂k₂  ∂v/∂k₃  ∂v/∂k₄ ]

Expanded:

J = [ θ_d·x/r    0       1    0    fx·θ³·x/r  fx·θ⁵·x/r  fx·θ⁷·x/r  fx·θ⁹·x/r ]
    [    0    θ_d·y/r    0    1    fy·θ³·y/r  fy·θ⁵·y/r  fy·θ⁷·y/r  fy·θ⁹·y/r ]

Arguments

• p_cam - 3D point in camera coordinate frame.

Returns

Returns the 2×8 intrinsic Jacobian matrix.

References

• Kannala & Brandt, “A Generic Camera Model and Calibration Method for Conventional, Wide-Angle, and Fish-Eye Lenses”, IEEE PAMI 2006
• Verified against numerical differentiation in test_jacobian_intrinsics_numerical()

Implementation Note

For numerical stability, when r (radial distance) is very small (near optical axis), the Jacobian is set to zero as the projection becomes degenerate in this region.

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.
• k1..k4 must be finite.

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

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

Returns the string identifier for the camera model.

Returns

The string "kannala_brandt".

const INTRINSIC_DIM: usize = 8

Number of intrinsic parameters (compile-time constant).

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

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 KannalaBrandtCamera

fn clone(&self) -> KannalaBrandtCamera

Returns a duplicate of the value.

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

Performs copy-assignment from source.

impl Debug for KannalaBrandtCamera

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

Formats the value using the given formatter.

impl From<&[f64]> for KannalaBrandtCamera

Create camera from slice of intrinsic parameters.

Layout

Expected parameter order: [fx, fy, cx, cy, k1, k2, k3, k4]

Panics

Panics if the slice has fewer than 8 elements.

fn from(params: &[f64]) -> KannalaBrandtCamera

Converts to this type from the input type.

impl From<&KannalaBrandtCamera> for [f64; 8]

Convert camera to fixed-size array of intrinsic parameters.

Layout

The parameters are ordered as: [fx, fy, cx, cy, k1, k2, k3, k4]

fn from(camera: &KannalaBrandtCamera) -> [f64; 8]

Converts to this type from the input type.

impl From<[f64; 8]> for KannalaBrandtCamera

Create camera from fixed-size array of intrinsic parameters.

Layout

Expected parameter order: [fx, fy, cx, cy, k1, k2, k3, k4]

fn from(params: [f64; 8]) -> KannalaBrandtCamera

Converts to this type from the input type.

impl PartialEq for KannalaBrandtCamera

fn eq(&self, other: &KannalaBrandtCamera) -> 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 Copy for KannalaBrandtCamera

impl StructuralPartialEq for KannalaBrandtCamera