so it can now make M, I think.

This HOPEFULLY now calculates the correct M, so that we can then start on the next steps

project/src/main.rs

@@ -1,9 +1,11 @@
 use anyhow::Result as AnyResult;
 use std::{fs::File, io::Read};
 
-use nalgebra as ng;
+use std::fmt::Debug;
 
-fn get_data(file_path: &str) -> AnyResult<Vec<u16>> {
+use nalgebra::{self as ng, Matrix, OMatrix};
+
+fn get_data(file_path: &str) -> AnyResult<Vec<i32>> {
     let mut file = File::open(file_path)?;
 
     let mut buf = "".to_string();
@@ -11,23 +13,89 @@ fn get_data(file_path: &str) -> AnyResult<Vec<u16>> {
 
     Ok(buf
         .split_whitespace()
-        .filter_map(|f| f.parse::<u16>().ok())
+        .filter_map(|f| f.parse::<i32>().ok())
         .collect())
 }
 
+fn create_matrix(
+    camera_points: ng::MatrixView<i32, ng::Dyn, ng::Const<3>>,
+    image_points: ng::MatrixView<i32, ng::Dyn, ng::Const<2>>,
+) -> ng::OMatrix<i32, ng::Dyn, ng::Const<12>> {
+    if camera_points.nrows() != image_points.nrows() {
+        panic!("Must have an equal number of rows between the image and camera points");
+    }
+
+    let rows = camera_points.nrows();
+
+    let iter = camera_points
+        .row_iter()
+        .zip(image_points.row_iter())
+        .flat_map(|(camera, image)| {
+            let x = camera[0];
+            let y = camera[1];
+            let z = camera[2];
+
+            let u = image[0];
+            let v = image[1];
+
+            let mut vec = vec![x, y, z, 1, 0, 0, 0, 0, -u * x, -u * y, -u * z, -u];
+            vec.append(&mut vec![
+                0,
+                0,
+                0,
+                0,
+                x,
+                y,
+                z,
+                1,
+                -v * x,
+                -v * y,
+                -v * z,
+                -v,
+            ]);
+            vec
+        });
+    ng::Matrix::from_row_iterator_generic(ng::Dyn(2 * rows), ng::U12, iter)
+}
+
 fn main() -> AnyResult<()> {
     let observe_data = get_data("../observe.dat")?;
-    let observe_mat = ng::MatrixXx2::from_vec_generic(
+    let observe_mat = ng::MatrixXx2::from_row_iterator_generic(
         ng::Dyn(observe_data.len() / 2),
         ng::Const::<2>,
         observe_data,
     );
-    println!("Observe: {:?}", observe_mat);
+
+    println!("Image:\n{:}", observe_mat);
 
     let model_data = get_data("../model.dat")?;
-    let model_mat =
-        ng::MatrixXx3::from_vec_generic(ng::Dyn(model_data.len() / 3), ng::Const::<3>, model_data);
-    println!("Observe: {:?}", model_mat);
+    let model_mat = ng::MatrixXx3::from_row_iterator_generic(
+        ng::Dyn(model_data.len() / 3),
+        ng::Const::<3>,
+        model_data,
+    );
+
+    println!("Camera:\n{:}", model_mat);
+
+    let q = create_matrix(model_mat.as_view(), observe_mat.as_view());
+
+    let q = ng::OMatrix::from_row_iterator_generic(
+        ng::Dyn(q.nrows()),
+        ng::U12,
+        q.iter().map(|num| *num as f32),
+    );
+
+    println!("Q:\n{:}", q);
+
+    let v_t = ng::SVD::new(q, false, true).v_t.expect("Expected");
+
+    println!("V:\n{:}", v_t);
+
+    let m_flat = v_t.row(v_t.nrows() - 1);
+
+    let m = ng::OMatrix::from_row_iterator_generic(ng::U3, ng::U4, m_flat.iter().copied());
+
+    println!("M:\n{:}", m);
 
     Ok(())
 }