updated to include basic drawing. Fully handles camera framing stuff now.

project/src/main.rs

@@ -1,9 +1,9 @@
 use anyhow::Result as AnyResult;
+use minifb as fb;
+use std::ops::Mul;
 use std::{fs::File, io::Read};
 
-use std::fmt::Debug;
-
-use nalgebra::{self as ng, Matrix, OMatrix};
+use nalgebra as ng;
 
 fn get_data(file_path: &str) -> AnyResult<Vec<i32>> {
     let mut file = File::open(file_path)?;
@@ -21,24 +21,30 @@ 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>> {
+    //Verify row counts are equal
     if camera_points.nrows() != image_points.nrows() {
         panic!("Must have an equal number of rows between the image and camera points");
     }
 
+    //Cache for later use
     let rows = camera_points.nrows();
 
     let iter = camera_points
-        .row_iter()
-        .zip(image_points.row_iter())
+        .row_iter() //Iterate over each row
+        .zip(image_points.row_iter()) //Zip up the rows of image points
         .flat_map(|(camera, image)| {
+            //Extract x,y,z from cords
             let x = camera[0];
             let y = camera[1];
             let z = camera[2];
 
+            //Extract uv from image
             let u = image[0];
             let v = image[1];
 
+            //Create a 12 long vector which is the first row
             let mut vec = vec![x, y, z, 1, 0, 0, 0, 0, -u * x, -u * y, -u * z, -u];
+            //Append another 12 elements to create another row
             vec.append(&mut vec![
                 0,
                 0,
@@ -53,12 +59,60 @@ fn create_matrix(
                 -v * z,
                 -v,
             ]);
+            //return the vec which can be converted into an iterator
             vec
         });
     ng::Matrix::from_row_iterator_generic(ng::Dyn(2 * rows), ng::U12, iter)
 }
 
-fn main() -> AnyResult<()> {
+/// Returns (K,R,t) in that order
+fn decompose_projection_matrix(
+    m: &ng::Matrix3x4<f64>,
+) -> (ng::Matrix3<f64>, ng::Matrix3<f64>, ng::Vector3<f64>) {
+    let a = m.fixed_columns::<3>(0);
+    let b = m.column(3);
+
+    // Reversal matrix J
+    let j = ng::Matrix3::new(0.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 0.0);
+
+    // RQ via QR on (J * A)^T
+    let ja_t = (j * a).transpose();
+    let qr = ja_t.qr();
+    let q = qr.q();
+    let r_qr = qr.r();
+
+    let mut k = j * r_qr.transpose() * j;
+    let mut r = j * q.transpose();
+
+    // Fix negative diagonal elements in K
+    for i in 0..3 {
+        if k[(i, i)] < 0.0 {
+            for row in 0..3 {
+                k[(row, i)] *= -1.0;
+            }
+            for col in 0..3 {
+                r[(i, col)] *= -1.0;
+            }
+        }
+    }
+
+    // Ensure proper rotation determinant
+    if r.determinant() < 0.0 {
+        r *= -1.0;
+        k *= -1.0;
+    }
+
+    // Extract translation vector t = K_raw^-1 * b
+    let t = k.lu().solve(&b).unwrap();
+
+    // Normalize K so K[2,2] == 1.0
+    let scale = k[(2, 2)];
+    k /= scale;
+
+    (k, r, t)
+}
+
+fn init() -> AnyResult<ng::Matrix3x4<f64>> {
     let observe_data = get_data("../observe.dat")?;
     let observe_mat = ng::MatrixXx2::from_row_iterator_generic(
         ng::Dyn(observe_data.len() / 2),
@@ -79,10 +133,10 @@ fn main() -> AnyResult<()> {
 
     let q = create_matrix(model_mat.as_view(), observe_mat.as_view());
 
-    let q = ng::OMatrix::from_row_iterator_generic(
+    let q = ng::OMatrix::from_iterator_generic(
         ng::Dyn(q.nrows()),
         ng::U12,
-        q.iter().map(|num| *num as f32),
+        q.iter().map(|num| *num as f64),
     );
 
     println!("Q:\n{:}", q);
@@ -97,5 +151,175 @@ fn main() -> AnyResult<()> {
 
     println!("M:\n{:}", m);
 
+    let model_mat = ng::OMatrix::from_iterator_generic(
+        ng::U4,
+        ng::Dyn(model_mat.nrows()),
+        model_mat
+            .row_iter()
+            .flat_map(|row| vec![row[0], row[1], row[2], 1]),
+    );
+
+    println!("P:\n{:}", model_mat);
+
+    let predicted_mat = ng::OMatrix::from_iterator_generic(
+        ng::U2,
+        ng::Dyn(model_mat.ncols()),
+        model_mat.column_iter().flat_map(|column| {
+            let new_column = &m.mul(&column.map(|x| x as f64));
+            let x = new_column[0];
+            let y = new_column[1];
+            let z = new_column[2];
+
+            vec![(x / z) as i32, (y / z) as i32]
+        }),
+    );
+
+    println!("Predict:\n{:}", predicted_mat);
+
+    let (k, r, t) = decompose_projection_matrix(&m);
+
+    println!("K:\n{:}\nR:\n{:}\nt:\n{:}", k, r, t);
+
+    let alpha = k[0];
+    let theta = 1.0_f64.atan2(k[3] / -alpha);
+    let beta = k[4] * theta.sin();
+    let u_0 = k[6];
+    let v_0 = k[7];
+
+    println!(
+        "alpha: {:}, theta: {:}, beta: {:}, u_0: {:}, v_0: {:}",
+        alpha, theta, beta, u_0, v_0
+    );
+
+    Ok(m)
+}
+
+//Returns (u,v)
+fn convert_3d_to_2d(m: &ng::Matrix3x4<f64>, x: f64, y: f64, z: f64) -> (i32, i32) {
+    let dim_3 = ng::Matrix4x1::new(x, y, z, 1.0);
+
+    let dim_2 = m * dim_3;
+
+    ((dim_2[0] / dim_2[2]) as i32, (dim_2[1] / dim_2[2]) as i32)
+}
+
+const WINDOW_HEIGHT: usize = 480;
+const WINDOW_WIDTH: usize = 640;
+
+fn main() -> AnyResult<()> {
+    //Handles all of the startup init code! Instead of keeping it in the way of the actual image drawing.
+    let m = init()?;
+    let mut window = fb::Window::new(
+        "Display Buffer",
+        WINDOW_WIDTH,
+        WINDOW_HEIGHT,
+        fb::WindowOptions::default(),
+    )?;
+
+    let mut x = Vec::new();
+    let mut y = Vec::new();
+    let mut z = Vec::new();
+
+    for i in 0..(10 * 10) {
+        // Constant 0
+        z.push(0);
+        // x will count up, the reset every 10
+        x.push(i % 10);
+        //y will only count up once every 10
+        y.push(i / 10);
+    }
+
+    for i in 0..(10 * 10) {
+        // Constant 10
+        y.push(10);
+        // x will count up, the reset every 10
+        x.push(i % 10);
+        //z will only count up once every 10
+        z.push(i / 10);
+    }
+
+    for i in 0..(10 * 10) {
+        // Constant 10
+        x.push(10);
+        // z will count up, the reset every 10
+        z.push(i % 10);
+        //y will only count up once every 10
+        y.push(i / 10);
+    }
+
+    let (u, v) = x
+        .into_iter()
+        .zip(y)
+        .zip(z)
+        .map(|((x, y), z)| convert_3d_to_2d(&m, x as f64, y as f64, z as f64))
+        .collect::<(Vec<i32>, Vec<i32>)>();
+
+    let mut buffer = vec![0 as u32; WINDOW_WIDTH * WINDOW_HEIGHT];
+
+    update_buffer_with_pixels(&mut buffer, u, v);
+
+    //Clone this, so we can clone this back into buffer later, for each frame. So we can keep the BG pixels.
+    let background_buffer = buffer.clone();
+
+    window.update_with_buffer(&buffer, WINDOW_WIDTH, WINDOW_HEIGHT)?;
+    window.set_target_fps(30);
+
+    let mut frame = 0.0;
+
+    while window.is_open() {
+        //Reload the background for each frame
+        buffer = background_buffer.clone();
+
+        let u = vec![0; 0];
+        let v = vec![0; 0];
+
+        let p1 = (2.0 + (frame / 30.0), 2.0, 2.0);
+        let p2 = (8.0 + (frame / 30.0), 8.0, 8.0);
+
+        let p1 = convert_3d_to_2d(&m, p1.0, p1.1, p1.2);
+        let p2 = convert_3d_to_2d(&m, p2.0, p2.1, p2.2);
+
+        let line = generate_line_between_verticies(p1.0, p1.1, p2.0, p2.1);
+
+        update_buffer_with_pixels(&mut buffer, line.0, line.1);
+
+        frame += 1.0;
+
+        if frame > 150.0 {
+            frame = 0.0;
+        }
+
+        //Required to close, and update frame;
+        if window.is_key_down(fb::Key::Q) {
+            break;
+        }
+        window.update_with_buffer(&buffer, WINDOW_WIDTH, WINDOW_HEIGHT)?;
+    }
+
     Ok(())
 }
+
+fn update_buffer_with_pixels(buf: &mut Vec<u32>, u: Vec<i32>, v: Vec<i32>) {
+    u.into_iter().zip(v).for_each(|(x, y)| {
+        let index = (y as usize) * WINDOW_WIDTH + (x as usize);
+        buf[index] = u32::MAX
+    });
+}
+
+fn generate_line_between_verticies(u_1: i32, v_1: i32, u_2: i32, v_2: i32) -> (Vec<i32>, Vec<i32>) {
+    let mut u = vec![0; 0];
+    let mut v = vec![0; 0];
+
+    let u_diff = u_2 - u_1;
+    let v_diff = v_2 - v_1;
+
+    let u_change = u_diff / 100;
+    let v_change = v_diff / 100;
+
+    for i in 0..100 {
+        u.push(u_1 + (i * u_change));
+        v.push(v_1 + (i * v_change));
+    }
+
+    (u, v)
+}