asd lab

Cargo.toml

@@ -10,11 +10,10 @@ opt-level = 2  # Maximum optimization for release
 image = "0.25.6"
 show-image = "0.14.1"
 ndarray = "0.16.0"
+ndarray-linalg = { version = "0.17.0", features = ["intel-mkl"] }
 rand = "0.9.1"
 rayon = "1.10.0"
 hdbscan = "0.10.0"
-nalgebra = "0.33.2"
 indicatif = "0.17.11"
 r2r = "0.9.5"
-futures-core = "0.3.31"
 futures = "0.3.31"

src/main.rs

@@ -2,25 +2,26 @@ mod pipeline;
 
 use rand::Rng;
 use rayon::prelude::*;
-use std::collections::VecDeque;
-use std::sync::{mpsc, Arc, Mutex};
+use std::sync::{mpsc, Arc};
 use std::thread;
-use std::time::{Duration, Instant};
+use std::time::{Duration};
 use indicatif::{MultiProgress, ProgressBar, ProgressStyle};
 use r2r::QosProfile;
 use futures::{executor::LocalPool, future, stream::StreamExt, task::LocalSpawnExt};
-use image::{ImageReader, Rgb, RgbImage};
-use show_image::{create_window, ImageInfo, ImageView, PixelFormat};
 use crate::pipeline::hdb_clusterer::create_pointcloud_labels_thread;
-use crate::pipeline::highlight_extractor::{create_extract_thread, Pointcloud};
+use crate::pipeline::highlight_extractor::{create_extract_thread};
 use crate::pipeline::m_estimator::create_mestimator_thread;
