From 952f4239d52f2eb813dba9bb18866850dc2b8aa1 Mon Sep 17 00:00:00 2001
From: Timo Schneider <timoschneider04@gmail.com>
Date: Mon, 12 May 2025 10:51:42 +0200
Subject: [PATCH] optimized m_estimator

---
 Cargo.toml                  |  3 +--
 src/pipeline/m_estimator.rs | 29 ++++++++++-------------------
 2 files changed, 11 insertions(+), 21 deletions(-)

diff --git a/Cargo.toml b/Cargo.toml
index aa40fc2..1c70b72 100644
--- a/Cargo.toml
+++ b/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"
\ No newline at end of file
diff --git a/src/pipeline/m_estimator.rs b/src/pipeline/m_estimator.rs
index 25ef19f..08f8e82 100644
--- a/src/pipeline/m_estimator.rs
+++ b/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();