sophus_sensor/projections/
perspective.rs

1use core::borrow::Borrow;
2
3use sophus_autodiff::{
4    linalg::{
5        IsMatrix,
6        IsScalar,
7        IsVector,
8    },
9    prelude::IsSingleScalar,
10};
11
12use crate::traits::IsProjection;
13
14/// Perspective camera projection - using z=1 plane
15///
16/// Projects a 3D point in the camera frame to a 2D point in the z=1 plane
17#[derive(Debug, Clone, Copy)]
18pub struct PerspectiveProjectionImpl<
19    S: IsScalar<BATCH, DM, DN>,
20    const BATCH: usize,
21    const DM: usize,
22    const DN: usize,
23> {
24    phantom: core::marker::PhantomData<S>,
25}
26
27impl<S: IsScalar<BATCH, DM, DN>, const BATCH: usize, const DM: usize, const DN: usize>
28    IsProjection<S, BATCH, DM, DN> for PerspectiveProjectionImpl<S, BATCH, DM, DN>
29{
30    fn proj<P>(point_in_camera: P) -> S::Vector<2>
31    where
32        P: Borrow<S::Vector<3>>,
33    {
34        let point_in_camera = point_in_camera.borrow();
35        S::Vector::<2>::from_array([
36            point_in_camera.elem(0) / point_in_camera.elem(2),
37            point_in_camera.elem(1) / point_in_camera.elem(2),
38        ])
39    }
40
41    fn unproj<P>(point_in_camera: P, extension: S) -> S::Vector<3>
42    where
43        P: Borrow<S::Vector<2>>,
44    {
45        let point_in_camera = point_in_camera.borrow();
46
47        S::Vector::<3>::from_array([
48            point_in_camera.elem(0) * extension,
49            point_in_camera.elem(1) * extension,
50            extension,
51        ])
52    }
53
54    fn dx_proj_x<P>(point_in_camera: P) -> S::Matrix<2, 3>
55    where
56        P: Borrow<S::Vector<3>>,
57    {
58        let point_in_camera = point_in_camera.borrow();
59
60        S::Matrix::<2, 3>::from_array2([
61            [
62                S::ones() / point_in_camera.elem(2),
63                S::zeros(),
64                -point_in_camera.elem(0) / (point_in_camera.elem(2) * point_in_camera.elem(2)),
65            ],
66            [
67                S::zeros(),
68                S::ones() / point_in_camera.elem(2),
69                -point_in_camera.elem(1) / (point_in_camera.elem(2) * point_in_camera.elem(2)),
70            ],
71        ])
72    }
73}
74
75/// Perspective projection for single scalar
76pub fn perspect_proj<
77    S: IsSingleScalar<DM, DN> + 'static + Send + Sync,
78    const DM: usize,
79    const DN: usize,
80>(
81    point_in_camera: &S::Vector<3>,
82) -> S::Vector<2> {
83    PerspectiveProjectionImpl::<S, 1, DM, DN>::proj(point_in_camera)
84}
sophus_sensor/projections/perspective.rs

sophus_sensor/projections/
perspective.rs
