patch_tracker/
patch.rs

1use image::imageops;
2use image::GrayImage;
3use nalgebra as na;
4use std::ops::AddAssign;
5
6use crate::image_utilities;
7
8pub const PATTERN52_SIZE: usize = 52;
9pub struct Pattern52 {
10    pub valid: bool,
11    pub mean: f32,
12    pub pos: na::SVector<f32, 2>,
13    pub data: [f32; PATTERN52_SIZE], // negative if the point is not valid
14    pub h_se2_inv_j_se2_t: na::SMatrix<f32, 3, PATTERN52_SIZE>,
15    pub pattern_scale_down: f32,
16}
17impl Pattern52 {
18    pub const PATTERN_RAW: [[f32; 2]; PATTERN52_SIZE] = [
19        [-3.0, 7.0],
20        [-1.0, 7.0],
21        [1.0, 7.0],
22        [3.0, 7.0],
23        [-5.0, 5.0],
24        [-3.0, 5.0],
25        [-1.0, 5.0],
26        [1.0, 5.0],
27        [3.0, 5.0],
28        [5.0, 5.0],
29        [-7.0, 3.0],
30        [-5.0, 3.0],
31        [-3.0, 3.0],
32        [-1.0, 3.0],
33        [1.0, 3.0],
34        [3.0, 3.0],
35        [5.0, 3.0],
36        [7.0, 3.0],
37        [-7.0, 1.0],
38        [-5.0, 1.0],
39        [-3.0, 1.0],
40        [-1.0, 1.0],
41        [1.0, 1.0],
42        [3.0, 1.0],
43        [5.0, 1.0],
44        [7.0, 1.0],
45        [-7.0, -1.0],
46        [-5.0, -1.0],
47        [-3.0, -1.0],
48        [-1.0, -1.0],
49        [1.0, -1.0],
50        [3.0, -1.0],
51        [5.0, -1.0],
52        [7.0, -1.0],
53        [-7.0, -3.0],
54        [-5.0, -3.0],
55        [-3.0, -3.0],
56        [-1.0, -3.0],
57        [1.0, -3.0],
58        [3.0, -3.0],
59        [5.0, -3.0],
60        [7.0, -3.0],
61        [-5.0, -5.0],
62        [-3.0, -5.0],
63        [-1.0, -5.0],
64        [1.0, -5.0],
65        [3.0, -5.0],
66        [5.0, -5.0],
67        [-3.0, -7.0],
68        [-1.0, -7.0],
69        [1.0, -7.0],
70        [3.0, -7.0],
71    ];
72
73    // verified
74    pub fn set_data_jac_se2(
75        &mut self,
76        greyscale_image: &GrayImage,
77        j_se2: &mut na::SMatrix<f32, PATTERN52_SIZE, 3>,
78    ) {
79        let mut num_valid_points = 0;
80        let mut sum: f32 = 0.0;
81        let mut grad_sum_se2 = na::SVector::<f32, 3>::zeros();
82
83        let mut jw_se2 = na::SMatrix::<f32, 2, 3>::identity();
84
85        for (i, pattern_pos) in Self::PATTERN_RAW.into_iter().enumerate() {
86            let p = self.pos
87                + na::SVector::<f32, 2>::new(
88                    pattern_pos[0] / self.pattern_scale_down,
89                    pattern_pos[1] / self.pattern_scale_down,
90                );
91            jw_se2[(0, 2)] = -pattern_pos[1] / self.pattern_scale_down;
92            jw_se2[(1, 2)] = pattern_pos[0] / self.pattern_scale_down;
93
94            if image_utilities::inbound(greyscale_image, p.x, p.y, 2) {
95                let val_grad = image_utilities::image_grad(greyscale_image, p.x, p.y);
96
97                self.data[i] = val_grad[0];
98                sum += val_grad[0];
99                let re = val_grad.fixed_rows::<2>(1).transpose() * jw_se2;
100                j_se2.set_row(i, &re);
101                grad_sum_se2.add_assign(j_se2.fixed_rows::<1>(i).transpose());
102                num_valid_points += 1;
103            } else {
104                self.data[i] = -1.0;
105            }
106        }
107
108        self.mean = sum / num_valid_points as f32;
109
110        let mean_inv = num_valid_points as f32 / sum;
111
112        for i in 0..Self::PATTERN_RAW.len() {
113            if self.data[i] >= 0.0 {
114                let rhs = grad_sum_se2.transpose() * self.data[i] / sum;
115                j_se2.fixed_rows_mut::<1>(i).add_assign(-rhs);
116                self.data[i] *= mean_inv;
117            } else {
118                j_se2.set_row(i, &na::SMatrix::<f32, 1, 3>::zeros());
119            }
120        }
121        *j_se2 *= mean_inv;
122    }
123    pub fn new(greyscale_image: &GrayImage, px: f32, py: f32) -> Pattern52 {
124        let mut j_se2 = na::SMatrix::<f32, PATTERN52_SIZE, 3>::zeros();
125        let mut p = Pattern52 {
126            valid: false,
127            mean: 1.0,
128            pos: na::SVector::<f32, 2>::new(px, py),
129            data: [0.0; PATTERN52_SIZE], // negative if the point is not valid
130            h_se2_inv_j_se2_t: na::SMatrix::<f32, 3, 52>::zeros(),
131            pattern_scale_down: 2.0,
132        };
133        p.set_data_jac_se2(greyscale_image, &mut j_se2);
134        let h_se2 = j_se2.transpose() * j_se2;
135        let mut h_se2_inv = na::SMatrix::<f32, 3, 3>::identity();
136
137        if let Some(x) = h_se2.cholesky() {
138            x.solve_mut(&mut h_se2_inv);
139            p.h_se2_inv_j_se2_t = h_se2_inv * j_se2.transpose();
140
141            // NOTE: while it's very unlikely we get a source patch with all black
142            // pixels, since points are usually selected at corners, it doesn't cost
143            // much to be safe here.
144
145            // all-black patch cannot be normalized; will result in mean of "zero" and
146            // H_se2_inv_J_se2_T will contain "NaN" and data will contain "inf"
147            p.valid = p.mean > f32::EPSILON
148                && p.h_se2_inv_j_se2_t.iter().all(|x| x.is_finite())
149                && p.data.iter().all(|x| x.is_finite());
150        }
151
152        p
153    }
154    pub fn residual(
155        &self,
156        greyscale_image: &GrayImage,
157        transformed_pattern: &na::SMatrix<f32, 2, PATTERN52_SIZE>,
158    ) -> Option<na::SVector<f32, PATTERN52_SIZE>> {
159        let mut sum: f32 = 0.0;
160        let mut num_valid_points = 0;
161        let mut residual = na::SVector::<f32, PATTERN52_SIZE>::zeros();
162        for i in 0..PATTERN52_SIZE {
163            if image_utilities::inbound(
164                greyscale_image,
165                transformed_pattern[(0, i)],
166                transformed_pattern[(1, i)],
167                2,
168            ) {
169                let p = imageops::interpolate_bilinear(
170                    greyscale_image,
171                    transformed_pattern[(0, i)],
172                    transformed_pattern[(1, i)],
173                );
174                residual[i] = p.unwrap().0[0] as f32;
175                sum += residual[i];
176                num_valid_points += 1;
177            } else {
178                residual[i] = -1.0;
179            }
180        }
181
182        // all-black patch cannot be normalized
183        if sum < f32::EPSILON {
184            return None;
185        }
186
187        let mut num_residuals = 0;
188
189        for i in 0..PATTERN52_SIZE {
190            if residual[i] >= 0.0 && self.data[i] >= 0.0 {
191                let val = residual[i];
192                residual[i] = num_valid_points as f32 * val / sum - self.data[i];
193                num_residuals += 1;
194            } else {
195                residual[i] = 0.0;
196            }
197        }
198        if num_residuals > PATTERN52_SIZE / 2 {
199            Some(residual)
200        } else {
201            None
202        }
203    }
204}
patch_tracker/patch.rs

patch_tracker/
patch.rs
