WIP for laser homography, with behaviour from OpenCV

Cargo.toml

@@ -18,3 +18,7 @@ zmq = "0.10.0"
 # (tip from bevy examples for fast compilation/performance trade-off)
 [profile.dev.package."*"]
 opt-level = 3
+
+[[bin]]
+name="renderer"
+path="src/main.rs"

examples/laser_points_homography.rs

@@ -0,0 +1,40 @@
+use bevy::math::Vec3;
+use trap_rust::trap::laser::{self, apply_homography_matrix};
+
+/*
+Compare output with the following python
+```python
+import numpy as np
+import cv2
+
+H =np.array([[ 2.47442963e+02, -7.01714050e+01, -9.71749119e+01],
+            [ 1.02328119e+01,  1.47185254e+02,  1.96295638e+02],
+            [-1.20921986e-03, -3.32735973e-02,  1.00000000e+00]])
+
+points =[[1.,1.0],[2.0,2.0]]
+print(cv2.perspectiveTransform(np.array([points]), H))
+```
+ */
+fn main(){
+    {
+        let point = Vec3::new(1.0,1.0,1.0);
+        let laser_h = laser::python_cv_h_into_mat3(laser::TMP_PYTHON_LASER_H);
+        let new_point = laser_h.mul_vec3(point);
+        println!("{} is wrong", new_point);
+    }
+    {
+        let point = Vec3::new(2.0,2.0,1.0);
+        let laser_h = laser::python_cv_h_into_mat3(laser::TMP_PYTHON_LASER_H);
+        let new_point = laser_h.mul_vec3(point);
+        println!("{} is also wrong", new_point);
+        let t = [new_point[0]/new_point[2],new_point[1]/new_point[2]];
+        println!("{:?} but can be fixed!", t);
+    }
+    {
+
+        let p = apply_homography_matrix(laser::python_cv_h_into_mat3(laser::TMP_PYTHON_LASER_H), &[1.,1.]);
+        println!("{:?} this is right as well!", p);
+        
+    }
+}   
+

src/lib.rs

@@ -0,0 +1 @@
+pub mod trap;

src/main.rs

@@ -7,6 +7,9 @@ use bevy_nannou::prelude::*;
 use bevy_nannou::NannouPlugin;
 use iyes_perf_ui::prelude::*;
 use nannou_laser::point::Rgb;
+use trap::laser::apply_homography_matrix;
+use trap::laser::python_cv_h_into_mat3;
+use trap::laser::TMP_PYTHON_LASER_H;
 use trap::shapes::PositionAndIntensity;
 use trap::tracks::LaserPoints;
 use trap::tracks::RenderableLines;
@@ -21,7 +24,8 @@ use std::fs;
 use std::time::Instant;
 use std::time::Duration;
 
