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

optimized m_estimator

Browse Source
This commit is contained in:
Timo Schneider
2025-05-12 10:51:42 +02:00
parent 2f3f49a657
commit 952f4239d5
2 changed files with 11 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
Cargo.toml
View File

@@ -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"

29
src/pipeline/m_estimator.rs
View File

@@ -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();