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

wip: lab

Browse Source
This commit is contained in:
Timo Schneider
2025-05-06 16:32:46 +02:00
parent a164201c9d
commit 85f718d578
2 changed files with 100 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
Cargo.toml
View File

@@ -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"

125
src/main.rs
View File

@@ -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<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);
@@ -131,12 +133,70 @@ fn create_transform_thread(pointcloud_rx : Receiver<Arc<Vec<Vec<f64>>>>) -> Rece
projection_rx
}
fn _create_filter_thread() {
fn create_mestimator_thread(lanes_rx: Receiver<Arc<[Vec<Vec<f64>>; 2]>>) -> Receiver<(Arc<Vec<f64>>, Arc<[Vec<Vec<f64>>; 2]>)> {
let (z_tx, z_rx) = mpsc::sync_channel::<(Arc<Vec<f64>>, Arc<[Vec<Vec<f64>>; 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::<Vec<f64>>::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<f64> = Array2::from_shape_vec((0, 4), Vec::<f64>::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<f64> = 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<f64> = 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::<Vec<_>>();
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::<Vec<_>>();
let mut v = Vec::<f64>::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<Arc<Vec<u8>>> {
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<u8> = img.clone().into_bytes();
let (img_tx, img_rx) = mpsc::sync_channel::<Arc<Vec<u8>>>(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<Duration> = 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::<f64>() / t.len() as f64);
now = Instant::now();
}
}