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

Filter settings per stream

Browse source
This commit is contained in:
Ruben van de Ven 2025-07-01 16:23:37 +02:00
parent 9aefe4f7f6
commit d989cdaa86
2 changed files with 274 additions and 288 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

517
src/bin/render_lines_gui.rs
View file

@ -2,21 +2,19 @@
//! A clone of the `laser_frame_stream.rs` example that allows for configuring laser settings via a
//! UI.
use bevy::math::Mat3;
use bevy_nannou::prelude::DARK_GRAY;
// use bevy_nannou::prelude::DARK_GRAY;
// use nannou::lyon::geom::euclid::Transform2D;
use nannou::{geom::Rect, math::map_range as nannou_map_range};
use nannou::prelude::*;
use nannou_egui::egui::emath::inverse_lerp;
// use nannou_egui::egui::emath::inverse_lerp;
use nannou_egui::{self, egui, Egui};
use nannou_laser::DacId;
use nannou_laser::{self as laser, util::map_range};
use nannou_laser::{self as laser};
use serde_json::Result;
use serde::{Serialize,Deserialize};
use trap_rust::trap::filters::{Filter, PointFilters};
use trap_rust::trap::filters::PointFilters;
use trap_rust::trap::laser::{LaserPoints, TMP_DESK_CLUBMAX};
use trap_rust::trap::tracks::CoordinateSpace;
use trap_rust::trap::{laser::{apply_homography_matrix, python_cv_h_into_mat3, LaserModel, TMP_PYTHON_LASER_H, DacConfig}, tracks::{RenderableLines}};
use trap_rust::trap::{laser::{python_cv_h_into_mat3, LaserModel, TMP_PYTHON_LASER_H, DacConfig}, tracks::{RenderableLines}};
use zmq::Socket;
use std::sync::{mpsc, Arc};
use std::time::{Instant, Duration};
@ -27,11 +25,17 @@ fn main() {
nannou::app(model).update(update).run();
}
struct Model {
pub struct StreamConfig{
pub stream: laser::FrameStream<LaserModel>,
pub config: DacConfig,
}
type StreamConfigMap = HashMap<DacId, StreamConfig>;
struct GuiModel {
// A handle to the laser API used for spawning streams and detecting DACs.
laser_api: Arc<laser::Api>,
// All of the live stream handles.
laser_streams: Vec<laser::FrameStream<LaserModel>>,
laser_streams: StreamConfigMap,
// A copy of the state that will live on the laser thread so we can present a GUI.
laser_model: LaserModel,
// A copy of the laser settings so that we can control them with the GUI.
@ -48,6 +52,7 @@ struct Model {
// dimming_factor: f32,
lost_alpha: f32,
connected: bool,
selected_stream: Option<DacId>,
// canvas_transform: Translation2D<f32, ScreenSpace, ScreenSpace>,
// dragging: bool,
}
@ -85,7 +90,8 @@ impl Default for LaserSettings {
fn setup_zmq() -> Socket{
let url = "tcp://100.109.175.82:99174";
// let url = "tcp://100.109.175.82:99174";
let url = "tcp://127.0.0.1:99174";
let context = zmq::Context::new();
let subscriber = context.socket(zmq::SUB).unwrap();
subscriber.set_conflate(true).unwrap(); // only keep latest entry
@ -101,7 +107,7 @@ fn setup_zmq() -> Socket{
// fn zmq_receive(subscriber: &Socket, laser_streams: &Vec<laser::FrameStream<LaserModel>>) {
/// Receive items if available on the queue and update Model with the new data
fn zmq_receive(model: &mut Model) {
fn zmq_receive(model: &mut GuiModel) {
let subscriber = &model.zmq;
let mut items = [
subscriber.as_poll_item(zmq::POLLIN)
@ -149,10 +155,10 @@ fn zmq_receive(model: &mut Model) {
// println!("receive {}", lines.lines.len());
for laser_stream in (&model.laser_streams).into_iter() {
for (_dac, stream_config) in (&model.laser_streams).into_iter() {
// let lines = get_laser_lines(version);
let lines_for_laser: RenderableLines = lines.clone();
let sending = laser_stream.send(move |laser| {
let sending = stream_config.stream.send(move |laser| {
let laser_lines: RenderableLines = lines_for_laser;
laser.current_lines = laser_lines;
});
@ -179,6 +185,7 @@ fn zmq_receive(model: &mut Model) {
type DacConfigMap = HashMap<DacId, DacConfig>;
// Some hardcoded config. Not spending time on reading/writing config atm.
fn get_dac_configs() -> DacConfigMap{
@ -225,10 +232,27 @@ fn get_dac_configs() -> DacConfigMap{
filters: PointFilters::default(),
}
);
dac_configs.insert(
DacId::EtherDream {
mac_address: [
18,
52,
86,
120,
154,
188,
],
},
DacConfig{
name: "Emulator".into(),
homography: python_cv_h_into_mat3(TMP_DESK_CLUBMAX),
filters: PointFilters::default(),
}
);
dac_configs
}
fn model(app: &App) -> Model {
fn model(app: &App) -> GuiModel {
// Create a window to receive keyboard events.
let w_id_lasersettings = app
.new_window()
@ -300,7 +324,7 @@ fn model(app: &App) -> Model {
});
// We'll use a `Vec` to collect laser streams as they appear.
let laser_streams = vec![];
let laser_streams = HashMap::new(); //vec![];
// A user-interface to tweak the settings.
let window = app.window(w_id_lasersettings).unwrap();
@ -310,7 +334,7 @@ fn model(app: &App) -> Model {
let current_lines = RenderableLines::new(); //Vec::new();
Model {
GuiModel {
laser_api,
laser_settings,
laser_model,
@ -323,84 +347,12 @@ fn model(app: &App) -> Model {
lost_alpha: 1.,
connected: true,
per_laser_config: get_dac_configs(),
selected_stream: None,
// canvas_transform: Transform2D
// dimming_factor: 1.,
}
}
// Draw lines or points based on the `DrawMode`.
// fn add_points<I>(points: I, scale: f32, frame: &mut laser::Frame)
// where
// I: IntoIterator,
// I::Item: AsRef<laser::Point>,
// {
// let points = points.into_iter().map(|p| {
// let mut p = p.as_ref().clone();
// p.position[0] *= scale;
// p.position[1] *= scale;
// p
// });
// frame.add_lines(points);
// }
const LASER_MIN: f32 = -1.0;
const LASER_MAX: f32 = 1.0;
fn within_laser_bounds(position: &[f32; 2]) -> bool {
!(position[0] < LASER_MIN || position[0] > LASER_MAX || position[1] < LASER_MIN || position[1] > LASER_MAX)
}
fn interpolate_to_bounds(a: &[f32;2], b: &[f32;2]) {
//let t_a = inverse_lerp(a)
}
// From ChatGTP: Lian-Barsky Algorithm for line segment cropping
fn clip_line_to_bounds(
p1: &[f32; 2],
p2: &[f32; 2],
) -> Option<([f32; 2], [f32; 2])> {
let min = [LASER_MIN, LASER_MIN];
let max = [LASER_MAX, LASER_MAX];
let dx = p2[0] - p1[0];
let dy = p2[1] - p1[1];
let mut t0 = 0.0;
let mut t1 = 1.0;
let checks = [
(-dx, p1[0] - min[0]), // Left
(dx, max[0] - p1[0]), // Right
(-dy, p1[1] - min[1]), // Bottom
(dy, max[1] - p1[1]), // Top
];
for (p, q) in checks {
if p == 0.0 {
if q < 0.0 {
return None; // Line is parallel and outside
}
} else {
let r = q / p;
if p < 0.0 {
if r > t1 {
return None;
} else if r > t0 {
t0 = r;
}
} else {
if r < t0 {
return None;
} else if r < t1 {
t1 = r;
}
}
}
}
let clipped_p1 = [p1[0] + t0 * dx, p1[1] + t0 * dy];
let clipped_p2 = [p1[0] + t1 * dx, p1[1] + t1 * dy];
Some((clipped_p1, clipped_p2))
}
fn laser_frame_producer(model: &mut LaserModel, frame: &mut laser::Frame){
let points: LaserPoints = (&model.current_lines).into();
@ -415,99 +367,13 @@ fn laser_frame_producer(model: &mut LaserModel, frame: &mut laser::Frame){
frame.add_lines(new_laser_points);
return;
// dbg!(&model.config.name);
// let mut new_points = Vec::new();
// let projected_positions: Vec<[f32;2]> = points.iter().map(|point| {
// let p = point.position;
// let new_position = match space {
// CoordinateSpace::World => apply_homography_matrix(model.config.homography, &p),
// CoordinateSpace::Laser => p,
// _ => panic!("Invalid coordinate space"),
// };
// // let new_position = apply_homography_matrix(LASER_H, &p);
// // let s = 1.; // when using TMP_PYTHON_LASER_H_FOR_NANNOU -- doesn't work?
// let s = 0xFFF as f32 / 2.; // when using TMP_PYTHON_LASER_H
// [new_position[0]/s - 1., new_position[1]/s - 1.]
// }).collect();
// for (id, position ) in projected_positions.iter().enumerate() {
// let point = points[id] ;
// let mut new_positions: Vec<[f32;2]> = Vec::new();
// // const LASER_MIN: f32 = -1.0;
// // const LASER_MAX: f32 = 1.0;
// if !within_laser_bounds(position) {
// let mut either = false;
// if id > 0 {
// let prev_position = projected_positions[id-1];
// if within_laser_bounds(&prev_position) {
// either = true;
// // interpolate with prev
// let clip = clip_line_to_bounds(&prev_position, position);
// if let Some((p1, p2)) = clip {
// new_positions.push(p1);
// new_positions.push(p2);
// }
// }
// }
// if id < (projected_positions.len()-1) {
// let next_position = projected_positions[id+1];
// if within_laser_bounds(&next_position) {
// either = true;
// // interpolate with next
// let clip = clip_line_to_bounds(position, &next_position);
// if let Some((p1, p2)) = clip {
// new_positions.push(p1);
// new_positions.push(p2);
// }
// }
// }
// if !either {
// // if neither prev nor next is withint bounds, point can be ditched
// continue;
// }
// } else {
// new_positions.push(position.clone());
// }
// let mut color = point.color.clone();
// if model.dimming < 1.0 {
// color[0] *= model.dimming;
// color[1] *= model.dimming;
// color[2] *= model.dimming;
// }
// for position in new_positions {
// // let pos: [f32; 2] = position.clone();
// let new_point = laser::Point {
// position,
// color,
// .. point.clone()
// };
// new_points.push(new_point);
// }
// }
// if new_points.len() < pointno {
// println!("Cropped Points {} (was: {})", new_points.len(), pointno);
// }
// // println!("{:?}", new_points);
// frame.add_lines(new_points);
}
fn raw_window_event(_app: &App, model: &mut Model, event: &nannou::winit::event::WindowEvent) {
fn raw_window_event(_app: &App, model: &mut GuiModel, event: &nannou::winit::event::WindowEvent) {
model.egui.handle_raw_event(event);
}
fn update(_app: &App, model: &mut Model, update: Update) {
fn update(_app: &App, model: &mut GuiModel, update: Update) {
// First, check for new laser DACs.
for dac in model.dac_rx.try_recv() {
println!("Detected DAC {:?}!", dac.id());
@ -522,53 +388,69 @@ fn update(_app: &App, model: &mut Model, update: Update) {
let stream = model
.laser_api
.new_frame_stream(model.laser_model.with_config(config), laser_frame_producer)
.detected_dac(dac)
.detected_dac(dac.clone())
.build()
.expect("failed to establish stream with newly detected DAC");
model.laser_streams.push(stream);
model.laser_streams.insert(dac.id(), StreamConfig{ stream, config: config.clone() });
}
// Check if any streams have dropped out (e.g network issues, DAC turned off) and attempt to
// start them again.
let mut dropped = vec![];
for (i, stream) in model.laser_streams.iter().enumerate() {
if stream.is_closed() {
dropped.push(i);
}
}
for i in dropped.into_iter().rev() {
let stream = model.laser_streams.remove(i);
let dac = stream
.dac()
.expect("`dac` returned `None` even though one was specified during stream creation");
let res = stream
.close()
.expect("stream was unexpectedly already closed from another stream handle")
.expect("failed to join stream thread");
if let Err(err) = res {
eprintln!("Stream closed due to an error: {}", err);
}
println!("attempting to restart stream with DAC {:?}", dac.id());
match model
.laser_api
.new_frame_stream(model.laser_model.clone(), laser_frame_producer)
.detected_dac(dac)
.build()
{
Err(err) => eprintln!("failed to restart stream: {}", err),
Ok(stream) => model.laser_streams.push(stream),
for (dac_id, stream_config) in model.laser_streams.iter() {
if stream_config.stream.is_closed() {
dropped.push(dac_id.clone());
}
}
for dac_id in dropped.into_iter().rev() {
// let stream = ;
let s = model.laser_streams.remove(&dac_id);
if model.selected_stream == Some(dac_id){
model.selected_stream = None;
}
if let Some(stream_config) = s{
let dac = stream_config.stream
.dac()
.expect("`dac` returned `None` even though one was specified during stream creation");
let res = stream_config.stream
.close()
.expect("stream was unexpectedly already closed from another stream handle")
.expect("failed to join stream thread");
if let Err(err) = res {
eprintln!("Stream closed due to an error: {}", err);
}
// TODO: keeps looping on disconnect.
println!("attempting to restart stream with DAC {:?}", dac.id());
let dac_id = dac.id();
match model
.laser_api
.new_frame_stream(model.laser_model.clone(), laser_frame_producer)
.detected_dac(dac)
.build()
{
Err(err) => eprintln!("failed to restart stream: {}", err),
Ok(stream) => {model.laser_streams.insert(dac_id, StreamConfig{stream, config: stream_config.config});},
}
}
}
// check if new messages have arrived. Update the model with new data.
zmq_receive(model);
// Update the GUI.
let Model {
let GuiModel {
ref mut egui,
ref laser_streams,
ref mut laser_streams,
ref mut laser_model,
ref mut laser_settings,
ref mut per_laser_config,
ref mut selected_stream,
ref mut current_lines,
..
} = *model;
@ -585,34 +467,21 @@ fn update(_app: &App, model: &mut Model, update: Update) {
}
ui.heading("Laser Points");
ui.separator();
ui.add(egui::Label::new(format!("Lines {}", current_lines.lines.len())));
ui.add(egui::Label::new(format!("Points {}", current_lines.point_count())));
ui.add(egui::Label::new(format!("Lines {}", model.current_lines.lines.len())));
ui.add(egui::Label::new(format!("Points {}", model.current_lines.point_count())));
ui.heading("Laser Settings");
if ui
.add(egui::Slider::new(&mut model.laser_model.dimming, 0.0..=1.).text("Dimming"))
.changed()
{
for laser_stream in laser_streams {
let factor = model.laser_model.dimming;
// let lines = get_laser_lines(version);
laser_stream.send(move |laser| {
laser.dimming = factor;
}).unwrap();
}
}
ui.heading("General settings");
if ui
.add(egui::Slider::new(&mut laser_settings.point_hz, 1_000..=50_000).text("DAC PPS"))
.changed()
{
let hz = laser_settings.point_hz;
for stream in laser_streams {
stream.set_point_hz(hz).ok();
for (_dac_id, stream) in laser_streams.iter() {
stream.stream.set_point_hz(hz).ok();
}
}
if ui
@ -620,20 +489,20 @@ fn update(_app: &App, model: &mut Model, update: Update) {
.changed()
{
let latency = laser_settings.latency_points;
for stream in laser_streams {
stream.set_latency_points(latency).ok();
for (_dac_id, stream) in laser_streams.iter() {
stream.stream.set_latency_points(latency).ok();
}
}
if ui
.add(egui::Slider::new(&mut laser_settings.frame_hz, 1..=120).text("Target FPS"))
.changed()
{
let hz = laser_settings.frame_hz;
for stream in laser_streams {
stream.set_frame_hz(hz).ok();
}
{
let hz = laser_settings.frame_hz;
for (_dac_id, stream) in laser_streams.iter() {
stream.stream.set_frame_hz(hz).ok();
}
}
ui.separator();
ui.heading("Laser Path Interpolation");
@ -642,34 +511,34 @@ fn update(_app: &App, model: &mut Model, update: Update) {
.checkbox(&mut laser_settings.enable_optimisations, "Optimize Path")
.changed()
{
for stream in laser_streams {
stream
for (_dac_id, stream_config) in laser_streams.iter() {
stream_config.stream
.enable_optimisations(laser_settings.enable_optimisations)
.ok();
}
}
if ui
.add(
egui::Slider::new(&mut laser_settings.distance_per_point, 0.01..=1.0)
.add(
egui::Slider::new(&mut laser_settings.distance_per_point, 0.01..=1.0)
.text("Distance Per Point"),
)
)
.changed()
{
let distance = laser_settings.distance_per_point;
for stream in laser_streams {
stream.set_distance_per_point(distance).ok();
for (_dac_id, stream) in laser_streams.iter() {
stream.stream.set_distance_per_point(distance).ok();
}
}
if ui
.add(
egui::Slider::new(&mut laser_settings.blank_delay_points, 0..=32)
.text("Blank Delay (Points)"),
.text("Blank Delay (Points)"),
)
.changed()
{
let delay = laser_settings.blank_delay_points;
for stream in laser_streams {
stream.set_blank_delay_points(delay).ok();
for (_dac_id, stream) in laser_streams.iter() {
stream.stream.set_blank_delay_points(delay).ok();
}
}
let mut degrees = rad_to_deg(laser_settings.radians_per_point);
@ -679,39 +548,119 @@ fn update(_app: &App, model: &mut Model, update: Update) {
{
let radians = deg_to_rad(degrees);
laser_settings.radians_per_point = radians;
for stream in laser_streams {
stream.set_radians_per_point(radians).ok();
for (_dac_id, stream) in laser_streams.iter() {
stream.stream.set_radians_per_point(radians).ok();
}
}
ui.heading("Connected DACs");
if laser_streams.len() < 1 {
ui.label("no streams");
ui.separator();
ui.heading("Laser specific settings");
if laser_streams.is_empty() {
ui.label("No dacs available");
} else {
ui.horizontal(|ui| {
for (dac_id, _stream) in laser_streams.iter() {
ui.selectable_value(
selected_stream,
Some(dac_id.clone()),
if let Some(config) = per_laser_config.get(&dac_id) { config.name.clone() } else { "DAC".into() }
);
}
});
}
for stream in laser_streams {
let dac: laser::DetectedDac = stream
.dac()
.expect("`dac` returned `None` even though one was specified during stream creation");
ui.add(egui::Label::new(format!("{:?}", dac.id())));
if let Some(selected_stream_value) = selected_stream {
ui.separator();
ui.add(egui::Label::new(format!("{:?}", selected_stream_value)));
let stream_config: &mut StreamConfig = laser_streams.get_mut(&selected_stream_value).expect("Selected stream not found in configs");
if ui
// todo : from custom dac config:
.add(egui::Slider::new(&mut model.laser_model.dimming, 0.0..=1.).text("Dimming"))
.changed()
{
for laser_stream in laser_streams {
let factor = model.laser_model.dimming;
// let lines = get_laser_lines(version);
laser_stream.send(move |laser| {
// laser: LaserModel
laser.config.filters.dim.intensity = factor;
// laser.dimming = factor;
.add(egui::Slider::new(&mut stream_config.config.filters.dim.intensity, 0.0..=1.).text("Dimming"))
.changed()
{
let factor = stream_config.config.filters.dim.intensity;
stream_config.stream.send(move |laser_model: &mut LaserModel| {
laser_model.config.filters.dim.intensity = factor;
}).unwrap();
}
}
if ui
.add(egui::Slider::new(&mut stream_config.config.filters.scale.factor, 0.0..=2.).text("Scale"))
.changed()
{
let factor = stream_config.config.filters.scale.factor;
stream_config.stream.send(move |laser_model: &mut LaserModel| {
laser_model.config.filters.scale.factor = factor;
}).unwrap();
}
if ui
.add(egui::Slider::new(&mut stream_config.config.filters.pincushion.k_x, -1.0..=1.).text("Pincushion x"))
.changed()
{
let factor = stream_config.config.filters.pincushion.k_x;
stream_config.stream.send(move |laser_model: &mut LaserModel| {
laser_model.config.filters.pincushion.k_x = factor;
}).unwrap();
}
if ui
.add(egui::Slider::new(&mut stream_config.config.filters.pincushion.k_y, -1.0..=1.).text("Pincushion y"))
.changed()
{
let factor = stream_config.config.filters.pincushion.k_y;
stream_config.stream.send(move |laser_model: &mut LaserModel| {
laser_model.config.filters.pincushion.k_y = factor;
}).unwrap();
}
if ui
.checkbox(&mut stream_config.config.filters.crop.enabled ,"Crop")
.changed()
{
let enabled = stream_config.config.filters.crop.enabled;
stream_config.stream.send(move |laser_model: &mut LaserModel| {
laser_model.config.filters.crop.enabled = enabled;
}).unwrap();
}
// ui.heading("Connected DACs");
// if laser_streams.is_empty() {
// ui.label("no streams");
// }
// for (_dac_id, stream_config) in laser_streams.iter_mut() {
// let dac: laser::DetectedDac = stream_config.stream
// .dac()
// .expect("`dac` returned `None` even though one was specified during stream creation");
// ui.add(egui::Label::new(format!("{:?}", dac.id())));
// if ui
// // todo : from custom dac config:
// .add(egui::Slider::new(&mut stream_config.config.filters.dim.intensity, 0.0..=1.).text("Dimming"))
// .changed()
// {
// for (_dac_id, laser_stream) in laser_streams.iter() {
// let factor = model.laser_model.dimming;
// // let lines = get_laser_lines(version);
// laser_stream.stream.send(move |laser| {
// // laser: LaserModel
// laser.config.filters.dim.intensity = factor;
// // laser.dimming = factor;
// }).unwrap();
// }
// }
// }
//if egui::ComboBox::from_label("Homography")
// .selected_text(format!("{radio:?}"))
@ -722,23 +671,25 @@ fn update(_app: &App, model: &mut Model, update: Update) {
// })
// .changed() {
// let sending = laser_stream.send(move |laser| {
// let laser_lines: RenderableLines = lines_for_laser;
// laser.current_lines = laser_lines;
// });
// if let Err(e) = sending {
// println!("Error sending to laser! {e:?}");
// }
// };
// let laser_lines: RenderableLines = lines_for_laser;
// laser.current_lines = laser_lines;
// });
// if let Err(e) = sending {
// println!("Error sending to laser! {e:?}");
// }
// };
} else {
ui.label("Select a DAC");
}
});
}
fn view_laser_settings(_app: &App, model: &Model, frame: Frame) {
fn view_laser_settings(_app: &App, model: &GuiModel, frame: Frame) {
model.egui.draw_to_frame(&frame).unwrap();
}
fn view_line_canvas(app: &App, model: &Model, frame: Frame) {
fn view_line_canvas(app: &App, model: &GuiModel, frame: Frame) {
// get canvas to draw on
let draw = app.draw();
@ -832,15 +783,15 @@ fn style() -> egui::Style {
style
}
fn mouse_moved(_app: &App, _model: &mut Model, _pos: Point2) {
fn mouse_moved(_app: &App, _model: &mut GuiModel, _pos: Point2) {
}
fn mouse_pressed(_app: &App, _model: &mut Model, _button: MouseButton) {
fn mouse_pressed(_app: &App, _model: &mut GuiModel, _button: MouseButton) {
// _model.dragging
}
fn mouse_released(_app: &App, _model: &mut Model, _button: MouseButton) {}
fn mouse_released(_app: &App, _model: &mut GuiModel, _button: MouseButton) {}
fn mouse_wheel(_app: &App, _model: &mut Model, _dt: MouseScrollDelta, _phase: TouchPhase) {
fn mouse_wheel(_app: &App, _model: &mut GuiModel, _dt: MouseScrollDelta, _phase: TouchPhase) {
// canvas zoom
}

45
src/trap/filters.rs
View file

@ -18,6 +18,7 @@ pub struct HomographyFilter {
#[derive(Serialize, Deserialize, Clone)]
pub struct CropFilter {
pub enabled: bool
}
#[derive(Serialize, Deserialize, Clone)]
@ -25,6 +26,11 @@ pub struct DimFilter {
pub intensity: f32
}
#[derive(Serialize, Deserialize, Clone)]
pub struct ScaleFilter {
pub factor: f32
}
#[derive(Serialize, Deserialize, Clone)]
pub struct PincushionFilter {
pub k_x: f32,
@ -35,17 +41,19 @@ pub struct PincushionFilter {
// TODO consider moving to struct?
pub enum PointFilter {
Homography(HomographyFilter),
Crop(CropFilter),
Dim(DimFilter),
Scale(ScaleFilter),
Pincushion(PincushionFilter),
Crop(CropFilter),
}
pub struct PointFilterList(Vec<PointFilter>); // deprecated
#[derive(Serialize, Deserialize, Clone)]
pub struct PointFilters{
pub homography: HomographyFilter,
pub dim: DimFilter,
pub homography: HomographyFilter,
pub scale: ScaleFilter,
pub pincushion: PincushionFilter,
pub crop: CropFilter,
}
@ -68,6 +76,7 @@ impl PointFilters {
pub fn apply(&self, points: &LaserPoints) -> LaserPoints{
let mut p = self.dim.apply(points);
p = self.homography.apply(&p);
p = self.scale.apply(&p);
p = self.pincushion.apply(&p);
p = self.crop.apply(&p);
p
@ -85,8 +94,9 @@ impl Default for PointFilters {
Self {
homography: HomographyFilter::default(),
dim: DimFilter{intensity: 0.5},
scale: ScaleFilter { factor: 1. },
pincushion: PincushionFilter{k_x: 0., k_y: 0.},
crop: CropFilter{},
crop: CropFilter{ enabled: true },
}
}
}
@ -186,6 +196,14 @@ fn clip_line_to_bounds(
impl Filter for CropFilter {
fn apply(&self, points: &LaserPoints) -> LaserPoints {
if !self.enabled {
// don't modify if disabled
return LaserPoints{
points: points.points.clone(),
space: points.space,
};
}
let space = points.space;
let mut new_points = Vec::new();
@ -266,6 +284,23 @@ impl Filter for DimFilter {
}
}
impl Filter for ScaleFilter {
fn apply(&self, points: &LaserPoints) -> LaserPoints {
let new_points = points.points.iter().map(|point| {
let mut position = point.position.clone();
if self.factor != 1.0 {
position[0] *= self.factor;
position[1] *= self.factor;
}
Point::new(position, point.color)
}).collect();
LaserPoints {
points: new_points,
space: points.space
}
}
}
impl Filter for PincushionFilter {
// The formula for pincushion distortion is: r_u = r_d * (1 + k * r_d^2)
// see also https://stackoverflow.com/a/6227310
@ -273,14 +308,14 @@ impl Filter for PincushionFilter {
// becomes trivial
fn apply(&self, points: &LaserPoints) -> LaserPoints{
let space = points.space;
dbg!(&space);
// dbg!(&space);
// assert!(!matches!(space, CoordinateSpace::Laser));
let projected_positions: Vec<laser::Point> = points.points.iter().map(|point| {
let p = point.position;
let new_position = [
p[0] * (1. + self.k_x * p[0].powi(2)),
p[1] * (1. + self.k_x * p[1].powi(2))
p[1] * (1. + self.k_y * p[1].powi(2))
];
laser::Point {