visual_odometry_rs/core/
multires.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 functions for generation of multi-resolution data.
6
7use nalgebra::{DMatrix, Scalar};
8
9use crate::core::gradient;
10
11/// Recursively generate a pyramid of matrices where each following level
12/// is half the previous resolution, computed with the mean of each 2x2 block.
13///
14/// It consumes the original matrix to keep it as first level of the pyramid without copy.
15/// If you need it later, simply use something like `pyramid[0]`.
16///
17/// PS: since we are using 2x2 blocs,
18/// border information is lost for odd resolutions.
19/// Some precision is also left to keep the pyramid data as `u8`.
20#[allow(clippy::cast_possible_truncation)]
21pub fn mean_pyramid(max_levels: usize, mat: DMatrix<u8>) -> Vec<DMatrix<u8>> {
22    limited_sequence(max_levels, mat, |m| {
23        halve(m, |a, b, c, d| {
24            let a = u16::from(a);
25            let b = u16::from(b);
26            let c = u16::from(c);
27            let d = u16::from(d);
28            ((a + b + c + d) / 4) as u8
29        })
30    })
31}
32
33/// Recursively apply a function transforming an image
34/// until it's not possible anymore or the max length is reached.
35///
36/// Using `max_length = 0` has the same effect than `max_length = 1` since
37/// the result vector contains always at least one matrix (the init matrix).
38pub fn limited_sequence<F: Fn(&T) -> Option<T>, T>(max_length: usize, data: T, f: F) -> Vec<T> {
39    let mut length = 1;
40    let f_limited = |x: &T| {
41        if length < max_length {
42            length += 1;
43            f(x)
44        } else {
45            None
46        }
47    };
48    sequence(data, f_limited)
49}
50
51/// Recursively apply a function transforming data
52/// until it's not possible anymore.
53pub fn sequence<F: FnMut(&T) -> Option<T>, T>(data: T, mut f: F) -> Vec<T> {
54    let mut s = Vec::new();
55    s.push(data);
56    while let Some(new_data) = f(s.last().unwrap()) {
57        s.push(new_data);
58    }
59    s
60}
61
62/// Halve the resolution of a matrix by applying a function to each 2x2 block.
63///
64/// If one size of the matrix is < 2 then this function returns None.
65/// If one size is odd, its last line/column is dropped.
66#[allow(clippy::many_single_char_names)]
67pub fn halve<F, T, U>(mat: &DMatrix<T>, f: F) -> Option<DMatrix<U>>
68where
69    F: Fn(T, T, T, T) -> U,
70    T: Scalar,
71    U: Scalar,
72{
73    let (r, c) = mat.shape();
74    let half_r = r / 2;
75    let half_c = c / 2;
76    if half_r == 0 || half_c == 0 {
77        None
78    } else {
79        let half_mat = DMatrix::<U>::from_fn(half_r, half_c, |i, j| {
80            let a = mat[(2 * i, 2 * j)];
81            let b = mat[(2 * i + 1, 2 * j)];
82            let c = mat[(2 * i, 2 * j + 1)];
83            let d = mat[(2 * i + 1, 2 * j + 1)];
84            f(a, b, c, d)
85        });
86        Some(half_mat)
87    }
88}
89
90// Gradients stuff ###################################################
91
92/// Compute centered gradients norm at each resolution from
93/// the image at the higher resolution.
94///
95/// As a consequence there is one less level in the gradients pyramid.
96pub fn gradients_squared_norm(multires_mat: &[DMatrix<u8>]) -> Vec<DMatrix<u16>> {
97    let nb_levels = multires_mat.len();
98    multires_mat
99        .iter()
100        .take(nb_levels - 1)
101        .map(|mat| {
102            halve(mat, gradient::bloc_squared_norm)
103                .expect("There is an issue in gradients_squared_norm")
104        })
105        .collect()
106}
107
108/// Compute centered gradients at each resolution from
109/// the image at the higher resolution.
110///
111/// As a consequence there is one less level in the gradients pyramid.
112pub fn gradients_xy(multires_mat: &[DMatrix<u8>]) -> Vec<(DMatrix<i16>, DMatrix<i16>)> {
113    // TODO: maybe it would be better to return Vec<DMatrix<(i16,i16)>>,
114    // to colocate the x and y gradient and do only one "halve" call?
115    let nb_levels = multires_mat.len();
116    multires_mat
117        .iter()
118        .take(nb_levels - 1)
119        .map(|mat| {
120            (
121                halve(mat, gradient::bloc_x).expect("There is an issue in gradients_xy x."),
122                halve(mat, gradient::bloc_y).expect("There is an issue in gradients_xy y."),
123            )
124        })
125        .collect()
126}
visual_odometry_rs/core/multires.rs

visual_odometry_rs/core/
multires.rs
