• Home
  • Line#
  • Scopes#
  • Navigate#
  • Raw
  • Download
1 // Copyright 2024 Google LLC
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 //
7 //     https://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
14 
15 use rand::{prelude::SliceRandom, rngs::ThreadRng, thread_rng};
16 
17 use std::collections::BTreeMap;
18 
19 type TrueDistance = f32; // meters
20 type EstimatedDistance = f32; // meters
21 
22 /// The data is organized as a `BTreeMap` for efficient lookup and interpolation.
23 pub struct RangingDataSet {
24     /// Stores ranging data in the form of (true distance, [estimated distances]) pairs.
25     /// The map keys are true distances in u16 centimeters.
26     data: BTreeMap<u16, Vec<EstimatedDistance>>,
27 }
28 
29 impl RangingDataSet {
30     /// Creates a new `RangingDataSet` instance by loading ranging data.
31     /// Data is in a format where each entry is a tuple of
32     /// (true distance, estimated distance), typically representing samples from a
33     /// ranging sensor.
new(ranging_data: Option<Vec<(TrueDistance, EstimatedDistance)>>) -> Self34     pub fn new(ranging_data: Option<Vec<(TrueDistance, EstimatedDistance)>>) -> Self {
35         // Use sample_ranging_data.csv if ranging_data is not provided.
36         #[allow(clippy::excessive_precision)]
37         let mut sample_ranging_data: Vec<(TrueDistance, EstimatedDistance)> =
38             ranging_data.unwrap_or(include!("sample_ranging_data.csv"));
39         // Convert to centimeters as Pica uses centimeters for ranging units
40         sample_ranging_data = sample_ranging_data
41             .into_iter()
42             .map(|(true_dist, est_dist)| (true_dist * 100.0, est_dist * 100.0))
43             .collect::<Vec<(TrueDistance, EstimatedDistance)>>();
44 
45         // Process the sample_raning_data into BTreeMap
46         let mut data: BTreeMap<u16, Vec<EstimatedDistance>> = BTreeMap::new();
47         for (true_distance, estimated_distance) in sample_ranging_data {
48             // Convert true_distance into u16 centimeters
49             data.entry((true_distance * 100.0).round() as u16)
50                 .or_default()
51                 .push(estimated_distance);
52         }
53         RangingDataSet { data }
54     }
55 
56     /// Samples an estimated distance for the given true distance.
57     ///
58     /// # Arguments
59     ///
60     /// * `distance` - The true distance for which an estimated distance is required.
61     /// * `option_rng` - An optional random number generator
62     ///     (if not provided, a default one will be used)
63     ///
64     /// # Returns
65     ///
66     /// An estimated distance sampled from the dataset.
67     /// If the exact true distance is found in the dataset,
68     ///     a random estimated distance is chosen from its associated values.
69     /// If the true distance falls between known values,
70     ///     linear interpolation is used to estimate a distance.
71     /// If the true distance is outside the range of known values,
72     ///     the distance itself is returned as the estimated distance.
sample( &self, distance: TrueDistance, option_rng: Option<ThreadRng>, ) -> EstimatedDistance73     pub fn sample(
74         &self,
75         distance: TrueDistance,
76         option_rng: Option<ThreadRng>,
77     ) -> EstimatedDistance {
78         // Generate a new ThreadRng if not provided
79         let mut rng = option_rng.unwrap_or(thread_rng());
80         // Convert TrueDistance into u16 centimeters.
81         let distance_u16 = (distance * 100.0).round() as u16;
82 
83         // Random sampling if distance is an existing data key
84         if let Some(vec_estimated_distance) = self.data.get(&distance_u16) {
85             return *vec_estimated_distance.choose(&mut rng).unwrap();
86         }
87 
88         // Linear Interpolation if provided TrueDistance lies in between data keys
89         let lower = self.data.range(..=&distance_u16).next_back();
90         let upper = self.data.range(&distance_u16..).next();
91         match (lower, upper) {
92             (Some((lower_key, lower_vals)), Some((upper_key, upper_vals))) => {
93                 let x1 = *lower_key as f32 / 100.0;
94                 let y1 = *lower_vals.choose(&mut rng).unwrap();
95                 let x2 = *upper_key as f32 / 100.0;
96                 let y2 = *upper_vals.choose(&mut rng).unwrap();
97                 y1 + (distance - x1) * (y2 - y1) / (x2 - x1)
98             }
99             _ => distance,
100         }
101     }
102 }
103 
104 #[cfg(test)]
105 mod tests {
106     use super::*;
107 
sample_ranging_data_set() -> RangingDataSet108     fn sample_ranging_data_set() -> RangingDataSet {
109         let ranging_data = vec![(0.0, 0.2), (1.0, 0.9), (2.0, 1.9), (2.0, 2.1)];
110         RangingDataSet::new(Some(ranging_data))
111     }
112 
113     #[test]
test_sample_ranging_data_set()114     fn test_sample_ranging_data_set() {
115         let ranging_data_set = sample_ranging_data_set();
116         // Linear Interpolation
117         assert_eq!(ranging_data_set.sample(50., None), 55.);
118         // Exact distance found in dataset
119         assert!([190., 210.].contains(&ranging_data_set.sample(200., None)));
120         // Out of Range
121         assert_eq!(ranging_data_set.sample(300., None), 300.);
122     }
123 }
124