Struct RadTanCamera 

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

A Radial-Tangential camera model with 9 intrinsic parameters.

Fields

pinhole: PinholeParams

distortion: DistortionModel

Implementations

impl RadTanCamera

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

Creates a new Radial-Tangential (Brown-Conrady) camera.

Arguments

• pinhole - The pinhole parameters (fx, fy, cx, cy).
• distortion - The distortion model. This must be DistortionModel::BrownConrady with parameters { k1, k2, p1, p2, k3 }.

Errors

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

pub fn linear_estimation( &mut self, points_3d: &Matrix3xX<f64>, points_2d: &Matrix2xX<f64>, ) -> Result<(), CameraModelError>

Performs linear estimation to initialize distortion parameters from point correspondences.

This method estimates the radial distortion coefficients [k1, k2, k3] using a linear least squares approach given 3D-2D point correspondences. Tangential distortion parameters (p1, p2) are set to 0.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 RadTanCamera

fn project( &self, p_cam: &Vector3<f64>, ) -> Result<Vector2<f64>, CameraModelError>

Projects a 3D point in the camera frame to 2D image coordinates.

Mathematical Formula

Combines radial distortion (k₁, k₂, k₃) and tangential distortion (p₁, p₂).

Arguments

• p_cam - The 3D point in the camera coordinate frame.

Returns

• Ok(uv) - The 2D image coordinates if valid.
• Err - If the point is at or behind the camera.

fn unproject( &self, point_2d: &Vector2<f64>, ) -> Result<Vector3<f64>, CameraModelError>

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

Algorithm

Iterative Newton-Raphson with Jacobian matrix:

1. Start with the undistorted estimate.
2. Compute distortion and Jacobian.
3. Update estimate: p′ = p′ − J⁻¹ · f(p′).
4. Repeat until convergence.

Arguments

• point_2d - The 2D point in image coordinates.

Returns

• Ok(ray) - The normalized 3D ray direction.
• Err - If the iteration fails to converge.

fn jacobian_point(&self, p_cam: &Vector3<f64>) -> Self::PointJacobian

Computes the Jacobian of the projection with respect to the 3D point coordinates (2×3).

Mathematical Derivation

For the Radial-Tangential (Brown-Conrady) model, projection is:

