visual_odometry_rs/core/
gradient.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 function to compute gradients
6
7use nalgebra as na;
8
9/// Compute a centered gradient.
10///
11/// 1/2 * ( img(i+1,j) - img(i-1,j), img(i,j+1) - img(i,j-1) )
12///
13/// Gradients of pixels at the border of the image are set to 0.
14#[allow(clippy::similar_names)]
15pub fn centered(img: &na::DMatrix<u8>) -> (na::DMatrix<i16>, na::DMatrix<i16>) {
16    // TODO: might be better to return DMatrix<(i16,i16)>?
17    let (nb_rows, nb_cols) = img.shape();
18    let top = img.slice((0, 1), (nb_rows - 2, nb_cols - 2));
19    let bottom = img.slice((2, 1), (nb_rows - 2, nb_cols - 2));
20    let left = img.slice((1, 0), (nb_rows - 2, nb_cols - 2));
21    let right = img.slice((1, 2), (nb_rows - 2, nb_cols - 2));
22    let mut grad_x = na::DMatrix::zeros(nb_rows, nb_cols);
23    let mut grad_y = na::DMatrix::zeros(nb_rows, nb_cols);
24    let mut grad_x_inner = grad_x.slice_mut((1, 1), (nb_rows - 2, nb_cols - 2));
25    let mut grad_y_inner = grad_y.slice_mut((1, 1), (nb_rows - 2, nb_cols - 2));
26    for j in 0..nb_cols - 2 {
27        for i in 0..nb_rows - 2 {
28            grad_x_inner[(i, j)] = (i16::from(right[(i, j)]) - i16::from(left[(i, j)])) / 2;
29            grad_y_inner[(i, j)] = (i16::from(bottom[(i, j)]) - i16::from(top[(i, j)])) / 2;
30        }
31    }
32    (grad_x, grad_y)
33}
34
35/// Compute squared gradient norm from x and y gradient matrices.
36#[allow(clippy::cast_possible_truncation)]
37#[allow(clippy::cast_sign_loss)]
38pub fn squared_norm(grad_x: &na::DMatrix<i16>, grad_y: &na::DMatrix<i16>) -> na::DMatrix<u16> {
39    grad_x.zip_map(grad_y, |gx, gy| {
40        let gx = i32::from(gx);
41        let gy = i32::from(gy);
42        (gx * gx + gy * gy) as u16
43    })
44}
45
46/// Compute squared gradient norm directly from the image.
47#[allow(clippy::cast_possible_truncation)]
48#[allow(clippy::cast_sign_loss)]
49pub fn squared_norm_direct(im: &na::DMatrix<u8>) -> na::DMatrix<u16> {
50    let (nb_rows, nb_cols) = im.shape();
51    let top = im.slice((0, 1), (nb_rows - 2, nb_cols - 2));
52    let bottom = im.slice((2, 1), (nb_rows - 2, nb_cols - 2));
53    let left = im.slice((1, 0), (nb_rows - 2, nb_cols - 2));
54    let right = im.slice((1, 2), (nb_rows - 2, nb_cols - 2));
55    let mut squared_norm_mat = na::DMatrix::zeros(nb_rows, nb_cols);
56    let mut grad_inner = squared_norm_mat.slice_mut((1, 1), (nb_rows - 2, nb_cols - 2));
57    for j in 0..nb_cols - 2 {
58        for i in 0..nb_rows - 2 {
59            let gx = i32::from(right[(i, j)]) - i32::from(left[(i, j)]);
60            let gy = i32::from(bottom[(i, j)]) - i32::from(top[(i, j)]);
61            grad_inner[(i, j)] = ((gx * gx + gy * gy) / 4) as u16;
62        }
63    }
64    squared_norm_mat
65}
66
67// BLOCS 2x2 ###################################################################
68
69/// Horizontal gradient in a 2x2 pixels block.
70///
71/// The block is of the form:
72///   a c
73///   b d
74pub fn bloc_x(a: u8, b: u8, c: u8, d: u8) -> i16 {
75    let a = i16::from(a);
76    let b = i16::from(b);
77    let c = i16::from(c);
78    let d = i16::from(d);
79    (c + d - a - b) / 2
80}
81
82/// Vertical gradient in a 2x2 pixels block.
83///
84/// The block is of the form:
85///   a c
86///   b d
87pub fn bloc_y(a: u8, b: u8, c: u8, d: u8) -> i16 {
88    let a = i16::from(a);
89    let b = i16::from(b);
90    let c = i16::from(c);
91    let d = i16::from(d);
92    (b - a + d - c) / 2
93}
94
95/// Gradient squared norm in a 2x2 pixels block.
96///
97/// The block is of the form:
98///   a c
99///   b d
100#[allow(clippy::cast_possible_truncation)]
101#[allow(clippy::cast_sign_loss)]
102pub fn bloc_squared_norm(a: u8, b: u8, c: u8, d: u8) -> u16 {
103    let a = i32::from(a);
104    let b = i32::from(b);
105    let c = i32::from(c);
106    let d = i32::from(d);
107    let dx = c + d - a - b;
108    let dy = b - a + d - c;
109    // I have checked that the max value is in u16.
110    ((dx * dx + dy * dy) / 4) as u16
111}
visual_odometry_rs/core/gradient.rs

visual_odometry_rs/core/
gradient.rs
