From f7451f787c1373666b371884518b0454aa8e367d Mon Sep 17 00:00:00 2001 From: rsasaki0109 Date: Wed, 15 Jul 2026 17:01:31 +0900 Subject: [PATCH] Add Epic 105 camera calibration contracts --- CHANGELOG.md | 6 + bench/opencv_calibration_comparison/README.md | 11 + bench/opencv_calibration_comparison/run.py | 88 ++ bench/opencv_comparison/manifest.json | 4 +- bench/opencv_comparison/run.py | 1 + crates/spatialrust-camera/Cargo.toml | 4 + .../spatialrust-camera/benches/calibration.rs | 80 ++ crates/spatialrust-camera/src/calibration.rs | 791 ++++++++++++++++++ crates/spatialrust-camera/src/distortion.rs | 78 +- crates/spatialrust-camera/src/lib.rs | 9 +- crates/spatialrust-platform/src/stability.rs | 1 + crates/spatialrust-py/spatialrust.pyi | 16 +- crates/spatialrust-py/src/lib.rs | 69 ++ crates/spatialrust-py/tests/test_bindings.py | 35 + docs/API_STABILITY.md | 2 +- docs/ARCHITECTURE.md | 3 + docs/ROADMAP.md | 18 +- notes/2026-07-15_epic105_calibration.md | 28 + 18 files changed, 1238 insertions(+), 6 deletions(-) create mode 100644 bench/opencv_calibration_comparison/README.md create mode 100644 bench/opencv_calibration_comparison/run.py create mode 100644 crates/spatialrust-camera/benches/calibration.rs create mode 100644 crates/spatialrust-camera/src/calibration.rs create mode 100644 notes/2026-07-15_epic105_calibration.md diff --git a/CHANGELOG.md b/CHANGELOG.md index 8922818..451ddb0 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -21,6 +21,12 @@ removed no sooner than the next major (see `docs/API_STABILITY.md`). ### Added +- **Camera calibration contracts (Epic 105)**: robust pinhole intrinsics, + Kannala–Brandt4 fisheye fitting, supplied-rotation stereo and hand-eye + translation solves, and fixed-camera sparse point bundle adjustment. All + solvers return common RMS/max/iteration receipts, validate rotations and + indices, and use deterministic small dense math without a native optimizer. + - **Texture-backed GPU image chains (Epic 104)**: `GpuImage` now uses pooled `rgba8uint` textures instead of component-expanded storage buffers. Explicit upload/readback receipts compose through copy, RGB-to-gray, box blur, nearest diff --git a/bench/opencv_calibration_comparison/README.md b/bench/opencv_calibration_comparison/README.md new file mode 100644 index 0000000..0eb0681 --- /dev/null +++ b/bench/opencv_calibration_comparison/README.md @@ -0,0 +1,11 @@ +# OpenCV calibration comparison + +Build/install the Python extension and run: + +```bash +python bench/opencv_calibration_comparison/run.py \ + --output target/opencv-comparison/calibration.json +``` + +The suite uses OpenCV projection/fisheye conventions to synthesize observations, +then gates SpatialRust pinhole and Kannala–Brandt4 parameter recovery. diff --git a/bench/opencv_calibration_comparison/run.py b/bench/opencv_calibration_comparison/run.py new file mode 100644 index 0000000..be810d8 --- /dev/null +++ b/bench/opencv_calibration_comparison/run.py @@ -0,0 +1,88 @@ +"""OpenCV correctness comparison for Epic 105 camera calibration contracts.""" + +from __future__ import annotations + +import argparse +import sys +from pathlib import Path + +import cv2 +import numpy as np +import spatialrust as sr + +sys.path.insert(0, str(Path(__file__).resolve().parents[1])) +from opencv_comparison.report import emit_report, environment, make_report + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser() + parser.add_argument("--output", type=Path) + return parser.parse_args() + + +def main() -> None: + args = parse_args() + camera_points = np.array( + [ + [x * 0.07, y * 0.06, 1.5 + 0.025 * abs(x + y)] + for y in range(-4, 5) + for x in range(-5, 6) + ], + dtype=np.float64, + ) + expected_intrinsics = np.array([720.0, 715.0, 640.0, 360.0]) + camera_matrix = np.array( + [[720.0, 0.0, 640.0], [0.0, 715.0, 360.0], [0.0, 0.0, 1.0]], + dtype=np.float64, + ) + pixels, _ = cv2.projectPoints( + camera_points, np.zeros(3), np.zeros(3), camera_matrix, np.zeros(5) + ) + pinhole = sr.calibrate_pinhole_camera( + camera_points, pixels[:, 0, :], 1280, 720 + ) + intrinsics_error = float( + np.max(np.abs(np.asarray(pinhole[:4]) - expected_intrinsics)) + ) + + expected_fisheye = np.array([0.025, -0.003, 0.0004, -0.00002]) + theta = np.linspace(0.06, 1.2, 20, dtype=np.float64) + rays = np.column_stack((np.tan(theta), np.zeros_like(theta), np.ones_like(theta))) + fisheye_pixels, _ = cv2.fisheye.projectPoints( + rays[:, None, :], + np.zeros(3), + np.zeros(3), + np.eye(3, dtype=np.float64), + expected_fisheye, + ) + distorted_radius = fisheye_pixels[:, 0, 0] + fisheye = sr.calibrate_fisheye_angles(theta, distorted_radius) + fisheye_error = float( + np.max(np.abs(np.asarray(fisheye[:4]) - expected_fisheye)) + ) + status = "pass" if intrinsics_error <= 1e-8 and fisheye_error <= 1e-8 else "fail" + report = make_report( + suite="opencv-calibration", + kind="correctness", + status=status, + environment_receipt=environment( + opencv_version=cv2.__version__, spatialrust_version=sr.__version__ + ), + results={ + "pinhole_max_parameter_error": intrinsics_error, + "pinhole_rms_pixels": pinhole[4], + "fisheye_max_coefficient_error": fisheye_error, + "fisheye_rms_normalized_radius": fisheye[4], + "thresholds": { + "pinhole_max_parameter_error": 1e-8, + "fisheye_max_coefficient_error": 1e-8, + }, + }, + ) + emit_report(report, args.output) + if status != "pass": + raise SystemExit(1) + + +if __name__ == "__main__": + main() diff --git a/bench/opencv_comparison/manifest.json b/bench/opencv_comparison/manifest.json index fc69d69..6c7f7d5 100644 --- a/bench/opencv_comparison/manifest.json +++ b/bench/opencv_comparison/manifest.json @@ -22,7 +22,9 @@ { "id": "canny", "domain": "imgproc", "modes": ["allocate"] }, { "id": "morphology_open", "domain": "imgproc", "modes": ["allocate"] }, { "id": "orb", "domain": "feature2d", "modes": ["allocate"] }, - { "id": "stereo_bm", "domain": "calib3d", "modes": ["allocate"] }, + { "id": "stereo_bm", "domain": "calib3d", "modes": ["allocate"] }, + { "id": "pinhole_calibration", "domain": "calib3d", "modes": ["allocate"] }, + { "id": "fisheye_calibration", "domain": "calib3d", "modes": ["allocate"] }, { "id": "depth_to_xyz", "domain": "rgbd", "modes": ["allocate", "reuse"] }, { "id": "rgbd_to_point_cloud", "domain": "spatial-e2e", "modes": ["allocate"] }, { "id": "ai_preprocess", "domain": "dnn-adapter", "modes": ["allocate", "reuse"] }, diff --git a/bench/opencv_comparison/run.py b/bench/opencv_comparison/run.py index 401a0ed..482aa0a 100644 --- a/bench/opencv_comparison/run.py +++ b/bench/opencv_comparison/run.py @@ -12,6 +12,7 @@ ROOT = Path(__file__).resolve().parents[2] SUITES = { + "calibration": ROOT / "bench" / "opencv_calibration_comparison" / "run.py", "vision": ROOT / "bench" / "opencv_vision_comparison" / "run.py", "vision-performance": ROOT / "bench" diff --git a/crates/spatialrust-camera/Cargo.toml b/crates/spatialrust-camera/Cargo.toml index f57735a..7c5501d 100644 --- a/crates/spatialrust-camera/Cargo.toml +++ b/crates/spatialrust-camera/Cargo.toml @@ -23,3 +23,7 @@ criterion.workspace = true [[bench]] name = "rgbd" harness = false + +[[bench]] +name = "calibration" +harness = false diff --git a/crates/spatialrust-camera/benches/calibration.rs b/crates/spatialrust-camera/benches/calibration.rs new file mode 100644 index 0000000..9a2075b --- /dev/null +++ b/crates/spatialrust-camera/benches/calibration.rs @@ -0,0 +1,80 @@ +use criterion::{black_box, criterion_group, criterion_main, BenchmarkId, Criterion, Throughput}; +use spatialrust_camera::{ + bundle_adjust_points, calibrate_pinhole, BundleObservation, BundleProblem, BundleView, + CalibrationOptions, CameraIntrinsics, PinholeCamera, PinholeObservation, RigidTransform3, +}; +use spatialrust_math::{Mat3, Vec3}; + +fn benchmark_calibration(c: &mut Criterion) { + let camera = PinholeCamera::new( + CameraIntrinsics::try_new(800.0, 805.0, 640.0, 360.0, 1280, 720).unwrap(), + ); + for &count in &[100_usize, 1_000] { + let observations = (0..count) + .map(|index| { + let x = index % 31; + let y = (index / 31) % 23; + let point = Vec3::new( + x as f64 * 0.02 - 0.3, + y as f64 * 0.02 - 0.2, + 2.0 + index as f64 * 1e-4, + ); + PinholeObservation { camera_point: point, pixel: camera.project(point).unwrap() } + }) + .collect::>(); + let mut group = c.benchmark_group("calibrate_pinhole"); + group.throughput(Throughput::Elements(count as u64)); + group.bench_with_input(BenchmarkId::from_parameter(count), &observations, |b, input| { + b.iter(|| { + calibrate_pinhole(black_box(input), 1280, 720, CalibrationOptions::default()) + .unwrap() + }); + }); + group.finish(); + } + + let views = [0.0, -0.3, 0.3] + .into_iter() + .map(|x| BundleView { + camera, + camera_from_world: RigidTransform3 { + rotation: Mat3::::identity(), + translation: Vec3::new(x, 0.0, 0.0), + }, + }) + .collect::>(); + let truth = (0..100) + .map(|index| Vec3::new(index as f64 * 0.005 - 0.25, (index % 13) as f64 * 0.01 - 0.06, 2.5)) + .collect::>(); + let observations = truth + .iter() + .enumerate() + .flat_map(|(point_index, &point)| { + views.iter().enumerate().map(move |(view_index, view)| { + let camera_point = view.camera_from_world.transform_point(point); + BundleObservation { + view_index, + point_index, + pixel: view.camera.project(camera_point).unwrap(), + } + }) + }) + .collect::>(); + c.bench_function("bundle_adjust_fixed_cameras/100_points_3_views", |b| { + b.iter_batched( + || BundleProblem { + views: views.clone(), + points: truth.iter().map(|point| *point + Vec3::new(0.02, -0.01, 0.05)).collect(), + observations: observations.clone(), + }, + |mut problem| { + bundle_adjust_points(black_box(&mut problem), CalibrationOptions::default()) + .unwrap() + }, + criterion::BatchSize::SmallInput, + ); + }); +} + +criterion_group!(benches, benchmark_calibration); +criterion_main!(benches); diff --git a/crates/spatialrust-camera/src/calibration.rs b/crates/spatialrust-camera/src/calibration.rs new file mode 100644 index 0000000..a7af01b --- /dev/null +++ b/crates/spatialrust-camera/src/calibration.rs @@ -0,0 +1,791 @@ +//! Deterministic camera calibration contracts and small dense solvers. + +use spatialrust_math::{solve_linear_system, LeastSquaresResult, Mat3, Vec2, Vec3}; + +use crate::{CameraIntrinsics, KannalaBrandt4, PinholeCamera}; + +/// Calibration input or numerical failure. +#[derive(Clone, Debug, PartialEq, thiserror::Error)] +pub enum CalibrationError { + /// The dataset is empty, non-finite, inconsistent, or underconstrained. + #[error("invalid calibration dataset: {0}")] + InvalidDataset(String), + /// A normal equation was singular or ill-conditioned. + #[error("calibration normal equation is singular")] + Singular, + /// Projection failed while evaluating residuals. + #[error("calibration projection failed: {0}")] + Projection(String), +} + +/// Shared robust least-squares controls. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct CalibrationOptions { + /// Maximum robust/refinement iterations. + pub max_iterations: usize, + /// Huber transition in residual units; must be finite and positive. + pub huber_delta: f64, + /// Parameter-step convergence threshold. + pub convergence_tolerance: f64, +} + +impl Default for CalibrationOptions { + fn default() -> Self { + Self { max_iterations: 12, huber_delta: 2.0, convergence_tolerance: 1e-10 } + } +} + +/// Common numerical receipt returned by calibration solvers. +#[derive(Clone, Copy, Debug, Default, PartialEq)] +pub struct CalibrationReport { + /// Root-mean-square residual in pixels or the solver's documented units. + pub rms_residual: f64, + /// Maximum residual magnitude. + pub max_residual: f64, + /// Number of observations evaluated. + pub observation_count: usize, + /// Iterations executed. + pub iterations: usize, + /// Whether the parameter step reached the configured tolerance. + pub converged: bool, +} + +/// Known camera-space point and observed image pixel for mono calibration. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct PinholeObservation { + /// Point expressed in the camera frame. + pub camera_point: Vec3, + /// Measured image pixel. + pub pixel: Vec2, +} + +/// Fits `fx, fy, cx, cy` from known camera-space points with robust reweighting. +pub fn calibrate_pinhole( + observations: &[PinholeObservation], + width: usize, + height: usize, + options: CalibrationOptions, +) -> Result<(PinholeCamera, CalibrationReport), CalibrationError> { + validate_options(options)?; + if observations.len() < 4 { + return Err(CalibrationError::InvalidDataset( + "pinhole calibration needs at least four observations".to_owned(), + )); + } + let normalized = observations + .iter() + .map(|observation| { + validate_point_pixel(observation.camera_point, observation.pixel)?; + Ok(( + observation.camera_point.x / observation.camera_point.z, + observation.camera_point.y / observation.camera_point.z, + )) + }) + .collect::, CalibrationError>>()?; + let mut weights = vec![1.0; observations.len()]; + let mut previous = [0.0; 4]; + let mut parameters = [0.0; 4]; + let mut converged = false; + let mut iterations = 0; + for iteration in 0..options.max_iterations.max(1) { + let x_values = normalized.iter().map(|value| value.0).collect::>(); + let y_values = normalized.iter().map(|value| value.1).collect::>(); + let u_values = observations.iter().map(|value| value.pixel.x).collect::>(); + let v_values = observations.iter().map(|value| value.pixel.y).collect::>(); + let (fx, cx) = fit_slope_intercept(&x_values, &u_values, &weights)?; + let (fy, cy) = fit_slope_intercept(&y_values, &v_values, &weights)?; + parameters = [fx, fy, cx, cy]; + if fx <= 0.0 || fy <= 0.0 { + return Err(CalibrationError::InvalidDataset( + "calibrated focal lengths are not positive".to_owned(), + )); + } + iterations = iteration + 1; + let step = parameters + .iter() + .zip(previous) + .map(|(current, old)| (current - old).abs()) + .fold(0.0, f64::max); + let residuals = observations + .iter() + .zip(&normalized) + .map(|(observation, &(x, y))| { + (fx.mul_add(x, cx) - observation.pixel.x) + .hypot(fy.mul_add(y, cy) - observation.pixel.y) + }) + .collect::>(); + update_huber_weights(&residuals, options.huber_delta, &mut weights); + if iteration > 0 && step <= options.convergence_tolerance { + converged = true; + break; + } + previous = parameters; + } + let intrinsics = CameraIntrinsics::try_new( + parameters[0], + parameters[1], + parameters[2], + parameters[3], + width, + height, + ) + .map_err(|error| CalibrationError::InvalidDataset(error.to_string()))?; + let camera = PinholeCamera::new(intrinsics); + let residuals = observations + .iter() + .map(|observation| { + camera + .project(observation.camera_point) + .map(|pixel| pixel_distance(pixel, observation.pixel)) + .map_err(|error| CalibrationError::Projection(error.to_string())) + }) + .collect::, _>>()?; + Ok((camera, report(&residuals, iterations, converged))) +} + +/// Fisheye angle/radius sample in normalized image coordinates. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct FisheyeObservation { + /// Incident ray angle in radians. + pub theta: f64, + /// Measured distorted normalized radius. + pub distorted_radius: f64, +} + +/// Fits a Kannala–Brandt four-coefficient angle polynomial. +pub fn calibrate_fisheye( + observations: &[FisheyeObservation], +) -> Result<(KannalaBrandt4, CalibrationReport), CalibrationError> { + if observations.len() < 4 { + return Err(CalibrationError::InvalidDataset( + "fisheye calibration needs at least four non-zero angles".to_owned(), + )); + } + let mut normal = vec![vec![0.0; 4]; 4]; + let mut rhs = vec![0.0; 4]; + for observation in observations { + if !observation.theta.is_finite() + || !observation.distorted_radius.is_finite() + || observation.theta <= 0.0 + { + return Err(CalibrationError::InvalidDataset( + "fisheye samples must have finite positive theta".to_owned(), + )); + } + let theta2 = observation.theta * observation.theta; + let row = [theta2, theta2.powi(2), theta2.powi(3), theta2.powi(4)]; + let target = observation.distorted_radius / observation.theta - 1.0; + accumulate_normal(&mut normal, &mut rhs, &row, target, 1.0); + } + let values = solved(normal, rhs)?; + let model = KannalaBrandt4 { k1: values[0], k2: values[1], k3: values[2], k4: values[3] }; + let residuals = observations + .iter() + .map(|observation| { + let theta2 = observation.theta * observation.theta; + let predicted = observation.theta + * (1.0 + + theta2 + * (model.k1 + + theta2 * (model.k2 + theta2 * (model.k3 + theta2 * model.k4)))); + (predicted - observation.distorted_radius).abs() + }) + .collect::>(); + Ok((model, report(&residuals, 1, true))) +} + +/// Rigid transform represented as a rotation matrix and translation. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct RigidTransform3 { + /// Source-to-destination rotation. + pub rotation: Mat3, + /// Source-to-destination translation. + pub translation: Vec3, +} + +impl RigidTransform3 { + /// Applies the transform to a point. + #[must_use] + pub fn transform_point(self, point: Vec3) -> Vec3 { + self.rotation.mul_vec3(point) + self.translation + } +} + +/// Matched 3D point expressed in left and right camera frames. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct StereoPointPair { + /// Point in the left camera frame. + pub left: Vec3, + /// Same point in the right camera frame. + pub right: Vec3, +} + +/// Stereo calibration result with explicit right-from-left transform. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct StereoCalibration { + /// Left calibrated camera. + pub left: PinholeCamera, + /// Right calibrated camera. + pub right: PinholeCamera, + /// Transform mapping left-camera points into the right camera. + pub right_from_left: RigidTransform3, + /// 3D alignment residual receipt. + pub report: CalibrationReport, +} + +/// Fits stereo translation for a supplied relative rotation. +pub fn calibrate_stereo_translation( + left: PinholeCamera, + right: PinholeCamera, + rotation: Mat3, + pairs: &[StereoPointPair], +) -> Result { + if pairs.len() < 3 { + return Err(CalibrationError::InvalidDataset( + "stereo calibration needs at least three point pairs".to_owned(), + )); + } + validate_rotation(rotation)?; + let mut translation = Vec3::new(0.0, 0.0, 0.0); + for pair in pairs { + validate_vec3(pair.left)?; + validate_vec3(pair.right)?; + translation = translation + pair.right - rotation.mul_vec3(pair.left); + } + let inverse_count = 1.0 / pairs.len() as f64; + translation = Vec3::new( + translation.x * inverse_count, + translation.y * inverse_count, + translation.z * inverse_count, + ); + let transform = RigidTransform3 { rotation, translation }; + let residuals = pairs + .iter() + .map(|pair| (transform.transform_point(pair.left) - pair.right).length()) + .collect::>(); + Ok(StereoCalibration { + left, + right, + right_from_left: transform, + report: report(&residuals, 1, true), + }) +} + +/// Robot/camera relative motions for `A X = X B` hand-eye calibration. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct HandEyeMotionPair { + /// Robot/end-effector motion `A`. + pub robot_motion: RigidTransform3, + /// Camera motion `B`. + pub camera_motion: RigidTransform3, +} + +/// Solves hand-eye translation for a supplied hand-eye rotation. +pub fn calibrate_hand_eye_translation( + pairs: &[HandEyeMotionPair], + hand_eye_rotation: Mat3, +) -> Result<(RigidTransform3, CalibrationReport), CalibrationError> { + if pairs.len() < 2 { + return Err(CalibrationError::InvalidDataset( + "hand-eye translation needs at least two motion pairs".to_owned(), + )); + } + validate_rotation(hand_eye_rotation)?; + let mut normal = vec![vec![0.0; 3]; 3]; + let mut rhs = vec![0.0; 3]; + for pair in pairs { + validate_rotation(pair.robot_motion.rotation)?; + validate_rotation(pair.camera_motion.rotation)?; + let target = hand_eye_rotation.mul_vec3(pair.camera_motion.translation) + - pair.robot_motion.translation; + for row in 0..3 { + let coefficients = [ + pair.robot_motion.rotation.m[row][0] - if row == 0 { 1.0 } else { 0.0 }, + pair.robot_motion.rotation.m[row][1] - if row == 1 { 1.0 } else { 0.0 }, + pair.robot_motion.rotation.m[row][2] - if row == 2 { 1.0 } else { 0.0 }, + ]; + accumulate_normal( + &mut normal, + &mut rhs, + &coefficients, + [target.x, target.y, target.z][row], + 1.0, + ); + } + } + let values = solved(normal, rhs)?; + let result = RigidTransform3 { + rotation: hand_eye_rotation, + translation: Vec3::new(values[0], values[1], values[2]), + }; + let residuals = pairs + .iter() + .map(|pair| { + let left = pair.robot_motion.rotation.mul_vec3(result.translation) + + pair.robot_motion.translation; + let right = + result.rotation.mul_vec3(pair.camera_motion.translation) + result.translation; + let rotation_error = matrix_distance( + pair.robot_motion.rotation.mul_mat3(result.rotation), + result.rotation.mul_mat3(pair.camera_motion.rotation), + ); + (left - right).length().hypot(rotation_error) + }) + .collect::>(); + Ok((result, report(&residuals, 1, true))) +} + +/// Calibrated camera and world-to-camera pose used by bundle adjustment. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct BundleView { + /// Camera intrinsics/distortion. + pub camera: PinholeCamera, + /// World-to-camera transform. + pub camera_from_world: RigidTransform3, +} + +/// One 2D observation of a bundle point. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct BundleObservation { + /// Index into [`BundleProblem::views`]. + pub view_index: usize, + /// Index into [`BundleProblem::points`]. + pub point_index: usize, + /// Measured image pixel. + pub pixel: Vec2, +} + +/// Sparse fixed-camera bundle problem. +#[derive(Clone, Debug, PartialEq)] +pub struct BundleProblem { + /// Fixed calibrated camera views. + pub views: Vec, + /// Mutable world-space points. + pub points: Vec>, + /// Sparse image observations. + pub observations: Vec, +} + +/// Refines world-space points while keeping calibrated camera poses fixed. +pub fn bundle_adjust_points( + problem: &mut BundleProblem, + options: CalibrationOptions, +) -> Result { + validate_options(options)?; + validate_bundle(problem)?; + let mut converged = false; + let mut iterations = 0; + for iteration in 0..options.max_iterations.max(1) { + let mut max_step: f64 = 0.0; + for point_index in 0..problem.points.len() { + let mut normal = vec![vec![0.0; 3]; 3]; + let mut rhs = vec![0.0; 3]; + let mut count = 0; + for observation in + problem.observations.iter().filter(|value| value.point_index == point_index) + { + let view = problem.views[observation.view_index]; + let point = problem.points[point_index]; + let predicted = project_world(view, point)?; + let residual = + [predicted.x - observation.pixel.x, predicted.y - observation.pixel.y]; + let magnitude = residual[0].hypot(residual[1]); + let weight = huber_weight(magnitude, options.huber_delta); + const STEP: f64 = 1e-6; + let shifted_x = project_world(view, point + Vec3::new(STEP, 0.0, 0.0))?; + let shifted_y = project_world(view, point + Vec3::new(0.0, STEP, 0.0))?; + let shifted_z = project_world(view, point + Vec3::new(0.0, 0.0, STEP))?; + let jacobian = [ + [ + (shifted_x.x - predicted.x) / STEP, + (shifted_y.x - predicted.x) / STEP, + (shifted_z.x - predicted.x) / STEP, + ], + [ + (shifted_x.y - predicted.y) / STEP, + (shifted_y.y - predicted.y) / STEP, + (shifted_z.y - predicted.y) / STEP, + ], + ]; + for row in 0..2 { + accumulate_normal( + &mut normal, + &mut rhs, + &jacobian[row], + -residual[row], + weight, + ); + } + count += 1; + } + if count >= 2 { + let step = solved(normal, rhs)?; + let delta = Vec3::new(step[0], step[1], step[2]); + problem.points[point_index] = problem.points[point_index] + delta; + max_step = max_step.max(delta.length()); + } + } + iterations = iteration + 1; + if max_step <= options.convergence_tolerance { + converged = true; + break; + } + } + let residuals = bundle_residuals(problem)?; + Ok(report(&residuals, iterations, converged)) +} + +fn validate_bundle(problem: &BundleProblem) -> Result<(), CalibrationError> { + if problem.views.is_empty() || problem.points.is_empty() || problem.observations.is_empty() { + return Err(CalibrationError::InvalidDataset( + "bundle problem must be non-empty".to_owned(), + )); + } + for view in &problem.views { + validate_rotation(view.camera_from_world.rotation)?; + validate_vec3(view.camera_from_world.translation)?; + } + for observation in &problem.observations { + if observation.view_index >= problem.views.len() + || observation.point_index >= problem.points.len() + { + return Err(CalibrationError::InvalidDataset( + "bundle observation index out of bounds".to_owned(), + )); + } + if !observation.pixel.x.is_finite() || !observation.pixel.y.is_finite() { + return Err(CalibrationError::InvalidDataset("bundle pixel must be finite".to_owned())); + } + } + Ok(()) +} + +fn bundle_residuals(problem: &BundleProblem) -> Result, CalibrationError> { + problem + .observations + .iter() + .map(|observation| { + project_world( + problem.views[observation.view_index], + problem.points[observation.point_index], + ) + .map(|pixel| pixel_distance(pixel, observation.pixel)) + }) + .collect() +} + +fn project_world(view: BundleView, point: Vec3) -> Result, CalibrationError> { + view.camera + .project(view.camera_from_world.transform_point(point)) + .map_err(|error| CalibrationError::Projection(error.to_string())) +} + +fn validate_options(options: CalibrationOptions) -> Result<(), CalibrationError> { + if options.max_iterations == 0 + || !options.huber_delta.is_finite() + || options.huber_delta <= 0.0 + || !options.convergence_tolerance.is_finite() + || options.convergence_tolerance <= 0.0 + { + return Err(CalibrationError::InvalidDataset("invalid solver options".to_owned())); + } + Ok(()) +} + +fn validate_point_pixel(point: Vec3, pixel: Vec2) -> Result<(), CalibrationError> { + validate_vec3(point)?; + if point.z <= 0.0 || !pixel.x.is_finite() || !pixel.y.is_finite() { + return Err(CalibrationError::InvalidDataset( + "camera points need positive depth and finite pixels".to_owned(), + )); + } + Ok(()) +} + +fn validate_vec3(value: Vec3) -> Result<(), CalibrationError> { + if !value.x.is_finite() || !value.y.is_finite() || !value.z.is_finite() { + return Err(CalibrationError::InvalidDataset("3D values must be finite".to_owned())); + } + Ok(()) +} + +fn validate_rotation(rotation: Mat3) -> Result<(), CalibrationError> { + if rotation.m.iter().flatten().any(|value| !value.is_finite()) { + return Err(CalibrationError::InvalidDataset( + "rotation coefficients must be finite".to_owned(), + )); + } + let identity_error = + matrix_distance(rotation.transpose().mul_mat3(rotation), Mat3::::identity()); + let determinant = rotation.m[0][0] + * (rotation.m[1][1] * rotation.m[2][2] - rotation.m[1][2] * rotation.m[2][1]) + - rotation.m[0][1] + * (rotation.m[1][0] * rotation.m[2][2] - rotation.m[1][2] * rotation.m[2][0]) + + rotation.m[0][2] + * (rotation.m[1][0] * rotation.m[2][1] - rotation.m[1][1] * rotation.m[2][0]); + if identity_error > 1e-6 || (determinant - 1.0).abs() > 1e-6 { + return Err(CalibrationError::InvalidDataset( + "rotation matrix must be right-handed and orthonormal".to_owned(), + )); + } + Ok(()) +} + +fn matrix_distance(left: Mat3, right: Mat3) -> f64 { + left.m + .iter() + .flatten() + .zip(right.m.iter().flatten()) + .map(|(left, right)| (left - right).powi(2)) + .sum::() + .sqrt() +} + +fn fit_slope_intercept( + x: &[f64], + y: &[f64], + weights: &[f64], +) -> Result<(f64, f64), CalibrationError> { + let mut normal = vec![vec![0.0; 2]; 2]; + let mut rhs = vec![0.0; 2]; + for ((&x, &y), &weight) in x.iter().zip(y).zip(weights) { + accumulate_normal(&mut normal, &mut rhs, &[x, 1.0], y, weight); + } + let values = solved(normal, rhs)?; + Ok((values[0], values[1])) +} + +fn accumulate_normal( + normal: &mut [Vec], + rhs: &mut [f64], + row: &[f64], + target: f64, + weight: f64, +) { + for i in 0..row.len() { + rhs[i] += weight * row[i] * target; + for j in 0..row.len() { + normal[i][j] += weight * row[i] * row[j]; + } + } +} + +fn solved(normal: Vec>, rhs: Vec) -> Result, CalibrationError> { + match solve_linear_system(normal, rhs) { + LeastSquaresResult::Solved(values) => Ok(values), + LeastSquaresResult::Singular => Err(CalibrationError::Singular), + } +} + +fn update_huber_weights(residuals: &[f64], delta: f64, weights: &mut [f64]) { + for (weight, &residual) in weights.iter_mut().zip(residuals) { + *weight = huber_weight(residual, delta); + } +} + +fn huber_weight(residual: f64, delta: f64) -> f64 { + if residual <= delta || residual <= f64::EPSILON { + 1.0 + } else { + delta / residual + } +} + +fn pixel_distance(left: Vec2, right: Vec2) -> f64 { + (left.x - right.x).hypot(left.y - right.y) +} + +fn report(residuals: &[f64], iterations: usize, converged: bool) -> CalibrationReport { + let sum_squared = residuals.iter().map(|value| value * value).sum::(); + CalibrationReport { + rms_residual: (sum_squared / residuals.len().max(1) as f64).sqrt(), + max_residual: residuals.iter().copied().fold(0.0, f64::max), + observation_count: residuals.len(), + iterations, + converged, + } +} + +#[cfg(test)] +mod tests { + use super::*; + + fn camera() -> PinholeCamera { + PinholeCamera::new(CameraIntrinsics::try_new(500.0, 510.0, 320.0, 240.0, 640, 480).unwrap()) + } + + fn rotation_z(angle: f64) -> Mat3 { + let (sin, cos) = angle.sin_cos(); + Mat3::from_rows([cos, -sin, 0.0], [sin, cos, 0.0], [0.0, 0.0, 1.0]) + } + + fn rotation_x(angle: f64) -> Mat3 { + let (sin, cos) = angle.sin_cos(); + Mat3::from_rows([1.0, 0.0, 0.0], [0.0, cos, -sin], [0.0, sin, cos]) + } + + fn rotation_y(angle: f64) -> Mat3 { + let (sin, cos) = angle.sin_cos(); + Mat3::from_rows([cos, 0.0, sin], [0.0, 1.0, 0.0], [-sin, 0.0, cos]) + } + + #[test] + fn robust_mono_recovers_intrinsics_with_one_outlier() { + let expected = camera(); + let mut observations = Vec::new(); + for y in -3_i32..=3 { + for x in -4_i32..=4 { + let point = + Vec3::new(x as f64 * 0.08, y as f64 * 0.07, 1.5 + (x + y).abs() as f64 * 0.03); + observations.push(PinholeObservation { + camera_point: point, + pixel: expected.project(point).unwrap(), + }); + } + } + observations[0].pixel.x += 80.0; + let (calibrated, report) = calibrate_pinhole( + &observations, + 640, + 480, + CalibrationOptions { huber_delta: 1.0, ..CalibrationOptions::default() }, + ) + .unwrap(); + assert!((calibrated.intrinsics.fx - 500.0).abs() < 0.2); + assert!((calibrated.intrinsics.fy - 510.0).abs() < 1e-8); + assert!((calibrated.intrinsics.cx - 320.0).abs() < 0.1); + assert_eq!(report.observation_count, observations.len()); + } + + #[test] + fn fisheye_fit_recovers_angle_polynomial() { + let expected = KannalaBrandt4 { k1: 0.03, k2: -0.004, k3: 0.0005, k4: -0.00003 }; + let observations = (1..=12) + .map(|index| { + let theta = index as f64 * 0.08; + let theta2 = theta * theta; + FisheyeObservation { + theta, + distorted_radius: theta + * (1.0 + + theta2 + * (expected.k1 + + theta2 + * (expected.k2 + + theta2 * (expected.k3 + theta2 * expected.k4)))), + } + }) + .collect::>(); + let (actual, report) = calibrate_fisheye(&observations).unwrap(); + assert!((actual.k1 - expected.k1).abs() < 1e-9); + assert!((actual.k4 - expected.k4).abs() < 1e-9); + assert!(report.rms_residual < 1e-12); + } + + #[test] + fn stereo_translation_matches_known_transform() { + let camera = camera(); + let rotation = rotation_z(0.03); + let translation = Vec3::new(-0.2, 0.01, 0.005); + let pairs = (0..8) + .map(|index| { + let left = Vec3::new(index as f64 * 0.1 - 0.3, 0.05 * index as f64, 2.0); + StereoPointPair { left, right: rotation.mul_vec3(left) + translation } + }) + .collect::>(); + let result = calibrate_stereo_translation(camera, camera, rotation, &pairs).unwrap(); + assert!((result.right_from_left.translation - translation).length() < 1e-12); + assert!(result.report.rms_residual < 1e-12); + } + + #[test] + fn hand_eye_translation_satisfies_ax_xb() { + let expected = Vec3::new(0.12, -0.04, 0.3); + let pairs = [rotation_x(0.4), rotation_y(-0.7), rotation_z(1.1)] + .into_iter() + .enumerate() + .map(|(index, rotation)| { + let robot_translation = Vec3::new(0.03 * index as f64, 0.02, -0.01); + HandEyeMotionPair { + robot_motion: RigidTransform3 { rotation, translation: robot_translation }, + camera_motion: RigidTransform3 { + rotation, + translation: robot_translation + (rotation.mul_vec3(expected) - expected), + }, + } + }) + .collect::>(); + let (result, report) = + calibrate_hand_eye_translation(&pairs, Mat3::::identity()).unwrap(); + assert!((result.translation - expected).length() < 1e-10); + assert!(report.rms_residual < 1e-10); + } + + #[test] + fn fixed_camera_bundle_reduces_reprojection_error() { + let camera = camera(); + let views = vec![ + BundleView { + camera, + camera_from_world: RigidTransform3 { + rotation: Mat3::::identity(), + translation: Vec3::new(0.0, 0.0, 0.0), + }, + }, + BundleView { + camera, + camera_from_world: RigidTransform3 { + rotation: Mat3::::identity(), + translation: Vec3::new(-0.4, 0.0, 0.0), + }, + }, + ]; + let truth = [Vec3::new(0.1, -0.1, 2.5), Vec3::new(-0.2, 0.15, 3.0)]; + let mut observations = Vec::new(); + for (point_index, &point) in truth.iter().enumerate() { + for (view_index, &view) in views.iter().enumerate() { + observations.push(BundleObservation { + view_index, + point_index, + pixel: project_world(view, point).unwrap(), + }); + } + } + let mut problem = BundleProblem { + views, + points: truth.iter().map(|point| *point + Vec3::new(0.08, -0.04, 0.2)).collect(), + observations, + }; + let before = report(&bundle_residuals(&problem).unwrap(), 0, false).rms_residual; + let after = bundle_adjust_points(&mut problem, CalibrationOptions::default()).unwrap(); + assert!(after.rms_residual < before * 1e-4); + assert!(after.rms_residual < 1e-6); + } + + #[test] + fn calibration_contracts_reject_invalid_rotations_and_indices() { + let camera = camera(); + let reflection = Mat3::from_rows([1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, -1.0]); + let pairs = + [StereoPointPair { left: Vec3::new(0.0, 0.0, 1.0), right: Vec3::new(0.0, 0.0, 1.0) }; + 3]; + assert!(calibrate_stereo_translation(camera, camera, reflection, &pairs).is_err()); + + let mut problem = BundleProblem { + views: vec![BundleView { + camera, + camera_from_world: RigidTransform3 { + rotation: Mat3::::identity(), + translation: Vec3::new(0.0, 0.0, 0.0), + }, + }], + points: vec![Vec3::new(0.0, 0.0, 2.0)], + observations: vec![BundleObservation { + view_index: 1, + point_index: 0, + pixel: Vec2 { x: 0.0, y: 0.0 }, + }], + }; + assert!(bundle_adjust_points(&mut problem, CalibrationOptions::default()).is_err()); + } +} diff --git a/crates/spatialrust-camera/src/distortion.rs b/crates/spatialrust-camera/src/distortion.rs index 7166b8f..4693e8e 100644 --- a/crates/spatialrust-camera/src/distortion.rs +++ b/crates/spatialrust-camera/src/distortion.rs @@ -15,6 +15,72 @@ pub struct BrownConrady { pub k3: f64, } +/// Kannala–Brandt four-coefficient fisheye model. +#[derive(Clone, Copy, Debug, Default, PartialEq)] +pub struct KannalaBrandt4 { + /// Cubic angle coefficient. + pub k1: f64, + /// Fifth-order angle coefficient. + pub k2: f64, + /// Seventh-order angle coefficient. + pub k3: f64, + /// Ninth-order angle coefficient. + pub k4: f64, +} + +impl KannalaBrandt4 { + /// Distorts normalized pinhole coordinates using the equidistant angle polynomial. + #[must_use] + pub fn distort(self, point: Vec2) -> Vec2 { + let radius = point.x.hypot(point.y); + if radius <= f64::EPSILON { + return point; + } + let theta = radius.atan(); + let theta2 = theta * theta; + let distorted_theta = theta + * (1.0 + + theta2 * (self.k1 + theta2 * (self.k2 + theta2 * (self.k3 + theta2 * self.k4)))); + let scale = distorted_theta / radius; + Vec2 { x: point.x * scale, y: point.y * scale } + } + + /// Iteratively removes fisheye distortion from normalized coordinates. + #[must_use] + pub fn undistort(self, point: Vec2) -> Vec2 { + let distorted_radius = point.x.hypot(point.y); + if distorted_radius <= f64::EPSILON { + return point; + } + let mut theta = distorted_radius.min(std::f64::consts::FRAC_PI_2 - 1e-6); + for _ in 0..16 { + let theta2 = theta * theta; + let theta4 = theta2 * theta2; + let theta6 = theta4 * theta2; + let theta8 = theta4 * theta4; + let value = theta + * (1.0 + self.k1 * theta2 + self.k2 * theta4 + self.k3 * theta6 + self.k4 * theta8) + - distorted_radius; + let derivative = 1.0 + + 3.0 * self.k1 * theta2 + + 5.0 * self.k2 * theta4 + + 7.0 * self.k3 * theta6 + + 9.0 * self.k4 * theta8; + if derivative.abs() < 1e-12 { + break; + } + let step = value / derivative; + theta -= step; + if step.abs() < 1e-13 { + break; + } + } + let radius = theta.tan(); + let scale = radius / distorted_radius; + Vec2 { x: point.x * scale, y: point.y * scale } + } +} + impl BrownConrady { /// Returns whether all coefficients are zero. #[must_use] @@ -73,7 +139,7 @@ impl BrownConrady { #[cfg(test)] mod tests { - use super::BrownConrady; + use super::{BrownConrady, KannalaBrandt4}; use spatialrust_math::Vec2; #[test] @@ -85,4 +151,14 @@ mod tests { assert!((recovered.y - point.y).abs() < 1e-9); } } + + #[test] + fn fisheye_roundtrip() { + let model = KannalaBrandt4 { k1: 0.02, k2: -0.003, k3: 0.0004, k4: -0.00002 }; + for point in [Vec2 { x: -0.8, y: 0.5 }, Vec2 { x: 0.2, y: -0.1 }] { + let recovered = model.undistort(model.distort(point)); + assert!((recovered.x - point.x).abs() < 1e-10); + assert!((recovered.y - point.y).abs() < 1e-10); + } + } } diff --git a/crates/spatialrust-camera/src/lib.rs b/crates/spatialrust-camera/src/lib.rs index 3683933..f9e95c6 100644 --- a/crates/spatialrust-camera/src/lib.rs +++ b/crates/spatialrust-camera/src/lib.rs @@ -3,13 +3,20 @@ #![deny(unsafe_code)] #![warn(missing_docs)] +mod calibration; mod distortion; mod model; /// Dense RGB-D fill may use audited AVX2 kernels on x86_64. #[allow(unsafe_code)] mod rgbd; -pub use distortion::BrownConrady; +pub use calibration::{ + bundle_adjust_points, calibrate_fisheye, calibrate_hand_eye_translation, calibrate_pinhole, + calibrate_stereo_translation, BundleObservation, BundleProblem, BundleView, CalibrationError, + CalibrationOptions, CalibrationReport, FisheyeObservation, HandEyeMotionPair, + PinholeObservation, RigidTransform3, StereoCalibration, StereoPointPair, +}; +pub use distortion::{BrownConrady, KannalaBrandt4}; pub use model::{CameraError, CameraIntrinsics, PinholeCamera}; pub use rgbd::{ depth_to_point_cloud, depth_to_xyz_dense, depth_to_xyz_dense_into, rgbd_to_point_cloud, diff --git a/crates/spatialrust-platform/src/stability.rs b/crates/spatialrust-platform/src/stability.rs index b9387db..4884dbf 100644 --- a/crates/spatialrust-platform/src/stability.rs +++ b/crates/spatialrust-platform/src/stability.rs @@ -114,6 +114,7 @@ impl StabilityRegistry { registry.register(path, ApiStabilityClass::Stable); } let provisional = [ + "spatialrust-camera::calibration", "spatialrust-vision::geometry", "spatialrust-vision::stereo", "spatialrust-vision::optical-flow", diff --git a/crates/spatialrust-py/spatialrust.pyi b/crates/spatialrust-py/spatialrust.pyi index 381e40d..1bcd632 100644 --- a/crates/spatialrust-py/spatialrust.pyi +++ b/crates/spatialrust-py/spatialrust.pyi @@ -27,7 +27,8 @@ __all__: list[str] = [ "ransac_sphere", "ransac_cylinder", "chamfer_distance", "hausdorff_distance", "apply_transform", "recenter", "scale", "normalize_unit_sphere", "merge", "centroid", "bounding_box", "oriented_bounding_box", "voxelize", "range_image", - "rgbd_to_point_cloud", "depth_to_xyz", "filter2d_image", "gaussian_blur_image", + "rgbd_to_point_cloud", "depth_to_xyz", "calibrate_pinhole_camera", + "calibrate_fisheye_angles", "filter2d_image", "gaussian_blur_image", "median_blur_image", "bilateral_filter_image", "sobel_image", "scharr_image", "laplacian_image", "pyr_down_image", "pyr_up_image", "morphology_image", "threshold_image", "otsu_threshold_image", "adaptive_threshold_image", @@ -116,6 +117,19 @@ def depth_to_xyz( """ ... +def calibrate_pinhole_camera( + camera_points: _F64Array, + pixels: _F64Array, + width: int, + height: int, + huber_delta: float = ..., + max_iterations: int = ..., +) -> tuple[float, float, float, float, float, float]: ... + +def calibrate_fisheye_angles( + theta: _F64Array, distorted_radius: _F64Array +) -> tuple[float, float, float, float, float]: ... + def rgbd_to_point_cloud( depth: _F32Array, color: _U8Array, diff --git a/crates/spatialrust-py/src/lib.rs b/crates/spatialrust-py/src/lib.rs index 55fe2dd..fa57255 100644 --- a/crates/spatialrust-py/src/lib.rs +++ b/crates/spatialrust-py/src/lib.rs @@ -27,6 +27,10 @@ use spatialrust::ai::{ RunOptions as AiRunOptions, SessionOptions as AiSessionOptions, }; +use spatialrust::camera::{ + calibrate_fisheye as calibrate_fisheye_native, calibrate_pinhole as calibrate_pinhole_native, + CalibrationOptions, FisheyeObservation, PinholeObservation, +}; use spatialrust::core::{PointBuffer, PointBufferSet, SpatialMetadata}; use spatialrust::features::{ orient_normals_consistent, BoundaryConfig, BoundaryDetector, FeatureEstimator, @@ -693,6 +697,69 @@ fn gray_u8_image_from_numpy(array: PyReadonlyArray2<'_, u8>) -> PyResult, + pixels: PyReadonlyArray2<'_, f64>, + width: usize, + height: usize, + huber_delta: f64, + max_iterations: usize, +) -> PyResult<(f64, f64, f64, f64, f64, f64)> { + let points = camera_points.as_array(); + let pixels = pixels.as_array(); + if points.shape().len() != 2 || points.shape()[1] != 3 { + return Err(PyValueError::new_err("camera_points must have shape (N, 3)")); + } + if pixels.shape() != [points.shape()[0], 2] { + return Err(PyValueError::new_err("pixels must have shape (N, 2)")); + } + let observations = (0..points.shape()[0]) + .map(|index| PinholeObservation { + camera_point: Vec3::new(points[[index, 0]], points[[index, 1]], points[[index, 2]]), + pixel: spatialrust::Vec2 { x: pixels[[index, 0]], y: pixels[[index, 1]] }, + }) + .collect::>(); + let (camera, report) = calibrate_pinhole_native( + &observations, + width, + height, + CalibrationOptions { max_iterations, huber_delta, ..CalibrationOptions::default() }, + ) + .map_err(to_py_err)?; + Ok(( + camera.intrinsics.fx, + camera.intrinsics.fy, + camera.intrinsics.cx, + camera.intrinsics.cy, + report.rms_residual, + report.max_residual, + )) +} + +/// Fits Kannala–Brandt4 coefficients from incident angles and distorted radii. +#[pyfunction] +fn calibrate_fisheye_angles( + theta: PyReadonlyArray1<'_, f64>, + distorted_radius: PyReadonlyArray1<'_, f64>, +) -> PyResult<(f64, f64, f64, f64, f64)> { + let theta = theta.as_array(); + let radii = distorted_radius.as_array(); + if theta.len() != radii.len() { + return Err(PyValueError::new_err("theta and distorted_radius lengths must match")); + } + let observations = theta + .iter() + .copied() + .zip(radii.iter().copied()) + .map(|(theta, distorted_radius)| FisheyeObservation { theta, distorted_radius }) + .collect::>(); + let (model, report) = calibrate_fisheye_native(&observations).map_err(to_py_err)?; + Ok((model.k1, model.k2, model.k3, model.k4, report.rms_residual)) +} + fn cloud_from_xyz(arr: PyReadonlyArray2<'_, f32>) -> PyResult { let view = arr.as_array(); let shape = view.shape(); @@ -3073,6 +3140,8 @@ fn spatialrust_module(m: &Bound<'_, PyModule>) -> PyResult<()> { m.add_function(wrap_pyfunction!(range_image, m)?)?; m.add_function(wrap_pyfunction!(depth_to_xyz, m)?)?; m.add_function(wrap_pyfunction!(rgbd_to_point_cloud, m)?)?; + m.add_function(wrap_pyfunction!(calibrate_pinhole_camera, m)?)?; + m.add_function(wrap_pyfunction!(calibrate_fisheye_angles, m)?)?; m.add_function(wrap_pyfunction!(filter2d_image, m)?)?; m.add_function(wrap_pyfunction!(gaussian_blur_image, m)?)?; m.add_function(wrap_pyfunction!(median_blur_image, m)?)?; diff --git a/crates/spatialrust-py/tests/test_bindings.py b/crates/spatialrust-py/tests/test_bindings.py index 522cadc..155f51b 100644 --- a/crates/spatialrust-py/tests/test_bindings.py +++ b/crates/spatialrust-py/tests/test_bindings.py @@ -736,3 +736,38 @@ def test_tensor_zero_copy_dlpack_import_retains_producer(dtype): copied = np.from_dlpack(imported.copy()) np.testing.assert_array_equal(copied, np.arange(12, dtype=dtype).reshape(3, 4)) assert "DLPackTensorView(" in repr(imported) + + +def test_calibration_bindings_recover_intrinsics_and_fisheye(): + points = np.array( + [ + [x * 0.1, y * 0.08, 2.0 + 0.03 * abs(x + y)] + for y in range(-3, 4) + for x in range(-4, 5) + ], + dtype=np.float64, + ) + pixels = np.column_stack( + ( + 500.0 * points[:, 0] / points[:, 2] + 320.0, + 510.0 * points[:, 1] / points[:, 2] + 240.0, + ) + ) + result = sr.calibrate_pinhole_camera(points, pixels, 640, 480) + np.testing.assert_allclose(result[:4], [500.0, 510.0, 320.0, 240.0], atol=1e-9) + assert result[4] < 1e-9 + + expected = np.array([0.03, -0.004, 0.0005, -0.00003]) + theta = np.linspace(0.08, 1.0, 12, dtype=np.float64) + theta2 = theta * theta + radius = theta * ( + 1.0 + + theta2 + * ( + expected[0] + + theta2 * (expected[1] + theta2 * (expected[2] + theta2 * expected[3])) + ) + ) + fitted = sr.calibrate_fisheye_angles(theta, radius) + np.testing.assert_allclose(fitted[:4], expected, atol=1e-9) + assert fitted[4] < 1e-12 diff --git a/docs/API_STABILITY.md b/docs/API_STABILITY.md index 0838d3e..edf82ac 100644 --- a/docs/API_STABILITY.md +++ b/docs/API_STABILITY.md @@ -104,7 +104,7 @@ spatialrust- / feature- | `spatialrust-interchange` | Provisional | glTF JSON mesh bridge; USDA ASCII OpenUSD stage adapter | | `spatialrust-distribute` | Provisional | Partition graphs, topo schedules, backpressure queues, named measurable transfers | | `spatialrust-platform` | Provisional | Stability registry, conformance summaries, security checklist, LTS policy, performance budgets, release gate | -| `spatialrust-camera` | Stable foundation | Pinhole/Brown–Conrady and named RGB-D conversion entry points; future calibration solvers are additive and provisional | +| `spatialrust-camera` | Stable foundation | Pinhole/Brown–Conrady and named RGB-D conversion entry points are stable; mono/stereo/fisheye/hand-eye/BA calibration contracts are additive and provisional | | `spatialrust-vision` | Stable foundation | Errors, borders, resize/filter entry points, reusable resize/gray/normalize/CHW outputs, detection/dense and Feature2D data contracts are stable; geometry/stereo/flow/AI adapters remain provisional | | `spatialrust-search` | Stable with features | KD-tree behind `search-kdtree`; **chunked query traits** and **`search-parallel`** provisional | | `spatialrust-filtering` | Provisional | GPU thresholds may move | diff --git a/docs/ARCHITECTURE.md b/docs/ARCHITECTURE.md index 7debc5c..99f44d4 100644 --- a/docs/ARCHITECTURE.md +++ b/docs/ARCHITECTURE.md @@ -72,6 +72,9 @@ Runtime adapter identity and synchronization are explicit (`adapter_info`, `wait_idle`), and `recycle` returns textures to the runtime pool. `spatialrust-image-io` depends on storage, never the reverse; standard codecs are additive, while TIFF and OpenEXR remain independently gated. +Calibration datasets, robust solver controls, and residual receipts live in +`spatialrust-camera`; they depend only on `spatialrust-math` small dense solves. +External nonlinear optimizers may be added only behind dedicated features. `spatialrust-vision` keeps preprocessing, Feature2D, geometry/multiview (H/F/E, PnP, sparse LK, stereo BM), warp, detection, dense-map, and spatial bridges in separate additive features. Geometry depends on `spatialrust-camera` only and does diff --git a/docs/ROADMAP.md b/docs/ROADMAP.md index 32e5c53..567a357 100644 --- a/docs/ROADMAP.md +++ b/docs/ROADMAP.md @@ -381,7 +381,7 @@ uses the standard completion gates above and lands as one reviewable PR. | 102 | Complete | 101 | Stabilize the image/camera/vision 1.0 contract and cross-platform conformance | | 103 | Complete | 101–102 | SIMD/parallel CPU kernel dispatch, reusable outputs, and measured allocation control | | 104 | Complete | 89, 101–103 | Texture-backed GPU Image v2 and device-resident resize/filter/edge/morphology chains | -| 105 | Planned | 88, 101–102 | Mono/stereo/fisheye/hand-eye calibration and bundle-adjustment contracts | +| 105 | Complete | 88, 101–102 | Mono/stereo/fisheye/hand-eye calibration and bundle-adjustment contracts | | 106 | Planned | 92, 101–105 | Dense flow, tracking, background modeling, and feature-gated video stream adapters | | 107 | Planned | 93, 101–106 | Stronger local features, robust tracking, and visual/RGB-D odometry integration | | 108 | Planned | 101–107 | Feature-gated computational photography and panorama stitching | @@ -454,3 +454,19 @@ The reference low-power adapter measured the five-stage resident chain at 0.963 ms (VGA), 3.504 ms (1080p), and 13.523 ms (4K), with explicit device synchronization. A chain receipt contains one upload, named device stages, no mid-chain readback, and one readback only when the caller requests host data. + +### Epic 105 delivery slices + +| Slice | Status | Scope | Evidence | +| --- | --- | --- | --- | +| 105A | Complete | Shared robust solver options and RMS/max/iteration receipts | `CalibrationOptions`, `CalibrationReport` | +| 105B | Complete | Robust mono intrinsics and Kannala–Brandt4 fisheye fitting | synthetic outlier and angle-polynomial recovery tests | +| 105C | Complete | Stereo and hand-eye transforms with supplied-rotation translation solves | 3D alignment and `AX = XB` residual tests | +| 105D | Complete | Sparse fixed-camera point bundle adjustment | multi-view numerical-Jacobian convergence test | +| 105E | Complete | Calibration workload coverage and provisional API registration | 100/1000 observation and 100-point/3-view Criterion groups | + +Calibration solvers live in `spatialrust-camera`, use small deterministic dense +normal equations, and introduce no native optimizer dependency. Supplied +rotations are checked for finite, right-handed orthonormal form. The first BA +contract intentionally fixes calibrated camera poses and refines world points; +joint pose/intrinsics optimization remains additive and provisional. diff --git a/notes/2026-07-15_epic105_calibration.md b/notes/2026-07-15_epic105_calibration.md new file mode 100644 index 0000000..13ae025 --- /dev/null +++ b/notes/2026-07-15_epic105_calibration.md @@ -0,0 +1,28 @@ +# Epic 105: camera calibration contracts + +Epic 105 places calibration ownership in `spatialrust-camera`. The common +`CalibrationOptions` and `CalibrationReport` contracts make robust thresholds, +iterations, convergence, RMS, max residual, and observation counts explicit. + +Delivered solvers cover: + +- robust `fx/fy/cx/cy` fitting from known camera-space point observations; +- Kannala–Brandt4 four-coefficient fisheye angle fitting and round-trip mapping; +- stereo translation fitting for a supplied relative rotation; +- hand-eye translation fitting for a supplied hand-eye rotation under `AX = XB`; +- sparse world-point bundle adjustment with fixed calibrated camera poses. + +Synthetic tests include a mono outlier, exact fisheye coefficients, non-trivial +stereo rotation, hand-eye motions about three axes, and two-view BA convergence. +All supplied rotations are checked for finite coefficients, orthonormality, and +positive unit determinant. Singular normal equations and invalid observation +indices return typed errors rather than panicking. + +The fixed-camera BA slice establishes data ownership and residual behavior +without pulling Ceres or another native optimizer into default builds. Joint +camera-pose/intrinsics refinement can extend the provisional contracts later. + +The OpenCV 4.10 comparison uses `projectPoints` and +`fisheye.projectPoints` to generate convention-compatible observations. On the +reference run, SpatialRust recovered pinhole parameters within `3.41e-13` and +fisheye coefficients within `4.62e-14`, below the `1e-8` gates.