clustering

src/main.rs

@@ -1,5 +1,5 @@
-use std::collections::VecDeque;
+use std::collections::{HashMap, VecDeque};
-use std::iter::zip;
+use std::iter::{zip, Map};
 use std::sync::{mpsc, Arc};
 use std::sync::mpsc::Receiver;
 use std::thread;
@@ -9,6 +9,7 @@ use image::{DynamicImage, ImageReader, Rgb, RgbImage};
 use rand::{Rng};
 use rayon::prelude::*;
 use show_image::{create_window, ImageInfo, ImageView, PixelFormat};
+use show_image::event::VirtualKeyCode::V;
 
 fn create_extract_thread(image_rx: Receiver<Arc<Vec<u8>>>, width: u32, threshold: u8, downsample: f64) -> Receiver<Arc<Vec<Vec<f64>>>> {
     let (tx, rx) = mpsc::sync_channel::<Arc<Vec<Vec<f64>>>>(1);
@@ -19,12 +20,12 @@ fn create_extract_thread(image_rx: Receiver<Arc<Vec<u8>>>, width: u32, threshold
             let poi = image
                 .par_iter()
                 .enumerate()
-                .filter(move |(_, pixel)| pixel > &&threshold)
+                .filter(|(_, pixel)| pixel > &&threshold)
-                .filter(move |_| {
+                .filter(|_| {
                     let mut rng = rand::rng();
                     rng.random::<u32>() < max_rng
                 })
-                .map(move |(i, _)| {
+                .map(|(i, _)| {
                     vec![ i as f64 % width as f64 - width_2, i as f64 / width as f64 ]
                 });
 
@@ -36,25 +37,66 @@ fn create_extract_thread(image_rx: Receiver<Arc<Vec<u8>>>, width: u32, threshold
     rx
 }
 
-fn create_cluster_labels_thread(pointcloud_rx: Receiver<Arc<Vec<Vec<f64>>>>) -> Receiver<(Arc<Vec<Vec<f64>>>, Arc<Vec<i32>>)> {
+fn create_pointcloud_labels_thread(pointcloud_rx: Receiver<Arc<Vec<Vec<f64>>>>) -> Receiver<(Arc<Vec<Vec<f64>>>, Arc<Vec<i32>>)> {
-    let (clusters_tx, clusters_rx) = mpsc::sync_channel::<(Arc<Vec<Vec<f64>>>, Arc<Vec<i32>>)>(1);
+    let (pointcloud_labels_tx, pointcloud_labels_rx) = mpsc::sync_channel::<(Arc<Vec<Vec<f64>>>, Arc<Vec<i32>>)>(1);
 
     thread::spawn(move || {
         for pointcloud in pointcloud_rx {
             let params = HdbscanHyperParams::builder()
-                .epsilon(100000.0)
+                .epsilon(1.0)
                 .min_samples(3)
                 .allow_single_cluster(false)
                 .min_cluster_size(pointcloud.len() / 8)
                 .build();
             let labels = Arc::new(Hdbscan::new(&pointcloud, params).cluster().unwrap());
-            clusters_tx.send((pointcloud, labels)).unwrap() ;
+            pointcloud_labels_tx.send((pointcloud, labels)).unwrap() ;
+        }
+    });
+
+    pointcloud_labels_rx
+}
+
+fn create_cluster_separator_thread(cluster_labels: Receiver<(Arc<Vec<Vec<f64>>>, Arc<Vec<i32>>)>) -> Receiver<Arc<HashMap::<i32, Vec<Vec<f64>>>>> {
+    let (clusters_tx, clusters_rx) = mpsc::sync_channel::<Arc<HashMap::<i32, Vec<Vec<f64>>>>>(1);
+
+    thread::spawn(move || {
+        for (pointcloud, labels) in cluster_labels {
+            let mut clusters = HashMap::<i32, Vec<Vec<f64>>>::with_capacity(5);
+
+            let _ = zip(pointcloud.iter(), labels.iter()).for_each(|(point, label)| {
+                if !clusters.contains_key(label) { clusters.insert(*label, Vec::new()); }
+
+                clusters.get_mut(label).unwrap().push(point.clone());
+            });
+
+
+            clusters_tx.send(Arc::new(clusters)).unwrap() ;
         }
     });
 
     clusters_rx
 }
 
+fn create_isolate_lane_thread(clusters_rx: Receiver<Arc<HashMap::<i32, Vec<Vec<f64>>>>>) -> Receiver<[Vec<Vec<f64>>; 2]> {
+    let (lanes_tx, lanes_rx) = mpsc::sync_channel::<[Vec<Vec<f64>>; 2]>(1);
+
+    thread::spawn(move || {
+        for clusters in clusters_rx {
+            let mut averages = clusters.iter().filter(|(label, _)| **label != -1).map(|(label, cluster)| {
+                (*label, cluster.iter().map(|point| point[0]).sum::<f64>() / (cluster.len() as f64))
+            }).collect::<Vec<(i32, f64)>>();
+
+            averages.sort_by(|(_, a), (_, b)| a.partial_cmp(b).unwrap());
+
+            lanes_tx.send([clusters.get(&averages[0].0).unwrap().clone(), clusters.get(&averages[1].0 ).unwrap().clone()]).unwrap() ;
+
+        }
+    });
+
+    lanes_rx
+}
+
+
 fn create_transform_thread() {
     thread::spawn(move || {
         //let pts :Vec<Vec<f64>> = Vec::new(x_points, y_points, 1-array)
@@ -68,7 +110,7 @@ fn create_filter_thread() {
 }
 
 fn dummy_image_spawner_thread() -> Receiver<Arc<Vec<u8>>> {
-    let img: DynamicImage = ImageReader::open("./images/lane_detection_loop_80.png").unwrap().decode().unwrap();
+    let img: DynamicImage = ImageReader::open("./images/lane_detection_loop_60.png").unwrap().decode().unwrap();
     let img_data: Vec<u8> = img.clone().into_bytes();
 
     let (img_tx, img_rx) = mpsc::sync_channel::<Arc<Vec<u8>>>(1);
@@ -86,17 +128,22 @@ fn dummy_image_spawner_thread() -> Receiver<Arc<Vec<u8>>>{
 fn main() {
     let image_rx = dummy_image_spawner_thread();
     let pointcloud_rx = create_extract_thread(image_rx, 800, 196, 0.05);
-    let clusters_rx = create_cluster_labels_thread(pointcloud_rx);
+    let cluster_labels_rx = create_pointcloud_labels_thread(pointcloud_rx);
+    let clusters_rx = create_cluster_separator_thread(cluster_labels_rx);
+    let lanes = create_isolate_lane_thread(clusters_rx);
 
     let window = create_window("image", Default::default()).unwrap();
     let mut t: VecDeque<Duration> = VecDeque::with_capacity(25);
     let mut now: Instant = Instant::now();
 
-    for (image, labels) in clusters_rx {
+    for lanes in lanes.iter() {
+
         let mut rgb_image = RgbImage::new(800, 800);
-        zip(image.iter(), labels.iter()).for_each(|(pixel, label)| {
+
-            if *label != -1 {
+        let colors = vec![Rgb([0, 255, 0]), Rgb([255, 0, 0])];
-                let color = vec![Rgb([255, 0, 0]), Rgb([0, 255, 0]), Rgb([0, 0, 255]), Rgb([0, 0, 0])][*label as usize];
+        for (i, lane) in lanes.iter().enumerate() {
+            let color = colors[i];
+            let _ = lane.iter().for_each(|(pixel)| {
                 let x = (pixel[0] + 400f64) as u32;
                 rgb_image.put_pixel(x, pixel[1] as u32, color);
                 if pixel[0] > 0.0 {
@@ -109,10 +156,12 @@ fn main() {
                     rgb_image.put_pixel(x, pixel[1] as u32 - 1, color);
                 }
                 if pixel[1] < 399f64 {
-                    rgb_image.put_pixel(x, pixel[1] as u32 + 1, Rgb([*label as u8 * 8, 0, 0]));
+                    rgb_image.put_pixel(x, pixel[1] as u32 + 1, color);
-                }
                 }
             });
+        }
+
+
         let image = ImageView::new(ImageInfo::new(PixelFormat::Rgb8, 800, 800), rgb_image.iter().as_slice());
         window.set_image("camera", image).unwrap();