-mod trap;
+
+pub mod trap;
 
 fn main() {
     let mut app = App::new();
@@ -100,6 +104,8 @@ fn setup_laser(mut commands: Commands) {
 
 fn setup(mut commands: Commands) {
     // Spawn a camera for our main window
+    // TODO: look into https://docs.rs/visioncortex/latest/visioncortex/struct.PerspectiveTransform.html
+
     commands.spawn(render::NannouCamera);
 
 }
@@ -121,6 +127,13 @@ fn text2points(position_and_intensity: PositionAndIntensity) -> laser::Point{
     laser::Point::new(p, color)
 }
 
+// impl Into<Mat3> for [[f32;3]; 3] {
+//     fn from(m) -> Self{
+
+//     }
+// }
+
+const LASER_H: Mat3 = python_cv_h_into_mat3(TMP_PYTHON_LASER_H);
 
 fn laser_frame_producer(model: &mut LaserModel, frame: &mut laser::Frame){
     
@@ -134,8 +147,33 @@ fn laser_frame_producer(model: &mut LaserModel, frame: &mut laser::Frame){
     
     // let points = positions.iter().cloned().map(text2points);
     let points = model.current_points.clone();
-    println!("Points {}", points.len());
-    frame.add_lines(points);
+
+    // let laser_cv_h: [[f32;3]; 3] = [[ 2.47442963e+02, -7.01714050e+01, -9.71749119e+01],
+    // [ 1.02328119e+01,  1.47185254e+02,  1.96295638e+02],
+    // [-1.20921986e-03, -3.32735973e-02,  1.00000000e+00]];
+    
+
+    let mut new_points = Vec::new();
+    for point in points.into_iter() {
+        let p = point.position;
+        let new_position = apply_homography_matrix(LASER_H, &p);
+        // let new_position = laser_h.mul_vec3(v);
+        // TODO: current matrix is shifted by 0xFFF/2 and scaled by 0xFFF/2
+        let s = 0xFFF as f32;
+
+        let new_point = laser::Point {
+            position: [new_position[0]/s, new_position[1]/s],
+            .. point
+        };
+        new_points.push(new_point);
+    }
+
+    // let mut src_points: Vector<Point2f> = Vector::new();
+    // let mut dst_points: Vector<Point2f> = Vector::new();
+    // let homography_transform_matrix = Mat::new();
+    println!("Points {}", new_points.len());
+    println!("{:?}", new_points);
+    frame.add_lines(new_points);
 }
 
 
@@ -149,8 +187,6 @@ fn get_laser_lines(use_second: bool) -> Vec<nannou_laser::Point>{
 
 }
 
-#[derive(Component)]
-pub struct WindowColor(Color);
 
 #[derive(Component)]
 struct LaserTimer {
@@ -169,7 +205,7 @@ fn exit_system(keys: Res<ButtonInput<KeyCode>>, mut exit: EventWriter<AppExit>)
 fn update(
     // mut commands: Commands,
     // keys: Res<ButtonInput<KeyCode>>,
-    draws: Query<(&Draw, Option<&WindowColor>)>,
+    draws: Query<&Draw>,
     mut lasers: Query<(&mut LaserApi, &mut LaserTimer)>,
     tracks: Query<(&Track, &SpawnedTime)>,
     time: Res<Time>,
@@ -198,12 +234,8 @@ fn update(
         println!(" - sent");
     }
 
-    for (draw, window_color) in draws.iter() {
-        if let Some(window_color) = window_color {
-            draw.background().color(window_color.0);
-        } else {
-            draw.background().color(DIM_GRAY);
-        }
+    for draw in draws.iter() {
+        draw.background().color(DIM_GRAY);
 
         draw.ellipse().color(LIGHT_GRAY).w_h(100.0, 100.0);
 

src/trap/laser.rs

@@ -0,0 +1,21 @@
+use bevy::prelude::*;
+
+pub const TMP_PYTHON_LASER_H: [[f32;3];3] = [[ 2.47442963e+02, -7.01714050e+01, -9.71749119e+01],
+[ 1.02328119e+01,  1.47185254e+02,  1.96295638e+02],
+[-1.20921986e-03, -3.32735973e-02,  1.00000000e+00]];
+
+pub const fn python_cv_h_into_mat3(m: [[f32;3]; 3]) -> Mat3{
+    Mat3::from_cols(
+        Vec3::new(m[0][0], m[1][0], m[2][0]),
+        Vec3::new(m[0][1], m[1][1], m[2][1]),
+        Vec3::new(m[0][2], m[1][2], m[2][2]),
+    )
+}
+
+/// Transform point with transform (homography) matrix
+/// this behaviour is conguent with openCV perspectiveTransform
+pub fn apply_homography_matrix(h: Mat3, p: &[f32; 2]) -> [f32; 2]{
+    let v_in = Vec3::new(p[0],p[1],1.0);
+    let v_out = h.mul_vec3(v_in);
+    [v_out[0] / v_out[2], v_out[1] / v_out[2]]
+}

src/trap/mod.rs

@@ -8,3 +8,5 @@ pub mod tracks;
 // pub use grid::GridPlugin;
 
 pub mod shapes;
+
+pub mod laser;

src/trap/scenario.rs

@@ -1,4 +1,15 @@
+use bevy::prelude::*;
+
 enum Scene{
-    LEVEL,
-    TEST
+    FirstPrediction,
+    Lost, //temporarily lost
+    Gone,
+
+}
+
+#[derive(Bundle)]
+struct ScenarioBundle{
+    track: Track,
+    scene: Scene,
+    prediction: Track
 }