Struct DoubleSphereCamera 

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

Double Sphere camera model with 6 parameters.

Fields

pinhole: PinholeParams

distortion: DistortionModel

Implementations

impl DoubleSphereCamera

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

Creates a new Double Sphere camera model.

Arguments

• pinhole - Pinhole camera parameters (fx, fy, cx, cy).
• distortion - Distortion model (must be DistortionModel::DoubleSphere).

Returns

Returns a new DoubleSphereCamera instance if the parameters are valid.

Errors

Returns CameraModelError if:

• The distortion model is not DoubleSphere.
• Parameters are invalid (e.g., negative focal length, invalid alpha).

Example

use apex_camera_models::{DoubleSphereCamera, PinholeParams, DistortionModel};

let pinhole = PinholeParams::new(300.0, 300.0, 320.0, 240.0)?;
let distortion = DistortionModel::DoubleSphere { xi: -0.2, alpha: 0.6 };
let camera = DoubleSphereCamera::new(pinhole, distortion)?;

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 alpha parameter using a linear least squares approach given 3D-2D point correspondences. It assumes the intrinsic parameters (fx, fy, cx, cy) are already set. The xi parameter is initialized to 0.0.

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 DoubleSphereCamera

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

Projects a 3D point to 2D image coordinates.

Mathematical Formula

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

Arguments

• p_cam - 3D point in camera coordinate frame.

Returns

Returns the 2D image coordinates if valid.

Errors

Returns CameraModelError if:

• The point fails the geometric projection condition (ProjectionOutOfBounds).
• The denominator is too small, indicating the point is at the camera center (PointAtCameraCenter).

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

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

Algorithm

Algebraic solution for double sphere inverse projection.

Arguments

• point_2d - 2D point in image coordinates.

Returns

Returns the normalized 3D ray direction.

Errors

Returns CameraModelError::PointOutsideImage if the unprojection condition fails.

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

Checks if a 3D point can be validly projected. 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

Given the Double Sphere projection model:

d₁ = √(x² + y² + z²)              // Distance to origin
w = ξ·d₁ + z                      // Intermediate value
d₂ = √(x² + y² + w²)              // Second sphere distance
denom = α·d₂ + (1-α)·w            // Denominator
u = fx · (x/denom) + cx           // Pixel u-coordinate
v = fy · (y/denom) + cy           // Pixel v-coordinate

Derivatives of intermediate quantities:

∂d₁/∂x = x/d₁,  ∂d₁/∂y = y/d₁,  ∂d₁/∂z = z/d₁
∂w/∂x = ξ·(∂d₁/∂x) = ξx/d₁
∂w/∂y = ξ·(∂d₁/∂y) = ξy/d₁
∂w/∂z = ξ·(∂d₁/∂z) + 1 = ξz/d₁ + 1

Derivative of d₂:

Since d₂ = √(x² + y² + w²), using chain rule:

∂d₂/∂x = (x + w·∂w/∂x) / d₂ = (x + w·ξx/d₁) / d₂
∂d₂/∂y = (y + w·∂w/∂y) / d₂ = (y + w·ξy/d₁) / d₂
∂d₂/∂z = (w·∂w/∂z) / d₂ = w·(ξz/d₁ + 1) / d₂

Derivative of denominator:

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

Derivatives of pixel coordinates (quotient rule):

For u = fx·(x/denom) + cx:

∂u/∂x = fx · ∂(x/denom)/∂x
      = fx · (denom·1 - x·∂denom/∂x) / denom²
      = fx · (denom - x·∂denom/∂x) / denom²

∂u/∂y = fx · (0 - x·∂denom/∂y) / denom²
      = -fx·x·∂denom/∂y / denom²

∂u/∂z = -fx·x·∂denom/∂z / denom²

Similarly for v = fy·(y/denom) + cy:

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

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 2x3 Jacobian matrix.

References

• Usenko et al., “The Double Sphere Camera Model”, 3DV 2018 (Supplementary Material)
• Verified against numerical differentiation in tests

Implementation Note

The implementation uses the chain rule systematically through intermediate quantities d₁, w, d₂, and denom to ensure numerical stability and code clarity.

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

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

Computes ∂π/∂K where K = [fx, fy, cx, cy, ξ, α] are the intrinsic parameters.

Mathematical Derivation

The intrinsic parameters consist of:

1. Linear parameters: fx, fy, cx, cy (pinhole projection)
2. Distortion parameters: ξ (xi), α (alpha) (Double Sphere specific)

Projection Model Recap

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

Part 1: Linear Parameters (fx, fy, cx, cy)

