optimized m_estimator

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/pipeline/highlight_extractor.rs

@@ -2,7 +2,7 @@ use std::sync::{mpsc, Arc, Mutex};
 use std::sync::mpsc::Receiver;
 use std::thread;
 use std::time::{Duration, Instant};
-use rand::Rng;
+use rand::{rng, Rng};
 use r2r::sensor_msgs::msg::Image;
 
 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 mut rng = rand::rng();
         Arc::new(self.data
             .iter()
             .enumerate() 
-            .filter(|(i, _)| *i as u32 / self.width >= self.height / 2)
-            .filter(|(_, pixel)| **pixel > threshold)
+            .map(|(i, pixel)| (pixel, i as f64 % self.width as f64, i as f64 / self.width as f64))
+            .filter(|(pixel, _, _)| **pixel > threshold)
             .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];

src/pipeline/m_estimator.rs

@@ -2,9 +2,9 @@ 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;
 
 pub type LaneEstimation = Arc<Vec<f64>>;
 pub fn create_mestimator_thread(lanes_rx: Receiver<PointcloudLabeled>) -> (Receiver<LaneEstimation>, Arc<Mutex<(Duration, Duration)>>) {
@@ -22,14 +22,15 @@ 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();
 
@@ -42,17 +43,7 @@ pub fn create_mestimator_thread(lanes_rx: Receiver<PointcloudLabeled>) -> (Recei
 
                 let w = Array2::from_diag(&Array1::from_vec(res));
 
-                let first_part: Array2<f64> = H_t.dot(&w).dot(&H);
-
-                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();
+                z = H_t.dot(&w).dot(&H).dot(&H_t).inv().unwrap().dot(&w).dot(&y).to_vec();
             }
 
             let end = Instant::now();