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