cv_convert_fork/
with_opencv_nalgebra.rs

1use crate::nalgebra::{self as na, geometry as geo};
2use crate::opencv::{calib3d, core as core_cv, prelude::*};
3use crate::{common::*, FromCv, TryFromCv, TryIntoCv};
4
5// Note for future maintainers: Since the matrixes need to accommodate any size Matrix, we are using na::OMatrix instead of SMatrix.
6
7/// A pair of rvec and tvec from OpenCV, standing for rotation and translation.
8#[derive(Debug, Clone)]
9pub struct OpenCvPose<T> {
10    pub rvec: T,
11    pub tvec: T,
12}
13
14impl TryFromCv<OpenCvPose<&core_cv::Point3d>> for geo::Isometry3<f64> {
15    type Error = Error;
16
17    fn try_from_cv(pose: OpenCvPose<&core_cv::Point3d>) -> Result<Self> {
18        (&pose).try_into_cv()
19    }
20}
21
22impl TryFromCv<&OpenCvPose<&core_cv::Point3d>> for geo::Isometry3<f64> {
23    type Error = Error;
24
25    fn try_from_cv(pose: &OpenCvPose<&core_cv::Point3d>) -> Result<Self> {
26        let OpenCvPose { rvec, tvec } = *pose;
27        let rotation = {
28            let rvec_mat = {
29                let core_cv::Point3_ { x, y, z, .. } = *rvec;
30                core_cv::Mat::from_slice(&[x, y, z])?
31            };
32            let mut rotation_mat = core_cv::Mat::zeros(3, 3, core_cv::CV_64FC1)?.to_mat()?;
33            calib3d::rodrigues(&rvec_mat, &mut rotation_mat, &mut core_cv::Mat::default())?;
34            let rotation_matrix: na::Matrix3<f64> = TryFromCv::try_from_cv(rotation_mat)?;
35            geo::UnitQuaternion::from_matrix(&rotation_matrix)
36        };
37
38        let translation = {
39            let core_cv::Point3_ { x, y, z } = *tvec;
40            geo::Translation3::new(x, y, z)
41        };
42
43        let isometry = geo::Isometry3::from_parts(translation, rotation);
44        Ok(isometry)
45    }
46}
47
48impl TryFromCv<&OpenCvPose<core_cv::Point3d>> for geo::Isometry3<f64> {
49    type Error = Error;
50
51    fn try_from_cv(from: &OpenCvPose<core_cv::Point3d>) -> Result<Self> {
52        let OpenCvPose { rvec, tvec } = from;
53        TryFromCv::try_from_cv(OpenCvPose { rvec, tvec })
54    }
55}
56
57impl TryFromCv<OpenCvPose<core_cv::Point3d>> for geo::Isometry3<f64> {
58    type Error = Error;
59
60    fn try_from_cv(from: OpenCvPose<core_cv::Point3d>) -> Result<Self> {
61        TryFromCv::try_from_cv(&from)
62    }
63}
64
65impl TryFromCv<OpenCvPose<&core_cv::Mat>> for geo::Isometry3<f64> {
66    type Error = Error;
67
68    fn try_from_cv(from: OpenCvPose<&core_cv::Mat>) -> Result<Self> {
69        (&from).try_into_cv()
70    }
71}
72
73impl TryFromCv<&OpenCvPose<&core_cv::Mat>> for geo::Isometry3<f64> {
74    type Error = Error;
75
76    fn try_from_cv(from: &OpenCvPose<&core_cv::Mat>) -> Result<Self> {
77        let OpenCvPose {
78            rvec: rvec_mat,
79            tvec: tvec_mat,
80        } = *from;
81        let rvec = core_cv::Point3d::try_from_cv(rvec_mat)?;
82        let tvec = core_cv::Point3d::try_from_cv(tvec_mat)?;
83        let isometry = TryFromCv::try_from_cv(OpenCvPose { rvec, tvec })?;
84        Ok(isometry)
85    }
86}
87
88impl TryFromCv<&OpenCvPose<core_cv::Mat>> for geo::Isometry3<f64> {
89    type Error = Error;
90
91    fn try_from_cv(from: &OpenCvPose<core_cv::Mat>) -> Result<Self> {
92        let OpenCvPose { rvec, tvec } = from;
93        TryFromCv::try_from_cv(OpenCvPose { rvec, tvec })
94    }
95}
96
97impl TryFromCv<OpenCvPose<core_cv::Mat>> for geo::Isometry3<f64> {
98    type Error = Error;
99
100    fn try_from_cv(from: OpenCvPose<core_cv::Mat>) -> Result<Self> {
101        let OpenCvPose { rvec, tvec } = &from;
102        TryFromCv::try_from_cv(OpenCvPose { rvec, tvec })
103    }
104}
105
106impl<T> TryFromCv<&geo::Isometry3<T>> for OpenCvPose<core_cv::Point3_<T>>
107where
108    T: core_cv::DataType + na::RealField,
109{
110    type Error = Error;
111
112    fn try_from_cv(from: &geo::Isometry3<T>) -> Result<OpenCvPose<core_cv::Point3_<T>>> {
113        let geo::Isometry3 {
114            rotation,
115            translation,
116            ..
117        } = from;
118
119        let rvec = {
120            let rotation_mat =
121                core_cv::Mat::try_from_cv(rotation.to_rotation_matrix().into_inner())?;
122            let mut rvec_mat = core_cv::Mat::zeros(3, 1, core_cv::CV_64FC1)?.to_mat()?;
123            calib3d::rodrigues(&rotation_mat, &mut rvec_mat, &mut core_cv::Mat::default())?;
124            let rvec = core_cv::Point3_::new(
125                *rvec_mat.at_2d::<T>(0, 0)?,
126                *rvec_mat.at_2d::<T>(1, 0)?,
127                *rvec_mat.at_2d::<T>(2, 0)?,
128            );
129            rvec
130        };
131        let tvec = core_cv::Point3_::new(translation.x, translation.y, translation.z);
132
133        Ok(OpenCvPose { rvec, tvec })
134    }
135}
136
137impl<T> TryFromCv<geo::Isometry3<T>> for OpenCvPose<core_cv::Point3_<T>>
138where
139    T: core_cv::DataType + na::RealField,
140{
141    type Error = Error;
142
143    fn try_from_cv(from: geo::Isometry3<T>) -> Result<OpenCvPose<core_cv::Point3_<T>>> {
144        TryFromCv::try_from_cv(&from)
145    }
146}
147
148impl TryFromCv<&geo::Isometry3<f64>> for OpenCvPose<core_cv::Mat> {
149    type Error = Error;
150
151    fn try_from_cv(from: &geo::Isometry3<f64>) -> Result<OpenCvPose<core_cv::Mat>> {
152        let geo::Isometry3 {
153            rotation,
154            translation,
155            ..
156        } = from;
157
158        let rvec = {
159            let rotation_mat: Mat =
160                TryFromCv::try_from_cv(rotation.to_rotation_matrix().into_inner())?;
161            let mut rvec_mat = core_cv::Mat::zeros(3, 1, core_cv::CV_64FC1)?.to_mat()?;
162            calib3d::rodrigues(&rotation_mat, &mut rvec_mat, &mut core_cv::Mat::default())?;
163            rvec_mat
164        };
165        let tvec = core_cv::Mat::from_slice(&[translation.x, translation.y, translation.z])?;
166
167        Ok(OpenCvPose { rvec, tvec })
168    }
169}
170
171impl TryFromCv<geo::Isometry3<f64>> for OpenCvPose<core_cv::Mat> {
172    type Error = Error;
173
174    fn try_from_cv(from: geo::Isometry3<f64>) -> Result<OpenCvPose<core_cv::Mat>> {
175        TryFromCv::try_from_cv(&from)
176    }
177}
178
179impl TryFromCv<&geo::Isometry3<f32>> for OpenCvPose<core_cv::Mat> {
180    type Error = Error;
181
182    fn try_from_cv(from: &geo::Isometry3<f32>) -> Result<OpenCvPose<core_cv::Mat>> {
183        let geo::Isometry3 {
184            rotation,
185            translation,
186            ..
187        } = from;
188
189        let rvec = {
190            let rotation_mat = Mat::try_from_cv(rotation.to_rotation_matrix().into_inner())?;
191            let mut rvec_mat = Mat::zeros(3, 1, core_cv::CV_32FC1)?.to_mat()?;
192            calib3d::rodrigues(&rotation_mat, &mut rvec_mat, &mut core_cv::Mat::default())?;
193            rvec_mat
194        };
195        let tvec = Mat::from_slice(&[translation.x, translation.y, translation.z])?
196            .t()?
197            .to_mat()?;
198
199        Ok(OpenCvPose { rvec, tvec })
200    }
201}
202
203impl TryFromCv<geo::Isometry3<f32>> for OpenCvPose<core_cv::Mat> {
204    type Error = Error;
205
206    fn try_from_cv(from: geo::Isometry3<f32>) -> Result<OpenCvPose<core_cv::Mat>> {
207        TryFromCv::try_from_cv(&from)
208    }
209}
210
211impl<N, R, C> TryFromCv<&core_cv::Mat> for na::OMatrix<N, R, C>
212where
213    N: na::Scalar + core_cv::DataType,
214    R: na::Dim,
215    C: na::Dim,
216    na::base::default_allocator::DefaultAllocator: na::base::allocator::Allocator<N, R, C>,
217{
218    type Error = Error;
219
220    fn try_from_cv(from: &core_cv::Mat) -> Result<Self> {
221        let shape = from.size()?;
222        {
223            let check_height = R::try_to_usize()
224                .map(|size| size == shape.height as usize)
225                .unwrap_or(true);
226            let check_width = C::try_to_usize()
227                .map(|size| size == shape.width as usize)
228                .unwrap_or(true);
229            let has_same_shape = check_height && check_width;
230            ensure!(has_same_shape, "input and output matrix shapes differ");
231        }
232
233        let rows: Result<Vec<&[N]>, _> = (0..shape.height)
234            .map(|row_idx| from.at_row::<N>(row_idx))
235            .collect();
236        let rows = rows?;
237        let values: Vec<N> = rows
238            .into_iter()
239            .flat_map(|row| row.iter().cloned())
240            .collect();
241
242        Ok(Self::from_row_slice_generic(
243            R::from_usize(shape.height as usize),
244            C::from_usize(shape.width as usize),
245            &values,
246        ))
247    }
248}
249
250impl<N, R, C> TryFromCv<core_cv::Mat> for na::OMatrix<N, R, C>
251where
252    N: na::Scalar + core_cv::DataType,
253    R: na::Dim,
254    C: na::Dim,
255    na::base::default_allocator::DefaultAllocator: na::base::allocator::Allocator<N, R, C>,
256{
257    type Error = Error;
258
259    fn try_from_cv(from: core_cv::Mat) -> Result<Self> {
260        TryFromCv::try_from_cv(&from)
261    }
262}
263
264impl<N, R, C, S> TryFromCv<&na::Matrix<N, R, C, S>> for core_cv::Mat
265where
266    N: na::Scalar + core_cv::DataType,
267    R: na::Dim,
268    C: na::Dim,
269    S: na::base::storage::Storage<N, R, C>,
270    na::base::default_allocator::DefaultAllocator: na::base::allocator::Allocator<N, C, R>,
271{
272    type Error = Error;
273
274    fn try_from_cv(from: &na::Matrix<N, R, C, S>) -> Result<Self> {
275        let nrows = from.nrows();
276        let mat =
277            core_cv::Mat::from_slice(from.transpose().as_slice())?.reshape(1, nrows as i32)?;
278        Ok(mat)
279    }
280}
281
282impl<N, R, C, S> TryFromCv<na::Matrix<N, R, C, S>> for core_cv::Mat
283where
284    N: na::Scalar + core_cv::DataType,
285    R: na::Dim,
286    C: na::Dim,
287    S: na::base::storage::Storage<N, R, C>,
288    na::base::default_allocator::DefaultAllocator: na::base::allocator::Allocator<N, C, R>,
289{
290    type Error = Error;
291
292    fn try_from_cv(from: na::Matrix<N, R, C, S>) -> Result<Self> {
293        TryFromCv::try_from_cv(&from)
294    }
295}
296
297impl<T> FromCv<&na::Point2<T>> for core_cv::Point_<T>
298where
299    T: na::Scalar + Copy,
300{
301    fn from_cv(from: &na::Point2<T>) -> Self {
302        core_cv::Point_::new(from.x, from.y)
303    }
304}
305
306impl<T> FromCv<na::Point2<T>> for core_cv::Point_<T>
307where
308    T: na::Scalar + Copy,
309{
310    fn from_cv(from: na::Point2<T>) -> Self {
311        FromCv::from_cv(&from)
312    }
313}
314
315impl<T> FromCv<&core_cv::Point_<T>> for na::Point2<T>
316where
317    T: na::Scalar + Copy,
318{
319    fn from_cv(from: &core_cv::Point_<T>) -> Self {
320        Self::new(from.x, from.y)
321    }
322}
323
324impl<T> FromCv<core_cv::Point_<T>> for na::Point2<T>
325where
326    T: na::Scalar + Copy,
327{
328    fn from_cv(from: core_cv::Point_<T>) -> Self {
329        FromCv::from_cv(&from)
330    }
331}
332
333impl<T> FromCv<&na::Point3<T>> for core_cv::Point3_<T>
334where
335    T: na::Scalar + Copy,
336{
337    fn from_cv(from: &na::Point3<T>) -> Self {
338        Self::new(from.x, from.y, from.z)
339    }
340}
341
342impl<T> FromCv<na::Point3<T>> for core_cv::Point3_<T>
343where
344    T: na::Scalar + Copy,
345{
346    fn from_cv(from: na::Point3<T>) -> Self {
347        FromCv::from_cv(&from)
348    }
349}
350
351impl<T> FromCv<&core_cv::Point3_<T>> for na::Point3<T>
352where
353    T: na::Scalar + Copy,
354{
355    fn from_cv(from: &core_cv::Point3_<T>) -> Self {
356        Self::new(from.x, from.y, from.z)
357    }
358}
359
360impl<T> FromCv<core_cv::Point3_<T>> for na::Point3<T>
361where
362    T: na::Scalar + Copy,
363{
364    fn from_cv(from: core_cv::Point3_<T>) -> Self {
365        FromCv::from_cv(&from)
366    }
367}
368
369impl<N, const D: usize> TryFromCv<&geo::Translation<N, D>> for core_cv::Mat
370where
371    N: na::Scalar + core_cv::DataType,
372{
373    type Error = Error;
374
375    fn try_from_cv(translation: &geo::Translation<N, D>) -> Result<Self> {
376        let mat = core_cv::Mat::from_exact_iter(translation.vector.into_iter().copied())?;
377        Ok(mat)
378    }
379}
380
381impl<N, const D: usize> TryFromCv<geo::Translation<N, D>> for core_cv::Mat
382where
383    N: na::Scalar + core_cv::DataType,
384{
385    type Error = Error;
386
387    fn try_from_cv(translation: geo::Translation<N, D>) -> Result<Self> {
388        TryFromCv::try_from_cv(&translation)
389    }
390}
391
392#[cfg(test)]
393mod tests {
394    use super::*;
395    use crate::nalgebra::{U2, U3};
396    use crate::opencv::core as core_cv;
397    use crate::{IntoCv, TryIntoCv};
398    use anyhow::Result;
399    use approx::abs_diff_eq;
400    use rand::prelude::*;
401    use std::f64;
402
403    #[test]
404    fn convert_opencv_nalgebra() -> Result<()> {
405        let mut rng = rand::thread_rng();
406
407        for _ in 0..5000 {
408            // FromCv
409            {
410                let cv_point = core_cv::Point2d::new(rng.gen(), rng.gen());
411                let na_point = na::Point2::<f64>::from_cv(&cv_point);
412                ensure!(
413                    abs_diff_eq!(cv_point.x, na_point.x) && abs_diff_eq!(cv_point.y, na_point.y),
414                    "point conversion failed"
415                );
416            }
417
418            // IntoCv
419            {
420                let cv_point = core_cv::Point2d::new(rng.gen(), rng.gen());
421                let na_point: na::Point2<f64> = cv_point.into_cv();
422                ensure!(
423                    abs_diff_eq!(cv_point.x, na_point.x) && abs_diff_eq!(cv_point.y, na_point.y),
424                    "point conversion failed"
425                );
426            }
427
428            // TryFromCv
429            {
430                let na_mat = na::DMatrix::<f64>::from_vec(
431                    2,
432                    3,
433                    vec![
434                        rng.gen(),
435                        rng.gen(),
436                        rng.gen(),
437                        rng.gen(),
438                        rng.gen(),
439                        rng.gen(),
440                    ],
441                );
442                let cv_mat = core_cv::Mat::try_from_cv(&na_mat)?;
443                ensure!(
444                    abs_diff_eq!(cv_mat.at_2d(0, 0)?, na_mat.get((0, 0)).unwrap())
445                        && abs_diff_eq!(cv_mat.at_2d(0, 1)?, na_mat.get((0, 1)).unwrap())
446                        && abs_diff_eq!(cv_mat.at_2d(0, 2)?, na_mat.get((0, 2)).unwrap())
447                        && abs_diff_eq!(cv_mat.at_2d(1, 0)?, na_mat.get((1, 0)).unwrap())
448                        && abs_diff_eq!(cv_mat.at_2d(1, 1)?, na_mat.get((1, 1)).unwrap())
449                        && abs_diff_eq!(cv_mat.at_2d(1, 2)?, na_mat.get((1, 2)).unwrap()),
450                    "matrix conversion failed"
451                );
452            }
453
454            // TryIntoCv
455            {
456                let na_mat = na::DMatrix::<f64>::from_vec(
457                    2,
458                    3,
459                    vec![
460                        rng.gen(),
461                        rng.gen(),
462                        rng.gen(),
463                        rng.gen(),
464                        rng.gen(),
465                        rng.gen(),
466                    ],
467                );
468                let cv_mat: core_cv::Mat = (&na_mat).try_into_cv()?;
469                ensure!(
470                    abs_diff_eq!(cv_mat.at_2d(0, 0)?, na_mat.get((0, 0)).unwrap())
471                        && abs_diff_eq!(cv_mat.at_2d(0, 1)?, na_mat.get((0, 1)).unwrap())
472                        && abs_diff_eq!(cv_mat.at_2d(0, 2)?, na_mat.get((0, 2)).unwrap())
473                        && abs_diff_eq!(cv_mat.at_2d(1, 0)?, na_mat.get((1, 0)).unwrap())
474                        && abs_diff_eq!(cv_mat.at_2d(1, 1)?, na_mat.get((1, 1)).unwrap())
475                        && abs_diff_eq!(cv_mat.at_2d(1, 2)?, na_mat.get((1, 2)).unwrap()),
476                    "matrix conversion failed"
477                );
478            }
479        }
480        Ok(())
481    }
482
483    #[test]
484    fn matrix_nalgebra_to_opencv_test() -> Result<()> {
485        let input = na::OMatrix::<i32, U3, U2>::from_row_slice(&[1, 2, 3, 4, 5, 6]);
486        let (nrows, ncols) = input.shape();
487        let output = core_cv::Mat::try_from_cv(input)?;
488        let output_shape = output.size()?;
489        ensure!(
490            output.channels() == 1
491                && nrows == output_shape.height as usize
492                && ncols == output_shape.width as usize,
493            "the shape does not match"
494        );
495        Ok(())
496    }
497
498    #[test]
499    fn matrix_opencv_to_nalgebra_test() -> Result<()> {
500        let input = Mat::from_slice_2d(&[&[1, 2, 3], &[4, 5, 6]])?;
501        let input_shape = input.size()?;
502        let output = na::OMatrix::<i32, U2, U3>::try_from_cv(input)?;
503        ensure!(
504            output.nrows() == input_shape.height as usize
505                && output.ncols() == input_shape.width as usize,
506            "the shape does not match"
507        );
508        Ok(())
509    }
510
511    #[test]
512    fn rvec_tvec_conversion() -> Result<()> {
513        let mut rng = rand::thread_rng();
514
515        for _ in 0..5000 {
516            let orig_isometry = {
517                let rotation = na::UnitQuaternion::from_euler_angles(
518                    rng.gen_range(0.0..(f64::consts::PI * 2.0)),
519                    rng.gen_range(0.0..(f64::consts::PI * 2.0)),
520                    rng.gen_range(0.0..(f64::consts::PI * 2.0)),
521                );
522                let translation = na::Translation3::new(rng.gen(), rng.gen(), rng.gen());
523                na::Isometry3::from_parts(translation, rotation)
524            };
525            let pose = OpenCvPose::<Mat>::try_from_cv(orig_isometry)?;
526            let recovered_isometry = na::Isometry3::<f64>::try_from_cv(pose)?;
527
528            ensure!(
529                (orig_isometry.to_homogeneous() - recovered_isometry.to_homogeneous()).norm()
530                    <= 1e-6,
531                "the recovered isometry is not consistent the original isometry"
532            );
533        }
534        Ok(())
535    }
536}
cv_convert_fork/with_opencv_nalgebra.rs

cv_convert_fork/
with_opencv_nalgebra.rs
