cv_core/
pose.rs

1use crate::{Bearing, CameraPoint, FeatureWorldMatch, Projective, Skew3, WorldPoint};
2use derive_more::{AsMut, AsRef, From, Into};
3use nalgebra::{
4    IsometryMatrix3, Matrix4, Matrix4x6, Matrix6x4, Rotation3, Vector3, Vector4, Vector6,
5};
6use sample_consensus::Model;
7
8/// This trait is implemented by all the different poses in this library:
9///
10/// * [`CameraToWorld`] - Transforms [`CameraPoint`] into [`WorldPoint`]
11/// * [`WorldToCamera`] - Transforms [`WorldPoint`] into [`CameraPoint`]
12/// * [`CameraToCamera`] - Transforms [`CameraPoint`] from one camera into [`CameraPoint`] for another camera
13pub trait Pose: From<IsometryMatrix3<f64>> + Clone + Copy {
14    type InputPoint: Projective;
15    type OutputPoint: Projective;
16    type Inverse: Pose;
17
18    /// Retrieve the isometry.
19    fn isometry(self) -> IsometryMatrix3<f64>;
20
21    /// Creates a pose with no change in position or orientation.
22    fn identity() -> Self {
23        IsometryMatrix3::identity().into()
24    }
25
26    /// Takes the inverse of the pose.
27    fn inverse(self) -> Self::Inverse {
28        self.isometry().inverse().into()
29    }
30
31    /// Applies a scale factor to the pose (scales the translation component)
32    fn scale(self, scale: f64) -> Self {
33        let mut isometry = self.isometry();
34        isometry.translation.vector *= scale;
35        isometry.into()
36    }
37
38    /// Create the pose from rotation and translation.
39    fn from_parts(translation: Vector3<f64>, rotation: Rotation3<f64>) -> Self {
40        IsometryMatrix3::from_parts(translation.into(), rotation).into()
41    }
42
43    /// Retrieve the homogeneous matrix.
44    fn homogeneous(self) -> Matrix4<f64> {
45        self.isometry().to_homogeneous()
46    }
47
48    /// Retrieve the se(3) representation of the pose.
49    fn se3(self) -> Vector6<f64> {
50        let isometry = self.isometry();
51        let t = isometry.translation.vector;
52        let r: Skew3 = isometry.rotation.into();
53        Vector6::new(t.x, t.y, t.z, r.x, r.y, r.z)
54    }
55
56    /// Set the se(3) representation of the pose.
57    fn from_se3(se3: Vector6<f64>) -> Self {
58        let translation = se3.xyz();
59        let rotation = Skew3(Vector3::new(se3[3], se3[4], se3[5])).into();
60        Self::from_parts(translation, rotation)
61    }
62
63    /// Transform the given point to an output point, while also retrieving both Jacobians.
64    ///
65    /// The following things are returned in this order:
66    ///
67    /// * The output point of the transformation
68    /// * The Jacobian of the output in respect to the input point
69    /// * The Jacobian of the output in respect to the pose in se(3) (with translation components before so(3) components)
70    fn transform_jacobians(
71        self,
72        input: Self::InputPoint,
73    ) -> (Self::OutputPoint, Matrix4<f64>, Matrix4x6<f64>) {
74        let (rotated, output) = pose_rotated_output(self, input);
75        let jacobian_input = pose_jacobian_input(self);
76        let jacobian_self = pose_jacobian_self(self, rotated, output);
77        (output.into(), jacobian_input, jacobian_self)
78    }
79
80    /// Transform the given point to an output point, while also retrieving the input Jacobian.
81    ///
82    /// The following things are returned in this order:
83    ///
84    /// * The output point of the transformation
85    /// * The Jacobian of the output in respect to the input point
86    fn transform_jacobian_input(
87        self,
88        input: Self::InputPoint,
89    ) -> (Self::OutputPoint, Matrix4<f64>) {
90        let output = pose_output(self, input);
91        let jacobian_input = pose_jacobian_input(self);
92        (output.into(), jacobian_input)
93    }
94
95    /// Transform the given point to an output point, while also retrieving the transform Jacobian.
96    ///
97    /// The following things are returned in this order:
98    ///
99    /// * The output point of the transformation
100    /// * The Jacobian of the output in respect to the pose in se(3) (with translation components before so(3) components)
101    fn transform_jacobian_self(
102        self,
103        input: Self::InputPoint,
104    ) -> (Self::OutputPoint, Matrix4x6<f64>) {
105        let (rotated, output) = pose_rotated_output(self, input);
106        let jacobian_self = pose_jacobian_self(self, rotated, output);
107        (output.into(), jacobian_self)
108    }
109
110    /// Transform the given point to an output point, while also retrieving the transform Jacobian.
111    ///
112    /// The following things are returned in this order:
113    ///
114    /// * The output point of the transformation
115    /// * The Jacobian of the output in respect to the pose in se(3) (with translation components before so(3) components)
116    fn transform(self, input: Self::InputPoint) -> Self::OutputPoint {
117        pose_output(self, input).into()
118    }
119}
120
121/// Retrieves the output coordinate from the pose and input.
122fn pose_output<P: Pose>(pose: P, input: P::InputPoint) -> Vector4<f64>
123where
124    P::InputPoint: From<Vector4<f64>>,
125{
126    pose.isometry().to_homogeneous() * input.homogeneous()
127}
128
129/// Retrieves the rotated and output coordinates (in that order) from the pose and input.
130fn pose_rotated_output<P: Pose>(pose: P, input: P::InputPoint) -> (Vector4<f64>, Vector4<f64>)
131where
132    P::InputPoint: From<Vector4<f64>>,
133{
134    let rotated = pose.isometry().rotation.to_homogeneous() * input.homogeneous();
135    let output = pose.isometry().to_homogeneous() * input.homogeneous();
136    (rotated, output)
137}
138
139/// Retrieves the Jacobian relating the output to the input.
140fn pose_jacobian_input<P: Pose>(pose: P) -> Matrix4<f64> {
141    pose.isometry().to_homogeneous()
142}
143
144/// Retrieves the Jacobian relating the output to the pose in se(3)
145fn pose_jacobian_self<P: Pose>(
146    pose: P,
147    rotated: Vector4<f64>,
148    output: Vector4<f64>,
149) -> Matrix4x6<f64> {
150    // The translation homogeneous matrix
151    //
152    // This is also the jacobian of the output in respect to the rotation output.
153    let translation = pose.isometry().translation.to_homogeneous();
154
155    // dP/dT (Jacobian of output point in respect to translation component)
156    let dp_dt = Matrix4::<f64>::identity() * output.w;
157
158    // dP/ds (Jacobian of output point in respect to skew component)
159    let dp_ds = translation * Skew3::jacobian_self(rotated.xyz()).to_homogeneous();
160
161    // dP/dT,s (Jacobian of 3d camera point in respect to translation and skew)
162    Matrix6x4::<f64>::from_rows(&[
163        dp_dt.row(0),
164        dp_dt.row(1),
165        dp_dt.row(2),
166        dp_ds.row(0),
167        dp_ds.row(1),
168        dp_ds.row(2),
169    ])
170    .transpose()
171}
172
173/// This contains a world pose, which is a pose of the world relative to the camera.
174/// This maps [`WorldPoint`] into [`CameraPoint`], changing an absolute position into
175/// a vector relative to the camera.
176#[derive(Debug, Clone, Copy, PartialEq, AsMut, AsRef, From, Into)]
177pub struct WorldToCamera(pub IsometryMatrix3<f64>);
178
179impl Pose for WorldToCamera {
180    type InputPoint = WorldPoint;
181    type OutputPoint = CameraPoint;
182    type Inverse = CameraToWorld;
183
184    fn isometry(self) -> IsometryMatrix3<f64> {
185        self.into()
186    }
187}
188
189impl<P> Model<FeatureWorldMatch<P>> for WorldToCamera
190where
191    P: Bearing,
192{
193    fn residual(&self, data: &FeatureWorldMatch<P>) -> f64 {
194        let WorldToCamera(iso) = *self;
195        let FeatureWorldMatch(feature, world) = data;
196
197        let new_bearing = (iso.to_homogeneous() * world.0).xyz().normalize();
198        let bearing_vector = feature.bearing();
199        1.0 - bearing_vector.dot(&new_bearing)
200    }
201}
202
203/// This contains a camera pose, which is a pose of the camera relative to the world.
204/// This transforms camera points (with depth as `z`) into world coordinates.
205/// This also tells you where the camera is located and oriented in the world.
206#[derive(Debug, Clone, Copy, PartialEq, AsMut, AsRef, From, Into)]
207pub struct CameraToWorld(pub IsometryMatrix3<f64>);
208
209impl Pose for CameraToWorld {
210    type InputPoint = CameraPoint;
211    type OutputPoint = WorldPoint;
212    type Inverse = WorldToCamera;
213
214    fn isometry(self) -> IsometryMatrix3<f64> {
215        self.into()
216    }
217}
218
219/// This contains a relative pose, which is a pose that transforms the [`CameraPoint`]
220/// of one image into the corresponding [`CameraPoint`] of another image. This transforms
221/// the point from the camera space of camera `A` to camera `B`.
222///
223/// Camera space for a given camera is defined as thus:
224///
225/// * Origin is the optical center
226/// * Positive z axis is forwards
227/// * Positive y axis is up
228/// * Positive x axis is right
229///
230/// Note that this is a left-handed coordinate space.
231#[derive(Debug, Clone, Copy, PartialEq, AsMut, AsRef, From, Into)]
232pub struct CameraToCamera(pub IsometryMatrix3<f64>);
233
234impl Pose for CameraToCamera {
235    type InputPoint = CameraPoint;
236    type OutputPoint = CameraPoint;
237    type Inverse = CameraToCamera;
238
239    fn isometry(self) -> IsometryMatrix3<f64> {
240        self.into()
241    }
242}
cv_core/pose.rs

cv_core/
pose.rs
