maybe x and y were swapped all along

src/main.rs

@@ -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 first_part: Array2<f64> = H_t.dot(&w).dot(&H);
-                let second_part = first_part.inv().unwrap();
+
+                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 l = (0.5 * z[0]  + const_part) * (800.0/45.0);;
-            let r = -0.5 * z[0] + const_part;
+            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]);
             }
         });