These have direct, simple derivatives:

Focal lengths (fx, fy):

∂u/∂fx = x/denom    (coefficient of fx in u)
∂u/∂fy = 0          (fy doesn't affect u)
∂v/∂fx = 0          (fx doesn't affect v)
∂v/∂fy = y/denom    (coefficient of fy in v)

Principal point (cx, cy):

∂u/∂cx = 1          (additive constant)
∂u/∂cy = 0          (cy doesn't affect u)
∂v/∂cx = 0          (cx doesn't affect v)
∂v/∂cy = 1          (additive constant)

Part 2: Distortion Parameters (ξ, α)

These affect the projection through the denominator term.

Derivative w.r.t. ξ (xi):

Since w = ξ·d₁ + z and d₂ = √(x² + y² + w²), we have:

∂w/∂ξ = d₁

∂d₂/∂ξ = ∂d₂/∂w · ∂w/∂ξ
       = (w/d₂) · d₁
       = w·d₁/d₂

∂denom/∂ξ = α·∂d₂/∂ξ + (1-α)·∂w/∂ξ
          = α·(w·d₁/d₂) + (1-α)·d₁
          = d₁·[α·w/d₂ + (1-α)]

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

∂u/∂ξ = fx · ∂(x/denom)/∂ξ
      = fx · (-x/denom²) · ∂denom/∂ξ
      = -fx·x·∂denom/∂ξ / denom²

Similarly:

∂v/∂ξ = -fy·y·∂denom/∂ξ / denom²

Derivative w.r.t. α (alpha):

Since denom = α·d₂ + (1-α)·w:

∂denom/∂α = d₂ - w

∂u/∂α = -fx·x·(d₂ - w) / denom²
∂v/∂α = -fy·y·(d₂ - w) / denom²

Final Jacobian Matrix (2×6)

J = [ ∂u/∂fx  ∂u/∂y  ∂u/∂cx  ∂u/∂cy  ∂u/∂ξ  ∂u/∂α ]
    [ ∂v/∂x  ∂v/∂y  ∂v/∂cx  ∂v/∂cy  ∂v/∂ξ  ∂v/∂α ]

  = [ x/denom    0       1       0      -fx·x·∂denom/∂ξ/denom²  -fx·x·(d₂-w)/denom² ]
    [   0     y/denom    0       1      -fy·y·∂denom/∂ξ/denom²  -fy·y·(d₂-w)/denom² ]

Arguments

• p_cam - 3D point in camera coordinate frame.

Returns

Returns the 2x6 Intrinsic Jacobian matrix.

References

• Usenko et al., “The Double Sphere Camera Model”, 3DV 2018
• Verified against numerical differentiation in tests

Implementation Note

The implementation computes all intermediate values (d₁, w, d₂, denom) first, then applies the chain rule derivatives systematically.

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

Validates camera parameters.

Validation Rules

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

Errors

Returns CameraModelError if any parameter violates the 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::DoubleSphere).

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

Returns the string identifier for the camera model.

Returns

The string "double_sphere".

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: &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 DoubleSphereCamera

fn clone(&self) -> DoubleSphereCamera

Returns a duplicate of the value.

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

Performs copy-assignment from source.

impl Debug for DoubleSphereCamera

Provides a debug string representation for [DoubleSphereModel].

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

Formats the value using the given formatter.

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

Convert DoubleSphereCamera to fixed-size parameter array.

Returns intrinsic parameters as [fx, fy, cx, cy, xi, alpha]

fn from(camera: &DoubleSphereCamera) -> Self

Converts to this type from the input type.

impl From<&DoubleSphereCamera> for DVector<f64>

Convert DoubleSphereCamera to parameter vector.

Returns intrinsic parameters in the order: [fx, fy, cx, cy, xi, alpha]

fn from(camera: &DoubleSphereCamera) -> Self

Converts to this type from the input type.

impl From<[f64; 6]> for DoubleSphereCamera

Create DoubleSphereCamera from fixed-size parameter array.

Parameter Order

params = [fx, fy, cx, cy, xi, alpha]

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

Converts to this type from the input type.

impl PartialEq for DoubleSphereCamera

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

Create DoubleSphereCamera from parameter slice.

Panics

Panics if the slice has fewer than 6 elements.

Parameter Order

params = [fx, fy, cx, cy, xi, alpha]

type Error = CameraModelError

The type returned in the event of a conversion error.

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

Performs the conversion.

impl Copy for DoubleSphereCamera

impl StructuralPartialEq for DoubleSphereCamera