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

fixed stats

Browse Source
This commit is contained in:
Timo Schneider
2025-05-07 10:26:22 +02:00
parent 7aa8c0d139
commit bd0fc4c802
1 changed files with 57 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

90
src/main.rs
View File

@@ -8,19 +8,22 @@ use std::iter::zip;
use std::sync::mpsc::Receiver; use std::sync::mpsc::Receiver;
use std::sync::{mpsc, Arc, Mutex}; use std::sync::{mpsc, Arc, Mutex};
use std::thread; use std::thread;
use std::thread::sleep;
use std::time::{Duration, Instant}; use std::time::{Duration, Instant};
use indicatif::{MultiProgress, ProgressBar, ProgressStyle}; use indicatif::{MultiProgress, ProgressBar, ProgressStyle};
use nalgebra::{partial_sort2, DMatrix}; use nalgebra::{DMatrix};
use ndarray::{Array1, Array2}; use ndarray::{Array1, Array2};
fn create_extract_thread(image_rx: Receiver<Arc<Vec<u8>>>, width: u32, threshold: u8, downsample: f64) -> (Receiver<Arc<Vec<Vec<f64>>>>, Arc<Mutex<f64>>) { fn create_extract_thread(image_rx: Receiver<Arc<Vec<u8>>>, width: u32, threshold: u8, downsample: f64) -> (Receiver<Arc<Vec<Vec<f64>>>>, Arc<Mutex<(Duration, Duration)>>) {
let (tx, rx) = mpsc::sync_channel::<Arc<Vec<Vec<f64>>>>(1); let (tx, rx) = mpsc::sync_channel::<Arc<Vec<Vec<f64>>>>(1);
let usage = Arc::new(Mutex::new(0.0)); let usage = Arc::new(Mutex::new((Duration::ZERO, Duration::ZERO)));
let local_usage = usage.clone(); let local_usage = usage.clone();
thread::spawn(move || { thread::spawn(move || {
let mut t_loop = Instant::now();
for image in image_rx { for image in image_rx {
let start = Instant::now(); let start = Instant::now();
let max_rng = (u32::MAX as f64 * downsample) as u32; let max_rng = (u32::MAX as f64 * downsample) as u32;
let poi = image let poi = image
.par_iter() .par_iter()
@@ -38,18 +41,20 @@ fn create_extract_thread(image_rx: Receiver<Arc<Vec<u8>>>, width: u32, threshold
vec.par_extend(poi); vec.par_extend(poi);
let end = Instant::now(); let end = Instant::now();
tx.send(Arc::new(vec)).expect("TODO: panic message"); tx.send(Arc::new(vec)).expect("TODO: panic message");
*local_usage.lock().unwrap() = (end - start).as_secs_f64() / start.elapsed().as_secs_f64(); *local_usage.lock().unwrap() = (end - start, t_loop.elapsed());
t_loop = Instant::now();
} }
}); });
(rx, usage) (rx, usage)
} }
fn create_pointcloud_labels_thread(pointcloud_rx: Receiver<Arc<Vec<Vec<f64>>>>) -> (Receiver<(Arc<Vec<Vec<f64>>>, Arc<Vec<i32>>)>, Arc<Mutex<f64>>) { fn create_pointcloud_labels_thread(pointcloud_rx: Receiver<Arc<Vec<Vec<f64>>>>) -> (Receiver<(Arc<Vec<Vec<f64>>>, Arc<Vec<i32>>)>, Arc<Mutex<(Duration, Duration)>>) {
let (pointcloud_labels_tx, pointcloud_labels_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);
let usage = Arc::new(Mutex::new(0.0)); let usage = Arc::new(Mutex::new((Duration::ZERO, Duration::ZERO)));
let local_usage = usage.clone(); let local_usage = usage.clone();
thread::spawn(move || { thread::spawn(move || {
let mut t_loop = Instant::now();
for pointcloud in pointcloud_rx { for pointcloud in pointcloud_rx {
let start = Instant::now(); let start = Instant::now();
let params = HdbscanHyperParams::builder() let params = HdbscanHyperParams::builder()
@@ -61,21 +66,24 @@ fn create_pointcloud_labels_thread(pointcloud_rx: Receiver<Arc<Vec<Vec<f64>>>>)
let labels = Arc::new(Hdbscan::new(&pointcloud, params).cluster().unwrap()); let labels = Arc::new(Hdbscan::new(&pointcloud, params).cluster().unwrap());
let end = Instant::now(); let end = Instant::now();
pointcloud_labels_tx.send((pointcloud, labels)).unwrap(); pointcloud_labels_tx.send((pointcloud, labels)).unwrap();
*local_usage.lock().unwrap() = (end - start).as_secs_f64() / start.elapsed().as_secs_f64(); *local_usage.lock().unwrap() = (end - start, t_loop.elapsed());
t_loop = Instant::now();
} }
}); });
(pointcloud_labels_rx, usage) (pointcloud_labels_rx, usage)
} }
fn create_cluster_separator_thread(cluster_labels: Receiver<(Arc<Vec<Vec<f64>>>, Arc<Vec<i32>>)>) -> (Receiver<Arc<HashMap<i32, Vec<Vec<f64>>>>>, Arc<Mutex<f64>>) { fn create_cluster_separator_thread(cluster_labels: Receiver<(Arc<Vec<Vec<f64>>>, Arc<Vec<i32>>)>) -> (Receiver<Arc<HashMap<i32, Vec<Vec<f64>>>>>, Arc<Mutex<(Duration, Duration)>>) {
let (clusters_tx, clusters_rx) = mpsc::sync_channel::<Arc<HashMap<i32, Vec<Vec<f64>>>>>(1); let (clusters_tx, clusters_rx) = mpsc::sync_channel::<Arc<HashMap<i32, Vec<Vec<f64>>>>>(1);
let usage = Arc::new(Mutex::new(0.0)); let usage = Arc::new(Mutex::new((Duration::ZERO, Duration::ZERO)));
let local_usage = usage.clone(); let local_usage = usage.clone();
thread::spawn(move || { thread::spawn(move || {
let mut t_loop = Instant::now();
for (pointcloud, labels) in cluster_labels { for (pointcloud, labels) in cluster_labels {
let start = Instant::now(); let start = Instant::now();
let mut clusters = HashMap::<i32, Vec<Vec<f64>>>::with_capacity(5); let mut clusters = HashMap::<i32, Vec<Vec<f64>>>::with_capacity(5);
let _ = zip(pointcloud.iter(), labels.iter()).for_each(|(point, label)| { let _ = zip(pointcloud.iter(), labels.iter()).for_each(|(point, label)| {
@@ -86,21 +94,24 @@ fn create_cluster_separator_thread(cluster_labels: Receiver<(Arc<Vec<Vec<f64>>>,
let end = Instant::now(); let end = Instant::now();
clusters_tx.send(Arc::new(clusters)).unwrap(); clusters_tx.send(Arc::new(clusters)).unwrap();
*local_usage.lock().unwrap() = (end - start).as_secs_f64() / start.elapsed().as_secs_f64(); *local_usage.lock().unwrap() = (end - start, t_loop.elapsed());
t_loop = Instant::now();
} }
}); });
(clusters_rx, usage) (clusters_rx, usage)
} }
fn create_isolate_lane_thread(clusters_rx: Receiver<Arc<HashMap<i32, Vec<Vec<f64>>>>>) -> (Receiver<Arc<[Vec<Vec<f64>>; 2]>>, Arc<Mutex<f64>>) { fn create_isolate_lane_thread(clusters_rx: Receiver<Arc<HashMap<i32, Vec<Vec<f64>>>>>) -> (Receiver<Arc<[Vec<Vec<f64>>; 2]>>, Arc<Mutex<(Duration, Duration)>>) {
let (lanes_tx, lanes_rx) = mpsc::sync_channel::<Arc<[Vec<Vec<f64>>; 2]>>(1); let (lanes_tx, lanes_rx) = mpsc::sync_channel::<Arc<[Vec<Vec<f64>>; 2]>>(1);
let usage = Arc::new(Mutex::new(0.0)); let usage = Arc::new(Mutex::new((Duration::ZERO, Duration::ZERO)));
let local_usage = usage.clone(); let local_usage = usage.clone();
thread::spawn(move || { thread::spawn(move || {
let mut t_loop = Instant::now();
for clusters in clusters_rx { for clusters in clusters_rx {
let start = Instant::now(); let start = Instant::now();
let mut averages = clusters.iter().filter(|(label, _)| **label != -1).map(|(label, cluster)| { 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)) (*label, cluster.iter().map(|point| point[0]).sum::<f64>() / (cluster.len() as f64))
}).collect::<Vec<(i32, f64)>>(); }).collect::<Vec<(i32, f64)>>();
@@ -110,7 +121,8 @@ fn create_isolate_lane_thread(clusters_rx: Receiver<Arc<HashMap<i32, Vec<Vec<f64
if averages.len() < 2 {continue;} if averages.len() < 2 {continue;}
let end = Instant::now(); let end = Instant::now();
lanes_tx.send(Arc::new([clusters.get(&averages[0].0).unwrap().clone(), clusters.get(&averages[1].0 ).unwrap().clone()])).unwrap() ; lanes_tx.send(Arc::new([clusters.get(&averages[0].0).unwrap().clone(), clusters.get(&averages[1].0 ).unwrap().clone()])).unwrap() ;
*local_usage.lock().unwrap() = (end - start).as_secs_f64() / start.elapsed().as_secs_f64(); *local_usage.lock().unwrap() = (end - start, t_loop.elapsed());
t_loop = Instant::now();
} }
}); });
@@ -118,9 +130,9 @@ fn create_isolate_lane_thread(clusters_rx: Receiver<Arc<HashMap<i32, Vec<Vec<f64
} }
fn create_transform_thread(pointcloud_rx : Receiver<Arc<Vec<Vec<f64>>>>) -> (Receiver<Arc<Vec<Vec<f64>>>>, Arc<Mutex<f64>>) { fn create_transform_thread(pointcloud_rx : Receiver<Arc<Vec<Vec<f64>>>>) -> (Receiver<Arc<Vec<Vec<f64>>>>, Arc<Mutex<(Duration, Duration)>>) {
let (projection_tx, projection_rx) = mpsc::sync_channel::<Arc<Vec<Vec<f64>>>>(1); let (projection_tx, projection_rx) = mpsc::sync_channel::<Arc<Vec<Vec<f64>>>>(1);
let usage = Arc::new(Mutex::new(0.0)); let usage = Arc::new(Mutex::new((Duration::ZERO, Duration::ZERO)));
let local_usage = usage.clone(); let local_usage = usage.clone();
thread::spawn(move || { thread::spawn(move || {
@@ -130,6 +142,8 @@ fn create_transform_thread(pointcloud_rx : Receiver<Arc<Vec<Vec<f64>>>>) -> (Rec
[2.57498016e-18, -2.73825295e-03, 1.00000000e00], [2.57498016e-18, -2.73825295e-03, 1.00000000e00],
]; ];
let mut t_loop = Instant::now();
for pointcloud in pointcloud_rx { for pointcloud in pointcloud_rx {
let start = Instant::now(); let start = Instant::now();
let projection = pointcloud let projection = pointcloud
@@ -153,24 +167,27 @@ fn create_transform_thread(pointcloud_rx : Receiver<Arc<Vec<Vec<f64>>>>) -> (Rec
let end = Instant::now(); let end = Instant::now();
projection_tx.send(Arc::new(projection)).unwrap(); projection_tx.send(Arc::new(projection)).unwrap();
*local_usage.lock().unwrap() = (end - start).as_secs_f64() / start.elapsed().as_secs_f64(); *local_usage.lock().unwrap() = (end - start, t_loop.elapsed());
t_loop = Instant::now();
} }
}); });
(projection_rx, usage) (projection_rx, usage)
} }
fn create_mestimator_thread(lanes_rx: Receiver<Arc<[Vec<Vec<f64>>; 2]>>) -> (Receiver<(Arc<Vec<f64>>, Arc<[Vec<Vec<f64>>; 2]>)>, Arc<Mutex<f64>>) { fn create_mestimator_thread(lanes_rx: Receiver<Arc<[Vec<Vec<f64>>; 2]>>) -> (Receiver<(Arc<Vec<f64>>, Arc<[Vec<Vec<f64>>; 2]>)>, Arc<Mutex<(Duration, Duration)>>) {
let (z_tx, z_rx) = mpsc::sync_channel::<(Arc<Vec<f64>>, Arc<[Vec<Vec<f64>>; 2]>)>(1); let (z_tx, z_rx) = mpsc::sync_channel::<(Arc<Vec<f64>>, Arc<[Vec<Vec<f64>>; 2]>)>(1);
let usage = Arc::new(Mutex::new(0.0)); let usage = Arc::new(Mutex::new((Duration::ZERO, Duration::ZERO)));
let local_usage = usage.clone(); let local_usage = usage.clone();
let c = 0.1 * 2.3849; let c = 0.1 * 2.3849;
thread::spawn(move || { thread::spawn(move || {
let mut z = vec![4.0, -2.0, 0.0, 0.0]; let mut z = vec![4.0, -2.0, 0.0, 0.0];
let mut t_loop = Instant::now();
for lanes in lanes_rx { for lanes in lanes_rx {
let start = Instant::now(); let start = Instant::now();
let mut p = Vec::<Vec<f64>>::new(); let mut p = Vec::<Vec<f64>>::new();
p.append(&mut lanes[0].clone()); p.append(&mut lanes[0].clone());
p.append(&mut lanes[1].clone()); p.append(&mut lanes[1].clone());
@@ -225,7 +242,8 @@ fn create_mestimator_thread(lanes_rx: Receiver<Arc<[Vec<Vec<f64>>; 2]>>) -> (Rec
let end = Instant::now(); let end = Instant::now();
z_tx.send((Arc::new(z.clone()), lanes)).unwrap(); z_tx.send((Arc::new(z.clone()), lanes)).unwrap();
*local_usage.lock().unwrap() = (end - start).as_secs_f64() / start.elapsed().as_secs_f64(); *local_usage.lock().unwrap() = (end - start, t_loop.elapsed());
t_loop = Instant::now();
} }
}); });
@@ -242,6 +260,7 @@ fn dummy_image_spawner_thread() -> Receiver<Arc<Vec<u8>>> {
thread::spawn(move || { thread::spawn(move || {
loop { loop {
img_tx.send(Arc::new(img_data.clone())).unwrap(); img_tx.send(Arc::new(img_data.clone())).unwrap();
sleep(Duration::from_millis(16));
} }
}); });
@@ -259,7 +278,7 @@ fn main() {
let (z_rx, usage_detect) = create_mestimator_thread(lanes_rx); let (z_rx, usage_detect) = create_mestimator_thread(lanes_rx);
let multi_stats = MultiProgress::new(); let multi_stats = MultiProgress::new();
let p_style = ProgressStyle::default_bar().template("{prefix:15} {bar:50.gray/white} {percent:>3}%").unwrap(); let p_style = ProgressStyle::default_bar().template("{prefix:15} {bar:50.gray/white} {pos:>6} us | {len:>6} us").unwrap();
let p_extract = multi_stats.add(ProgressBar::new(100).with_prefix("Extract").with_style(p_style.clone())); let p_extract = multi_stats.add(ProgressBar::new(100).with_prefix("Extract").with_style(p_style.clone()));
let p_transform = multi_stats.add(ProgressBar::new(100).with_prefix("Transform").with_style(p_style.clone())); let p_transform = multi_stats.add(ProgressBar::new(100).with_prefix("Transform").with_style(p_style.clone()));
let p_cluster = multi_stats.add(ProgressBar::new(100).with_prefix("Cluster").with_style(p_style.clone())); let p_cluster = multi_stats.add(ProgressBar::new(100).with_prefix("Cluster").with_style(p_style.clone()));
@@ -268,7 +287,7 @@ fn main() {
let p_detect = multi_stats.add(ProgressBar::new(100).with_prefix("Detect").with_style(p_style.clone())); let p_detect = multi_stats.add(ProgressBar::new(100).with_prefix("Detect").with_style(p_style.clone()));
let p_max = multi_stats.add(ProgressBar::new(100).with_prefix("Pipeline load").with_style(ProgressStyle::default_bar().template("{prefix:15} {bar:50.red/white} {percent:>3}%").unwrap())); let p_max = multi_stats.add(ProgressBar::new(100).with_prefix("Pipeline load").with_style(ProgressStyle::default_bar().template("{prefix:15} {bar:50.red/white} {percent:>3}%").unwrap()));
let p_avg_ms = multi_stats.add(ProgressBar::new(100).with_prefix("Pipeline delay").with_style(ProgressStyle::default_bar().template("{prefix:15} {bar:50.cyan/white} {pos:>3} ms").unwrap())); let p_avg_ms = multi_stats.add(ProgressBar::new(100_000).with_prefix("Pipeline delay").with_style(ProgressStyle::default_bar().template("{prefix:15} {bar:50.cyan/white} {pos:>6} us").unwrap()));
let p_framerate = multi_stats.add(ProgressBar::new(150).with_prefix("Framerate").with_style(ProgressStyle::default_bar().template("{prefix:15} {bar:50.blue/white} {pos:>3} FPS").unwrap())); let p_framerate = multi_stats.add(ProgressBar::new(150).with_prefix("Framerate").with_style(ProgressStyle::default_bar().template("{prefix:15} {bar:50.blue/white} {pos:>3} FPS").unwrap()));
let window = create_window("image", Default::default()).unwrap(); let window = create_window("image", Default::default()).unwrap();
@@ -288,19 +307,24 @@ fn main() {
let u_detect = *usage_detect.lock().unwrap(); let u_detect = *usage_detect.lock().unwrap();
let u_vec = vec![u_extract, u_transform, u_cluster, u_split, u_isolate, u_detect]; let u_vec = vec![u_extract, u_transform, u_cluster, u_split, u_isolate, u_detect];
let u_total: f64 = u_vec.iter().sum(); let u_total: f64 = u_vec.iter().map(|(t, _)| t.as_secs_f64()).sum();
p_extract.set_position((u_extract / u_total * 100.0) as u64); p_extract.set_length(u_extract.1.as_micros() as u64);
p_transform.set_position((u_transform / u_total * 100.0) as u64); p_extract.set_position(u_extract.0.as_micros() as u64);
p_cluster.set_position((u_cluster / u_total * 100.0) as u64); p_transform.set_length(u_transform.1.as_micros() as u64);
p_split.set_position((u_split / u_total * 100.0) as u64); p_transform.set_position(u_transform.0.as_micros() as u64);
p_isolate.set_position((u_isolate / u_total * 100.0) as u64); p_cluster.set_length(u_cluster.1.as_micros() as u64);
p_detect.set_position((u_detect / u_total * 100.0) as u64); p_cluster.set_position(u_cluster.0.as_micros() as u64);
p_split.set_length(u_split.1.as_micros() as u64);
p_split.set_position(u_split.0.as_micros() as u64);
p_isolate.set_length(u_isolate.1.as_micros() as u64);
p_isolate.set_position(u_isolate.0.as_micros() as u64);
p_detect.set_length(u_detect.1.as_micros() as u64);
p_detect.set_position(u_detect.0.as_micros() as u64);
p_max.set_position((100.0 * *u_vec.iter().max_by(|a, b| a.partial_cmp(b).unwrap()).unwrap()) as u64); p_max.set_position(u_vec.iter().map(|(t, d)| ((t.as_secs_f64() / d.as_secs_f64()) * 100.0) as u64).max().unwrap());
let frame_s = t.iter().map(|d| d.as_secs_f64()).sum::<f64>() / t.len() as f64; p_avg_ms.set_position(u_vec.iter().map(|(t, _)| t).sum::<Duration>().as_micros() as u64);
p_avg_ms.set_position((frame_s * 1000.0 * u_total) as u64); p_framerate.set_position((1.0 / (t.iter().map(|d| d.as_secs_f64()).sum::<f64>() / t.len() as f64)) as u64);
p_framerate.set_position((1.0 / frame_s) as u64);
let mut rgb_image = RgbImage::new(800, 800); let mut rgb_image = RgbImage::new(800, 800);
@@ -337,7 +361,7 @@ fn main() {
}); });
let image = ImageView::new(ImageInfo::new(PixelFormat::Rgb8, 800, 800), 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(); //window.set_image("camera", image).unwrap();
if t.len() == 25 { if t.len() == 25 {
t.pop_front(); t.pop_front();