diff --git a/Cargo.toml b/Cargo.toml index bffa623..19b6810 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -17,4 +17,8 @@ zmq = "0.10.0" # Enable max optimizations for dependencies, but not for our code: # (tip from bevy examples for fast compilation/performance trade-off) [profile.dev.package."*"] -opt-level = 3 \ No newline at end of file +opt-level = 3 + +[[bin]] +name="renderer" +path="src/main.rs" \ No newline at end of file diff --git a/examples/laser_points_homography.rs b/examples/laser_points_homography.rs new file mode 100644 index 0000000..af3284c --- /dev/null +++ b/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); + + } +} + diff --git a/src/lib.rs b/src/lib.rs new file mode 100644 index 0000000..b737964 --- /dev/null +++ b/src/lib.rs @@ -0,0 +1 @@ +pub mod trap; \ No newline at end of file diff --git a/src/main.rs b/src/main.rs index 8d06ce3..acd72c1 100644 --- a/src/main.rs +++ b/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 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 = Vector::new(); + // let mut dst_points: Vector = 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{ } -#[derive(Component)] -pub struct WindowColor(Color); #[derive(Component)] struct LaserTimer { @@ -169,7 +205,7 @@ fn exit_system(keys: Res>, mut exit: EventWriter) fn update( // mut commands: Commands, // keys: Res>, - draws: Query<(&Draw, Option<&WindowColor>)>, + draws: Query<&Draw>, mut lasers: Query<(&mut LaserApi, &mut LaserTimer)>, tracks: Query<(&Track, &SpawnedTime)>, time: Res