New homography option
This commit is contained in:
Ruben van de Ven
parent
9c178f8db1
commit
2d909f771d
4 changed files with 466 additions and 113 deletions
376
src/main.rs
376
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<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 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<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.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<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]);
|
||||
|
||||
// 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<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) {
|
||||
|
|
|
|||
|
|
@ -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
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
|
|
|
|||
|
|
@ -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)]
|
||||
|
|
|
|||
Loading…
Reference in a new issue