From 85f718d578a5ed1533eeee2d3c2b20fe0543d3f2 Mon Sep 17 00:00:00 2001 From: Timo Schneider Date: Tue, 6 May 2025 16:32:46 +0200 Subject: [PATCH] wip: lab --- Cargo.toml | 2 + src/main.rs | 125 ++++++++++++++++++++++++++++++++++++++++------------ 2 files changed, 100 insertions(+), 27 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index eb198de..27e9a2e 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -9,6 +9,8 @@ opt-level = 2 # Maximum optimization for release [dependencies] image = "0.25.6" show-image = "0.14.1" +ndarray = "0.16.0" +ndarray-linalg = { version = "0.17.0" } rand = "0.9.1" rayon = "1.10.0" hdbscan = "0.10.0" \ No newline at end of file diff --git a/src/main.rs b/src/main.rs index 65c7c0f..0aa79fc 100644 --- a/src/main.rs +++ b/src/main.rs @@ -9,6 +9,8 @@ use std::sync::mpsc::Receiver; use std::sync::{mpsc, Arc}; use std::thread; use std::time::{Duration, Instant}; +use ndarray::{Array1, Array2}; +use ndarray_linalg::Inverse; fn create_extract_thread(image_rx: Receiver>>, width: u32, threshold: u8, downsample: f64) -> Receiver>>> { let (tx, rx) = mpsc::sync_channel::>>>(1); @@ -131,12 +133,70 @@ fn create_transform_thread(pointcloud_rx : Receiver>>>) -> Rece projection_rx } -fn _create_filter_thread() { +fn create_mestimator_thread(lanes_rx: Receiver>; 2]>>) -> Receiver<(Arc>, Arc<[Vec>; 2]>)> { + let (z_tx, z_rx) = mpsc::sync_channel::<(Arc>, Arc<[Vec>; 2]>)>(1); + let c = 0.2 * 2.3849; + thread::spawn(move || { + let mut z_start = vec![4.0, -2.0, 0.0, 0.0]; + + for lanes in lanes_rx { + let mut p = Vec::>::new(); + p.append(&mut lanes[0].clone()); + p.append(&mut lanes[1].clone()); + let y = Array1::from_vec(p.iter().map(|point| point[1]).collect()); + + let mut H: Array2 = Array2::from_shape_vec((0, 4), Vec::::new()).unwrap(); + + lanes[0].clone().into_iter().map(|point| point[0]).for_each(|x| { + let a = vec![0.5, -1.0, -x, 0.5*x.powi(2)]; + let b: Array1 = Array1::from_vec(a); + H.push_row(b.view()).unwrap() + }); + lanes[1].clone().into_iter().map(|point| point[0]).for_each(|x| { + let a = vec![-0.5, -1.0, -x, 0.5*x.powi(2)]; + let b: Array1 = Array1::from_vec(a); + H.push_row(b.view()).unwrap() + }); + + let H_t = H.t(); + + let mut z = z_start.clone(); + + for _ in 0..5 { + let res_l = lanes[0] + .iter() + .map(|point| point[1] - (0.5 * z[0] - z[1] - point[0] * z[2] + 0.5 * z[3] * point[0].powi(2))) + .map(|r| 1.0/(1.0 + (r/c).powi(2))) + .collect::>(); + + let res_r = lanes[1] + .iter() + .map(|point| point[1] - (-0.5 * z[0] - z[1] - point[0] * z[2] + 0.5 * z[3] * point[0].powi(2))) + .map(|r| 1.0/(1.0 + (r/c).powi(2))) + .collect::>(); + + let mut v = Vec::::new(); + v.append(&mut res_l.clone()); + v.append(&mut res_r.clone()); + + let w = Array2::from_diag(&Array1::from_vec(v)); + + let first_part = H_t.dot(&w).dot(&H); + let second_part = first_part.inv().unwrap(); + z = second_part.dot(&H_t).dot(&w).dot(&y).to_vec(); + } + + z_tx.send((Arc::new(z.clone()), lanes)).unwrap(); + } + }); + + z_rx } + fn dummy_image_spawner_thread() -> Receiver>> { - let img: DynamicImage = ImageReader::open("./images/lane_detection_loop_20.png").unwrap().decode().unwrap(); + let img: DynamicImage = ImageReader::open("./images/lane_detection_loop_80.png").unwrap().decode().unwrap(); let img_data: Vec = img.clone().into_bytes(); let (img_tx, img_rx) = mpsc::sync_channel::>>(1); @@ -157,44 +217,54 @@ fn main() { let projection_rx = create_transform_thread(pointcloud_rx); let cluster_labels_rx = create_pointcloud_labels_thread(projection_rx); let clusters_rx = create_cluster_separator_thread(cluster_labels_rx); - let lanes = create_isolate_lane_thread(clusters_rx); + let lanes_rx = create_isolate_lane_thread(clusters_rx); + let z_rx = create_mestimator_thread(lanes_rx); let window = create_window("image", Default::default()).unwrap(); let mut t: VecDeque = VecDeque::with_capacity(25); let mut now: Instant = Instant::now(); - for lanes in lanes { + //for z in z_rx {} + + + for (z, lanes) in z_rx { let mut rgb_image = RgbImage::new(800, 800); 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] * 15.0 + rgb_image.width() as f64 / 2.0) as u32; - let y = rgb_image.height() - (pixel[1] * 15.0) as u32; - if x > 799 || y > 799 {} - else { - rgb_image.put_pixel(x, y, color); - if x > 0 { - rgb_image.put_pixel(x - 1, y, color); - } - if x < 799 { - rgb_image.put_pixel(x + 1, y, color); - } - if y > 0 { - rgb_image.put_pixel(x, y - 1, color); - } - if y < 799 { - rgb_image.put_pixel(x, y + 1, color); - } - } + println!("{:?}", z); - }); - } + (0..rgb_image.height()).for_each(|x| { + let x_m = x as f64 * (45.0 / 800.0); + let const_part = - z[1] - x_m * z[2] + 0.5 * z[3] * x_m.powi(2); + let l = 0.5 * z[0] + const_part; + let r = -0.5 * z[0] + const_part; + if l.abs() < rgb_image.width() as f64 / 2.0 - 1.0 { + rgb_image.put_pixel((rgb_image.width() as f64 /2.0 - l) as u32, rgb_image.height() - 1 - x, colors[0]); + } + if r.abs() < rgb_image.width() as f64 / 2.0 - 1.0 { + rgb_image.put_pixel((rgb_image.width() as f64/2.0 - r) as u32, rgb_image.height() - 1 - x, colors[1]); + } + }); + + lanes[0].iter().for_each(|point| { + let x = point[0] * 800.0/45.0 + rgb_image.width() as f64 / 2.0; + let y = rgb_image.height() as f64 - 1.0 - point[1] * 800.0/45.0; + if x < rgb_image.width() as f64 && y < rgb_image.width() as f64 { + rgb_image.put_pixel(x as u32, y as u32, Rgb([0, 255, 255])); + } + }); + + lanes[1].iter().for_each(|point| { + let x = point[0] * 800.0/45.0 + rgb_image.width() as f64 / 2.0; + let y = rgb_image.height() as f64 - 1.0 - point[1] * 800.0/45.0; + if x < rgb_image.width() as f64 && y < rgb_image.width() as f64 { + rgb_image.put_pixel(x as u32, y as u32, Rgb([255, 255, 0])); + } + }); let image = ImageView::new(ImageInfo::new(PixelFormat::Rgb8, 800, 800), rgb_image.iter().as_slice()); window.set_image("camera", image).unwrap(); @@ -206,4 +276,5 @@ fn main() { println!("Avg: {:.6} s", t.iter().map(|d| d.as_secs_f64()).sum::() / t.len() as f64); now = Instant::now(); } + }