braid_mvg/
intrinsics.rs

1// Copyright (C) The Strand-Braid Authors
2// SPDX-License-Identifier: MIT OR Apache-2.0
3
4use nalgebra as na;
5use nalgebra::RealField;
6
7use opencv_ros_camera::RosOpenCvIntrinsics;
8
9/// Axis along which to mirror camera intrinsic parameters.
10///
11/// This enum specifies which axis to use when creating a mirrored version
12/// of camera intrinsic parameters, typically used for stereo camera setups
13/// or when cameras have different orientations.
14#[derive(Debug, Clone, Copy, PartialEq, Eq)]
15pub enum MirrorAxis {
16    /// Mirror along the left-right (horizontal) axis.
17    ///
18    /// This effectively flips the camera horizontally, reversing the x-coordinates.
19    LeftRight,
20    /// Mirror along the up-down (vertical) axis.
21    ///
22    /// This effectively flips the camera vertically, reversing the y-coordinates.
23    UpDown,
24}
25
26/// return a copy of this camera whose x coordinate is (image_width-x)
27pub fn mirror<R: RealField + Copy>(
28    self_: &RosOpenCvIntrinsics<R>,
29    axis: MirrorAxis,
30) -> Option<RosOpenCvIntrinsics<R>> {
31    if !self_.rect.is_identity(na::convert(1.0e-7)) {
32        None
33    } else {
34        let mut i2 = self_.clone();
35        let x = match axis {
36            MirrorAxis::LeftRight => {
37                i2.k[(0, 0)] = -i2.k[(0, 0)];
38                i2.k[(0, 1)] = -i2.k[(0, 1)];
39                i2.p[(0, 0)] = -i2.p[(0, 0)];
40                i2.p[(0, 1)] = -i2.p[(0, 1)];
41                i2
42            }
43            MirrorAxis::UpDown => {
44                i2.k[(1, 1)] = -i2.k[(1, 1)];
45                i2.p[(1, 1)] = -i2.p[(1, 1)];
46                i2
47            }
48        };
49        // call new() to recompute cache
50        Some(RosOpenCvIntrinsics::from_components(x.p, x.k, x.distortion, x.rect).unwrap())
51    }
52}
53
54#[cfg(test)]
55mod tests {
56    use na::geometry::Point2;
57    use nalgebra as na;
58    use nalgebra::{DefaultAllocator, U3, U7, allocator::Allocator};
59
60    #[test]
61    fn test_serde() {
62        let expected = crate::make_default_intrinsics();
63        let buf = serde_json::to_string(&expected).unwrap();
64        let actual: crate::RosOpenCvIntrinsics<f64> = serde_json::from_str(&buf).unwrap();
65        assert!(expected == actual);
66    }
67
68    #[test]
69    fn test_mirror()
70    where
71        DefaultAllocator: Allocator<U7, U3>,
72    {
73        use cam_geom::{IntrinsicParameters, Points};
74        use nalgebra::{OMatrix, U3, U7};
75
76        use crate::intrinsics::{MirrorAxis::*, mirror};
77
78        #[rustfmt::skip]
79        let pts = Points::new(
80            OMatrix::<f64, U7, U3>::from_row_slice(
81                &[0.0,  0.0, 1.0,
82                1.0,  0.0, 1.0,
83                0.0,  1.0, 1.0,
84                1.0,  1.0, 1.0,
85                -1.0,  0.0, 1.0,
86                0.0, -1.0, 1.0,
87                -1.0, -1.0, 1.0]
88            )
89        );
90
91        for axis in &[LeftRight, UpDown] {
92            for (name, cam) in crate::tests::get_test_cameras().iter() {
93                let cam = cam.intrinsics();
94                let lr_mirror = mirror(cam, *axis).unwrap();
95
96                let orig_pixels = cam.camera_to_pixel(&pts);
97                let lr_pixels = lr_mirror.camera_to_pixel(&pts);
98
99                println!("{name}, axis: {axis:?}");
100                for i in 0..orig_pixels.data.nrows() {
101                    let expected = match axis {
102                        LeftRight => {
103                            // TODO make comparison testing for symmetric
104                            // reflection without getting cx from parameters.
105                            let cx = cam.p[(0, 2)];
106                            let expected_x = cx + (cx - orig_pixels.data[(i, 0)]);
107                            let expected_y = orig_pixels.data[(i, 1)];
108                            Point2::new(expected_x, expected_y)
109                        }
110                        UpDown => {
111                            // TODO make comparison testing for symmetric
112                            // reflection without getting cy from parameters.
113                            let cy = cam.p[(1, 2)];
114                            let expected_x = orig_pixels.data[(i, 0)];
115                            let expected_y = cy + (cy - orig_pixels.data[(i, 1)]);
116                            Point2::new(expected_x, expected_y)
117                        }
118                    };
119                    println!(
120                        "orig: {:?}, expected: {:?}, lr: {:?}",
121                        orig_pixels.data.row(i),
122                        expected,
123                        lr_pixels.data.row(i)
124                    );
125                    let eps = 1e-10;
126                    approx::assert_relative_eq!(expected[0], lr_pixels.data[(i, 0)], epsilon = eps);
127                    approx::assert_relative_eq!(expected[1], lr_pixels.data[(i, 1)], epsilon = eps);
128                }
129            }
130        }
131    }
132}
braid_mvg/intrinsics.rs

braid_mvg/
intrinsics.rs