x' = x/z,  y' = y/z  (normalized coordinates)
r² = x'² + y'²
radial = 1 + k₁·r² + k₂·r⁴ + k₃·r⁶
dx = 2·p₁·x'·y' + p₂·(r² + 2·x'²)  (tangential distortion)
dy = p₁·(r² + 2·y'²) + 2·p₂·x'·y'  (tangential distortion)
x'' = radial·x' + dx
y'' = radial·y' + dy
u = fx·x'' + cx
v = fy·y'' + cy

Chain Rule Application

This is the most complex Jacobian due to coupled radial + tangential distortion. The chain rule must be applied through multiple stages:

1. ∂(x’,y’)/∂(x,y,z): Normalized coordinate derivatives
2. ∂(x’‘,y’‘)/∂(x’,y’): Distortion derivatives (radial + tangential)
3. ∂(u,v)/∂(x’‘,y’’): Final projection derivatives

Normalized coordinate derivatives (x’ = x/z, y’ = y/z):

∂x'/∂x = 1/z,   ∂x'/∂y = 0,     ∂x'/∂z = -x/z²
∂y'/∂x = 0,     ∂y'/∂y = 1/z,   ∂y'/∂z = -y/z²

Distortion derivatives:

The distorted coordinates are: x’’ = radial·x’ + dx, y’’ = radial·y’ + dy

Radial Distortion Component

radial = 1 + k₁·r² + k₂·r⁴ + k₃·r⁶
∂radial/∂r² = k₁ + 2k₂·r² + 3k₃·r⁴
∂r²/∂x' = 2x',  ∂r²/∂y' = 2y'

Tangential Distortion Component

For dx = 2p₁x’y’ + p₂(r² + 2x’²):

∂dx/∂x' = 2p₁y' + p₂(2x' + 4x') = 2p₁y' + 6p₂x'
∂dx/∂y' = 2p₁x' + 2p₂y'

For dy = p₁(r² + 2y’²) + 2p₂x’y’:

∂dy/∂x' = 2p₁x' + 2p₂y'
∂dy/∂y' = p₁(2y' + 4y') + 2p₂x' = 6p₁y' + 2p₂x'

Combined Distorted Coordinate Derivatives

For x’’ = radial·x’ + dx:

∂x''/∂x' = radial + x'·∂radial/∂r²·∂r²/∂x' + ∂dx/∂x'
         = radial + x'·dradial_dr2·2x' + 2p₁y' + 6p₂x'
         = radial + 2x'²·dradial_dr2 + 2p₁y' + 6p₂x'

∂x''/∂y' = x'·∂radial/∂r²·∂r²/∂y' + ∂dx/∂y'
         = x'·dradial_dr2·2y' + 2p₁x' + 2p₂y'
         = 2x'y'·dradial_dr2 + 2p₁x' + 2p₂y'

For y’’ = radial·y’ + dy:

∂y''/∂x' = y'·∂radial/∂r²·∂r²/∂x' + ∂dy/∂x'
         = y'·dradial_dr2·2x' + 2p₁x' + 2p₂y'
         = 2x'y'·dradial_dr2 + 2p₁x' + 2p₂y'

∂y''/∂y' = radial + y'·∂radial/∂r²·∂r²/∂y' + ∂dy/∂y'
         = radial + y'·dradial_dr2·2y' + 6p₁y' + 2p₂x'
         = radial + 2y'²·dradial_dr2 + 6p₁y' + 2p₂x'

Final projection derivatives (u = fx·x’’ + cx, v = fy·y’’ + cy):

∂u/∂x'' = fx,  ∂u/∂y'' = 0
∂v/∂x'' = 0,   ∂v/∂y'' = fy

Chain rule:

∂u/∂x = fx·(∂x''/∂x'·∂x'/∂x + ∂x''/∂y'·∂y'/∂x)
      = fx·(∂x''/∂x'·1/z + 0)
      = fx·∂x''/∂x'·1/z

∂u/∂y = fx·(∂x''/∂x'·∂x'/∂y + ∂x''/∂y'·∂y'/∂y)
      = fx·(0 + ∂x''/∂y'·1/z)
      = fx·∂x''/∂y'·1/z

∂u/∂z = fx·(∂x''/∂x'·∂x'/∂z + ∂x''/∂y'·∂y'/∂z)
      = fx·(∂x''/∂x'·(-x'/z) + ∂x''/∂y'·(-y'/z))

Similar derivations apply for ∂v/∂x, ∂v/∂y, ∂v/∂z.

Matrix Form

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

References

• Brown, “Decentering Distortion of Lenses”, Photogrammetric Engineering 1966
• OpenCV Camera Calibration Documentation
• Hartley & Zisserman, “Multiple View Geometry”, Chapter 7

Numerical Verification

Verified against numerical differentiation in test_jacobian_point_numerical().

fn jacobian_intrinsics(&self, p_cam: &Vector3<f64>) -> Self::IntrinsicJacobian

Computes the Jacobian of the projection with respect to intrinsic parameters (2×9).

Mathematical Derivation

The Radial-Tangential camera has 9 intrinsic parameters: [fx, fy, cx, cy, k₁, k₂, p₁, p₂, k₃]

Projection Equations

x' = x/z,  y' = y/z
r² = x'² + y'²
radial = 1 + k₁·r² + k₂·r⁴ + k₃·r⁶
dx = 2·p₁·x'·y' + p₂·(r² + 2·x'²)
dy = p₁·(r² + 2·y'²) + 2·p₂·x'·y'
x'' = radial·x' + dx
y'' = radial·y' + dy
u = fx·x'' + cx
v = fy·y'' + cy

Jacobian Structure

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

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

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

Radial distortion coefficients (k₁, k₂, k₃):

Each k_i affects the radial distortion component:

∂radial/∂k₁ = r²
∂radial/∂k₂ = r⁴
∂radial/∂k₃ = r⁶

By chain rule (x’’ = radial·x’ + dx, y’’ = radial·y’ + dy):

∂x''/∂k₁ = x'·r²
∂x''/∂k₂ = x'·r⁴
∂x''/∂k₃ = x'·r⁶

∂y''/∂k₁ = y'·r²
∂y''/∂k₂ = y'·r⁴
∂y''/∂k₃ = y'·r⁶

Then:

∂u/∂k₁ = fx·x'·r²
∂u/∂k₂ = fx·x'·r⁴
∂u/∂k₃ = fx·x'·r⁶

∂v/∂k₁ = fy·y'·r²
∂v/∂k₂ = fy·y'·r⁴
∂v/∂k₃ = fy·y'·r⁶

Tangential distortion coefficients (p₁, p₂):

∂dx/∂p₁ = 2x'y',  ∂dy/∂p₁ = r² + 2y'²
∂u/∂p₁ = fx·2x'y',  ∂v/∂p₁ = fy·(r² + 2y'²)
∂dx/∂p₂ = r² + 2x'²,  ∂dy/∂p₂ = 2x'y'
∂u/∂p₂ = fx·(r² + 2x'²),  ∂v/∂p₂ = fy·2x'y'

Matrix form:

J = [ x''  0   1  0  fx·x'·r²  fx·x'·r⁴  fx·2x'y'    fx·(r²+2x'²)  fx·x'·r⁶ ]
    [  0  y''  0  1  fy·y'·r²  fy·y'·r⁴  fy·(r²+2y'²) fy·2x'y'      fy·y'·r⁶ ]

References

• Brown, “Decentering Distortion of Lenses”, 1966
• OpenCV Camera Calibration Documentation
• Hartley & Zisserman, “Multiple View Geometry”, Chapter 7

Numerical Verification

Verified in test_jacobian_intrinsics_numerical() with tolerance < 1e-5.

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

Validates the camera parameters.

Validation Rules

• fx, fy must be positive (> 0)
• fx, fy must be finite
• cx, cy must be finite
• k₁, k₂, p₁, p₂, k₃ must be finite

Errors

Returns CameraModelError if any parameter violates validation rules.

fn get_pinhole_params(&self) -> PinholeParams

Returns the pinhole parameters.

Returns

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

fn get_distortion(&self) -> DistortionModel

Returns the distortion model parameters.

Returns

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

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

Returns the name of the camera model.

Returns

The string "rad_tan".

const INTRINSIC_DIM: usize = 9

Number of intrinsic parameters (compile-time constant).

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

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: &Vector3<f64>, pose: &SE3, ) -> (Self::PointJacobian, SMatrix<f64, 3, 6>)

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

fn project_batch(&self, points_cam: &Matrix3xX<f64>) -> Matrix2xX<f64>

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

impl Clone for RadTanCamera

fn clone(&self) -> RadTanCamera

Returns a duplicate of the value.

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

Performs copy-assignment from source.

impl Debug for RadTanCamera

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

Formats the value using the given formatter.

impl From<&[f64]> for RadTanCamera

Creates a camera from a slice of intrinsic parameters.

Layout

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

Panics

Panics if the slice has fewer than 9 elements.

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

Converts to this type from the input type.

impl From<&RadTanCamera> for [f64; 9]

Converts the camera parameters to a fixed-size array.

Layout

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

fn from(camera: &RadTanCamera) -> Self

Converts to this type from the input type.

impl From<&RadTanCamera> for DVector<f64>

Converts the camera parameters to a dynamic vector.

Layout

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

fn from(camera: &RadTanCamera) -> Self

Converts to this type from the input type.

impl From<[f64; 9]> for RadTanCamera

Creates a camera from a fixed-size array of intrinsic parameters.

Layout

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

fn from(params: [f64; 9]) -> Self

Converts to this type from the input type.

impl PartialEq for RadTanCamera

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

impl StructuralPartialEq for RadTanCamera