From ee46faf311ab0196b7e61b21beb4e55354b0430f Mon Sep 17 00:00:00 2001 From: timoschneider Date: Mon, 5 May 2025 20:57:41 +0200 Subject: [PATCH] clustering --- src/main.rs | 87 +++++++++++++++++++++++++++++++++++++++++------------ 1 file changed, 68 insertions(+), 19 deletions(-) diff --git a/src/main.rs b/src/main.rs index 0c252fd..95c584e 100644 --- a/src/main.rs +++ b/src/main.rs @@ -1,5 +1,5 @@ -use std::collections::VecDeque; -use std::iter::zip; +use std::collections::{HashMap, VecDeque}; +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>>, width: u32, threshold: u8, downsample: f64) -> Receiver>>> { let (tx, rx) = mpsc::sync_channel::>>>(1); @@ -19,12 +20,12 @@ fn create_extract_thread(image_rx: Receiver>>, width: u32, threshold let poi = image .par_iter() .enumerate() - .filter(move |(_, pixel)| pixel > &&threshold) - .filter(move |_| { + .filter(|(_, pixel)| pixel > &&threshold) + .filter(|_| { let mut rng = rand::rng(); rng.random::() < 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>>, width: u32, threshold rx } -fn create_cluster_labels_thread(pointcloud_rx: Receiver>>>) -> Receiver<(Arc>>, Arc>)> { - let (clusters_tx, clusters_rx) = mpsc::sync_channel::<(Arc>>, Arc>)>(1); +fn create_pointcloud_labels_thread(pointcloud_rx: Receiver>>>) -> Receiver<(Arc>>, Arc>)> { + let (pointcloud_labels_tx, pointcloud_labels_rx) = mpsc::sync_channel::<(Arc>>, Arc>)>(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>>, Arc>)>) -> Receiver>>>> { + let (clusters_tx, clusters_rx) = mpsc::sync_channel::>>>>(1); + + thread::spawn(move || { + for (pointcloud, labels) in cluster_labels { + let mut clusters = HashMap::>>::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>>>>) -> Receiver<[Vec>; 2]> { + let (lanes_tx, lanes_rx) = mpsc::sync_channel::<[Vec>; 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::() / (cluster.len() as f64)) + }).collect::>(); + + 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::new(x_points, y_points, 1-array) @@ -67,8 +109,8 @@ fn create_filter_thread() { } -fn dummy_image_spawner_thread() -> Receiver>>{ - let img: DynamicImage = ImageReader::open("./images/lane_detection_loop_80.png").unwrap().decode().unwrap(); +fn dummy_image_spawner_thread() -> Receiver>> { + let img: DynamicImage = ImageReader::open("./images/lane_detection_loop_60.png").unwrap().decode().unwrap(); let img_data: Vec = img.clone().into_bytes(); let (img_tx, img_rx) = mpsc::sync_channel::>>(1); @@ -86,17 +128,22 @@ fn dummy_image_spawner_thread() -> Receiver>>{ 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 = 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 color = vec![Rgb([255, 0, 0]), Rgb([0, 255, 0]), Rgb([0, 0, 255]), Rgb([0, 0, 0])][*label as usize]; + + let colors = vec![Rgb([0, 255, 0]), Rgb([255, 0, 0])]; + 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();