patch_tracker/
patch.rs

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

patch_tracker/
patch.rs
