cv_core/
triangulation.rs

1use crate::{Bearing, CameraPoint, CameraToCamera, Pose, WorldPoint, WorldToCamera};
2
3/// This trait is for algorithms which allow you to triangulate a point from two or more observances.
4/// Each observance is a [`WorldToCamera`] and a [`Bearing`].
5pub trait TriangulatorObservances {
6    fn triangulate_observances<B: Bearing>(
7        &self,
8        pairs: impl IntoIterator<Item = (WorldToCamera, B)>,
9    ) -> Option<WorldPoint>;
10}
11
12/// This trait allows you to take one relative pose from camera `A` to camera `B` and two bearings `a` and `b` from
13/// their respective cameras to triangulate a point from the perspective of camera `A`.
14pub trait TriangulatorRelative {
15    fn triangulate_relative<A: Bearing, B: Bearing>(
16        &self,
17        relative_pose: CameraToCamera,
18        a: A,
19        b: B,
20    ) -> Option<CameraPoint>;
21}
22
23impl<T> TriangulatorRelative for T
24where
25    T: TriangulatorObservances,
26{
27    fn triangulate_relative<A: Bearing, B: Bearing>(
28        &self,
29        CameraToCamera(pose): CameraToCamera,
30        a: A,
31        b: B,
32    ) -> Option<CameraPoint> {
33        use core::iter::once;
34
35        // We use the first camera as the "world".
36        // The first pose maps the first camera to itself (the world).
37        // The second pose maps the first camera (the world) to the second camera (the camera).
38        // This is how we convert the `CameraToCamera` into a `WorldToCamera`.
39        self.triangulate_observances(
40            once((WorldToCamera::identity(), a.bearing()))
41                .chain(once((WorldToCamera(pose), b.bearing()))),
42        )
43        .map(|p| CameraPoint(p.0))
44    }
45}
cv_core/triangulation.rs

cv_core/
triangulation.rs
