opencv_ros_camera/
lib.rs

1#![doc = include_str!("../README.md")]
2#![deny(rust_2018_idioms, unsafe_code, missing_docs)]
3#![cfg_attr(not(feature = "std"), no_std)]
4
5#[cfg(not(feature = "std"))]
6extern crate core as std;
7
8#[cfg(feature = "serde-serialize")]
9use serde::{Deserialize, Serialize};
10
11use nalgebra::{
12    allocator::Allocator,
13    base::storage::{Owned, Storage},
14    convert, one, zero, DefaultAllocator, Dim, Matrix3, OMatrix, RealField, SMatrix, Vector2,
15    Vector3, Vector5, U1, U2, U3,
16};
17
18use cam_geom::{
19    coordinate_system::CameraFrame, ray_bundle_types::SharedOriginRayBundle, Bundle,
20    IntrinsicParameters, Pixels, Points, RayBundle,
21};
22
23#[cfg(feature = "std")]
24mod ros_file_support;
25#[cfg(feature = "std")]
26pub use ros_file_support::{NamedIntrinsicParameters, RosCameraInfo, RosMatrix};
27
28#[cfg(feature = "serde-serialize")]
29pub use ros_file_support::from_ros_yaml;
30
31/// Possible errors.
32#[derive(Debug)]
33#[cfg_attr(feature = "std", derive(thiserror::Error))]
34#[non_exhaustive]
35pub enum Error {
36    #[cfg_attr(feature = "std", error("invalid input"))]
37    /// invalid input
38    InvalidInput,
39    #[cfg_attr(feature = "std", error("error parsing YAML"))]
40    /// error parsing YAML
41    YamlParseError,
42    #[cfg_attr(feature = "std", error("unknown distortion model"))]
43    /// unknown distortion model
44    UnknownDistortionModel,
45    #[cfg_attr(feature = "std", error("bad matrix size"))]
46    /// bad matrix size
47    BadMatrixSize,
48}
49
50#[cfg(feature = "serde-serialize")]
51impl std::convert::From<serde_yaml::Error> for Error {
52    #[inline]
53    fn from(_orig: serde_yaml::Error) -> Self {
54        Error::YamlParseError
55    }
56}
57
58/// Result type
59pub type Result<T> = std::result::Result<T, Error>;
60
61/// A perspective camera model with distortion compatible with OpenCV and ROS.
62///
63/// This camera model is compatible with OpenCV and ROS, including stereo
64/// rectification and Brown-Conrady
65/// [distortion](https://en.wikipedia.org/wiki/Distortion_(optics)). To load
66/// from a ROS YAML file, see the [`from_ros_yaml`](fn.from_ros_yaml.html)
67/// function.
68///
69/// See [this page](http://wiki.ros.org/image_pipeline/CameraInfo) for an
70/// expanded definition of the parameters.
71///
72/// To convert from a
73/// [`NamedIntrinsicParameters`](struct.NamedIntrinsicParameters.html) struct,
74/// use its
75/// [`intrinsics`](struct.NamedIntrinsicParameters.html#structfield.intrinsics)
76/// field.
77///
78/// See the [module-level documentation for more information](index.html).
79#[derive(Clone, PartialEq)]
80pub struct RosOpenCvIntrinsics<R: RealField> {
81    /// If these intrinsics have zero skew, they are "opencv compatible" and this is `true`.
82    pub is_opencv_compatible: bool,
83    /// The intrinsic parameter matrix `P`.
84    pub p: SMatrix<R, 3, 4>,
85    /// The intrinsic parameter matrix `K`. Scaled from `P`.
86    pub k: SMatrix<R, 3, 3>,
87    /// The non-linear distortion parameters `D` specifying image warping.
88    pub distortion: Distortion<R>,
89    /// The stereo rectification matrix.
90    pub rect: SMatrix<R, 3, 3>,
91    cache: Cache<R>,
92}
93
94impl<R: RealField> std::fmt::Debug for RosOpenCvIntrinsics<R> {
95    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
96        f.debug_struct("RosOpenCvIntrinsics")
97            .field("p", &self.p)
98            .field("distortion", &self.distortion)
99            .field("rect", &self.rect)
100            .finish()
101    }
102}
103
104impl<R: RealField> From<cam_geom::IntrinsicParametersPerspective<R>> for RosOpenCvIntrinsics<R> {
105    fn from(orig: cam_geom::IntrinsicParametersPerspective<R>) -> Self {
106        Self::from_params(orig.fx(), orig.skew(), orig.fy(), orig.cx(), orig.cy())
107    }
108}
109
110#[derive(Debug, Clone, PartialEq)]
111struct Cache<R: RealField> {
112    pnorm: SMatrix<R, 3, 4>,
113    rect_t: Matrix3<R>,
114    rti: Matrix3<R>,
115}
116
117/// Undistorted 2D pixel locations
118///
119/// See [the wikipedia page on
120/// distortion](https://en.wikipedia.org/wiki/Distortion_(optics)) for a
121/// discussion. This type represents pixel coordinates which have been
122/// undistorted.
123///
124/// This is a newtype wrapping an `nalgebra::Matrix`.
125pub struct UndistortedPixels<R: RealField, NPTS: Dim, STORAGE> {
126    /// The undistorted pixel coordinates.
127    pub data: nalgebra::Matrix<R, NPTS, U2, STORAGE>,
128}
129
130impl<R: RealField> RosOpenCvIntrinsics<R> {
131    /// Construct intrinsics from raw components.
132    ///
133    /// Returns `Err(Error::InvalidInput)` if `rect` cannot be inverted.
134    pub fn from_components(
135        p: SMatrix<R, 3, 4>,
136        k: SMatrix<R, 3, 3>,
137        distortion: Distortion<R>,
138        rect: SMatrix<R, 3, 3>,
139    ) -> Result<Self> {
140        let is_opencv_compatible = p[(0, 1)] == zero();
141        let pnorm = p.clone() / p[(2, 2)].clone();
142        let rect_t = rect.transpose();
143        let mut rti = Matrix3::<R>::identity();
144        if !nalgebra::linalg::try_invert_to(rect_t.clone(), &mut rti) {
145            return Err(Error::InvalidInput);
146        }
147
148        let cache = Cache { pnorm, rect_t, rti };
149        Ok(Self {
150            is_opencv_compatible,
151            p,
152            k,
153            distortion,
154            rect,
155            cache,
156        })
157    }
158
159    /// Construct intrinsics from individual parameters with no distortion.
160    ///
161    /// `fx` and `fy` are the horizontal and vertical focal lengths. `skew` is
162    /// the pixel skew (typically near zero). `cx` and `cy` is the center of the
163    /// optical axis in pixel coordinates.
164    #[inline]
165    pub fn from_params(fx: R, skew: R, fy: R, cx: R, cy: R) -> Self {
166        Self::from_params_with_distortion(fx, skew, fy, cx, cy, Distortion::zero())
167    }
168
169    /// Construct intrinsics from individual parameters.
170    ///
171    /// `fx` and `fy` are the horizontal and vertical focal lengths. `skew` is
172    /// the pixel skew (typically near zero). `cx` and `cy` is the center of the
173    /// optical axis in pixel coordinates. `distortion` is a vector of the
174    /// distortion terms.
175    pub fn from_params_with_distortion(
176        fx: R,
177        skew: R,
178        fy: R,
179        cx: R,
180        cy: R,
181        distortion: Distortion<R>,
182    ) -> Self {
183        let zero: R = zero();
184        let one: R = one();
185        let p = SMatrix::<R, 3, 4>::new(
186            fx.clone(),
187            skew.clone(),
188            cx.clone(),
189            zero.clone(),
190            zero.clone(),
191            fy.clone(),
192            cy.clone(),
193            zero.clone(),
194            zero.clone(),
195            zero.clone(),
196            one.clone(),
197            zero.clone(),
198        );
199        let k =
200            SMatrix::<R, 3, 3>::new(fx, skew, cx, zero.clone(), fy, cy, zero.clone(), zero, one);
201        let rect = Matrix3::<R>::identity();
202        // Since rect can be inverted, this will not fail and we can unwrap.
203        Self::from_components(p, k, distortion, rect).unwrap()
204    }
205
206    /// Convert undistorted pixel coordinates to distorted pixel coordinates.
207    ///
208    /// This will take coordinates from, e.g. a linear camera model, warp them
209    /// into their distorted counterparts. This distortion thus models the
210    /// action of a real lens.
211    #[allow(clippy::many_single_char_names)]
212    pub fn distort<NPTS, IN>(
213        &self,
214        undistorted: &UndistortedPixels<R, NPTS, IN>,
215    ) -> Pixels<R, NPTS, Owned<R, NPTS, U2>>
216    where
217        NPTS: Dim,
218        IN: nalgebra::base::storage::Storage<R, NPTS, U2>,
219        DefaultAllocator: Allocator<NPTS, U2>,
220    {
221        let mut result = Pixels::new(OMatrix::zeros_generic(
222            NPTS::from_usize(undistorted.data.nrows()),
223            U2::from_usize(2),
224        ));
225
226        let p = &self.p;
227        let fx = p[(0, 0)].clone();
228        let cx = p[(0, 2)].clone();
229        let tx = p[(0, 3)].clone();
230        let fy = p[(1, 1)].clone();
231        let cy = p[(1, 2)].clone();
232        let ty = p[(1, 3)].clone();
233
234        let one: R = one();
235        let two: R = convert(2.0);
236
237        let d = &self.distortion;
238        let k1 = d.radial1();
239        let k2 = d.radial2();
240        let p1 = d.tangential1();
241        let p2 = d.tangential2();
242        let k3 = d.radial3();
243
244        let k = &self.k;
245        let kfx = k[(0, 0)].clone();
246        let kcx = k[(0, 2)].clone();
247        let kfy = k[(1, 1)].clone();
248        let kcy = k[(1, 2)].clone();
249
250        for i in 0..undistorted.data.nrows() {
251            let x = (undistorted.data[(i, 0)].clone() - cx.clone() - tx.clone()) / fx.clone();
252            let y = (undistorted.data[(i, 1)].clone() - cy.clone() - ty.clone()) / fy.clone();
253
254            let xy1 = Vector3::new(x.clone(), y.clone(), one.clone());
255            let xyw = self.cache.rect_t.clone() * xy1;
256            let xp = xyw[0].clone() / xyw[2].clone();
257            let yp = xyw[1].clone() / xyw[2].clone();
258
259            let r2 = xp.clone() * xp.clone() + yp.clone() * yp.clone();
260            let r4 = r2.clone() * r2.clone();
261            let r6 = r4.clone() * r2.clone();
262            let a1 = two.clone() * xp.clone() * yp.clone();
263
264            let barrel = one.clone()
265                + k1.clone() * r2.clone()
266                + k2.clone() * r4.clone()
267                + k3.clone() * r6.clone();
268            let xpp = xp.clone() * barrel.clone()
269                + p1.clone() * a1.clone()
270                + p2.clone() * (r2.clone() + two.clone() * (xp.clone() * xp.clone()));
271            let ypp = yp.clone() * barrel.clone()
272                + p1.clone() * (r2.clone() + two.clone() * (yp.clone() * yp.clone()))
273                + p2.clone() * a1.clone();
274
275            let u = xpp.clone() * kfx.clone() + kcx.clone();
276            let v = ypp.clone() * kfy.clone() + kcy.clone();
277
278            result.data[(i, 0)] = u;
279            result.data[(i, 1)] = v;
280        }
281        result
282    }
283
284    /// Convert distorted pixel coordinates to undistorted pixel coordinates.
285    ///
286    /// This will take distorted coordinates from, e.g. detections from a real
287    /// camera image, and undo the effect of the distortion model. This
288    /// "undistortion" thus converts coordinates from a real lens into
289    /// coordinates that can be used with a linear camera model.
290    ///
291    /// This method calls [undistort_ext](Self::undistort_ext) using the default
292    /// termination criteria.
293    pub fn undistort<NPTS, IN>(
294        &self,
295        distorted: &Pixels<R, NPTS, IN>,
296    ) -> UndistortedPixels<R, NPTS, Owned<R, NPTS, U2>>
297    where
298        NPTS: Dim,
299        IN: nalgebra::base::storage::Storage<R, NPTS, U2>,
300        DefaultAllocator: Allocator<NPTS, U2>,
301    {
302        self.undistort_ext(distorted, None)
303    }
304
305    /// Convert distorted pixel coordinates to undistorted pixel coordinates.
306    ///
307    /// This will take distorted coordinates from, e.g. detections from a real
308    /// camera image, and undo the effect of the distortion model. This
309    /// "undistortion" thus converts coordinates from a real lens into
310    /// coordinates that can be used with a linear camera model.
311    ///
312    /// If the termination criteria are not specified, the default of five
313    /// iterations is used.
314    #[allow(clippy::many_single_char_names)]
315    pub fn undistort_ext<NPTS, IN>(
316        &self,
317        distorted: &Pixels<R, NPTS, IN>,
318        criteria: impl Into<Option<TermCriteria>>,
319    ) -> UndistortedPixels<R, NPTS, Owned<R, NPTS, U2>>
320    where
321        NPTS: Dim,
322        IN: nalgebra::base::storage::Storage<R, NPTS, U2>,
323        DefaultAllocator: Allocator<NPTS, U2>,
324    {
325        let criteria = criteria.into().unwrap_or(TermCriteria::MaxIter(5));
326        let mut result = UndistortedPixels {
327            data: OMatrix::zeros_generic(
328                NPTS::from_usize(distorted.data.nrows()),
329                U2::from_usize(2),
330            ),
331        };
332
333        let k = &self.k;
334        let fx = k[(0, 0)].clone();
335        let cx = k[(0, 2)].clone();
336        let fy = k[(1, 1)].clone();
337        let cy = k[(1, 2)].clone();
338
339        let p = &self.p;
340        let fxp = p[(0, 0)].clone();
341        let cxp = p[(0, 2)].clone();
342        let fyp = p[(1, 1)].clone();
343        let cyp = p[(1, 2)].clone();
344
345        let d = &self.distortion;
346
347        let t1 = d.tangential1();
348        let t2 = d.tangential2();
349
350        let one: R = one();
351        let two: R = convert(2.0);
352
353        for i in 0..distorted.data.nrows() {
354            // Apply intrinsic parameters to get normalized, distorted coordinates
355            let xd = (distorted.data[(i, 0)].clone() - cx.clone()) / fx.clone();
356            let yd = (distorted.data[(i, 1)].clone() - cy.clone()) / fy.clone();
357
358            let mut x = xd.clone();
359            let mut y = yd.clone();
360            let mut count = 0;
361
362            loop {
363                if let TermCriteria::MaxIter(max_count) = criteria {
364                    count += 1;
365                    if count > max_count {
366                        break;
367                    }
368                }
369
370                let r2 = x.clone() * x.clone() + y.clone() * y.clone();
371                let icdist = one.clone()
372                    / (one.clone()
373                        + ((d.radial3() * r2.clone() + d.radial2()) * r2.clone() + d.radial1())
374                            * r2.clone());
375                let delta_x = two.clone() * t1.clone() * x.clone() * y.clone()
376                    + t2.clone() * (r2.clone() + two.clone() * x.clone() * x.clone());
377                let delta_y = t1.clone() * (r2.clone() + two.clone() * y.clone() * y.clone())
378                    + two.clone() * t2.clone() * x.clone() * y.clone();
379                x = (xd.clone() - delta_x) * icdist.clone();
380                y = (yd.clone() - delta_y) * icdist.clone();
381
382                if let TermCriteria::Eps(eps) = criteria {
383                    let r2 = x.clone() * x.clone() + y.clone() * y.clone();
384                    let cdist = one.clone()
385                        + ((d.radial3() * r2.clone() + d.radial2()) * r2.clone() + d.radial1())
386                            * r2.clone();
387                    let delta_x = two.clone() * t1.clone() * x.clone() * y.clone()
388                        + t2.clone() * (r2.clone() + two.clone() * x.clone() * x.clone());
389                    let delta_y = t1.clone() * (r2.clone() + two.clone() * y.clone() * y.clone())
390                        + two.clone() * t2.clone() * x.clone() * y.clone();
391                    let xp0 = x.clone() * cdist.clone() + delta_x.clone();
392                    let yp0 = y.clone() * cdist.clone() + delta_y.clone();
393
394                    let xywt = self.cache.rti.clone() * Vector3::new(xp0, yp0, one.clone());
395                    let xp = xywt[0].clone() / xywt[2].clone();
396                    let yp = xywt[1].clone() / xywt[2].clone();
397
398                    let up = x.clone() * fxp.clone() + cxp.clone();
399                    let vp = y.clone() * fyp.clone() + cyp.clone();
400
401                    let error = (Vector2::new(xp, yp) - Vector2::new(up, vp)).norm();
402                    if error < convert(eps) {
403                        break;
404                    }
405                }
406            }
407
408            let xp = x;
409            let yp = y;
410
411            let uh = Vector3::new(xp.clone(), yp.clone(), one.clone());
412            let xywt = self.cache.rti.clone() * uh.clone();
413            let x = xywt[0].clone() / xywt[2].clone();
414            let y = xywt[1].clone() / xywt[2].clone();
415
416            let up = x.clone() * fxp.clone() + cxp.clone();
417            let vp = y.clone() * fyp.clone() + cyp.clone();
418            result.data[(i, 0)] = up.clone();
419            result.data[(i, 1)] = vp.clone();
420        }
421        result
422    }
423
424    /// Convert 3D coordinates in `CameraFrame` to undistorted pixel coords.
425    pub fn camera_to_undistorted_pixel<IN, NPTS>(
426        &self,
427        camera: &Points<CameraFrame, R, NPTS, IN>,
428    ) -> UndistortedPixels<R, NPTS, Owned<R, NPTS, U2>>
429    where
430        IN: Storage<R, NPTS, U3>,
431        NPTS: Dim,
432        DefaultAllocator: Allocator<NPTS, U2>,
433        DefaultAllocator: Allocator<U1, U2>,
434    {
435        let mut result = UndistortedPixels {
436            data: OMatrix::zeros_generic(NPTS::from_usize(camera.data.nrows()), U2::from_usize(2)),
437        };
438
439        // TODO: can we remove this loop?
440        for i in 0..camera.data.nrows() {
441            let x = nalgebra::Point3::new(
442                camera.data[(i, 0)].clone(),
443                camera.data[(i, 1)].clone(),
444                camera.data[(i, 2)].clone(),
445            )
446            .to_homogeneous();
447            let rst = self.p.clone() * x;
448
449            result.data[(i, 0)] = rst[0].clone() / rst[2].clone();
450            result.data[(i, 1)] = rst[1].clone() / rst[2].clone();
451        }
452        result
453    }
454
455    /// Convert undistorted pixel coordinates to 3D coords in the `CameraFrame`.
456    pub fn undistorted_pixel_to_camera<IN, NPTS>(
457        &self,
458        undistorteds: &UndistortedPixels<R, NPTS, IN>,
459    ) -> RayBundle<CameraFrame, SharedOriginRayBundle<R>, R, NPTS, Owned<R, NPTS, U3>>
460    where
461        IN: Storage<R, NPTS, U2>,
462        NPTS: Dim,
463        DefaultAllocator: Allocator<NPTS, U3>,
464        DefaultAllocator: Allocator<U1, U2>,
465    {
466        let p = self.cache.pnorm.clone();
467
468        let mut result = RayBundle::new_shared_zero_origin(OMatrix::zeros_generic(
469            NPTS::from_usize(undistorteds.data.nrows()),
470            U3::from_usize(3),
471        ));
472
473        // TODO: eliminate this loop
474        for i in 0..undistorteds.data.nrows() {
475            // Create a slice view of a single pixel coordinate.
476            let undistorted = UndistortedPixels {
477                data: undistorteds.data.row(i),
478            };
479
480            let uv_rect_x = undistorted.data[(0, 0)].clone();
481            let uv_rect_y = undistorted.data[(0, 1)].clone();
482
483            // Convert undistorted point into camcoords.
484            let y = (uv_rect_y.clone() - p[(1, 2)].clone() - p[(1, 3)].clone()) / p[(1, 1)].clone();
485            let x = (uv_rect_x.clone()
486                - p[(0, 1)].clone() * y.clone()
487                - p[(0, 2)].clone()
488                - p[(0, 3)].clone())
489                / p[(0, 0)].clone();
490            let z = one();
491
492            result.data[(i, 0)] = x;
493            result.data[(i, 1)] = y;
494            result.data[(i, 2)] = z;
495        }
496        result
497    }
498
499    /// Return the horizontal focal length
500    #[inline]
501    pub fn fx(&self) -> R {
502        self.p[(0, 0)].clone()
503    }
504
505    /// Return the vertical focal length
506    #[inline]
507    pub fn fy(&self) -> R {
508        self.p[(1, 1)].clone()
509    }
510
511    /// Return the skew
512    #[inline]
513    pub fn skew(&self) -> R {
514        self.p[(0, 1)].clone()
515    }
516
517    /// Return the horizontal center
518    #[inline]
519    pub fn cx(&self) -> R {
520        self.p[(0, 2)].clone()
521    }
522
523    /// Return the vertical center
524    #[inline]
525    pub fn cy(&self) -> R {
526        self.p[(1, 2)].clone()
527    }
528}
529
530#[test]
531fn intrinsics_roundtrip() {
532    let i = RosOpenCvIntrinsics::from_params(1.0, 2.0, 3.0, 4.0, 5.0);
533    let i2 = RosOpenCvIntrinsics::from_params(i.fx(), i.skew(), i.fy(), i.cx(), i.cy());
534    assert_eq!(i, i2);
535}
536
537/// Specifies distortion using the Brown-Conrady "plumb bob" model.
538#[derive(Debug, Clone, PartialEq)]
539#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
540pub struct Distortion<R: RealField>(Vector5<R>);
541
542impl<R: RealField> Distortion<R> {
543    /// build from vector ordered [radial1, radial2, tangential1, tangential2, radial3]
544    #[inline]
545    pub fn from_opencv_vec(v: Vector5<R>) -> Self {
546        Distortion(v)
547    }
548
549    /// OpenCV ordered vector of distortion terms.
550    ///
551    /// The order is [radial1, radial2, tangential1, tangential2, radial3].
552    #[inline]
553    pub fn opencv_vec(&self) -> &Vector5<R> {
554        &self.0
555    }
556
557    /// Construct a zero distortion model.
558    #[inline]
559    pub fn zero() -> Self {
560        Distortion(Vector5::new(zero(), zero(), zero(), zero(), zero()))
561    }
562
563    /// The first radial distortion term, sometimes called `k1`.
564    #[inline]
565    pub fn radial1(&self) -> R {
566        self.0[0].clone()
567    }
568
569    /// The first radial distortion term, sometimes called `k1` (mutable reference).
570    #[inline]
571    pub fn radial1_mut(&mut self) -> &mut R {
572        &mut self.0[0]
573    }
574
575    /// The second radial distortion term, sometimes called `k2`.
576    #[inline]
577    pub fn radial2(&self) -> R {
578        self.0[1].clone()
579    }
580
581    /// The second radial distortion term, sometimes called `k2` (mutable reference).
582    #[inline]
583    pub fn radial2_mut(&mut self) -> &mut R {
584        &mut self.0[1]
585    }
586
587    /// The first tangential distortion term, sometimes called `p1`.
588    #[inline]
589    pub fn tangential1(&self) -> R {
590        self.0[2].clone()
591    }
592
593    /// The first tangential distortion term, sometimes called `p1` (mutable reference).
594    #[inline]
595    pub fn tangential1_mut(&mut self) -> &mut R {
596        &mut self.0[2]
597    }
598
599    /// The second tangential distortion term, sometimes called `p2`.
600    #[inline]
601    pub fn tangential2(&self) -> R {
602        self.0[3].clone()
603    }
604
605    /// The second tangential distortion term, sometimes called `p2` (mutable reference).
606    #[inline]
607    pub fn tangential2_mut(&mut self) -> &mut R {
608        &mut self.0[3]
609    }
610
611    /// The third radial distortion term, sometimes called `k3`.
612    #[inline]
613    pub fn radial3(&self) -> R {
614        self.0[4].clone()
615    }
616
617    /// The third radial distortion term, sometimes called `k3` (mutable reference).
618    #[inline]
619    pub fn radial3_mut(&mut self) -> &mut R {
620        &mut self.0[4]
621    }
622
623    /// Return `true` if there is approximately zero distortion, else `false`.
624    pub fn is_linear(&self) -> bool {
625        let v = &self.0;
626        let sum_squared = v.dot(v);
627        sum_squared < nalgebra::convert(1e-16)
628    }
629}
630
631impl<R: RealField> IntrinsicParameters<R> for RosOpenCvIntrinsics<R> {
632    type BundleType = SharedOriginRayBundle<R>;
633
634    fn pixel_to_camera<IN, NPTS>(
635        &self,
636        pixels: &Pixels<R, NPTS, IN>,
637    ) -> RayBundle<CameraFrame, Self::BundleType, R, NPTS, Owned<R, NPTS, U3>>
638    where
639        Self::BundleType: Bundle<R>,
640        IN: Storage<R, NPTS, U2>,
641        NPTS: Dim,
642        DefaultAllocator: Allocator<NPTS, U2>,
643        DefaultAllocator: Allocator<NPTS, U3>,
644        DefaultAllocator: Allocator<U1, U2>,
645    {
646        let undistorted = self.undistort::<NPTS, IN>(pixels);
647        self.undistorted_pixel_to_camera(&undistorted)
648    }
649
650    fn camera_to_pixel<IN, NPTS>(
651        &self,
652        camera: &Points<CameraFrame, R, NPTS, IN>,
653    ) -> Pixels<R, NPTS, Owned<R, NPTS, U2>>
654    where
655        IN: Storage<R, NPTS, U3>,
656        NPTS: Dim,
657        DefaultAllocator: Allocator<NPTS, U2>,
658    {
659        let undistorted = self.camera_to_undistorted_pixel(camera);
660        self.distort(&undistorted)
661    }
662}
663
664/// Extension trait to add `world_to_undistorted_pixel()` method.
665pub trait CameraExt<R: RealField> {
666    /// Convert 3D coordinates in the `WorldFrame` to undistorted pixel coordinates.
667    fn world_to_undistorted_pixel<NPTS, InStorage>(
668        &self,
669        world: &Points<cam_geom::WorldFrame, R, NPTS, InStorage>,
670    ) -> UndistortedPixels<R, NPTS, Owned<R, NPTS, U2>>
671    where
672        NPTS: Dim,
673        InStorage: Storage<R, NPTS, U3>,
674        DefaultAllocator: Allocator<NPTS, U3>,
675        DefaultAllocator: Allocator<NPTS, U2>;
676}
677
678impl<R: RealField> CameraExt<R> for cam_geom::Camera<R, RosOpenCvIntrinsics<R>> {
679    fn world_to_undistorted_pixel<NPTS, InStorage>(
680        &self,
681        world: &Points<cam_geom::WorldFrame, R, NPTS, InStorage>,
682    ) -> UndistortedPixels<R, NPTS, Owned<R, NPTS, U2>>
683    where
684        NPTS: Dim,
685        InStorage: Storage<R, NPTS, U3>,
686        DefaultAllocator: Allocator<NPTS, U3>,
687        DefaultAllocator: Allocator<NPTS, U2>,
688    {
689        let camera_frame = self.extrinsics().world_to_camera(world);
690        self.intrinsics().camera_to_undistorted_pixel(&camera_frame)
691    }
692}
693
694#[cfg(feature = "serde-serialize")]
695impl<R: RealField + serde::Serialize> serde::Serialize for RosOpenCvIntrinsics<R> {
696    fn serialize<S>(&self, serializer: S) -> std::result::Result<S::Ok, S::Error>
697    where
698        S: serde::Serializer,
699    {
700        use serde::ser::SerializeStruct;
701
702        // 4 is the number of fields we serialize from the struct.
703        let mut state = serializer.serialize_struct("RosOpenCvIntrinsics", 4)?;
704        state.serialize_field("p", &self.p)?;
705        state.serialize_field("k", &self.k)?;
706        state.serialize_field("distortion", &self.distortion)?;
707        state.serialize_field("rect", &self.rect)?;
708        state.end()
709    }
710}
711
712#[cfg(feature = "serde-serialize")]
713impl<'de, R: RealField + serde::Deserialize<'de>> serde::Deserialize<'de>
714    for RosOpenCvIntrinsics<R>
715{
716    fn deserialize<D>(deserializer: D) -> std::result::Result<Self, D::Error>
717    where
718        D: serde::Deserializer<'de>,
719    {
720        use serde::de;
721        use std::fmt;
722
723        #[derive(Deserialize)]
724        #[serde(field_identifier, rename_all = "lowercase")]
725        enum Field {
726            P,
727            K,
728            Distortion,
729            Rect,
730        }
731
732        struct IntrinsicParametersVisitor<'de, R2: RealField + serde::Deserialize<'de>>(
733            std::marker::PhantomData<&'de R2>,
734        );
735
736        impl<'de, R2: RealField + serde::Deserialize<'de>> serde::de::Visitor<'de>
737            for IntrinsicParametersVisitor<'de, R2>
738        {
739            type Value = RosOpenCvIntrinsics<R2>;
740
741            fn expecting(&self, formatter: &mut fmt::Formatter<'_>) -> fmt::Result {
742                formatter.write_str("struct RosOpenCvIntrinsics")
743            }
744
745            fn visit_seq<V>(
746                self,
747                mut seq: V,
748            ) -> std::result::Result<RosOpenCvIntrinsics<R2>, V::Error>
749            where
750                V: serde::de::SeqAccess<'de>,
751            {
752                let p = seq
753                    .next_element()?
754                    .ok_or_else(|| de::Error::invalid_length(0, &self))?;
755                let k = seq
756                    .next_element()?
757                    .ok_or_else(|| de::Error::invalid_length(1, &self))?;
758                let distortion = seq
759                    .next_element()?
760                    .ok_or_else(|| de::Error::invalid_length(1, &self))?;
761                let rect = seq
762                    .next_element()?
763                    .ok_or_else(|| de::Error::invalid_length(1, &self))?;
764                // Ok(RosOpenCvIntrinsics::from_components(p, k, distortion, rect))
765                RosOpenCvIntrinsics::from_components(p, k, distortion, rect)
766                    .map_err(|e| de::Error::custom(e))
767            }
768
769            fn visit_map<V>(
770                self,
771                mut map: V,
772            ) -> std::result::Result<RosOpenCvIntrinsics<R2>, V::Error>
773            where
774                V: serde::de::MapAccess<'de>,
775            {
776                let mut p = None;
777                let mut k = None;
778                let mut distortion = None;
779                let mut rect = None;
780                while let Some(key) = map.next_key()? {
781                    match key {
782                        Field::P => {
783                            if p.is_some() {
784                                return Err(de::Error::duplicate_field("p"));
785                            }
786                            p = Some(map.next_value()?);
787                        }
788                        Field::K => {
789                            if k.is_some() {
790                                return Err(de::Error::duplicate_field("k"));
791                            }
792                            k = Some(map.next_value()?);
793                        }
794                        Field::Distortion => {
795                            if distortion.is_some() {
796                                return Err(de::Error::duplicate_field("distortion"));
797                            }
798                            distortion = Some(map.next_value()?);
799                        }
800                        Field::Rect => {
801                            if rect.is_some() {
802                                return Err(de::Error::duplicate_field("rect"));
803                            }
804                            rect = Some(map.next_value()?);
805                        }
806                    }
807                }
808                let p = p.ok_or_else(|| de::Error::missing_field("p"))?;
809                let k = k.ok_or_else(|| de::Error::missing_field("k"))?;
810                let distortion =
811                    distortion.ok_or_else(|| de::Error::missing_field("distortion"))?;
812                let rect = rect.ok_or_else(|| de::Error::missing_field("rect"))?;
813                RosOpenCvIntrinsics::from_components(p, k, distortion, rect)
814                    .map_err(|e| de::Error::custom(e))
815            }
816        }
817
818        const FIELDS: &'static [&'static str] = &["p", "k", "distortion", "rect"];
819        deserializer.deserialize_struct(
820            "RosOpenCvIntrinsics",
821            FIELDS,
822            IntrinsicParametersVisitor(std::marker::PhantomData),
823        )
824    }
825}
826
827#[cfg(feature = "serde-serialize")]
828fn _test_intrinsics_is_serialize() {
829    // Compile-time test to ensure RosOpenCvIntrinsics implements Serialize trait.
830    fn implements<T: serde::Serialize>() {}
831    implements::<RosOpenCvIntrinsics<f64>>();
832}
833
834#[cfg(feature = "serde-serialize")]
835fn _test_intrinsics_is_deserialize() {
836    // Compile-time test to ensure RosOpenCvIntrinsics implements Deserialize trait.
837    fn implements<'de, T: serde::Deserialize<'de>>() {}
838    implements::<RosOpenCvIntrinsics<f64>>();
839}
840
841/// The type defines termination criteria for iterative algorithms.
842#[derive(Debug, Clone, Copy)]
843pub enum TermCriteria {
844    /// The maximum number of iterations.
845    MaxIter(usize),
846    /// The desired accuracy at which the iterative algorithm stops.
847    Eps(f64),
848}
opencv_ros_camera/lib.rs

opencv_ros_camera/
lib.rs
