From 2d909f771df7e932de217b682f3e5946efbce95c Mon Sep 17 00:00:00 2001 From: Ruben van de Ven Date: Thu, 16 Oct 2025 16:15:51 +0200 Subject: [PATCH] New homography option --- src/main.rs | 376 +++++++++++++++++++++++++++++++------------- src/trap/filters.rs | 184 +++++++++++++++++++++- src/trap/laser.rs | 16 +- src/trap/tracks.rs | 3 +- 4 files changed, 466 insertions(+), 113 deletions(-) diff --git a/src/main.rs b/src/main.rs index 05bc0d8..a1e4c70 100644 --- a/src/main.rs +++ b/src/main.rs @@ -2,6 +2,7 @@ //! A clone of the `laser_frame_stream.rs` example that allows for configuring laser settings via a //! UI. +use bevy::scene::ron::Map; // use bevy_nannou::prelude::DARK_GRAY; // use nannou::lyon::geom::euclid::Transform2D; use nannou::{geom::Rect, math::map_range as nannou_map_range}; @@ -12,7 +13,7 @@ use nannou_egui::{self, egui, Egui}; use nannou_laser::DacId; use nannou_laser::{self as laser}; use serde_json::Result; -use laserspace::trap::filters::PointFilters; +use laserspace::trap::filters::{MappedPoint, PointFilters}; use laserspace::trap::laser::{shape_rect, LaserPoints, LaserSpace, StreamSource, STREAM_SOURCES, TMP_DESK_CLUBMAX, Corner}; use laserspace::trap::tracks::CoordinateSpace; use laserspace::trap::utils::{closest_edge, split_on_blank}; @@ -40,6 +41,7 @@ use std::path::Path; // use egui_dropdown::DropDownBox; const CONFIG_FILE_PATH: &str = "./config.json"; +const DRAGGING_THRESHOLD: u32 = 30; // allow for a slight cursor movement to still detect a click fn main() { nannou::app(model).update(update).run(); @@ -79,11 +81,21 @@ struct GuiModel { canvas_scale: f32, canvas_translate: Vec2, canvas_dragging_corner: Option, + canvas_dragging_homography: Option, // TODO: merge canvas_dragging_* into some the DraggablePoint enum preview_dragging_point: Option, + preview_dragging_homography: Option, // canvas_transform: Translation2D, // dragging: bool, } +// TODO: use as single draggable + +enum DraggablePoint { + None, + AreaCorner(Corner), + HomographyPoint(usize), // TODO: Better yet, use &'a mut MappedPoint +} + struct LaserSettings { point_hz: u32, latency_points: u32, @@ -289,14 +301,14 @@ fn model(app: &App) -> GuiModel { // Create a window to receive keyboard events. let w_id_lasersettings = app .new_window() - .size(312, 530) + .size(312, 1024) // .key_pressed(key_pressed) .raw_event(raw_window_event) .view(view_laser_settings) .build() .unwrap(); - let w_id_linecanvas = app + let _w_id_linecanvas = app .new_window() .size(1024, 768) // .key_pressed(key_pressed) @@ -308,7 +320,7 @@ fn model(app: &App) -> GuiModel { .build() .unwrap(); - let w_id_laserpreview = app + let _w_id_laserpreview = app .new_window() .size(1024, 1024) // .key_pressed(key_pressed) @@ -400,7 +412,9 @@ fn model(app: &App) -> GuiModel { canvas_scale: 25., canvas_translate: Vec2::new(-300.,100.), canvas_dragging_corner: None, + canvas_dragging_homography: None, preview_dragging_point: None, + preview_dragging_homography: None, // canvas_transform: Transform2D // dimming_factor: 1., } @@ -409,15 +423,20 @@ fn model(app: &App) -> GuiModel { fn laser_frame_producer(model: &mut LaserModel, frame: &mut laser::Frame){ let current_points: LaserPoints = (&model.current_lines).into(); - let space = &model.current_lines.space; + // let space = &model.current_lines.space; // check which source should be used, and get points accordingly. // potentially ignoring the points coming from the stream - let points = model.config.source.get_shape(current_points); + let points = model.config.source.get_shape(current_points, &model.config); - let pointno = points.points.len(); + // let pointno = points.points.len(); + + let new_points = match points.space { + CoordinateSpace::RawLaser => points, + _ => model.config.filters.apply(&points) + }; + - let new_points = model.config.filters.apply(&points); let new_laser_points = new_points.points; // if new_laser_points.len() < pointno { // println!("Cropped Points {} (was: {})", new_laser_points.len(), pointno); @@ -901,10 +920,10 @@ fn view_line_canvas(app: &App, model: &GuiModel, frame: Frame) { let rect = shape_rect(LaserSpace::READY, 11); let points = config.filters.reverse(&rect); - + let is_selected_dac = model.selected_stream == Some(dac_id.clone()); let vertices = points.points.iter().map(|p| { - let color = if model.selected_stream == Some(dac_id.clone()) { + let color = if is_selected_dac { // Srgba::hex("e52d9f"); // ORCHID. to_srgba() srgba(229./255.,45./255.,159./255.,0.8) @@ -922,8 +941,8 @@ fn view_line_canvas(app: &App, model: &GuiModel, frame: Frame) { .join_round() .points_colored(vertices); - // draggable corners for the selected area - if model.selected_stream == Some(dac_id.clone()){ + if is_selected_dac { + // draggable corners for the selected area let rect = shape_rect(LaserSpace::READY, 1); // find corners let points = config.filters.reverse(&rect); for p in points.points { @@ -935,6 +954,25 @@ fn view_line_canvas(app: &App, model: &GuiModel, frame: Frame) { .color(srgba(229./255.,45./255.,159./255.,0.8)); } + + // draggable handles for additional homography mapping points + if let Some(mapped_points) = &config.filters.homography.mapped_points { + for (i, mp) in mapped_points.iter().enumerate() { + let pos = [mp.irl_point[0] * scale + translate_x, mp.irl_point[1] * -scale + translate_y]; + + let color; + if model.canvas_dragging_homography == Some(i) || model.preview_dragging_homography == Some(i) { + color = srgba(5./255.,255./255.,0./255.,0.8); + } else { + color = srgba(251./255.,0./255.,255./255.,0.5); + } + + draw.ellipse() + .x_y(pos[0], pos[1]) + .radius(3.) + .color(color); + } + } } } @@ -1004,15 +1042,18 @@ fn view_laser_preview(app: &App, model: &GuiModel, frame: Frame) { let current_points: LaserPoints = (&model.current_lines).into(); - let space = &model.current_lines.space; // check which source should be used, and get points accordingly. // potentially ignoring the points coming from the stream - let points = config.source.get_shape(current_points); + let points = config.source.get_shape(current_points, config); - let pointno = points.points.len(); - - let new_points = config.filters.apply(&points); + // let pointno = points.points.len(); + + let new_points = match points.space { + CoordinateSpace::RawLaser => points, + _ => config.filters.apply(&points) + }; + let new_laser_points = new_points.points; // draw as distinct lines (this is how it is send to post-processing) @@ -1039,7 +1080,26 @@ fn view_laser_preview(app: &App, model: &GuiModel, frame: Frame) { .points_colored(vertices); } - // 4. Last, draw identifier of laser on top + // 4. The points for mapping the homography if available + if let Some(mapped_points) = config.filters.homography.mapped_points.as_ref() { + for (i, mp) in mapped_points.iter().enumerate() { + let point = [mp.dac_point[0] * hw, mp.dac_point[1] * hh]; + + let color; + if model.canvas_dragging_homography == Some(i) || model.preview_dragging_homography == Some(i) { + color = srgba(5./255.,255./255.,0./255.,0.8); + } else { + color = srgba(251./255.,0./255.,255./255.,0.5); + } + + draw.ellipse() + .x_y(point[0], point[1]) + .radius(5.) + .color(color); + } + } + + // 5. Last, draw identifier of laser on top draw.text(&format!("{} {:?}", config.name, dac_id)) .h(win_rect.h()) .font_size(10) @@ -1075,6 +1135,24 @@ fn laser_preview_mouse_pressed(app: &App, model: &mut GuiModel, button: MouseBut // return // } + // 1. Logic for homography mapping points + if let Some(mapped_points) = config.filters.homography.mapped_points.as_ref() { + let point_distances: Vec<(usize, f32, [f32;2])> = mapped_points + .iter() + .enumerate() + .map(|(i, p)| { + (i, (laser_x - p.dac_point[0]).powi(2) + (laser_y - p.dac_point[1]).powi(2), p.dac_point) + }) + .collect(); + let (idx, dist_sq, closest_point) = point_distances.iter().min_by(|a,b| a.1.partial_cmp(&b.1).unwrap()).expect("No homography points existing?"); + if dist_sq.sqrt() <= (MATCH_DISTANCE / half_w) { + if button == MouseButton::Left { + model.preview_dragging_homography = Some(*idx); + } + return + } + } + // 1. Always min. 4 points. We need closest two. So sort first let mask_points = &config.filters.clip.mask; let point_distances: Vec<(usize, f32, [f32;2])> = mask_points @@ -1089,7 +1167,8 @@ fn laser_preview_mouse_pressed(app: &App, model: &mut GuiModel, button: MouseBut // = point_distances; // let (idx2, _, _) = point_distances.get(1).expect("No clipping points existing?"); - + + // 2a. if closest is within MATCH_DISTANCE. Select point for dragging // match_distance is in pixelspace. Convert to laser-space if dist_sq.sqrt() <= (MATCH_DISTANCE / half_w) { if button == MouseButton::Left { @@ -1104,11 +1183,10 @@ fn laser_preview_mouse_pressed(app: &App, model: &mut GuiModel, button: MouseBut } } - // 2a. if closest is within MATCH_DISTANCE. Select point for dragging + + // 2b. if not, find closest edge } else { - if button == MouseButton::Left { - // 2b. if not, find closest edge let insert_at = match closest_edge(&config.filters.clip.mask, [laser_x, laser_y]) { Some(idx) => (idx + 1) % config.filters.clip.mask.len(), None => return, @@ -1136,42 +1214,52 @@ fn laser_mouse_moved(app: &App, model: &mut GuiModel, pos: Point2) { Some(d) => d, }; - let point_idx = match &model.preview_dragging_point { - None => return, - Some(d) => d, - }; - - - let half_w = LASER_PREVIEW_WIDTH / 2.; let half_h = LASER_PREVIEW_HEIGHT / 2.; let laser_x = app.mouse.x / half_w; let laser_y = app.mouse.y / half_h; - - - // 0. Get config - let config = model.per_laser_config.get_mut(&dac_id).expect("Dac config unavailable"); + let config: &mut DacConfig = model.per_laser_config.get_mut(&dac_id).expect("Dac config unavailable"); - // 1. Move selected point to laser_x, laser_y - // config.filters.clip. - let point = config.filters.clip.mask.get_mut(*point_idx).unwrap(); - // set new position - *point = [laser_x, laser_y]; + if let Some(map_idx) = &model.preview_dragging_homography { + // 1. Move selected point to laser_x, laser_y + // config.filters.clip. + let point = config.filters.homography.move_dac_point(*map_idx, [laser_x, laser_y]); - // 3. update config in laser stream threat - let mask = config.filters.clip.mask.clone(); + // 3. update config in laser stream threat + let mat = config.filters.homography.homography_matrix.clone(); - let selected_laser_stream = model.laser_streams.get(&dac_id); - if let Some(stream) = selected_laser_stream { - stream.send(move |laser_model: &mut LaserModel| { - laser_model.config.filters.clip.mask = mask; - }).unwrap(); + let selected_laser_stream = model.laser_streams.get(&dac_id); + if let Some(stream) = selected_laser_stream { + stream.send(move |laser_model: &mut LaserModel| { + laser_model.config.filters.homography.homography_matrix = mat + }).unwrap(); + } + } + + + if let Some(point_idx) = &model.preview_dragging_point { + // 1. Move selected point to laser_x, laser_y + // config.filters.clip. + let point = config.filters.clip.mask.get_mut(*point_idx).unwrap(); + // set new position + *point = [laser_x, laser_y]; + + // 3. update config in laser stream threat + let mask = config.filters.clip.mask.clone(); + + let selected_laser_stream = model.laser_streams.get(&dac_id); + if let Some(stream) = selected_laser_stream { + stream.send(move |laser_model: &mut LaserModel| { + laser_model.config.filters.clip.mask = mask; + }).unwrap(); + } + } } @@ -1179,6 +1267,7 @@ fn laser_mouse_moved(app: &App, model: &mut GuiModel, pos: Point2) { fn laser_mouse_released(_app: &App, model: &mut GuiModel, _button: MouseButton) { // deselect point model.preview_dragging_point = None; + model.preview_dragging_homography = None; } fn draw_grid(draw: &Draw, win: &Rect, step: f32, weight: f32) { @@ -1238,49 +1327,61 @@ fn laser_corners_to_world(filters: &PointFilters) -> Vec<[f32;2]> { fn map_mouse_moved(_app: &App, model: &mut GuiModel, pos: Point2) { - let corner = match &model.canvas_dragging_corner { - None => return, - Some(c) => c, - }; let dac_id = match &model.selected_stream { None => return, Some(d) => d, }; - let config = model.per_laser_config.get_mut(&dac_id).expect("Dac config unavailable"); + let config: &mut DacConfig = model.per_laser_config.get_mut(&dac_id).expect("Dac config unavailable"); - // 1. For the dragging point, reverse the canvas space to world space: + // 0. For the dragging point, reverse the canvas space to world space: let world_point = [ (pos[0] - model.canvas_translate.x) / model.canvas_scale, (pos[1] - model.canvas_translate.y) / -model.canvas_scale, ]; - // 2. find existing corners in world space, replacing the - // dragging corner - let mut world_corners = laser_corners_to_world(&config.filters); - world_corners[corner.index()] = world_point; - - // 3. find the corners of the laser, correct them for all - // geometric filters, except the homography itself. - let laser_corners: LaserPoints = Corner::in_laser_space().into(); - let distorted_laser_corners: Vec<[f32;2]> = config.filters.reverse_without_homography(&laser_corners).into(); - + // 1. dragging corner + if let Some(corner) = &model.canvas_dragging_corner { - // 4. find new homography on pairs of points, and convert to compatible matrix type - let matches: Vec>> = world_corners.iter().zip(distorted_laser_corners).map(|(world, laser)| { - FeatureMatch( - nalgebra::Point2::new(world[0] as f64, world[1] as f64), - nalgebra::Point2::new(laser[0] as f64, laser[1] as f64), - ) - }).collect::>(); + // 1.1. find existing corners in world space, replacing the + // dragging corner + let mut world_corners = laser_corners_to_world(&config.filters); + world_corners[corner.index()] = world_point; + + // 1.2. find the corners of the laser, correct them for all + // geometric filters, except the homography itself. + let laser_corners: LaserPoints = Corner::in_laser_space().into(); + let distorted_laser_corners: Vec<[f32;2]> = config.filters.reverse_without_homography(&laser_corners).into(); + - let m = find_homography(matches).unwrap(); - let mat: Mat3 = Mat3::from_cols_array(&[m[0] as f32, m[1] as f32, m[2] as f32, m[3] as f32, m[4] as f32, m[5] as f32, m[6] as f32, m[7] as f32, m[8] as f32]); - - // 5. update config in Gui and laser stream threat - config.filters.homography.homography_matrix = mat.clone(); - + // 1.3. find new homography on pairs of points, and convert to compatible matrix type + let matches: Vec>> = world_corners.iter().zip(distorted_laser_corners).map(|(world, laser)| { + FeatureMatch( + nalgebra::Point2::new(world[0] as f64, world[1] as f64), + nalgebra::Point2::new(laser[0] as f64, laser[1] as f64), + ) + }).collect::>(); + + let m = find_homography(matches).unwrap(); + let mat: Mat3 = Mat3::from_cols_array(&[m[0] as f32, m[1] as f32, m[2] as f32, m[3] as f32, m[4] as f32, m[5] as f32, m[6] as f32, m[7] as f32, m[8] as f32]); + + // 1.4. update config in Gui + // config.filters.homography.homography_matrix = mat.clone(); + config.filters.homography.update_h(mat.clone()); + } + + // 2. dragging non-corner mapping point + if let Some(point_idx) = model.canvas_dragging_homography { + // 2.1 set value + config.filters.homography.move_mapped_point(point_idx, world_point); + + + } + + // 3. propagate to laser stream threat let selected_laser_stream = model.laser_streams.get(&dac_id); + let mat = config.filters.homography.homography_matrix.clone(); + if let Some(stream) = selected_laser_stream { stream.send(move |laser_model: &mut LaserModel| { laser_model.config.filters.homography.homography_matrix = mat; @@ -1291,46 +1392,101 @@ fn map_mouse_moved(_app: &App, model: &mut GuiModel, pos: Point2) { const MATCH_DISTANCE: f32 = 30.; // screen pixels -fn map_mouse_pressed(app: &App, model: &mut GuiModel, button: MouseButton) { - if button != MouseButton::Left { - // ignore - return; - } - +fn map_mouse_pressed(app: &App, model: &mut GuiModel, button: MouseButton) { + if let Some(dac_id) = &model.selected_stream { - let config = model.per_laser_config.get(&dac_id).expect("Dac config unavailable"); + let config: &mut DacConfig = model.per_laser_config.get_mut(&dac_id).expect("Dac config unavailable"); - // find close corner to drag - let corners = laser_corners_to_world(&config.filters); - let canvas_corners: Vec<[f32;2]> = corners.iter().map(|p| { - [ - p[0] * model.canvas_scale + model.canvas_translate.x, - p[1] * -model.canvas_scale + model.canvas_translate.y - ] - }).collect(); - dbg!("{:?}", &canvas_corners); - dbg!("{:?}, {:?}", &app.mouse.x, &app.mouse.y); + if button == MouseButton::Left { + // find close corner to drag + let corners = laser_corners_to_world(&config.filters); + let canvas_corners: Vec<[f32;2]> = corners.iter().map(|p| { + [ + p[0] * model.canvas_scale + model.canvas_translate.x, + p[1] * -model.canvas_scale + model.canvas_translate.y + ] + }).collect(); - let mut selected: Option = None; - for (i, c) in canvas_corners.iter().enumerate(){ - if (app.mouse.x - c[0]).abs() < MATCH_DISTANCE && (app.mouse.y - c[1]).abs() < MATCH_DISTANCE { - dbg!("close to corner! {:?}", &c); - match i { - 0 => selected = Some(Corner::TopLeft), - 1 => selected = Some(Corner::TopRight), - 2 => selected = Some(Corner::BottomRight), - 3 => selected = Some(Corner::BottomLeft), - _ => selected = None, + dbg!("{:?}", &canvas_corners); + dbg!("{:?}, {:?}", &app.mouse.x, &app.mouse.y); + + let mut selected: Option = None; + for (i, c) in canvas_corners.iter().enumerate(){ + if (app.mouse.x - c[0]).abs() < MATCH_DISTANCE && (app.mouse.y - c[1]).abs() < MATCH_DISTANCE { + dbg!("close to corner! {:?}", &c); + match i { + 0 => selected = Some(Corner::TopLeft), + 1 => selected = Some(Corner::TopRight), + 2 => selected = Some(Corner::BottomRight), + 3 => selected = Some(Corner::BottomLeft), + _ => selected = None, + } + break; } - break; + } + + dbg!("selected! {:?}", &selected); + model.canvas_dragging_corner = selected; + } + + // not nearby a corner, maybe nearby a homography point + if model.canvas_dragging_corner.is_none() { + let mut selected: Option = None; + if !&config.filters.homography.mapped_points.is_none() { + + let map_points = config.filters.homography.mapped_points.as_ref().unwrap(); + + let h_points: Vec<[f32;2]> = map_points.iter().map(|p| { + [ + p.irl_point[0] * model.canvas_scale + model.canvas_translate.x, + p.irl_point[1] * -model.canvas_scale + model.canvas_translate.y + ] + }).collect(); + + + for (i, c) in h_points.iter().enumerate(){ + if (app.mouse.x - c[0]).abs() < MATCH_DISTANCE && (app.mouse.y - c[1]).abs() < MATCH_DISTANCE { + dbg!("close to corner! {:?}", &c); + selected = Some(i); + break; + } + } + + if let Some(idx) = selected { + + dbg!("selected homography! {:?}", &selected); + dbg!("with mouse {:?}", &button); + if button == MouseButton::Left { + model.canvas_dragging_homography = selected; + } + if button == MouseButton::Right { + dbg!("selected homography to remove {:?}", &idx); + // TODO: remove + config.filters.homography.rm_mapped_point(idx); + } + + } + } + if selected.is_none() && button == MouseButton::Left { + dbg!("Adding homography point: {:?}", &app.mouse); + + let irl_point = [ + + (app.mouse.x - model.canvas_translate.x) / model.canvas_scale, + (app.mouse.y - model.canvas_translate.y) / -model.canvas_scale, + ]; + + dbg!("world point: {:?}", &irl_point); + + if let Ok(index) = config.filters.homography.add_mapped_point(irl_point) { + // only add if within bounds + model.canvas_dragging_homography = Some(index); + } } } - dbg!("selected! {:?}", &selected); - model.canvas_dragging_corner = selected; - } @@ -1350,7 +1506,15 @@ fn map_mouse_pressed(app: &App, model: &mut GuiModel, button: MouseButton) { } fn map_mouse_released(_app: &App, model: &mut GuiModel, _button: MouseButton) { - model.canvas_dragging_corner = None; + if !model.canvas_dragging_corner.is_none() { + model.canvas_dragging_corner = None; + return + } + + if !model.canvas_dragging_homography.is_none() { + model.canvas_dragging_homography = None; + return + } } fn map_wheel_zoom(_app: &App, model: &mut GuiModel, dt: MouseScrollDelta, _phase: TouchPhase) { diff --git a/src/trap/filters.rs b/src/trap/filters.rs index cf54f60..fd97f67 100644 --- a/src/trap/filters.rs +++ b/src/trap/filters.rs @@ -1,4 +1,6 @@ -use bevy::prelude::*; // for glam::f32::Mat3 +use bevy::prelude::*; +use cv_core::FeatureMatch; +use homography::find_homography; // for glam::f32::Mat3 use crate::trap::{laser::{apply_homography_matrix, Corner, LaserPoints}, tracks::CoordinateSpace, utils::clip_lines}; @@ -12,9 +14,16 @@ pub trait Filter { fn reverse(&self, points: &LaserPoints) -> LaserPoints; } +#[derive(Serialize, Deserialize, Clone, Debug)] +pub struct MappedPoint { + pub irl_point: [f32; 2], // in real life / world coordinates + pub dac_point: [f32; 2], // dac coordinates (ranged -1, 1) +} + #[derive(Serialize, Deserialize, Clone, Debug)] pub struct HomographyFilter { - pub homography_matrix: Mat3 + pub homography_matrix: Mat3, + pub mapped_points: Option>, } #[derive(Serialize, Deserialize, Clone, Debug)] @@ -141,6 +150,172 @@ impl Default for PointFilters { } } +impl Default for MappedPoint { + fn default() -> Self { + Self { + irl_point: [0., 0.], + dac_point: [0., 0.], + } + } +} + +impl HomographyFilter { + pub fn add_mapped_point(&mut self, irl_point: [f32; 2]) -> Result { + // 1. find position of the irl_point in laser space according to current homography + + let point = self._map_single_irl_point_to_dac(irl_point); + + if !Corner::within_bounds(point) { + return Err("Point not within laser bounds"); + } + + let p = MappedPoint{ + irl_point: irl_point, + dac_point: point, + }; + + // match self.mapped_points { + // None => { + // self.mapped_points = Some(vec![p]); + // 0 + // }, + // Some(mp) => { + // mp.push(p); + // mp.len() - 1 + // }, + // } + + + if let Some(mp) = self.mapped_points.as_mut() { + mp.push(p); + } else { + self.mapped_points = Some(vec![p]); + } + + Ok(self.mapped_points.as_ref().unwrap().len() - 1) + + } + + // move a point, updating the dac_point position (thus keeping homography neutral) + // used to accurately position the irl_point + pub fn move_mapped_point(&mut self, idx: usize, irl_point: [f32; 2]) { + let point = self._map_single_irl_point_to_dac(irl_point); + + let mp: &mut MappedPoint = self.mapped_points.as_mut().unwrap().get_mut(idx).unwrap(); + if !Corner::within_bounds(point) { + return; + } + + mp.irl_point = irl_point; + mp.dac_point = point; + } + + // move a dac point, recalculating homography + pub fn move_dac_point(&mut self, idx: usize, dac_point: [f32; 2]) { + let mp: &mut MappedPoint = self.mapped_points.as_mut().unwrap().get_mut(idx).unwrap(); + if !Corner::within_bounds(dac_point) { + return; + } + + mp.dac_point = dac_point; + + self.update_from_mapped_points(); + } + + pub fn _map_single_irl_point_to_dac(&self, irl_point: [f32; 2]) -> [f32; 2] { + let mut world_points: LaserPoints = vec![irl_point].into(); + world_points.space = CoordinateSpace::World; + + let laserpoints: Vec<[f32; 2]> = self.apply(&world_points).into(); + laserpoints[0] + } + + pub fn rm_mapped_point(&mut self, idx: usize) { + let mps = self.mapped_points.as_mut().unwrap(); + mps.remove(idx); + if mps.len() == 0 { + self.mapped_points = None + } + self.update_from_mapped_points(); + } + + pub fn get_h_from_mapped_points(&self) -> Mat3 { + let mat: Mat3; + + if self.mapped_points.as_ref().unwrap().len() < 4 { // we need 4 points for a unambiguous homography + mat = self.homography_matrix.clone(); + } else { + let matches: Vec>> = self.mapped_points.as_ref().unwrap().iter().map(|mapped_point| { + FeatureMatch( + nalgebra::Point2::new(mapped_point.irl_point[0] as f64, mapped_point.irl_point[1] as f64), + nalgebra::Point2::new(mapped_point.dac_point[0] as f64, mapped_point.dac_point[1] as f64), + ) + }).collect::>(); + + let m = find_homography(matches).unwrap(); + mat = Mat3::from_cols_array(&[m[0] as f32, m[1] as f32, m[2] as f32, m[3] as f32, m[4] as f32, m[5] as f32, m[6] as f32, m[7] as f32, m[8] as f32]); + } + + mat + } + + pub fn update_from_mapped_points(&mut self) { + let mat = self.get_h_from_mapped_points(); + self.homography_matrix = mat; + } + + pub fn update_h(&mut self, mat: Mat3) { + self.homography_matrix = mat; + + // change irl_points by reverse-applying filter to dac_points + if self.mapped_points.is_none() { + return + } + + let dac_points: Vec<[f32; 2]> = self.mapped_points.as_ref().unwrap().iter().map(|mapped_point| { + mapped_point.dac_point + }).collect(); + let laserpoints: LaserPoints = dac_points.into(); + + let world_points: Vec<[f32; 2]> = self.reverse(&laserpoints).into(); + + for (mut mp, world_point) in self.mapped_points.as_mut().unwrap().iter_mut().zip(world_points) { + mp.irl_point = world_point; + } + + } + + pub fn to_cursor_laserpoints(&self) -> LaserPoints{ + if self.mapped_points.is_none() { + return LaserPoints::default(); + } + + let cursor_points: Vec = self.mapped_points.as_ref().unwrap().iter().map(|mapped_point| { + let point = mapped_point.dac_point; + + const SIZE: f32 = 0.02; + let color = [1., 0., 1.]; + let blank = [0., 0., 0.]; + + vec![ + laser::Point::new([point[0] - SIZE, point[1]], blank ), + laser::Point::new([point[0] - SIZE, point[1]], color ), + laser::Point::new([point[0] + SIZE, point[1]], color ), + laser::Point::new([point[0] + SIZE, point[1]], blank ), + laser::Point::new([point[0], point[1] - SIZE], blank ), + laser::Point::new([point[0], point[1] - SIZE], color ), + laser::Point::new([point[0], point[1] + SIZE], color ), + laser::Point::new([point[0], point[1] + SIZE], blank ), + ] + }).flatten().collect(); + LaserPoints{ + points: cursor_points, + space: CoordinateSpace::RawLaser, // don't run these through filters anymore + } + } +} + + impl Filter for HomographyFilter { fn apply(&self, points: &LaserPoints) -> LaserPoints{ @@ -198,7 +373,10 @@ impl Filter for HomographyFilter { impl Default for HomographyFilter{ fn default() -> Self { - return Self { homography_matrix: Mat3::IDENTITY } + return Self { + homography_matrix: Mat3::IDENTITY, + mapped_points: None + } } } diff --git a/src/trap/laser.rs b/src/trap/laser.rs index e1a70fa..8709de1 100644 --- a/src/trap/laser.rs +++ b/src/trap/laser.rs @@ -127,17 +127,19 @@ pub enum StreamSource { Rectangle, Grid, // lines WorldGrid, // grid in world space + Homography, // points for homography alignment // Circle, // segments // Spiral, } // usefull to create pull downs with an iterator -pub const STREAM_SOURCES: [StreamSource; 5] = [ +pub const STREAM_SOURCES: [StreamSource; 6] = [ StreamSource::Disabled, StreamSource::CurrentLines, StreamSource::Rectangle, StreamSource::Grid, StreamSource::WorldGrid, + StreamSource::Homography, // StreamSource::Spiral ]; @@ -209,9 +211,10 @@ pub fn shape_rect(space: LaserSpace, steps: usize) -> LaserPoints { // the different shapes that override the provided lines if needed impl StreamSource{ - pub fn get_shape(&self, current_lines: LaserPoints) -> LaserPoints { + pub fn get_shape(&self, current_points: LaserPoints, config: &DacConfig) -> LaserPoints { + match self { - Self::CurrentLines => current_lines, + Self::CurrentLines => current_points, Self::Rectangle => { shape_rect(LaserSpace::READY, 11) }, @@ -303,6 +306,9 @@ impl StreamSource{ LaserPoints { points, space: CoordinateSpace::World } }, + Self::Homography => { + config.filters.homography.to_cursor_laserpoints() + } StreamSource::Disabled => LaserPoints::default(), // empty set _ => LaserPoints::default(), // empty set } @@ -322,6 +328,10 @@ impl Corner { vec!([-1.,1.], [1.,1.], [1., -1.], [-1.,-1.]) } + pub fn within_bounds(p: [f32;2] ) -> bool { + !(p[0] < -1. || p[0] > 1. || p[1] < -1. || p[1] > 1.) + } + pub fn index(&self) -> usize { match self { Self::TopLeft => 0, diff --git a/src/trap/tracks.rs b/src/trap/tracks.rs index 35cd774..04a55ea 100644 --- a/src/trap/tracks.rs +++ b/src/trap/tracks.rs @@ -76,7 +76,8 @@ pub enum CoordinateSpace { Image = 1, UndistortedImage = 2, World = 3, - Laser = 4, + Laser = 4, // already homography applied, other filters still run + RawLaser = 8, // no filters, not even clipping, pincushion, dim, etc apply } #[derive(Clone, Debug, Serialize, Deserialize)]