optimized highlight extractor

src/pipeline/highlight_extractor.rs

@@ -2,7 +2,7 @@ use std::sync::{mpsc, Arc, Mutex};
 use std::sync::mpsc::Receiver;
 use std::thread;
 use std::time::{Duration, Instant};
-use rand::Rng;
+use rand::{rng, Rng};
 use r2r::sensor_msgs::msg::Image;
 
 pub type Pointcloud = Arc<Vec<Vec<f64>>>;
@@ -19,18 +19,16 @@ impl ExtractHighlights for Image {
         ];
         
         let max_rng = (u32::MAX as f64 * downsample) as u32;
+        let mut rng = rand::rng();
         Arc::new(self.data
             .iter()
-            .enumerate()
-            .filter(|(i, _)| *i as u32 / self.width >= self.height / 2)
-            .filter(|(_, pixel)| **pixel > threshold)
+            .enumerate() 
+            .map(|(i, pixel)| (pixel, i as f64 % self.width as f64, i as f64 / self.width as f64))
+            .filter(|(pixel, _, _)| **pixel > threshold)
             .filter(|_| {
-                let mut rng = rand::rng();
                 rng.random::<u32>() < max_rng
             })
-            .map(|(i, _)| {
-                 let x = i as f64 % self.width as f64;
-                 let y = i as f64 / self.width as f64;
+            .map(|(_, x, y)| {
 
                  // Convert to homogeneous coordinates
                  let u = h_i[0][0] * x + h_i[0][1] * y + h_i[0][2];