cam_geom/
extrinsics.rs

1use nalgebra::geometry::{Isometry3, Point3, Rotation3, Translation, UnitQuaternion};
2use nalgebra::{
3    allocator::Allocator,
4    storage::{Owned, Storage},
5    DefaultAllocator, RealField,
6};
7use nalgebra::{convert, Dim, OMatrix, SMatrix, Unit, Vector3, U3};
8
9#[cfg(feature = "serde-serialize")]
10use serde::{Deserialize, Serialize};
11
12use crate::{
13    coordinate_system::{CameraFrame, WorldFrame},
14    Bundle, Points, RayBundle,
15};
16
17/// Defines the pose of a camera in the world coordinate system.
18#[derive(Clone, PartialEq)]
19#[cfg_attr(feature = "serde-serialize", derive(Serialize))]
20pub struct ExtrinsicParameters<R: RealField> {
21    pub(crate) rquat: UnitQuaternion<R>,
22    pub(crate) camcenter: Point3<R>,
23    #[cfg_attr(feature = "serde-serialize", serde(skip))]
24    pub(crate) cache: ExtrinsicsCache<R>,
25}
26
27impl<R: RealField> std::fmt::Debug for ExtrinsicParameters<R> {
28    fn fmt(&self, fmt: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
29        // This should match the auto derived Debug implementation but not print
30        // the cache field.
31        fmt.debug_struct("ExtrinsicParameters")
32            .field("rquat", &self.rquat)
33            .field("camcenter", &self.camcenter)
34            .finish()
35    }
36}
37
38#[derive(Clone, PartialEq)]
39pub(crate) struct ExtrinsicsCache<R: RealField> {
40    pub(crate) q: Rotation3<R>,
41    pub(crate) translation: Point3<R>,
42    pub(crate) qt: SMatrix<R, 3, 4>,
43    pub(crate) q_inv: Rotation3<R>,
44    pub(crate) camcenter_z0: Point3<R>,
45    pub(crate) pose: Isometry3<R>,
46    pub(crate) pose_inv: Isometry3<R>,
47}
48
49impl<R: RealField> std::fmt::Debug for ExtrinsicsCache<R> {
50    fn fmt(&self, _f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
51        // do not show cache
52        Ok(())
53    }
54}
55
56impl<R: RealField> ExtrinsicParameters<R> {
57    /// Create a new instance from a rotation and a camera center.
58    pub fn from_rotation_and_camcenter(rotation: UnitQuaternion<R>, camcenter: Point3<R>) -> Self {
59        let q = rotation.clone().to_rotation_matrix();
60        let translation = -(q.clone() * camcenter.clone());
61        #[rustfmt::skip]
62        let qt = {
63            let q = q.matrix();
64            SMatrix::<R,3,4>::new(
65                q[(0,0)].clone(), q[(0,1)].clone(), q[(0,2)].clone(), translation[0].clone(),
66                q[(1,0)].clone(), q[(1,1)].clone(), q[(1,2)].clone(), translation[1].clone(),
67                q[(2,0)].clone(), q[(2,1)].clone(), q[(2,2)].clone(), translation[2].clone(),
68            )
69        };
70        let q_inv = q.inverse();
71        let camcenter_z0 = Point3::from(Vector3::new(
72            camcenter[0].clone(),
73            camcenter[1].clone(),
74            convert::<_, R>(0.0),
75        ));
76        let pose = Isometry3::from_parts(
77            Translation {
78                vector: translation.clone().coords,
79            },
80            rotation.clone(),
81        );
82        let pose_inv = pose.inverse();
83        let cache = ExtrinsicsCache {
84            q,
85            translation,
86            qt,
87            q_inv,
88            camcenter_z0,
89            pose,
90            pose_inv,
91        };
92
93        Self {
94            rquat: rotation,
95            camcenter,
96            cache,
97        }
98    }
99
100    /// Create a new instance from an [`nalgebra::Isometry3`](https://docs.rs/nalgebra/latest/nalgebra/geometry/type.Isometry3.html).
101    pub fn from_pose(pose: &Isometry3<R>) -> Self {
102        let rquat = pose.clone().rotation;
103        let translation = pose.clone().translation.vector;
104        let q = rquat.inverse().to_rotation_matrix();
105        let camcenter = -(q * translation);
106        let cc = Point3 { coords: camcenter };
107        Self::from_rotation_and_camcenter(rquat, cc)
108    }
109
110    /// Create a new instance from a camera center, a lookat vector, and an up vector.
111    pub fn from_view(camcenter: &Vector3<R>, lookat: &Vector3<R>, up: &Unit<Vector3<R>>) -> Self {
112        let dir = lookat - camcenter;
113        let dir_unit = nalgebra::Unit::new_normalize(dir);
114        let q = UnitQuaternion::look_at_lh(dir_unit.as_ref(), up.as_ref());
115        let pi: R = convert(std::f64::consts::PI);
116
117        let q2 = UnitQuaternion::from_axis_angle(&dir_unit, pi);
118        let q3 = q * q2;
119
120        Self::from_rotation_and_camcenter(
121            q3,
122            Point3 {
123                coords: camcenter.clone(),
124            },
125        )
126    }
127
128    /// Return the camera center
129    #[inline]
130    pub fn camcenter(&self) -> &Point3<R> {
131        &self.camcenter
132    }
133
134    /// Return the camera pose
135    #[inline]
136    pub fn pose(&self) -> &Isometry3<R> {
137        &self.cache.pose
138    }
139
140    /// Return the pose as a 3x4 matrix
141    #[inline]
142    pub fn matrix(&self) -> &SMatrix<R, 3, 4> {
143        &self.cache.qt
144    }
145
146    /// Return the rotation part of the pose
147    ///
148    /// To obtain the rotation as a quaternion, use [Self::pose] to obtain an
149    /// [Isometry3] and access the [field@Isometry3::rotation] field.
150    #[inline]
151    pub fn rotation(&self) -> &Rotation3<R> {
152        &self.cache.q
153    }
154
155    /// Return the translation part of the pose
156    #[inline]
157    pub fn translation(&self) -> &Point3<R> {
158        &self.cache.translation
159    }
160
161    /// Return a unit vector aligned along our look (+Z) direction.
162    pub fn forward(&self) -> Unit<Vector3<R>> {
163        let pt_cam = Point3::new(R::zero(), R::zero(), R::one());
164        self.lookdir(&pt_cam)
165    }
166
167    /// Return a unit vector aligned along our up (-Y) direction.
168    pub fn up(&self) -> Unit<Vector3<R>> {
169        let pt_cam = Point3::new(R::zero(), -R::one(), R::zero());
170        self.lookdir(&pt_cam)
171    }
172
173    /// Return a unit vector aligned along our right (+X) direction.
174    pub fn right(&self) -> Unit<Vector3<R>> {
175        let pt_cam = Point3::new(R::one(), R::zero(), R::zero());
176        self.lookdir(&pt_cam)
177    }
178
179    /// Return a world coords unit vector aligned along the given direction
180    ///
181    /// `pt_cam` is specified in camera coords.
182    fn lookdir(&self, pt_cam: &Point3<R>) -> Unit<Vector3<R>> {
183        let cc = self.camcenter();
184        let pt = self.cache.pose_inv.transform_point(pt_cam) - cc;
185        nalgebra::Unit::new_normalize(pt)
186    }
187
188    /// Convert points in camera coordinates to world coordinates.
189    pub fn camera_to_world<NPTS, InStorage>(
190        &self,
191        cam_coords: &Points<CameraFrame, R, NPTS, InStorage>,
192    ) -> Points<WorldFrame, R, NPTS, Owned<R, NPTS, U3>>
193    where
194        NPTS: Dim,
195        InStorage: Storage<R, NPTS, U3>,
196        DefaultAllocator: Allocator<NPTS, U3>,
197    {
198        let mut world = Points::new(OMatrix::zeros_generic(
199            NPTS::from_usize(cam_coords.data.nrows()),
200            U3::from_usize(3),
201        ));
202
203        // Potential optimization: remove for loops
204        let in_mult = &cam_coords.data;
205        let out_mult = &mut world.data;
206
207        for i in 0..in_mult.nrows() {
208            let tmp = self.cache.pose_inv.transform_point(&Point3::new(
209                in_mult[(i, 0)].clone(),
210                in_mult[(i, 1)].clone(),
211                in_mult[(i, 2)].clone(),
212            ));
213            for j in 0..3 {
214                out_mult[(i, j)] = tmp[j].clone();
215            }
216        }
217        world
218    }
219
220    /// Convert rays in camera coordinates to world coordinates.
221    #[inline]
222    pub fn ray_camera_to_world<BType, NPTS, StorageCamera>(
223        &self,
224        camera: &RayBundle<CameraFrame, BType, R, NPTS, StorageCamera>,
225    ) -> RayBundle<WorldFrame, BType, R, NPTS, Owned<R, NPTS, U3>>
226    where
227        BType: Bundle<R>,
228        NPTS: Dim,
229        StorageCamera: Storage<R, NPTS, U3>,
230        DefaultAllocator: Allocator<NPTS, U3>,
231    {
232        camera.to_pose(self.cache.pose_inv.clone())
233    }
234
235    /// Convert points in world coordinates to camera coordinates.
236    pub fn world_to_camera<NPTS, InStorage>(
237        &self,
238        world: &Points<WorldFrame, R, NPTS, InStorage>,
239    ) -> Points<CameraFrame, R, NPTS, Owned<R, NPTS, U3>>
240    where
241        NPTS: Dim,
242        InStorage: Storage<R, NPTS, U3>,
243        DefaultAllocator: Allocator<NPTS, U3>,
244    {
245        let mut cam_coords = Points::new(OMatrix::zeros_generic(
246            NPTS::from_usize(world.data.nrows()),
247            U3::from_usize(3),
248        ));
249
250        // Potential optimization: remove for loops
251        let in_mult = &world.data;
252        let out_mult = &mut cam_coords.data;
253
254        for i in 0..in_mult.nrows() {
255            let tmp = self.cache.pose.transform_point(&Point3::new(
256                in_mult[(i, 0)].clone(),
257                in_mult[(i, 1)].clone(),
258                in_mult[(i, 2)].clone(),
259            ));
260            for j in 0..3 {
261                out_mult[(i, j)] = tmp[j].clone();
262            }
263        }
264        cam_coords
265    }
266}
267
268// So far, I could not figure out how to get serde derive to construct a cache
269// only after rquat and camcenter are created. Instead serde derive wants the
270// struct to be deserialized to implement the Default trait.
271#[cfg(feature = "serde-serialize")]
272impl<'de, R: RealField + serde::Deserialize<'de>> serde::Deserialize<'de>
273    for ExtrinsicParameters<R>
274{
275    fn deserialize<D>(deserializer: D) -> std::result::Result<Self, D::Error>
276    where
277        D: serde::Deserializer<'de>,
278    {
279        use serde::de;
280        use std::fmt;
281
282        #[derive(Deserialize)]
283        #[serde(field_identifier, rename_all = "lowercase")]
284        enum Field {
285            RQuat,
286            CamCenter,
287        }
288
289        struct ExtrinsicParametersVisitor<'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 ExtrinsicParametersVisitor<'de, R2>
295        {
296            type Value = ExtrinsicParameters<R2>;
297
298            fn expecting(&self, formatter: &mut fmt::Formatter<'_>) -> fmt::Result {
299                formatter.write_str("struct ExtrinsicParameters")
300            }
301
302            fn visit_seq<V>(
303                self,
304                mut seq: V,
305            ) -> std::result::Result<ExtrinsicParameters<R2>, V::Error>
306            where
307                V: serde::de::SeqAccess<'de>,
308            {
309                let rquat = seq
310                    .next_element()?
311                    .ok_or_else(|| de::Error::invalid_length(0, &self))?;
312                let camcenter = seq
313                    .next_element()?
314                    .ok_or_else(|| de::Error::invalid_length(1, &self))?;
315                Ok(ExtrinsicParameters::from_rotation_and_camcenter(
316                    rquat, camcenter,
317                ))
318            }
319
320            fn visit_map<V>(
321                self,
322                mut map: V,
323            ) -> std::result::Result<ExtrinsicParameters<R2>, V::Error>
324            where
325                V: serde::de::MapAccess<'de>,
326            {
327                let mut rquat = None;
328                let mut camcenter = None;
329                while let Some(key) = map.next_key()? {
330                    match key {
331                        Field::RQuat => {
332                            if rquat.is_some() {
333                                return Err(de::Error::duplicate_field("rquat"));
334                            }
335                            rquat = Some(map.next_value()?);
336                        }
337                        Field::CamCenter => {
338                            if camcenter.is_some() {
339                                return Err(de::Error::duplicate_field("camcenter"));
340                            }
341                            camcenter = Some(map.next_value()?);
342                        }
343                    }
344                }
345                let rquat = rquat.ok_or_else(|| de::Error::missing_field("rquat"))?;
346                let camcenter = camcenter.ok_or_else(|| de::Error::missing_field("camcenter"))?;
347                Ok(ExtrinsicParameters::from_rotation_and_camcenter(
348                    rquat, camcenter,
349                ))
350            }
351        }
352
353        const FIELDS: &[&str] = &["rquat", "camcenter"];
354        deserializer.deserialize_struct(
355            "ExtrinsicParameters",
356            FIELDS,
357            ExtrinsicParametersVisitor(std::marker::PhantomData),
358        )
359    }
360}
361
362#[cfg(feature = "serde-serialize")]
363fn _test_extrinsics_is_serialize() {
364    // Compile-time test to ensure ExtrinsicParameters implements Serialize trait.
365    fn implements<T: serde::Serialize>() {}
366    implements::<ExtrinsicParameters<f64>>();
367}
368
369#[cfg(feature = "serde-serialize")]
370fn _test_extrinsics_is_deserialize() {
371    // Compile-time test to ensure ExtrinsicParameters implements Deserialize trait.
372    fn implements<'de, T: serde::Deserialize<'de>>() {}
373    implements::<ExtrinsicParameters<f64>>();
374}
375
376#[cfg(test)]
377mod tests {
378    use super::*;
379    use nalgebra::{convert as c, UnitVector3};
380
381    #[test]
382    fn to_from_pose_f64() {
383        to_from_pose_generic::<f64>(1e-10)
384    }
385
386    #[test]
387    fn to_from_pose_f32() {
388        to_from_pose_generic::<f32>(1e-5)
389    }
390
391    fn to_from_pose_generic<R: RealField>(epsilon: R) {
392        let zero: R = convert(0.0);
393        let one: R = convert(1.0);
394
395        let e1 = ExtrinsicParameters::<R>::from_view(
396            &Vector3::new(c(1.2), c(3.4), c(5.6)), // camcenter
397            &Vector3::new(c(2.2), c(3.4), c(5.6)), // lookat
398            &nalgebra::Unit::new_normalize(Vector3::new(zero.clone(), zero.clone(), one.clone())), // up
399        );
400        let pose1 = e1.pose();
401        let e2 = ExtrinsicParameters::<R>::from_pose(pose1);
402
403        approx::assert_abs_diff_eq!(e1.rotation(), e2.rotation(), epsilon = epsilon.clone());
404        approx::assert_abs_diff_eq!(e1.camcenter(), e2.camcenter(), epsilon = epsilon.clone());
405    }
406
407    #[test]
408    fn roundtrip_f64() {
409        roundtrip_generic::<f64>(1e-10)
410    }
411
412    #[test]
413    fn roundtrip_f32() {
414        roundtrip_generic::<f32>(1e-5)
415    }
416
417    fn roundtrip_generic<R: RealField>(epsilon: R) {
418        let zero: R = convert(0.0);
419        let one: R = convert(1.0);
420
421        let e1 = ExtrinsicParameters::<R>::from_view(
422            &Vector3::new(c(1.2), c(3.4), c(5.6)), // camcenter
423            &Vector3::new(c(2.2), c(3.4), c(5.6)), // lookat
424            &nalgebra::Unit::new_normalize(Vector3::new(zero.clone(), zero.clone(), one.clone())), // up
425        );
426
427        #[rustfmt::skip]
428        let cam_coords = Points {
429            coords: std::marker::PhantomData,
430            data: SMatrix::<R, 4, 3>::new(
431                zero.clone(), zero.clone(), zero.clone(), // at camera center
432                zero.clone(), zero.clone(), one.clone(), // one unit in +Z - exactly in camera direction
433                one.clone(), zero.clone(), zero.clone(), // one unit in +X - right of camera axis
434                zero.clone(), one, zero, // one unit in +Y - down from camera axis
435            ),
436        };
437
438        #[rustfmt::skip]
439        let world_expected = SMatrix::<R, 4, 3>::new(
440            c(1.2), c(3.4), c(5.6),
441            c(2.2), c(3.4), c(5.6),
442            c(1.2), c(2.4), c(5.6),
443            c(1.2), c(3.4), c(4.6),
444        );
445
446        let world_actual = e1.camera_to_world(&cam_coords);
447        approx::assert_abs_diff_eq!(world_expected, world_actual.data, epsilon = epsilon.clone());
448
449        // test roundtrip
450        let camera_actual = e1.world_to_camera(&world_actual);
451        approx::assert_abs_diff_eq!(cam_coords.data, camera_actual.data, epsilon = epsilon);
452    }
453
454    #[test]
455    #[cfg(feature = "serde-serialize")]
456    fn test_serde() {
457        let expected = ExtrinsicParameters::<f64>::from_view(
458            &Vector3::new(1.2, 3.4, 5.6),                                // camcenter
459            &Vector3::new(2.2, 3.4, 5.6),                                // lookat
460            &nalgebra::Unit::new_normalize(Vector3::new(0.0, 0.0, 1.0)), // up
461        );
462        let buf = serde_json::to_string(&expected).unwrap();
463        let actual: crate::ExtrinsicParameters<f64> = serde_json::from_str(&buf).unwrap();
464        assert!(expected == actual);
465    }
466
467    #[test]
468    fn test_from_view() {
469        // These values had previously cause problems.
470        let camcenter = Vector3::new(10.0, 0.0, 10.0);
471        let lookat = Vector3::new(0.0, 0.0, 0.0);
472        let up = Unit::new_normalize(Vector3::new(0.0, 0.0, 1.0));
473        let e = ExtrinsicParameters::from_view(&camcenter, &lookat, &up);
474        let axis = UnitVector3::new_normalize(Vector3::new(
475            0.6785983445458471,
476            0.6785983445458469,
477            -0.28108463771482023,
478        ));
479        let expected_rot = UnitQuaternion::from_axis_angle(&axis, 2.5935642459694805);
480        approx::assert_abs_diff_eq!(camcenter, e.camcenter().coords);
481        approx::assert_abs_diff_eq!(expected_rot, e.pose().rotation);
482    }
483}
cam_geom/extrinsics.rs

cam_geom/
extrinsics.rs
