wip: lab

Cargo.toml

@@ -9,6 +9,8 @@ opt-level = 2  # Maximum optimization for release
 [dependencies]
 image = "0.25.6"
 show-image = "0.14.1"
+ndarray = "0.16.0"
+ndarray-linalg = { version = "0.17.0" }
 rand = "0.9.1"
 rayon = "1.10.0"
 hdbscan = "0.10.0"

src/main.rs

@@ -9,6 +9,8 @@ use std::sync::mpsc::Receiver;
 use std::sync::{mpsc, Arc};
 use std::thread;
 use std::time::{Duration, Instant};
+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);
@@ -131,12 +133,70 @@ fn create_transform_thread(pointcloud_rx : Receiver<Arc<Vec<Vec<f64>>>>) -> Rece
     projection_rx
 }
 
-fn _create_filter_thread() {
+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;
 
+    thread::spawn(move || {
+        let mut z_start = 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 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| {
+                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| {
+                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()
+            });
+
+            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(|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(|r| 1.0/(1.0 + (r/c).powi(2)))
+                    .collect::<Vec<_>>();
+
+                let mut v = Vec::<f64>::new();
+                v.append(&mut res_l.clone());
+                v.append(&mut res_r.clone());
+
+                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();
+                z = second_part.dot(&H_t).dot(&w).dot(&y).to_vec();
             }
 
+            z_tx.send((Arc::new(z.clone()), lanes)).unwrap();
+        }
+    });
+
+    z_rx
+}
+
+
 fn dummy_image_spawner_thread() -> Receiver<Arc<Vec<u8>>> {
-    let img: DynamicImage = ImageReader::open("./images/lane_detection_loop_20.png").unwrap().decode().unwrap();
+    let img: DynamicImage = ImageReader::open("./images/lane_detection_loop_80.png").unwrap().decode().unwrap();
     let img_data: Vec<u8> = img.clone().into_bytes();
 
     let (img_tx, img_rx) = mpsc::sync_channel::<Arc<Vec<u8>>>(1);
@@ -157,44 +217,54 @@ fn main() {
     let projection_rx = create_transform_thread(pointcloud_rx);
     let cluster_labels_rx = create_pointcloud_labels_thread(projection_rx);
     let clusters_rx = create_cluster_separator_thread(cluster_labels_rx);
-    let lanes = create_isolate_lane_thread(clusters_rx);
+    let lanes_rx = create_isolate_lane_thread(clusters_rx);
+    let z_rx = create_mestimator_thread(lanes_rx);
 
 
     let window = create_window("image", Default::default()).unwrap();
     let mut t: VecDeque<Duration> = VecDeque::with_capacity(25);
     let mut now: Instant = Instant::now();
 
-    for lanes in lanes {
+    //for z in z_rx {}
+
+
+    for (z, lanes) in z_rx {
 
         let mut rgb_image = RgbImage::new(800, 800);
 
         let colors = vec![Rgb([0, 255, 0]), Rgb([255, 0, 0])];
-        for (i, lane) in lanes.iter().enumerate() {
-            let color = colors[i];
-            let _ = lane.iter().for_each(|pixel| {
-                let x = (pixel[0] * 15.0 + rgb_image.width() as f64 / 2.0) as u32;
-                let y = rgb_image.height() - (pixel[1] * 15.0) as u32;
 
-                if x > 799 || y > 799  {}
+        println!("{:?}", z);
-                else {
-                    rgb_image.put_pixel(x, y, color);
-                    if x > 0 {
-                        rgb_image.put_pixel(x - 1, y, color);
-                    }
-                    if x < 799 {
-                        rgb_image.put_pixel(x + 1, y, color);
-                    }
-                    if y > 0 {
-                        rgb_image.put_pixel(x, y - 1, color);
-                    }
-                    if y < 799 {
-                        rgb_image.put_pixel(x, y + 1, color);
-                    }
-                }
 
+        (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;
+
+            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]);
+            }
         });
-        }
 
+        lanes[0].iter().for_each(|point| {
+            let x = point[0] * 800.0/45.0 + rgb_image.width() as f64 / 2.0;
+            let y = rgb_image.height() as f64 - 1.0 - point[1] * 800.0/45.0;
+            if x < rgb_image.width() as f64 && y < rgb_image.width() as f64 {
+                rgb_image.put_pixel(x as u32, y as u32, Rgb([0, 255, 255]));
+            }
+        });
+
+        lanes[1].iter().for_each(|point| {
+            let x = point[0] * 800.0/45.0 + rgb_image.width() as f64 / 2.0;
+            let y = rgb_image.height() as f64 - 1.0 - point[1] * 800.0/45.0;
+            if x < rgb_image.width() as f64 && y < rgb_image.width() as f64 {
+                rgb_image.put_pixel(x as u32, y as u32, Rgb([255, 255, 0]));
+            }
+        });
 
         let image = ImageView::new(ImageInfo::new(PixelFormat::Rgb8, 800, 800), rgb_image.iter().as_slice());
         window.set_image("camera", image).unwrap();
@@ -206,4 +276,5 @@ fn main() {
         println!("Avg: {:.6} s", t.iter().map(|d| d.as_secs_f64()).sum::<f64>() / t.len() as f64);
         now = Instant::now();
     }
+
 }