patch_tracker/
image_utilities.rs

1use image::{GenericImageView, GrayImage};
2use imageproc::corners::{corners_fast9, Corner};
3use nalgebra as na;
4
5pub fn image_grad(grayscale_image: &GrayImage, x: f32, y: f32) -> na::SVector<f32, 3> {
6    // inbound
7    let ix = x.floor() as u32;
8    let iy = y.floor() as u32;
9
10    let dx = x - ix as f32;
11    let dy = y - iy as f32;
12
13    let ddx = 1.0 - dx;
14    let ddy = 1.0 - dy;
15
16    let px0y0 = grayscale_image.get_pixel(ix, iy).0[0] as f32;
17    let px1y0 = grayscale_image.get_pixel(ix + 1, iy).0[0] as f32;
18    let px0y1 = grayscale_image.get_pixel(ix, iy + 1).0[0] as f32;
19    let px1y1 = grayscale_image.get_pixel(ix + 1, iy + 1).0[0] as f32;
20
21    let res0 = ddx * ddy * px0y0 + ddx * dy * px0y1 + dx * ddy * px1y0 + dx * dy * px1y1;
22
23    let pxm1y0 = grayscale_image.get_pixel(ix - 1, iy).0[0] as f32;
24    let pxm1y1 = grayscale_image.get_pixel(ix - 1, iy + 1).0[0] as f32;
25
26    let res_mx = ddx * ddy * pxm1y0 + ddx * dy * pxm1y1 + dx * ddy * px0y0 + dx * dy * px0y1;
27
28    let px2y0 = grayscale_image.get_pixel(ix + 2, iy).0[0] as f32;
29    let px2y1 = grayscale_image.get_pixel(ix + 2, iy + 1).0[0] as f32;
30
31    let res_px = ddx * ddy * px1y0 + ddx * dy * px1y1 + dx * ddy * px2y0 + dx * dy * px2y1;
32
33    let res1 = 0.5 * (res_px - res_mx);
34
35    let px0ym1 = grayscale_image.get_pixel(ix, iy - 1).0[0] as f32;
36    let px1ym1 = grayscale_image.get_pixel(ix + 1, iy - 1).0[0] as f32;
37
38    let res_my = ddx * ddy * px0ym1 + ddx * dy * px0y0 + dx * ddy * px1ym1 + dx * dy * px1y0;
39
40    let px0y2 = grayscale_image.get_pixel(ix, iy + 2).0[0] as f32;
41    let px1y2 = grayscale_image.get_pixel(ix + 1, iy + 2).0[0] as f32;
42
43    let res_py = ddx * ddy * px0y1 + ddx * dy * px0y2 + dx * ddy * px1y1 + dx * dy * px1y2;
44
45    let res2 = 0.5 * (res_py - res_my);
46
47    na::SVector::<f32, 3>::new(res0, res1, res2)
48}
49
50pub fn point_in_bound(keypoint: &Corner, height: u32, width: u32, radius: u32) -> bool {
51    keypoint.x >= radius
52        && keypoint.x <= width - radius
53        && keypoint.y >= radius
54        && keypoint.y <= height - radius
55}
56
57pub fn inbound(image: &GrayImage, x: f32, y: f32, radius: u32) -> bool {
58    let x = x.round() as u32;
59    let y = y.round() as u32;
60
61    x >= radius && y >= radius && x < image.width() - radius && y < image.height() - radius
62}
63
64pub fn se2_exp_matrix(a: &na::SVector<f32, 3>) -> na::SMatrix<f32, 3, 3> {
65    let theta = a[2];
66    let mut so2 = na::Rotation2::new(theta);
67    let sin_theta_by_theta;
68    let one_minus_cos_theta_by_theta;
69
70    if theta.abs() < f32::EPSILON {
71        let theta_sq = theta * theta;
72        sin_theta_by_theta = 1.0f32 - 1.0 / 6.0 * theta_sq;
73        one_minus_cos_theta_by_theta = 0.5f32 * theta - 1. / 24. * theta * theta_sq;
74    } else {
75        let cos = so2.matrix_mut_unchecked().m22;
76        let sin = so2.matrix_mut_unchecked().m21;
77        sin_theta_by_theta = sin / theta;
78        one_minus_cos_theta_by_theta = (1. - cos) / theta;
79    }
80    let mut se2_mat = na::SMatrix::<f32, 3, 3>::identity();
81    se2_mat.m11 = so2.matrix_mut_unchecked().m11;
82    se2_mat.m12 = so2.matrix_mut_unchecked().m12;
83    se2_mat.m21 = so2.matrix_mut_unchecked().m21;
84    se2_mat.m22 = so2.matrix_mut_unchecked().m22;
85    se2_mat.m13 = sin_theta_by_theta * a[0] - one_minus_cos_theta_by_theta * a[1];
86    se2_mat.m23 = one_minus_cos_theta_by_theta * a[0] + sin_theta_by_theta * a[1];
87    se2_mat
88}
89
90pub fn detect_key_points(
91    image: &GrayImage,
92    grid_size: u32,
93    current_corners: &Vec<Corner>,
94    num_points_in_cell: u32,
95) -> Vec<Corner> {
96    const EDGE_THRESHOLD: u32 = 19;
97    let h = image.height();
98    let w = image.width();
99    let mut all_corners = vec![];
100    let mut grids =
101        na::DMatrix::<i32>::zeros((h / grid_size + 1) as usize, (w / grid_size + 1) as usize);
102
103    let x_start = (w % grid_size) / 2;
104    let x_stop = x_start + grid_size * (w / grid_size - 1) + 1;
105
106    let y_start = (h % grid_size) / 2;
107    let y_stop = y_start + grid_size * (h / grid_size - 1) + 1;
108
109    // add existing corners to grid
110    for corner in current_corners {
111        if corner.x >= x_start
112            && corner.y >= y_start
113            && corner.x < x_stop + grid_size
114            && corner.y < y_stop + grid_size
115        {
116            let x = (corner.x - x_start) / grid_size;
117            let y = (corner.y - y_start) / grid_size;
118
119            grids[(y as usize, x as usize)] += 1;
120        }
121    }
122
123    for x in (x_start..x_stop).step_by(grid_size as usize) {
124        for y in (y_start..y_stop).step_by(grid_size as usize) {
125            if grids[(
126                ((y - y_start) / grid_size) as usize,
127                ((x - x_start) / grid_size) as usize,
128            )] > 0
129            {
130                continue;
131            }
132
133            let image_view = image.view(x, y, grid_size, grid_size).to_image();
134            let mut points_added = 0;
135            let mut threshold: u8 = 40;
136
137            while points_added < num_points_in_cell && threshold >= 10 {
138                let mut fast_corners = corners_fast9(&image_view, threshold);
139                fast_corners.sort_by(|a, b| a.score.partial_cmp(&b.score).unwrap());
140
141                for mut point in fast_corners {
142                    if points_added >= num_points_in_cell {
143                        break;
144                    }
145                    point.x += x;
146                    point.y += y;
147                    if point_in_bound(&point, h, w, EDGE_THRESHOLD) {
148                        all_corners.push(point);
149                        points_added += 1;
150                    }
151                }
152                threshold -= 5;
153            }
154        }
155    }
156    all_corners
157}
patch_tracker/image_utilities.rs

patch_tracker/
image_utilities.rs
