spatialrust_vision/
geometry.rs

1//! Checked data contracts for calibrated and uncalibrated multiview geometry.
2
3use spatialrust_camera::CameraIntrinsics;
4use spatialrust_math::{Mat3, Vec2, Vec3};
5
6use crate::{VisionError, VisionResult};
7
8/// One ordered pixel correspondence from a source image to a target image.
9#[derive(Clone, Copy, Debug, PartialEq)]
10pub struct PointCorrespondence2 {
11    source: Vec2<f64>,
12    target: Vec2<f64>,
13}
14
15impl PointCorrespondence2 {
16    /// Creates a correspondence with finite pixel coordinates.
17    pub fn try_new(source: Vec2<f64>, target: Vec2<f64>) -> VisionResult<Self> {
18        if ![source.x, source.y, target.x, target.y].into_iter().all(f64::is_finite) {
19            return Err(VisionError::InvalidParameter(
20                "2D correspondence coordinates must be finite".into(),
21            ));
22        }
23        Ok(Self { source, target })
24    }
25
26    /// Returns the source-image pixel.
27    pub const fn source(self) -> Vec2<f64> {
28        self.source
29    }
30
31    /// Returns the target-image pixel.
32    pub const fn target(self) -> Vec2<f64> {
33        self.target
34    }
35}
36
37/// Calibrated pinhole intrinsic matrix and its analytic inverse.
38#[derive(Clone, Copy, Debug, PartialEq)]
39pub struct CameraMatrix3 {
40    matrix: Mat3<f64>,
41    inverse: Mat3<f64>,
42}
43
44impl CameraMatrix3 {
45    /// Builds `K` and `K^-1` from validated camera intrinsics.
46    #[must_use]
47    pub fn from_intrinsics(intrinsics: CameraIntrinsics) -> Self {
48        let matrix = Mat3::from_rows(
49            [intrinsics.fx, 0.0, intrinsics.cx],
50            [0.0, intrinsics.fy, intrinsics.cy],
51            [0.0, 0.0, 1.0],
52        );
53        let inverse = Mat3::from_rows(
54            [1.0 / intrinsics.fx, 0.0, -intrinsics.cx / intrinsics.fx],
55            [0.0, 1.0 / intrinsics.fy, -intrinsics.cy / intrinsics.fy],
56            [0.0, 0.0, 1.0],
57        );
58        Self { matrix, inverse }
59    }
60
61    /// Builds a checked pinhole `K` with analytic inverse (zero skew).
62    pub(crate) fn try_from_pinhole(fx: f64, fy: f64, cx: f64, cy: f64) -> VisionResult<Self> {
63        if ![fx, fy, cx, cy].into_iter().all(f64::is_finite) || fx <= 0.0 || fy <= 0.0 {
64            return Err(VisionError::InvalidParameter(
65                "pinhole camera matrix requires finite positive focal lengths".into(),
66            ));
67        }
68        Ok(Self {
69            matrix: Mat3::from_rows([fx, 0.0, cx], [0.0, fy, cy], [0.0, 0.0, 1.0]),
70            inverse: Mat3::from_rows(
71                [1.0 / fx, 0.0, -cx / fx],
72                [0.0, 1.0 / fy, -cy / fy],
73                [0.0, 0.0, 1.0],
74            ),
75        })
76    }
77
78    /// Returns the row-major intrinsic matrix `K`.
79    pub const fn matrix(self) -> Mat3<f64> {
80        self.matrix
81    }
82
83    /// Returns the row-major inverse intrinsic matrix `K^-1`.
84    pub const fn inverse(self) -> Mat3<f64> {
85        self.inverse
86    }
87
88    /// Converts a pixel into the homogeneous normalized camera plane.
89    #[must_use]
90    pub fn normalize_pixel(self, pixel: Vec2<f64>) -> Vec3<f64> {
91        self.inverse.mul_vec3(Vec3::new(pixel.x, pixel.y, 1.0))
92    }
93}
94
95fn validate_projective_matrix(matrix: Mat3<f64>, name: &str) -> VisionResult<Mat3<f64>> {
96    if matrix.m.iter().flatten().any(|value| !value.is_finite()) {
97        return Err(VisionError::InvalidParameter(format!(
98            "{name} matrix elements must be finite"
99        )));
100    }
101    let norm_squared = matrix.m.iter().flatten().map(|value| value * value).sum::<f64>();
102    if norm_squared <= f64::EPSILON {
103        return Err(VisionError::InvalidParameter(format!("{name} matrix must not be all zero")));
104    }
105    Ok(matrix)
106}
107
108macro_rules! projective_model {
109    ($name:ident, $summary:literal, $label:literal) => {
110        #[doc = $summary]
111        #[derive(Clone, Copy, Debug, PartialEq)]
112        pub struct $name(Mat3<f64>);
113
114        impl $name {
115            /// Creates a checked finite, non-zero projective matrix.
116            pub fn try_new(matrix: Mat3<f64>) -> VisionResult<Self> {
117                Ok(Self(validate_projective_matrix(matrix, $label)?))
118            }
119
120            /// Returns the row-major matrix without changing its arbitrary scale.
121            pub const fn matrix(self) -> Mat3<f64> {
122                self.0
123            }
124        }
125    };
126}
127
128projective_model!(Homography3, "A pixel-to-pixel planar projective transform.", "homography");
129projective_model!(
130    Fundamental3,
131    "An uncalibrated two-view epipolar constraint matrix.",
132    "fundamental"
133);
134projective_model!(Essential3, "A calibrated two-view epipolar constraint matrix.", "essential");
135
136/// Deterministic robust-model sampling and inlier classification settings.
137#[derive(Clone, Copy, Debug, PartialEq)]
138pub struct RobustEstimationOptions {
139    /// Maximum accepted geometric residual in pixels or normalized units.
140    pub threshold: f64,
141    /// Desired probability of sampling an outlier-free minimal set.
142    pub confidence: f64,
143    /// Hard iteration limit.
144    pub max_iterations: usize,
145    /// Reproducible pseudo-random sampling seed.
146    pub seed: u64,
147}
148
149impl Default for RobustEstimationOptions {
150    fn default() -> Self {
151        Self { threshold: 1.0, confidence: 0.99, max_iterations: 2_000, seed: 0 }
152    }
153}
154
155impl RobustEstimationOptions {
156    /// Validates thresholds, confidence, and the iteration budget.
157    pub fn validate(self) -> VisionResult<Self> {
158        if !self.threshold.is_finite() || self.threshold <= 0.0 {
159            return Err(VisionError::InvalidParameter(
160                "geometry threshold must be finite and positive".into(),
161            ));
162        }
163        if !self.confidence.is_finite() || self.confidence <= 0.0 || self.confidence >= 1.0 {
164            return Err(VisionError::InvalidParameter(
165                "geometry confidence must be finite and in (0, 1)".into(),
166            ));
167        }
168        if self.max_iterations == 0 {
169            return Err(VisionError::InvalidParameter(
170                "geometry max_iterations must be positive".into(),
171            ));
172        }
173        Ok(self)
174    }
175}
176
177/// A geometric model paired with one inlier decision and residual per input row.
178#[derive(Clone, Debug, PartialEq)]
179pub struct GeometricEstimate<Model> {
180    model: Model,
181    inliers: Vec<bool>,
182    residuals: Vec<f64>,
183}
184
185/// Target-camera pose expressed in the source-camera coordinate frame.
186#[derive(Clone, Copy, Debug, PartialEq)]
187pub struct RelativePose {
188    rotation: Mat3<f64>,
189    translation: Vec3<f64>,
190}
191
192impl RelativePose {
193    /// Creates a checked proper rotation and finite non-zero translation.
194    pub fn try_new(rotation: Mat3<f64>, translation: Vec3<f64>) -> VisionResult<Self> {
195        validate_rotation(rotation, "relative pose")?;
196        if ![translation.x, translation.y, translation.z].into_iter().all(f64::is_finite) {
197            return Err(VisionError::InvalidParameter(
198                "relative pose translation must be finite".into(),
199            ));
200        }
201        if translation.length() <= f64::EPSILON {
202            return Err(VisionError::InvalidParameter(
203                "relative pose translation must be non-zero".into(),
204            ));
205        }
206        Ok(Self { rotation, translation })
207    }
208
209    /// Returns the source-to-target rotation.
210    pub const fn rotation(self) -> Mat3<f64> {
211        self.rotation
212    }
213
214    /// Returns the source-to-target translation, whose scale may be arbitrary.
215    pub const fn translation(self) -> Vec3<f64> {
216        self.translation
217    }
218}
219
220/// Object-to-camera pose `X_cam = R X_obj + t` used by absolute orientation (PnP).
221#[derive(Clone, Copy, Debug, PartialEq)]
222pub struct AbsolutePose {
223    rotation: Mat3<f64>,
224    translation: Vec3<f64>,
225}
226
227impl AbsolutePose {
228    /// Creates a checked proper rotation and finite translation (may be zero).
229    pub fn try_new(rotation: Mat3<f64>, translation: Vec3<f64>) -> VisionResult<Self> {
230        validate_rotation(rotation, "absolute pose")?;
231        if ![translation.x, translation.y, translation.z].into_iter().all(f64::is_finite) {
232            return Err(VisionError::InvalidParameter(
233                "absolute pose translation must be finite".into(),
234            ));
235        }
236        Ok(Self { rotation, translation })
237    }
238
239    /// Returns the object-to-camera rotation.
240    pub const fn rotation(self) -> Mat3<f64> {
241        self.rotation
242    }
243
244    /// Returns the object-to-camera translation.
245    pub const fn translation(self) -> Vec3<f64> {
246        self.translation
247    }
248
249    /// Transforms an object-frame point into the camera frame.
250    #[must_use]
251    pub fn transform_point(self, point: Vec3<f64>) -> Vec3<f64> {
252        self.rotation.mul_vec3(point) + self.translation
253    }
254}
255
256/// One ordered object-to-image correspondence for absolute pose estimation.
257#[derive(Clone, Copy, Debug, PartialEq)]
258pub struct ObjectImageCorrespondence {
259    object: Vec3<f64>,
260    image: Vec2<f64>,
261}
262
263impl ObjectImageCorrespondence {
264    /// Creates a correspondence with finite object and image coordinates.
265    pub fn try_new(object: Vec3<f64>, image: Vec2<f64>) -> VisionResult<Self> {
266        if ![object.x, object.y, object.z, image.x, image.y].into_iter().all(f64::is_finite) {
267            return Err(VisionError::InvalidParameter(
268                "object-image correspondence coordinates must be finite".into(),
269            ));
270        }
271        Ok(Self { object, image })
272    }
273
274    /// Returns the object-frame point.
275    pub const fn object(self) -> Vec3<f64> {
276        self.object
277    }
278
279    /// Returns the image-plane pixel.
280    pub const fn image(self) -> Vec2<f64> {
281        self.image
282    }
283}
284
285fn validate_rotation(rotation: Mat3<f64>, name: &str) -> VisionResult<()> {
286    if rotation.m.iter().flatten().any(|value| !value.is_finite()) {
287        return Err(VisionError::InvalidParameter(format!("{name} rotation must be finite")));
288    }
289    let orthogonality = rotation.transpose().mul_mat3(rotation);
290    let identity = Mat3::<f64>::identity();
291    let maximum_error = orthogonality
292        .m
293        .iter()
294        .flatten()
295        .zip(identity.m.iter().flatten())
296        .map(|(actual, expected)| (actual - expected).abs())
297        .fold(0.0_f64, f64::max);
298    if maximum_error > 1e-6 || determinant(rotation) < 1.0 - 1e-6 {
299        return Err(VisionError::InvalidParameter(format!(
300            "{name} rotation must be a proper orthonormal matrix"
301        )));
302    }
303    Ok(())
304}
305
306/// One two-view triangulation with cheirality and reprojection diagnostics.
307#[derive(Clone, Copy, Debug, PartialEq)]
308pub struct TriangulatedPoint {
309    position: Vec3<f64>,
310    source_depth: f64,
311    target_depth: f64,
312    reprojection_error: f64,
313}
314
315impl TriangulatedPoint {
316    pub(crate) fn try_new(
317        position: Vec3<f64>,
318        source_depth: f64,
319        target_depth: f64,
320        reprojection_error: f64,
321    ) -> VisionResult<Self> {
322        if ![position.x, position.y, position.z, source_depth, target_depth, reprojection_error]
323            .into_iter()
324            .all(f64::is_finite)
325            || reprojection_error < 0.0
326        {
327            return Err(VisionError::InvalidParameter(
328                "triangulation values must be finite and error non-negative".into(),
329            ));
330        }
331        Ok(Self { position, source_depth, target_depth, reprojection_error })
332    }
333
334    /// Returns the point in source-camera coordinates.
335    pub const fn position(self) -> Vec3<f64> {
336        self.position
337    }
338
339    /// Returns its signed source-camera depth.
340    pub const fn source_depth(self) -> f64 {
341        self.source_depth
342    }
343
344    /// Returns its signed target-camera depth.
345    pub const fn target_depth(self) -> f64 {
346        self.target_depth
347    }
348
349    /// Returns the mean normalized-plane reprojection distance.
350    pub const fn reprojection_error(self) -> f64 {
351        self.reprojection_error
352    }
353
354    /// Returns whether the point lies in front of both cameras.
355    pub fn has_positive_depth(self) -> bool {
356        self.source_depth > 0.0 && self.target_depth > 0.0
357    }
358}
359
360/// Essential-matrix pose disambiguation and per-correspondence triangulation.
361#[derive(Clone, Debug, PartialEq)]
362pub struct RelativePoseEstimate {
363    pose: RelativePose,
364    points: Vec<Option<TriangulatedPoint>>,
365    positive_depth_count: usize,
366}
367
368impl RelativePoseEstimate {
369    pub(crate) fn new(pose: RelativePose, points: Vec<Option<TriangulatedPoint>>) -> Self {
370        let positive_depth_count =
371            points.iter().flatten().filter(|point| point.has_positive_depth()).count();
372        Self { pose, points, positive_depth_count }
373    }
374
375    /// Returns the selected source-to-target pose.
376    pub const fn pose(&self) -> RelativePose {
377        self.pose
378    }
379
380    /// Returns one optional triangulation per input correspondence.
381    pub fn points(&self) -> &[Option<TriangulatedPoint>] {
382        &self.points
383    }
384
385    /// Returns the number of points in front of both cameras.
386    pub const fn positive_depth_count(&self) -> usize {
387        self.positive_depth_count
388    }
389}
390
391fn determinant(matrix: Mat3<f64>) -> f64 {
392    matrix.m[0][0] * (matrix.m[1][1] * matrix.m[2][2] - matrix.m[1][2] * matrix.m[2][1])
393        - matrix.m[0][1] * (matrix.m[1][0] * matrix.m[2][2] - matrix.m[1][2] * matrix.m[2][0])
394        + matrix.m[0][2] * (matrix.m[1][0] * matrix.m[2][1] - matrix.m[1][1] * matrix.m[2][0])
395}
396
397impl<Model> GeometricEstimate<Model> {
398    /// Creates a result whose inlier and residual arrays match the input count.
399    pub fn try_new(
400        model: Model,
401        correspondence_count: usize,
402        inliers: Vec<bool>,
403        residuals: Vec<f64>,
404    ) -> VisionResult<Self> {
405        if inliers.len() != correspondence_count || residuals.len() != correspondence_count {
406            return Err(VisionError::GeometryResultLayout {
407                correspondences: correspondence_count,
408                inliers: inliers.len(),
409                residuals: residuals.len(),
410            });
411        }
412        if residuals.iter().any(|value| !value.is_finite() || *value < 0.0) {
413            return Err(VisionError::InvalidParameter(
414                "geometry residuals must be finite and non-negative".into(),
415            ));
416        }
417        Ok(Self { model, inliers, residuals })
418    }
419
420    /// Returns the estimated model.
421    pub const fn model(&self) -> &Model {
422        &self.model
423    }
424
425    /// Returns one inlier decision per input correspondence.
426    pub fn inliers(&self) -> &[bool] {
427        &self.inliers
428    }
429
430    /// Returns one non-negative geometric residual per input correspondence.
431    pub fn residuals(&self) -> &[f64] {
432        &self.residuals
433    }
434
435    /// Returns the number of accepted correspondences.
436    pub fn inlier_count(&self) -> usize {
437        self.inliers.iter().filter(|&&value| value).count()
438    }
439}
440
441#[cfg(test)]
442mod tests {
443    use super::{
444        CameraMatrix3, GeometricEstimate, Homography3, PointCorrespondence2,
445        RobustEstimationOptions,
446    };
447    use crate::VisionError;
448    use spatialrust_camera::CameraIntrinsics;
449    use spatialrust_math::{Mat3, Vec2};
450
451    #[test]
452    fn correspondence_and_projective_matrix_validation_is_strict() {
453        assert!(PointCorrespondence2::try_new(Vec2 { x: 1.0, y: 2.0 }, Vec2 { x: 3.0, y: 4.0 },)
454            .is_ok());
455        assert!(PointCorrespondence2::try_new(
456            Vec2 { x: f64::NAN, y: 2.0 },
457            Vec2 { x: 3.0, y: 4.0 },
458        )
459        .is_err());
460        assert!(Homography3::try_new(Mat3::from_rows(
461            [0.0, 0.0, 0.0],
462            [0.0, 0.0, 0.0],
463            [0.0, 0.0, 0.0],
464        ))
465        .is_err());
466    }
467
468    #[test]
469    fn camera_matrix_normalizes_pixels_analytically() {
470        let intrinsics = CameraIntrinsics::try_new(500.0, 400.0, 320.0, 240.0, 640, 480).unwrap();
471        let camera = CameraMatrix3::from_intrinsics(intrinsics);
472        let normalized = camera.normalize_pixel(Vec2 { x: 420.0, y: 160.0 });
473        assert!((normalized.x - 0.2).abs() < 1e-12);
474        assert!((normalized.y + 0.2).abs() < 1e-12);
475        assert_eq!(normalized.z, 1.0);
476        assert_eq!(camera.matrix().mul_mat3(camera.inverse()), Mat3::<f64>::identity());
477    }
478
479    #[test]
480    fn estimate_layout_and_robust_options_are_checked() {
481        let model = Homography3::try_new(Mat3::<f64>::identity()).unwrap();
482        let estimate =
483            GeometricEstimate::try_new(model, 2, vec![true, false], vec![0.1, 2.0]).unwrap();
484        assert_eq!(estimate.inlier_count(), 1);
485        assert!(matches!(
486            GeometricEstimate::try_new(model, 2, vec![true], vec![0.1, 2.0]),
487            Err(VisionError::GeometryResultLayout { .. })
488        ));
489        assert!(RobustEstimationOptions { confidence: 1.0, ..Default::default() }
490            .validate()
491            .is_err());
492    }
493}
spatialrust_vision/geometry.rs

spatialrust_vision/
geometry.rs