+use r2r::geometry_msgs::msg::Vector3;
 
 #[show_image::main]
 fn main() {
     let ctx = r2r::Context::create().unwrap();
     let mut node = r2r::Node::create(ctx, "node", "").unwrap();
     let subscriber = node.subscribe::<r2r::sensor_msgs::msg::Image>("/camera/front", QosProfile::default()).unwrap();
-    let mut timer = node.create_wall_timer(Duration::from_millis(1000)).unwrap();
+    let lane_left_pub = node.create_publisher::<r2r::nav_msgs::msg::Path>("/lane_detection/left", QosProfile::default()).unwrap();
+    let lane_right_pub = node.create_publisher::<r2r::nav_msgs::msg::Path>("/lane_detection/right", QosProfile::default()).unwrap();
+    let pcd_pub = node.create_publisher::<r2r::sensor_msgs::msg::PointCloud2>("/lane_detection/highlights", QosProfile::default()).unwrap();
+    let cmd_vel_pub = node.create_publisher::<r2r::geometry_msgs::msg::Twist>("/cmd_vel_in", QosProfile::default()).unwrap();
 
     let (img_tx, img_rx) = mpsc::sync_channel::<Arc<r2r::sensor_msgs::msg::Image>>(1);
 
@@ -28,9 +29,9 @@ fn main() {
     let mut pool = LocalPool::new();
     let spawner = pool.spawner();
 
-    let (pointcloud_rx, usage_extract) = create_extract_thread(img_rx, 196, 0.025);
+    let (pointcloud_rx, usage_extract) = create_extract_thread(img_rx, 32, 0.05, pcd_pub);
     let (cluster_labels_rx, usage_cluster) = create_pointcloud_labels_thread(pointcloud_rx);
-    let (z_rx, usage_detect) = create_mestimator_thread(cluster_labels_rx);
+    let (z_rx, usage_detect) = create_mestimator_thread(cluster_labels_rx, lane_left_pub, lane_right_pub);
 
     let multi_stats = MultiProgress::new();
     let p_style = ProgressStyle::default_bar().template("{prefix:15} {bar:50.gray/white} {pos:>6} us | {len:>6} us").unwrap();
@@ -40,17 +41,10 @@ fn main() {
 
     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_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 mut t: VecDeque<Duration> = VecDeque::with_capacity(25);
-    let mut now: Instant = Instant::now();
-
-    let img = ImageReader::open("./images/lane_detection_loop_80.png").unwrap().decode().unwrap();
-    // Run the subscriber in one task, printing the messages
     spawner.spawn_local(async move {
         subscriber
-            .for_each(|mut msg| {
-                msg.data = img.clone().into_bytes();
+            .for_each(|msg| {
                 img_tx.send(Arc::new(msg)).unwrap();
                 future::ready(())
             })
@@ -58,34 +52,18 @@ fn main() {
     }).unwrap();
 
     thread::spawn(move || {
-        let window = create_window("image", Default::default()).unwrap();
         for z in z_rx {
-            let mut rgb_image = RgbImage::new(800, 800);
-
-            let colors = vec![Rgb([0, 255, 0]), Rgb([255, 0, 0])];
-
-            (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) * (800.0/45.0);;
-                let r = (-0.5 * z[0] + const_part) * (800.0/45.0);;
-
-                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]);
-                }
-            });
-
-            let image = ImageView::new(ImageInfo::new(PixelFormat::Rgb8, 800, 800), rgb_image.iter().as_slice());
-            window.set_image("camera", image).unwrap();
-
+            let x_m = 1.0;
+            let cmd_vel = r2r::geometry_msgs::msg::Twist {
+                linear: Vector3 {x: 1.0, y: 0.0, z: 0.0},
+                angular: Vector3 {x: 0.0, y: 0.0, z: -2.0 * ((-z[1] - x_m * z[2] + 0.5 * z[3] * x_m.powi(2)) / x_m).atan()},
+            };
+            cmd_vel_pub.publish(&cmd_vel).unwrap();
         }
     });
 
     loop {
-        node.spin_once(Duration::from_millis(10));
+        node.spin_once(Duration::from_millis(16));
         pool.run_until_stalled();
 
         let u_extract = *usage_extract.lock().unwrap();
@@ -93,7 +71,7 @@ fn main() {
         let u_detect = *usage_detect.lock().unwrap();
 
         let u_vec = vec![u_extract, u_cluster, u_detect];
-        
+
         p_extract.set_length(u_extract.1.as_micros() as u64);
         p_extract.set_position(u_extract.0.as_micros() as u64);
         p_cluster.set_length(u_cluster.1.as_micros() as u64);
@@ -103,13 +81,6 @@ fn main() {
 
         p_max.set_position(u_vec.iter().map(|(t, d)| ((t.as_secs_f64() / d.as_secs_f64()) * 100.0) as u64).max().unwrap());
         p_avg_ms.set_position(u_vec.iter().map(|(t, _)| t).sum::<Duration>().as_micros() as u64);
-        p_framerate.set_position((1.0 / (t.iter().map(|d| d.as_secs_f64()).sum::<f64>() / t.len() as f64)) as u64);
-
-        if t.len() == 25 {
-            t.pop_front();
-        }
-        t.push_back(now.elapsed());
-        now = Instant::now();
     }
 
 }

src/pipeline/hdb_clusterer.rs

@@ -26,7 +26,7 @@ impl SortLabeled for PointcloudLabeled {
             if !averages.contains_key(label) {
                 averages.insert(*label, Vec::new());
             }
-            averages.get_mut(label).unwrap().push(point[1]);
+            averages.get_mut(label).unwrap().push(point[0]);
         });
 
         let mut average: Vec<(i32, f64)> = averages.into_iter().map(|(label, y_values)| (label, y_values.iter().sum::<f64>() / y_values.len() as f64)).collect();
@@ -43,22 +43,20 @@ impl SortLabeled for PointcloudLabeled {
 
 impl Label for Pointcloud {
     fn label(&self) -> Option<PointcloudLabeled> {
-        if self.len() < 75 { return None }
+        if self.len() < 125 { return None }
 
         let params = HdbscanHyperParams::builder()
             .epsilon(10000.0)
             .min_samples(3)
             .allow_single_cluster(false)
-            .min_cluster_size(20)
+            .min_cluster_size(10)
             .build();
 
-        let labels = Hdbscan::new(self, params).cluster().unwrap();
-
-        let pointcloud_labeled: PointcloudLabeled = Arc::new(zip(self.iter(), labels)
-            .map(|(point, label)| (point.clone(), label))
-            .collect());
-
-        Some(pointcloud_labeled)
+        if let Ok(labels) = Hdbscan::new(self, params).cluster() {
+            Some(Arc::new(zip(self.iter(), labels)
+                .map(|(point, label)| (point.clone(), label))
+                .collect()))
+        } else { None }
     }
 }
 

src/pipeline/highlight_extractor.rs

@@ -2,35 +2,42 @@ use std::sync::{mpsc, Arc, Mutex};
 use std::sync::mpsc::Receiver;
 use std::thread;
 use std::time::{Duration, Instant};
-use rand::Rng;
+use r2r::Publisher;
+use rand::{rng, Rng};
 use r2r::sensor_msgs::msg::Image;
+use r2r::sensor_msgs::msg::{PointField, PointCloud2};
+use r2r::builtin_interfaces::msg::Time;
+use r2r::std_msgs::msg::Header;
 
 pub type Pointcloud = Arc<Vec<Vec<f64>>>;
 
 trait ExtractHighlights {
     fn get_highlights(&self, threshold: u8, downsample: f64) -> Pointcloud;
 }
+
+trait Visualize{
+    fn visualize(&self, pub_pcd: &Publisher<PointCloud2>);
+}
+
 impl ExtractHighlights for Image {
     fn get_highlights(&self, threshold: u8, downsample: f64) -> Pointcloud {
         let h_i = [
-            [-7.74914499e-03, 3.95733793e-18, 3.10353257e00],
-            [8.56519716e-18, 9.42313768e-05, -1.86052093e00],
-            [2.57498016e-18, -2.73825295e-03, 1.00000000e00],
+            [-1.31918135e-20, -1.55163853e-06,  2.44274454e-02],
+            [-5.89622642e-05, -3.10327706e-07,  2.37534136e-02],
+            [-5.57265378e-21,  6.51688183e-05, -2.59527061e-02],
         ];
-        
+
         let max_rng = (u32::MAX as f64 * downsample) as u32;
+        let mut rng = rand::rng();
         Arc::new(self.data
             .iter()
-            .enumerate()
-            .filter(|(i, _)| *i as u32 / self.width >= self.height / 2)
-            .filter(|(_, pixel)| **pixel > threshold)
+            .enumerate() 
+            .map(|(i, pixel)| (pixel, i as f64 % self.width as f64, i as f64 / self.width as f64))
+            .filter(|(pixel, _, h)| **pixel > threshold && **pixel < 100 && *h > self.height as f64 * 0.4)
             .filter(|_| {
-                let mut rng = rand::rng();
                 rng.random::<u32>() < max_rng
             })
-            .map(|(i, _)| {
-                 let x = i as f64 % self.width as f64;
-                 let y = i as f64 / self.width as f64;
+            .map(|(_, x, y)| {
 
                  // Convert to homogeneous coordinates
                  let u = h_i[0][0] * x + h_i[0][1] * y + h_i[0][2];
@@ -38,15 +45,53 @@ impl ExtractHighlights for Image {
                  let w = h_i[2][0] * x + h_i[2][1] * y + h_i[2][2];
 
                  let u_norm = u / w;
-                 let v_norm = v / w;
+                 let v_norm = -v / w;
 
-                 vec![u_norm, v_norm]
+                 vec![v_norm, u_norm]
             })
-            //.filter(|point| point[0].abs() <= 5.0)
+            .filter(|point| point[0].abs() <= 1.5 && point[1] <= 3.0)
             .collect())
     }
 }
-pub fn create_extract_thread(image_rx: Receiver<Arc<Image>>, threshold: u8, downsample: f64) -> (Receiver<Pointcloud>, Arc<Mutex<(Duration, Duration)>>) {
+
+impl Visualize for Pointcloud {
+    fn visualize(&self, pub_pcd: &Publisher<PointCloud2>) {
+        let header = Header {
+            stamp: Time {
+                sec: 0,
+                nanosec: 0,
+            },
+            frame_id: "front_camera_link".to_string(),
+        };
+
+        let pcd_bytes = self.
+            iter()
+            .flat_map(|point| vec![point[1], -point[0], -0.7])
+            .map(|f| (f as f32).to_le_bytes())
+            .flatten()
+            .collect::<Vec<u8>>();
+
+        let msg = PointCloud2 {
+            header,
+            height: 1,
+            width: self.len() as u32,
+            fields: vec![
+                PointField { name: "x".to_string(), offset: 0, datatype: 7, count: 1 },
+                PointField { name: "y".to_string(), offset: 4, datatype: 7, count: 1 },
+                PointField { name: "z".to_string(), offset: 8, datatype: 7, count: 1 },
+            ],
+            is_bigendian: false,
+            point_step: 12,
+            row_step: 12,
+            data: pcd_bytes,
+            is_dense: true,
+        };
+
+        pub_pcd.publish(&msg).unwrap();
+    }
+}
+
+pub fn create_extract_thread(image_rx: Receiver<Arc<Image>>, threshold: u8, downsample: f64, pub_pcd: Publisher<PointCloud2>) -> (Receiver<Pointcloud>, Arc<Mutex<(Duration, Duration)>>) {
     let (tx, rx) = mpsc::sync_channel::<Pointcloud>(1);
     let usage = Arc::new(Mutex::new((Duration::ZERO, Duration::ZERO)));
     let local_usage = usage.clone();
@@ -57,9 +102,10 @@ pub fn create_extract_thread(image_rx: Receiver<Arc<Image>>, threshold: u8, down
             let start = Instant::now();
             let poi = image.get_highlights(threshold, downsample);
             let end = Instant::now();
-            
+
+            poi.visualize(&pub_pcd);
             tx.send(poi).unwrap();
-            
+
             *local_usage.lock().unwrap() = (end - start, t_loop.elapsed());
             t_loop = Instant::now();
         }

src/pipeline/m_estimator.rs

@@ -2,12 +2,59 @@ use std::sync::{mpsc, Arc, Mutex};
 use std::sync::mpsc::Receiver;
 use std::thread;
 use std::time::{Duration, Instant};
-use nalgebra::DMatrix;
 use ndarray::{Array1, Array2};
 use crate::pipeline::hdb_clusterer::PointcloudLabeled;
+use ndarray_linalg::Inverse;
+use r2r::builtin_interfaces::msg::Time;
+use r2r::std_msgs::msg::Header;
+use r2r::nav_msgs::msg::Path;
+use r2r::geometry_msgs::msg::{Point, Pose, PoseStamped, Quaternion};
+use r2r::Publisher;
 
 pub type LaneEstimation = Arc<Vec<f64>>;
-pub fn create_mestimator_thread(lanes_rx: Receiver<PointcloudLabeled>) -> (Receiver<LaneEstimation>, Arc<Mutex<(Duration, Duration)>>) {
+
+trait Visualize {
+    fn visualize(&self, pub_left: &Publisher<Path>, pub_right: &Publisher<Path>);
+}
+
+impl Visualize for Vec<f64> {
+    fn visualize(&self, pub_left: &Publisher<Path>, pub_right: &Publisher<Path>) {
+        let mut lane_left = Path {
+            header: Header {stamp: Time {sec: 0, nanosec: 0}, frame_id: "front_camera_link".to_string()},
+            poses: Vec::<PoseStamped>::new()
+        };
+
+        let mut right_lane = Path {
+            header: Header {stamp: Time {sec: 0, nanosec: 0}, frame_id: "front_camera_link".to_string()},
+            poses: Vec::<PoseStamped>::new()
+        };
+
+
+        (10..250).for_each(|x| {
+            let x_m = x as f64 / 10.0;
+            let const_part = - self[1] - x_m * self[2] + 0.5 * self[3] * x_m.powi(2);
+            let l = 0.5 * self[0]  + const_part;
+            let r = -0.5 * self[0] + const_part;
+
+            if l.abs() < 10.0 {
+                lane_left.poses.push(PoseStamped {header: lane_left.header.clone(), pose: Pose {
+                    position: Point {x: x_m, y: -l, z: -0.7},
+                    orientation: Quaternion {x: 0.0, y: 0.0, z: 0.0, w: 0.0}
+                }})};
+
+            if r.abs() < 10.0 {
+                right_lane.poses.push(PoseStamped {header: lane_left.header.clone(), pose: Pose {
+                    position: Point {x: x_m, y: -r, z: -0.7},
+                    orientation: Quaternion {x: 0.0, y: 0.0, z: 0.0, w: 0.0}
+                }})};
+        });
+
+        pub_left.publish(&lane_left).unwrap();
+        pub_right.publish(&right_lane).unwrap();
+    }
+}
+
+pub fn create_mestimator_thread(lanes_rx: Receiver<PointcloudLabeled>, pub_left: Publisher<Path>, pub_right: Publisher<Path>) -> (Receiver<LaneEstimation>, Arc<Mutex<(Duration, Duration)>>) {
     let (z_tx, z_rx) = mpsc::sync_channel::<LaneEstimation>(1);
     let usage = Arc::new(Mutex::new((Duration::ZERO, Duration::ZERO)));
     let local_usage = usage.clone();
@@ -22,42 +69,39 @@ pub fn create_mestimator_thread(lanes_rx: Receiver<PointcloudLabeled>) -> (Recei
 
             let y = Array1::from_vec(lanes.iter().map(|(point, label)| point[0]).collect());
 
-            let mut H: Array2<f64> = Array2::from_shape_vec((0, 4), Vec::<f64>::new()).unwrap();
+            let mut H: Array2<f64> = Array2::zeros((lanes.len(), 4));
 
-            lanes.iter().for_each(|(point, label)| {
-                let a= vec![if *label == 0 {0.5} else {-0.5}, -1.0, -point[1], 0.5*point[1].powi(2)];
-        
-                let b: Array1<f64> = Array1::from_vec(a);
-                H.push_row(b.view()).unwrap()
-            });
+            for (i, (point, label)) in lanes.iter().enumerate() {
+                let y = point[1];
+                H[[i, 0]] = if *label == 0 { 0.5 } else { -0.5 };
+                H[[i, 1]] = -1.0;
+                H[[i, 2]] = -y;
+                H[[i, 3]] = 0.5 * y.powi(2);
+            }
 
             let H_t = H.t();
 
             for _ in 0..3 {
-                let res = lanes
-                    .iter()
-                    .map(|(point, label)| point[0] - (if *label == 0 {0.5} else {-0.5} * z[0] - z[1] - point[1] * z[2] + 0.5 * z[3] * point[1].powi(2)))
-                    .map(|r| 1.0/(1.0 + (r/c).powi(2)))
-                    .collect::<Vec<_>>();
+                let mut w = Array2::zeros((lanes.len(), lanes.len()));
 
-                let w = Array2::from_diag(&Array1::from_vec(res));
+                for (i, (point, label)) in lanes.iter().enumerate() {
+                    let r = point[0] - (if *label == 0 { 0.5 } else { -0.5 } * z[0] - z[1] - point[1] * z[2] + 0.5 * z[3] * point[1].powi(2));
+                    let diag_value = 1.0 / (1.0 + (r / c).powi(2));
 
-                let first_part: Array2<f64> = H_t.dot(&w).dot(&H);
+                    w[[i, i]] = diag_value;
+                }
 
-                let a_nalgebra = DMatrix::from_row_slice(4, 4, first_part.as_slice().unwrap());
-
-                // Perform the matrix inversion using nalgebra
-                let inv = a_nalgebra.pseudo_inverse(0.0).unwrap();
-
-                // Convert the nalgebra matrix back to Array2<f64>
-                let second_part: Array2<f64> = Array2::from_shape_vec((4, 4), inv.as_slice().to_vec()).unwrap();
-
-                z = second_part.dot(&H_t).dot(&w).dot(&y).to_vec();
+                if let Ok(first_part) = H_t.dot(&w).dot(&H).inv() {
+                    z = first_part.dot(&H_t).dot(&w).dot(&y).to_vec();
+                }
             }
 
             let end = Instant::now();
             z_tx.send(Arc::new(z.clone())).unwrap();
             *local_usage.lock().unwrap() = (end - start, t_loop.elapsed());
+
+            z.visualize(&pub_left, &pub_right);
+
             t_loop = Instant::now();
         }
     });