1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143

//! This module contains functions to perform various geometric algorithms.
//!
//! ## Triangulation of a point with a given camera transformation
//!
//! In this problem we have a [`RelativeCameraPose`] and two [`NormalizedKeyPoint`].
//! We want to find the point of intersection from the two cameras in camera A's space.
//!
//! - `p` the point we are trying to triangulate
//! - `a` the normalized keypoint on camera A
//! - `b` the normalized keypoint on camera B
//! - `O` the optical center of a camera
//! - `@` the virtual image plane
//!
//! ```no_build,no_run
//!                        @
//!                        @
//!               p--------b--------O
//!              /         @
//!             /          @
//!            /           @
//!           /            @
//!   @@@@@@@a@@@@@
//!         /
//!        /
//!       /
//!      O
//! ```
//!
//! //! Solutions to this problem:
//!
//! * [`triangulate_dlt`]
//!
//! ## Translation along a bearing given one prior depth
//!
//! This problem consumes a direction to translate along, a `from` [`CameraPoint`],
//! and a `to` [`NormalizedKeyPoint`] coordinate.
//!
//! - `t` the `translation` bearing vector
//! - `a` the `from` point
//! - `b` the `to` epipolar point
//! - `O` the optical center
//! - `@` the virtual image plane
//!
//! ```no_build,no_run
//!      t<---a
//!          /
//!         /
//! @@@b@@@/@@@@@
//!    |  /
//!    | /
//!    |/
//!    O
//! ```
//!
//! The `from` coordinate is the relative 3d coordinate in camera space before translation.
//!
//! The `to` coordinate is just a normalized keypoint that we wish to find the optimal translation
//! to reproject as close as possible to.
//!
//! The `translation` is a vector which will be scaled (multiplied) by the return value to
//! get the actual 3d translation to move from `from` to `to` in 3d space.
//!
//! Solutions to this problem:
//!
//! * [`triangulate_bearing_intersection`]
//! * [`triangulate_bearing_reproject`]
//!
//! It is recommended to use [`triangulate_bearing_reproject`], as it is incredibly cheap to compute.

use crate::{CameraPoint, NormalizedKeyPoint, UnscaledRelativeCameraPose};
use nalgebra::{Matrix4, Point3, RowVector4, Vector3};

/// This solves the point triangulation problem using
/// Algorithm 12 from "Multiple View Geometry in Computer Vision".
///
/// It is considered the "optimal" triangulation and is best when dealing with noise.
pub fn make_one_pose_dlt_triangulator(
    epsilon: f64,
    max_iterations: usize,
) -> impl Fn(UnscaledRelativeCameraPose, NormalizedKeyPoint, NormalizedKeyPoint) -> Option<Point3<f64>>
{
    move |pose, a, b| {
        let pose = pose.to_homogeneous();
        let a = a.bearing_unnormalized();
        let b = b.bearing_unnormalized();
        let mut design = Matrix4::zeros();
        design
            .row_mut(0)
            .copy_from(&RowVector4::new(-a.z, 0.0, a.x, 0.0));
        design
            .row_mut(1)
            .copy_from(&RowVector4::new(0.0, -a.z, a.y, 0.0));
        design
            .row_mut(2)
            .copy_from(&(b.x * pose.row(2) - b.z * pose.row(0)));
        design
            .row_mut(3)
            .copy_from(&(b.y * pose.row(2) - b.z * pose.row(1)));

        let svd = design.try_svd(false, true, epsilon, max_iterations)?;

        // Extract the null-space vector from V* corresponding to the smallest
        // singular value and then normalize it back from heterogeneous coordinates.
        svd.singular_values
            .iter()
            .enumerate()
            .min_by_key(|&(_, &n)| float_ord::FloatOrd(n))
            .map(|(ix, _)| svd.v_t.unwrap().row(ix).transpose().into_owned())
            .map(|h| (h.xyz() / h.w).into())
    }
}

/// This solves the translation along a bearing triangulation assuming that there is
/// a perfect intersection.
pub fn triangulate_bearing_intersection(
    bearing: Vector3<f64>,
    from: CameraPoint,
    to: NormalizedKeyPoint,
) -> Option<f64> {
    let from = from.0.coords;
    let to = to.bearing_unnormalized();

    let hv = to.cross(&-from);
    let h = hv.norm();
    let kv = to.cross(&bearing);
    let k = kv.norm();

    let l = h / k;

    Some(if hv.dot(&kv) > 0.0 { l } else { -l })
}

/// This solves the translation along a bearing triangulation by minimizing the reprojection error.
pub fn triangulate_bearing_reproject(
    bearing: Vector3<f64>,
    from: CameraPoint,
    to: NormalizedKeyPoint,
) -> Option<f64> {
    let a = to;
    let b = from;
    let t = bearing;
    Some((a.y * b.x - a.x * b.y) / (a.x * t.y - a.y * t.x))
}