cam_geom/
intrinsics_perspective.rs

1use nalgebra::{
2    allocator::Allocator,
3    convert,
4    storage::{Owned, Storage},
5    DefaultAllocator, Dim, Matrix, OMatrix, RealField, SMatrix, U1, U2, U3,
6};
7
8#[cfg(feature = "serde-serialize")]
9use serde::{Deserialize, Serialize};
10
11use crate::{
12    coordinate_system::CameraFrame, Bundle, Error, IntrinsicParameters, Pixels, Points, RayBundle,
13};
14
15use crate::ray_bundle_types::SharedOriginRayBundle;
16
17/// Parameters defining a pinhole perspective camera model.
18///
19/// These will be used to make the 3x4 intrinsic parameter matrix
20/// ```text
21/// [[fx, skew, cx, 0],
22///  [ 0,   fy, cy, 0],
23///  [ 0,    0,  1, 0]]
24/// ```
25///
26/// These parameters describe the intrinsic parameters, the transformation from
27/// camera coordinates to pixel coordinates, for a perspective camera model. For
28/// a full transformation from world coordinates to pixel coordinates, use a
29/// [`Camera`](struct.Camera.html), which can be constructed with these intinsic
30/// parameters and extrinsic parameters.
31///
32/// Read more about the [pinhole perspective
33/// projection](https://en.wikipedia.org/wiki/Pinhole_camera_model).
34///
35/// Can be converted into
36/// [`IntrinsicParametersPerspective`](struct.IntrinsicParametersPerspective.html)
37/// via the `.into()` method like so:
38///
39/// ```
40/// use cam_geom::*;
41/// let params = PerspectiveParams {
42///     fx: 100.0,
43///     fy: 100.0,
44///     skew: 0.0,
45///     cx: 640.0,
46///     cy: 480.0,
47/// };
48/// let intrinsics: IntrinsicParametersPerspective<_> = params.into();
49/// ```
50#[derive(Debug, Clone, PartialEq)]
51#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
52pub struct PerspectiveParams<R: RealField> {
53    /// Horizontal focal length.
54    pub fx: R,
55    /// Vertical focal length.
56    pub fy: R,
57    /// Skew between horizontal and vertical axes.
58    pub skew: R,
59    /// Horizontal component of the principal point.
60    pub cx: R,
61    /// Vertical component of the principal point.
62    pub cy: R,
63}
64
65impl<R: RealField> From<PerspectiveParams<R>> for IntrinsicParametersPerspective<R> {
66    fn from(params: PerspectiveParams<R>) -> Self {
67        use nalgebra::convert as c;
68        #[rustfmt::skip]
69        let cache_p = nalgebra::SMatrix::<R,3,4>::new(
70            params.fx.clone(), params.skew.clone(), params.cx.clone(), c(0.0),
71             c(0.0),     params.fy.clone(), params.cy.clone(), c(0.0),
72             c(0.0),        c(0.0), c(1.0), c(0.0),
73        );
74        Self { params, cache_p }
75    }
76}
77
78/// A pinhole perspective camera model. Implements [`IntrinsicParameters`](trait.IntrinsicParameters.html).
79///
80/// Create an `IntrinsicParametersPerspective` as described for
81/// [`PerspectiveParams`](struct.PerspectiveParams.html) by using `.into()`.
82#[derive(Clone, PartialEq)]
83#[cfg_attr(feature = "serde-serialize", derive(Serialize))]
84pub struct IntrinsicParametersPerspective<R: RealField> {
85    params: PerspectiveParams<R>,
86    #[cfg_attr(feature = "serde-serialize", serde(skip))]
87    pub(crate) cache_p: SMatrix<R, 3, 4>,
88}
89
90impl<R: RealField> IntrinsicParametersPerspective<R> {
91    /// Create a new instance given an intrinsic parameter matrix.
92    ///
93    /// Returns an error if the intrinsic parameter matrix is not normalized or
94    /// otherwise does not represent a perspective camera model.
95    pub fn from_normalized_3x4_matrix(p: SMatrix<R, 3, 4>) -> std::result::Result<Self, Error> {
96        let params: PerspectiveParams<R> = PerspectiveParams {
97            fx: p[(0, 0)].clone(),
98            fy: p[(1, 1)].clone(),
99            skew: p[(0, 1)].clone(),
100            cx: p[(0, 2)].clone(),
101            cy: p[(1, 2)].clone(),
102        };
103        if approx::relative_ne!(p[(0, 3)], nalgebra::convert(0.0)) {
104            return Err(Error::InvalidInput);
105        }
106
107        if approx::relative_ne!(p[(1, 0)], nalgebra::convert(0.0)) {
108            return Err(Error::InvalidInput);
109        }
110
111        if approx::relative_ne!(p[(1, 3)], nalgebra::convert(0.0)) {
112            return Err(Error::InvalidInput);
113        }
114
115        if approx::relative_ne!(p[(2, 0)], nalgebra::convert(0.0)) {
116            return Err(Error::InvalidInput);
117        }
118
119        if approx::relative_ne!(p[(2, 1)], nalgebra::convert(0.0)) {
120            return Err(Error::InvalidInput);
121        }
122
123        if approx::relative_ne!(p[(2, 2)], nalgebra::convert(1.0)) {
124            return Err(Error::InvalidInput); // camera matrix must be normalized
125        }
126
127        if approx::relative_ne!(p[(2, 3)], nalgebra::convert(0.0)) {
128            return Err(Error::InvalidInput);
129        }
130
131        Ok(params.into())
132    }
133
134    /// Get X focal length
135    #[inline]
136    pub fn fx(&self) -> R {
137        self.params.fx.clone()
138    }
139
140    /// Get Y focal length
141    #[inline]
142    pub fn fy(&self) -> R {
143        self.params.fy.clone()
144    }
145
146    /// Get skew
147    #[inline]
148    pub fn skew(&self) -> R {
149        self.params.skew.clone()
150    }
151
152    /// Get X center
153    #[inline]
154    pub fn cx(&self) -> R {
155        self.params.cx.clone()
156    }
157
158    /// Get Y center
159    #[inline]
160    pub fn cy(&self) -> R {
161        self.params.cy.clone()
162    }
163
164    /// Get intrinsic parameters
165    #[inline]
166    pub fn params(&self) -> &PerspectiveParams<R> {
167        &self.params
168    }
169
170    /// Create a 3x3 projection matrix.
171    #[inline]
172    pub(crate) fn as_intrinsics_matrix(
173        &self,
174    ) -> Matrix<R, U3, U3, nalgebra::ViewStorage<'_, R, U3, U3, U1, U3>> {
175        // TODO: implement similar functionality for orthographic camera and
176        // make a new trait which exposes this functionality. Note that not all
177        // intrinsic parameter implementations will be able to implement this
178        // hypothetical new trait, because not all cameras are linear.
179        self.cache_p.fixed_view::<3, 3>(0, 0)
180    }
181}
182
183impl<R> IntrinsicParameters<R> for IntrinsicParametersPerspective<R>
184where
185    R: RealField,
186{
187    type BundleType = SharedOriginRayBundle<R>;
188
189    fn pixel_to_camera<IN, NPTS>(
190        &self,
191        pixels: &Pixels<R, NPTS, IN>,
192    ) -> RayBundle<CameraFrame, Self::BundleType, R, NPTS, Owned<R, NPTS, U3>>
193    where
194        Self::BundleType: Bundle<R>,
195        IN: Storage<R, NPTS, U2>,
196        NPTS: Dim,
197        DefaultAllocator: Allocator<NPTS, U3>,
198    {
199        let one: R = convert(1.0);
200
201        // allocate zeros, fill later
202        let mut result = RayBundle::new_shared_zero_origin(OMatrix::zeros_generic(
203            NPTS::from_usize(pixels.data.nrows()),
204            U3::from_usize(3),
205        ));
206
207        let cam_dir = &mut result.data;
208
209        // It seems broadcasting is not (yet) supported in nalgebra, so we loop
210        // through the data. See
211        // https://discourse.nphysics.org/t/array-broadcasting-support/375/3 .
212
213        for i in 0..pixels.data.nrows() {
214            let u = pixels.data[(i, 0)].clone();
215            let v = pixels.data[(i, 1)].clone();
216
217            // point in camcoords at distance 1.0 from image plane
218            let y = (v - self.params.cy.clone()) / self.params.fy.clone();
219            cam_dir[(i, 0)] = (u - self.params.skew.clone() * y.clone() - self.params.cx.clone())
220                / self.params.fx.clone(); // x
221            cam_dir[(i, 1)] = y;
222            cam_dir[(i, 2)] = one.clone(); // z
223        }
224        result
225    }
226
227    fn camera_to_pixel<IN, NPTS>(
228        &self,
229        camera: &Points<CameraFrame, R, NPTS, IN>,
230    ) -> Pixels<R, NPTS, Owned<R, NPTS, U2>>
231    where
232        IN: Storage<R, NPTS, U3>,
233        NPTS: Dim,
234        DefaultAllocator: Allocator<NPTS, U2>,
235    {
236        let mut result = Pixels::new(OMatrix::zeros_generic(
237            NPTS::from_usize(camera.data.nrows()),
238            U2::from_usize(2),
239        ));
240
241        // It seems broadcasting is not (yet) supported in nalgebra, so we loop
242        // through the data. See
243        // https://discourse.nphysics.org/t/array-broadcasting-support/375/3 .
244
245        for i in 0..camera.data.nrows() {
246            let x = nalgebra::Point3::new(
247                camera.data[(i, 0)].clone(),
248                camera.data[(i, 1)].clone(),
249                camera.data[(i, 2)].clone(),
250            )
251            .to_homogeneous();
252            let rst = self.cache_p.clone() * x;
253            result.data[(i, 0)] = rst[0].clone() / rst[2].clone();
254            result.data[(i, 1)] = rst[1].clone() / rst[2].clone();
255        }
256        result
257    }
258}
259
260impl<R: RealField> std::fmt::Debug for IntrinsicParametersPerspective<R> {
261    fn fmt(&self, fmt: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
262        // This should match the auto derived Debug implementation but not print
263        // the cache_p field.
264        fmt.debug_struct("IntrinsicParametersPerspective")
265            .field("params", &self.params)
266            .finish()
267    }
268}
269
270// See note about serde derive for ExtrinsicParameters Deserialize, which
271// applies here, too.
272#[cfg(feature = "serde-serialize")]
273impl<'de, R: RealField + serde::Deserialize<'de>> serde::Deserialize<'de>
274    for IntrinsicParametersPerspective<R>
275{
276    fn deserialize<D>(deserializer: D) -> std::result::Result<Self, D::Error>
277    where
278        D: serde::Deserializer<'de>,
279    {
280        use serde::de;
281        use std::fmt;
282
283        #[derive(Deserialize)]
284        #[serde(field_identifier, rename_all = "lowercase")]
285        enum Field {
286            Params,
287        }
288
289        struct IntrinsicParametersPerspectiveVisitor<'de, R2: RealField + serde::Deserialize<'de>>(
290            std::marker::PhantomData<&'de R2>,
291        );
292
293        impl<'de, R2: RealField + serde::Deserialize<'de>> serde::de::Visitor<'de>
294            for IntrinsicParametersPerspectiveVisitor<'de, R2>
295        {
296            type Value = IntrinsicParametersPerspective<R2>;
297
298            fn expecting(&self, formatter: &mut fmt::Formatter<'_>) -> fmt::Result {
299                formatter.write_str("struct IntrinsicParametersPerspective")
300            }
301
302            fn visit_seq<V>(
303                self,
304                mut seq: V,
305            ) -> std::result::Result<IntrinsicParametersPerspective<R2>, V::Error>
306            where
307                V: serde::de::SeqAccess<'de>,
308            {
309                let params: PerspectiveParams<_> = seq
310                    .next_element()?
311                    .ok_or_else(|| de::Error::invalid_length(0, &self))?;
312                Ok(IntrinsicParametersPerspective::from(params))
313            }
314
315            fn visit_map<V>(
316                self,
317                mut map: V,
318            ) -> std::result::Result<IntrinsicParametersPerspective<R2>, V::Error>
319            where
320                V: serde::de::MapAccess<'de>,
321            {
322                let mut params = None;
323                while let Some(key) = map.next_key()? {
324                    match key {
325                        Field::Params => {
326                            if params.is_some() {
327                                return Err(de::Error::duplicate_field("params"));
328                            }
329                            params = Some(map.next_value()?);
330                        }
331                    }
332                }
333                let params: PerspectiveParams<_> =
334                    params.ok_or_else(|| de::Error::missing_field("params"))?;
335                Ok(IntrinsicParametersPerspective::from(params))
336            }
337        }
338
339        const FIELDS: &[&str] = &["params"];
340        deserializer.deserialize_struct(
341            "IntrinsicParametersPerspective",
342            FIELDS,
343            IntrinsicParametersPerspectiveVisitor(std::marker::PhantomData),
344        )
345    }
346}
347
348#[cfg(feature = "serde-serialize")]
349fn _test_is_serialize() {
350    // Compile-time test to ensure IntrinsicParametersPerspective implements Serialize trait.
351    fn implements<T: serde::Serialize>() {}
352    implements::<IntrinsicParametersPerspective<f64>>();
353}
354
355#[cfg(feature = "serde-serialize")]
356fn _test_is_deserialize() {
357    // Compile-time test to ensure IntrinsicParametersPerspective implements Deserialize trait.
358    fn implements<'de, T: serde::Deserialize<'de>>() {}
359    implements::<IntrinsicParametersPerspective<f64>>();
360}
361
362#[cfg(test)]
363mod tests {
364    use nalgebra::{SMatrix, Vector3};
365
366    use super::{IntrinsicParametersPerspective, PerspectiveParams};
367    use crate::camera::{roundtrip_camera, Camera};
368    use crate::extrinsics::ExtrinsicParameters;
369    use crate::intrinsic_test_utils::roundtrip_intrinsics;
370    use crate::Points;
371
372    #[test]
373    fn roundtrip() {
374        let params = PerspectiveParams {
375            fx: 100.0,
376            fy: 102.0,
377            skew: 0.1,
378            cx: 321.0,
379            cy: 239.9,
380        };
381
382        let cam: IntrinsicParametersPerspective<_> = params.into();
383        roundtrip_intrinsics(&cam, 640, 480, 5, 0, nalgebra::convert(1e-10));
384
385        let extrinsics = ExtrinsicParameters::from_view(
386            &Vector3::new(1.2, 3.4, 5.6),                                // camcenter
387            &Vector3::new(2.2, 3.4, 5.6),                                // lookat
388            &nalgebra::Unit::new_normalize(Vector3::new(0.0, 0.0, 1.0)), // up
389        );
390
391        let full_cam = Camera::new(cam, extrinsics);
392        roundtrip_camera(full_cam, 640, 480, 5, 0, nalgebra::convert(1e-10));
393    }
394
395    #[test]
396    fn reject_invalid_projection_matrix() {
397        #[rustfmt::skip]
398        let p_valid = nalgebra::SMatrix::<f64,3,4>::new(
399            10.0,  0.0, 0.0, 0.0,
400             0.0, 10.0, 0.0, 0.0,
401             0.0,  0.0, 1.0, 0.0,
402        );
403        assert!(IntrinsicParametersPerspective::from_normalized_3x4_matrix(p_valid).is_ok());
404
405        let mut p = p_valid;
406        p[(2, 2)] = 1.1;
407        assert!(IntrinsicParametersPerspective::from_normalized_3x4_matrix(p).is_err());
408
409        let mut p = p_valid;
410        p[(0, 3)] = 1.1;
411        assert!(IntrinsicParametersPerspective::from_normalized_3x4_matrix(p).is_err());
412
413        let mut p = p_valid;
414        p[(1, 0)] = 1.1;
415        assert!(IntrinsicParametersPerspective::from_normalized_3x4_matrix(p).is_err());
416
417        let mut p = p_valid;
418        p[(1, 3)] = 1.1;
419        assert!(IntrinsicParametersPerspective::from_normalized_3x4_matrix(p).is_err());
420
421        let mut p = p_valid;
422        p[(2, 0)] = 1.1;
423        assert!(IntrinsicParametersPerspective::from_normalized_3x4_matrix(p).is_err());
424
425        let mut p = p_valid;
426        p[(2, 1)] = 1.1;
427        assert!(IntrinsicParametersPerspective::from_normalized_3x4_matrix(p).is_err());
428
429        let mut p = p_valid;
430        p[(2, 2)] = 1.1;
431        assert!(IntrinsicParametersPerspective::from_normalized_3x4_matrix(p).is_err());
432    }
433
434    fn assert_is_pmat_same(
435        cam: &Camera<f64, IntrinsicParametersPerspective<f64>>,
436        pmat: &SMatrix<f64, 3, 4>,
437    ) {
438        let camcoord_pts = Points::new(SMatrix::<f64, 2, 3>::new(
439            1.23, 4.56, 7.89, // pt 1
440            1.0, 2.0, 3.0, // pt 2
441        ));
442
443        // Convert world to pixel using Camera method.
444        let pts1 = cam.world_to_pixel(&camcoord_pts);
445
446        for i in 0..pts1.data.nrows() {
447            let pt1 = pts1.data.row(i);
448
449            // Convert world to pixel using matrix multiply.
450            let cc = camcoord_pts.data.row(i);
451            let coords = nalgebra::Point3::new(cc[(0, 0)], cc[(0, 1)], cc[(0, 2)]);
452            let cc = pmat * coords.to_homogeneous();
453            let pt2 = SMatrix::<f64, 1, 2>::new(cc[0] / cc[2], cc[1] / cc[2]);
454
455            approx::assert_abs_diff_eq!(pt1[0], pt2[0], epsilon = 1e-5);
456            approx::assert_abs_diff_eq!(pt1[1], pt2[1], epsilon = 1e-5);
457        }
458    }
459
460    #[test]
461    fn test_to_from_pmat() {
462        for (name, cam) in &get_test_cameras() {
463            println!("\n\n\ntesting camera {}", name);
464
465            // Get camera matrix from this Camera instance and check it.
466            let pmat = cam.as_camera_matrix();
467            assert_is_pmat_same(cam, &pmat);
468
469            // Create a new Camera instance from this matrix and check it.
470            let cam2 = Camera::from_perspective_matrix(&pmat).unwrap();
471            let pmat2 = cam2.as_camera_matrix();
472            assert_is_pmat_same(&cam2, &pmat);
473            assert_is_pmat_same(cam, &pmat2);
474        }
475    }
476
477    fn get_test_cameras() -> Vec<(String, Camera<f64, IntrinsicParametersPerspective<f64>>)> {
478        let mut result = Vec::new();
479
480        // camera 1 - from perspective parameters
481        let params = PerspectiveParams {
482            fx: 100.0,
483            fy: 102.0,
484            skew: 0.1,
485            cx: 321.0,
486            cy: 239.9,
487        };
488
489        let cam: IntrinsicParametersPerspective<_> = params.into();
490        roundtrip_intrinsics(&cam, 640, 480, 5, 0, nalgebra::convert(1e-10));
491
492        let extrinsics = ExtrinsicParameters::from_view(
493            &Vector3::new(1.2, 3.4, 5.6),                                // camcenter
494            &Vector3::new(2.2, 3.4, 5.6),                                // lookat
495            &nalgebra::Unit::new_normalize(Vector3::new(0.0, 0.0, 1.0)), // up
496        );
497
498        let from_params = Camera::new(cam, extrinsics);
499        result.push(("from-params".into(), from_params));
500
501        // in the future - more cameras
502
503        result
504    }
505
506    #[test]
507    #[cfg(feature = "serde-serialize")]
508    fn test_serde() {
509        let params = PerspectiveParams {
510            fx: 100.0,
511            fy: 102.0,
512            skew: 0.1,
513            cx: 321.0,
514            cy: 239.9,
515        };
516
517        let expected: IntrinsicParametersPerspective<_> = params.into();
518
519        let buf = serde_json::to_string(&expected).unwrap();
520        let actual: IntrinsicParametersPerspective<f64> = serde_json::from_str(&buf).unwrap();
521        assert!(expected == actual);
522        assert!(actual.fx() == 100.0);
523        assert!(actual.fy() == 102.0);
524        assert!(actual.skew() == 0.1);
525        assert!(actual.cx() == 321.0);
526        assert!(actual.cy() == 239.9);
527    }
528}
cam_geom/intrinsics_perspective.rs

cam_geom/
intrinsics_perspective.rs
