security_vision/laserspace
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

New homography option

Browse source
This commit is contained in:
Ruben van de Ven 2025-10-16 16:15:51 +02:00
parent 9c178f8db1
commit 2d909f771d
4 changed files with 466 additions and 113 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

360
src/main.rs
View file

@ -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<Corner>,
canvas_dragging_homography: Option<usize>, // TODO: merge canvas_dragging_* into some the DraggablePoint enum
preview_dragging_point: Option<usize>,
preview_dragging_homography: Option<usize>,
// canvas_transform: Translation2D<f32, ScreenSpace, ScreenSpace>,
// 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 new_points = match points.space {
CoordinateSpace::RawLaser => points,
_ => model.config.filters.apply(&points)
};
let pointno = points.points.len();
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 pointno = points.points.len();
let new_points = match points.space {
CoordinateSpace::RawLaser => points,
_ => config.filters.apply(&points)
};
let new_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
@ -1090,6 +1168,7 @@ 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
} else {
// 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,49 +1214,60 @@ 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.homography.homography_matrix = mat
}).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.clip.mask = mask;
}).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();
}
}
}
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;
// 1. dragging corner
if let Some(corner) = &model.canvas_dragging_corner {
// 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.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();
// 4. find new homography on pairs of points, and convert to compatible matrix type
let matches: Vec<FeatureMatch<nalgebra::Point2<f64>>> = 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::<Vec<_>>();
// 1.3. find new homography on pairs of points, and convert to compatible matrix type
let matches: Vec<FeatureMatch<nalgebra::Point2<f64>>> = 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::<Vec<_>>();
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]);
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.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;
@ -1292,45 +1393,100 @@ 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;
}
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<Corner> = 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<Corner> = 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<usize> = 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) {

184
src/trap/filters.rs
View file

@ -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<Vec<MappedPoint>>,
}
#[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<usize, &'static str> {
// 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<FeatureMatch<nalgebra::Point2<f64>>> = 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::<Vec<_>>();
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<laser::Point> = 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
}
}
}

16
src/trap/laser.rs
View file

@ -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,

3
src/trap/tracks.rs
View file

@ -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)]