visual_odometry_rs/core/
camera.rs

1// This Source Code Form is subject to the terms of the Mozilla Public
2// License, v. 2.0. If a copy of the MPL was not distributed with this
3// file, You can obtain one at http://mozilla.org/MPL/2.0/.
4
5//! Helper types and functions to manipulate camera poses and projections.
6
7use nalgebra::Affine2;
8
9use crate::core::multires;
10use crate::misc::type_aliases::{Float, Iso3, Mat3, Point2, Point3, Vec3};
11
12/// A camera has intrinsic and extrinsic parameters.
13/// Warning: extrinsics here is the pose of the camera,
14/// not directly the projection matrix parameters.
15///
16/// As it stands, it is currently limited to the pinhole camera model.
17#[derive(PartialEq, Debug, Clone)]
18pub struct Camera {
19    /// Intrinsic parameters of the camera.
20    pub intrinsics: Intrinsics,
21    /// Extrinsic parameters of the camera pose.
22    pub extrinsics: Extrinsics,
23}
24
25impl Camera {
26    /// Initialize a camera from intrinsic and extrinsic parameters.
27    pub fn new(intrinsics: Intrinsics, extrinsics: Extrinsics) -> Self {
28        Self {
29            intrinsics,
30            extrinsics,
31        }
32    }
33
34    /// Project a 3D point into its corresponding pixel in the image generated by the camera.
35    /// Result is still in homogeneous coordinates (thus `Vec3`).
36    pub fn project(&self, point: Point3) -> Vec3 {
37        self.intrinsics
38            .project(extrinsics::project(&self.extrinsics, point))
39    }
40
41    /// From a 2D pixel position and a depth info,
42    /// back project this point into the 3D world.
43    pub fn back_project(&self, point: Point2, depth: Float) -> Point3 {
44        extrinsics::back_project(&self.extrinsics, self.intrinsics.back_project(point, depth))
45    }
46
47    /// Generate a multi-resolution camera.
48    /// Extrinsics are left intact, but intrinsics are scaled at each level.
49    pub fn multi_res(self, n: usize) -> Vec<Self> {
50        multires::limited_sequence(n, self, |cam| Some(cam.half_res()))
51    }
52
53    /// Generate a camera corresponding to an image with half resolution.
54    /// Extrinsics are left intact, but intrinsics are scaled.
55    pub fn half_res(&self) -> Self {
56        Self::new(self.intrinsics.half_res(), self.extrinsics)
57    }
58}
59
60// EXTRINSICS ##############################################
61
62/// Extrinsic parameters are represented by a rigid body motion (or direct isometry).
63pub type Extrinsics = Iso3;
64
65/// Module regrouping functions operating on extrinsics.
66pub mod extrinsics {
67    use super::*;
68
69    /// Project a 3D point from world coordinates to camera coordinates.
70    pub fn project(pose: &Extrinsics, point: Point3) -> Point3 {
71        pose.rotation.inverse() * (pose.translation.inverse() * point)
72    }
73
74    /// Back project a 3D point from camera coordinates to world coordinates.
75    pub fn back_project(pose: &Extrinsics, point: Point3) -> Point3 {
76        pose * point
77    }
78}
79
80// INTRINSICS ##############################################
81
82/// Intrinsic parameters of a pinhole camera model.
83#[derive(PartialEq, Debug, Clone)]
84pub struct Intrinsics {
85    /// Principal point (in the optical center axis) of the camera.
86    pub principal_point: (Float, Float),
87    /// Focal length in pixels along both axes.
88    pub focal: (Float, Float),
89    /// Skew of the camera, usually 0.0.
90    pub skew: Float,
91}
92
93impl Intrinsics {
94    /// Equivalent matrix representation of intrinsic parameters.
95    #[rustfmt::skip]
96    pub fn matrix(&self) -> Affine2<Float> {
97        Affine2::from_matrix_unchecked(Mat3::new(
98            self.focal.0, self.skew,    self.principal_point.0,
99            0.0,          self.focal.1, self.principal_point.1,
100            0.0,          0.0,          1.0,
101        ))
102    }
103
104    /// Generate a multi-resolution vector of intrinsic parameters.
105    /// Each level corresponds to a camera with half resolution.
106    pub fn multi_res(self, n: usize) -> Vec<Self> {
107        multires::limited_sequence(n, self, |intrinsics| Some(intrinsics.half_res()))
108    }
109
110    /// Compute intrinsic parameters of a camera with half resolution.
111    ///
112    /// Since the (0,0) coordinates correspond the center of the first pixel,
113    /// and not its top left corner, a shift of 0.5 is performed
114    /// for the principal point before and after the resolution scaling.
115    pub fn half_res(&self) -> Self {
116        let (cx, cy) = self.principal_point;
117        let (fx, fy) = self.focal;
118        Self {
119            principal_point: ((cx + 0.5) / 2.0 - 0.5, (cy + 0.5) / 2.0 - 0.5),
120            focal: (0.5 * fx, 0.5 * fy),
121            skew: self.skew,
122        }
123    }
124
125    /// Project a 3D point in camera coordinates into a 2D homogeneous image point.
126    pub fn project(&self, point: Point3) -> Vec3 {
127        Vec3::new(
128            self.focal.0 * point[0] + self.skew * point[1] + self.principal_point.0 * point[2],
129            self.focal.1 * point[1] + self.principal_point.1 * point[2],
130            point[2],
131        )
132    }
133
134    /// Back project a pixel with depth info into a 3D point in camera coordinates.
135    pub fn back_project(&self, point: Point2, depth: Float) -> Point3 {
136        let z = depth;
137        let y = (point[1] - self.principal_point.1) * z / self.focal.1;
138        let x = ((point[0] - self.principal_point.0) * z - self.skew * y) / self.focal.0;
139        Point3::new(x, y, z)
140    }
141}
visual_odometry_rs/core/camera.rs

visual_odometry_rs/core/
camera.rs
