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

optimized highlight extractor

Browse Source
This commit is contained in:
Timo Schneider
2025-05-12 10:07:37 +02:00
parent f783a2e4f2
commit 2f3f49a657
1 changed files with 6 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
src/pipeline/highlight_extractor.rs
View File

@@ -2,7 +2,7 @@ use std::sync::{mpsc, Arc, Mutex};
use std::sync::mpsc::Receiver; use std::sync::mpsc::Receiver;
use std::thread; use std::thread;
use std::time::{Duration, Instant}; use std::time::{Duration, Instant};
use rand::Rng; use rand::{rng, Rng};
use r2r::sensor_msgs::msg::Image; use r2r::sensor_msgs::msg::Image;
pub type Pointcloud = Arc<Vec<Vec<f64>>>; pub type Pointcloud = Arc<Vec<Vec<f64>>>;
@@ -19,18 +19,16 @@ impl ExtractHighlights for Image {
]; ];
let max_rng = (u32::MAX as f64 * downsample) as u32; let max_rng = (u32::MAX as f64 * downsample) as u32;
let mut rng = rand::rng();
Arc::new(self.data Arc::new(self.data
.iter() .iter()
.enumerate() .enumerate()
.filter(|(i, _)| *i as u32 / self.width >= self.height / 2) .map(|(i, pixel)| (pixel, i as f64 % self.width as f64, i as f64 / self.width as f64))
.filter(|(_, pixel)| **pixel > threshold) .filter(|(pixel, _, _)| **pixel > threshold)
.filter(|_| { .filter(|_| {
let mut rng = rand::rng();
rng.random::<u32>() < max_rng rng.random::<u32>() < max_rng
}) })
.map(|(i, _)| { .map(|(_, x, y)| {
let x = i as f64 % self.width as f64;
let y = i as f64 / self.width as f64;
// Convert to homogeneous coordinates // Convert to homogeneous coordinates
let u = h_i[0][0] * x + h_i[0][1] * y + h_i[0][2]; let u = h_i[0][0] * x + h_i[0][1] * y + h_i[0][2];