simple_icp/
adaptive_threshold.rs

1use nalgebra as na;
2
3pub struct AdaptiveThreshold {
4    // configurable parameters
5    min_motion_threshold: f64,
6    max_range: f64,
7
8    // Local cache for ccomputation
9    model_sse: f64,
10    num_samples: f64,
11}
12impl AdaptiveThreshold {
13    pub fn new(init_thres: f64, min_motion_threshold: f64, max_range: f64) -> AdaptiveThreshold {
14        AdaptiveThreshold {
15            min_motion_threshold,
16            max_range,
17            model_sse: init_thres * init_thres,
18            num_samples: 1.0,
19        }
20    }
21    pub fn compute_threshold(&self) -> f64 {
22        (self.model_sse / self.num_samples).sqrt()
23    }
24    pub fn update_model_deviation(&mut self, current_deviation: &na::Isometry3<f64>) {
25        let theta = current_deviation.rotation.angle();
26        let delta_rot = 2.0 * self.max_range * (theta / 2.0).sin();
27        let delta_trans = current_deviation.translation.vector.norm();
28        let model_error = delta_rot + delta_trans;
29        if model_error > self.min_motion_threshold {
30            self.model_sse += model_error * model_error;
31            self.num_samples += 1.0;
32        }
33    }
34}
simple_icp/adaptive_threshold.rs

simple_icp/
adaptive_threshold.rs
