timoschneider/ros2_lane_detect
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

added pipeline

Browse Source
This commit is contained in:
Timo Schneider
2025-05-05 16:23:53 +02:00
parent 66dae40de8
commit 6872c04aa9
1 changed files with 90 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files

138
src/main.rs
View File

@@ -1,84 +1,126 @@
use std::collections::VecDeque;
use std::iter::zip;
use std::sync::{mpsc, Arc};
use std::sync::mpsc::Receiver;
use std::thread;
use std::time::{Duration, Instant};
use hdbscan::{Hdbscan, HdbscanHyperParams};
use image::{DynamicImage, ImageReader, Rgb, RgbImage};
use rand::{Rng};
use show_image::{create_window, ImageInfo, ImageView, PixelFormat};
use rayon::prelude::*;
use show_image::{create_window, ImageInfo, ImageView, PixelFormat};
fn extract_highlights_and_distort(image: &Vec<u8>, width: u32, threshold: u8, downsample: f64) -> Vec<Vec<f64>> {
let max_rng = (u32::MAX as f64 * downsample) as u32;
let width_2 = width as f64 / 2.0;
let poi = image
.par_iter()
.enumerate()
.filter(move |(_, pixel)| pixel > &&threshold)
.filter(move |_| {
let mut rng = rand::rng();
rng.random::<u32>() < max_rng
})
.map(move |(i, _)| {
vec![ i as f64 % width as f64 - width_2, i as f64 / width as f64 ]
});
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);
thread::spawn(move || {
for image in image_rx {
let max_rng = (u32::MAX as f64 * downsample) as u32;
let width_2 = width as f64 / 2.0;
let poi = image
.par_iter()
.enumerate()
.filter(move |(_, pixel)| pixel > &&threshold)
.filter(move |_| {
let mut rng = rand::rng();
rng.random::<u32>() < max_rng
})
.map(move |(i, _)| {
vec![ i as f64 % width as f64 - width_2, i as f64 / width as f64 ]
});
let mut vec = Vec::new();
vec.par_extend(poi);
vec
let mut vec = Vec::new();
vec.par_extend(poi);
tx.send(Arc::new(vec)).expect("TODO: panic message");
}
});
rx
}
fn create_cluster_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);
thread::spawn(move || {
for pointcloud in pointcloud_rx {
let params = HdbscanHyperParams::builder()
.epsilon(100000.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() ;
}
});
clusters_rx
}
fn create_transform_thread() {
thread::spawn(move || {
//let pts :Vec<Vec<f64>> = Vec::new(x_points, y_points, 1-array)
//let X = H * pts
});
}
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_data: Vec<u8> = img.clone().into_bytes();
let (img_tx, img_rx) = mpsc::sync_channel::<Arc<Vec<u8>>>(1);
thread::spawn(move || {
loop {
img_tx.send(Arc::new(img_data.clone())).unwrap();
}
});
img_rx
}
#[show_image::main]
fn main() {
let img: DynamicImage = ImageReader::open("./images/lane_detection_loop_80.png").unwrap().decode().unwrap();
let width: u32 = img.width();
let height: u32 = img.height();
let img_data: Vec<u8> = img.clone().into_bytes();
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 window = create_window("image", Default::default()).unwrap();
let mut t: VecDeque<Duration> = VecDeque::with_capacity(25);
let params = HdbscanHyperParams::builder()
.epsilon(5.0)
.min_samples(5)
.min_cluster_size(100)
.build();
let mut now: Instant = Instant::now();
loop {
let start = Instant::now();
let poi = extract_highlights_and_distort(&img_data, width, 196, 0.05);
let clusterer = Hdbscan::new(&poi, params.clone());
let labels = clusterer.cluster().unwrap();
let mut rgb_image = RgbImage::new(width, height);
zip(poi, labels).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 x = (pixel[0] + width as f64 / 2.0) as u32;
for (image, labels) in clusters_rx {
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 x = (pixel[0] + 400f64) as u32;
rgb_image.put_pixel(x, pixel[1] as u32, color);
if pixel[0] > 0.0 {
rgb_image.put_pixel(x - 1, pixel[1] as u32, color);
}
if pixel[0] < width as f64 - 1.0 {
if pixel[0] < 399f64 {
rgb_image.put_pixel(x + 1, pixel[1] as u32, color);
}
if pixel[1] > 0.0 {
rgb_image.put_pixel(x, pixel[1] as u32 - 1, color);
}
if pixel[1] < height as f64 - 1.0 {
rgb_image.put_pixel(x, pixel[1] as u32 + 1, Rgb([label as u8 * 8, 0, 0]));
if pixel[1] < 399f64 {
rgb_image.put_pixel(x, pixel[1] as u32 + 1, Rgb([*label as u8 * 8, 0, 0]));
}
}
});
let image = ImageView::new(ImageInfo::new(PixelFormat::Rgb8, width, height), rgb_image.iter().as_slice());
let image = ImageView::new(ImageInfo::new(PixelFormat::Rgb8, 800, 800), rgb_image.iter().as_slice());
window.set_image("camera", image).unwrap();
if t.len() == 25 {
t.pop_front();
}
t.push_back(Instant::now() - start);
println!("Avg: {} FPS", 1.0 / (t.iter().map(|d| d.as_secs_f64()).sum::<f64>() / t.len() as f64));
t.push_back(now.elapsed());
println!("Avg: {:.6} s", (t.iter().map(|d| d.as_secs_f64()).sum::<f64>() / t.len() as f64));
now = Instant::now();
}
}