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

maybe x and y were swapped all along

Browse Source
This commit is contained in:
Timo Schneider
2025-05-06 19:42:12 +02:00
parent 85f718d578
commit e3034d8500
1 changed files with 23 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

39
src/main.rs
View File

@@ -9,8 +9,8 @@ use std::sync::mpsc::Receiver;
use std::sync::{mpsc, Arc};
use std::thread;
use std::time::{Duration, Instant};
use nalgebra::DMatrix;
use ndarray::{Array1, Array2};
use ndarray_linalg::Inverse;
fn create_extract_thread(image_rx: Receiver<Arc<Vec<u8>>>, width: u32, threshold: u8, downsample: f64) -> Receiver<Arc<Vec<Vec<f64>>>> {
let (tx, rx) = mpsc::sync_channel::<Arc<Vec<Vec<f64>>>>(1);
@@ -125,6 +125,7 @@ fn create_transform_thread(pointcloud_rx : Receiver<Arc<Vec<Vec<f64>>>>) -> Rece
vec![u_norm, v_norm]
})
.filter(|point| point[0].abs() <= 5.0)
.collect();
projection_tx.send(Arc::new(projection)).unwrap();
@@ -135,25 +136,25 @@ fn create_transform_thread(pointcloud_rx : Receiver<Arc<Vec<Vec<f64>>>>) -> Rece
fn create_mestimator_thread(lanes_rx: Receiver<Arc<[Vec<Vec<f64>>; 2]>>) -> Receiver<(Arc<Vec<f64>>, Arc<[Vec<Vec<f64>>; 2]>)> {
let (z_tx, z_rx) = mpsc::sync_channel::<(Arc<Vec<f64>>, Arc<[Vec<Vec<f64>>; 2]>)>(1);
let c = 0.2 * 2.3849;
let c = 0.1 * 2.3849;
thread::spawn(move || {
let mut z_start = vec![4.0, -2.0, 0.0, 0.0];
let mut z = vec![4.0, -2.0, 0.0, 0.0];
for lanes in lanes_rx {
let mut p = Vec::<Vec<f64>>::new();
p.append(&mut lanes[0].clone());
p.append(&mut lanes[1].clone());
let y = Array1::from_vec(p.iter().map(|point| point[1]).collect());
let y = Array1::from_vec(p.iter().map(|point| point[0]).collect());
let mut H: Array2<f64> = Array2::from_shape_vec((0, 4), Vec::<f64>::new()).unwrap();
lanes[0].clone().into_iter().map(|point| point[0]).for_each(|x| {
lanes[0].clone().into_iter().map(|point| point[1]).for_each(|x| {
let a = vec![0.5, -1.0, -x, 0.5*x.powi(2)];
let b: Array1<f64> = Array1::from_vec(a);
H.push_row(b.view()).unwrap()
});
lanes[1].clone().into_iter().map(|point| point[0]).for_each(|x| {
lanes[1].clone().into_iter().map(|point| point[1]).for_each(|x| {
let a = vec![-0.5, -1.0, -x, 0.5*x.powi(2)];
let b: Array1<f64> = Array1::from_vec(a);
H.push_row(b.view()).unwrap()
@@ -161,18 +162,16 @@ fn create_mestimator_thread(lanes_rx: Receiver<Arc<[Vec<Vec<f64>>; 2]>>) -> Rece
let H_t = H.t();
let mut z = z_start.clone();
for _ in 0..5 {
let res_l = lanes[0]
.iter()
.map(|point| point[1] - (0.5 * z[0] - z[1] - point[0] * z[2] + 0.5 * z[3] * point[0].powi(2)))
.map(|point| point[0] - (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 res_r = lanes[1]
.iter()
.map(|point| point[1] - (-0.5 * z[0] - z[1] - point[0] * z[2] + 0.5 * z[3] * point[0].powi(2)))
.map(|point| point[0] - (-0.5 * z[0] - z[1] - point[0] * z[2] + 0.5 * z[3] * point[1].powi(2)))
.map(|r| 1.0/(1.0 + (r/c).powi(2)))
.collect::<Vec<_>>();
@@ -182,8 +181,16 @@ fn create_mestimator_thread(lanes_rx: Receiver<Arc<[Vec<Vec<f64>>; 2]>>) -> Rece
let w = Array2::from_diag(&Array1::from_vec(v));
let first_part = H_t.dot(&w).dot(&H);
let second_part = first_part.inv().unwrap();
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();
}
@@ -239,14 +246,14 @@ fn main() {
(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;
let r = -0.5 * z[0] + const_part;
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]);
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]);
rgb_image.put_pixel((rgb_image.width() as f64/2.0 + r) as u32, rgb_image.height() - 1 - x, colors[1]);
}
});