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

basic filter implementation

Browse source
This commit is contained in:
Ruben van de Ven 2025-06-27 14:16:17 +02:00
parent 0ca03aa1e7
commit 9aefe4f7f6
6 changed files with 431 additions and 87 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

190
src/bin/render_lines_gui.rs
View file

@ -13,14 +13,16 @@ use nannou_laser::DacId;
use nannou_laser::{self as laser, util::map_range};
use serde_json::Result;
use serde::{Serialize,Deserialize};
use trap_rust::trap::laser::TMP_DESK_CLUBMAX;
use trap_rust::trap::filters::{Filter, 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::{LaserPoints, RenderableLines}};
use trap_rust::trap::{laser::{apply_homography_matrix, 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};
use std::collections::HashMap;
fn main() {
nannou::app(model).update(update).run();
}
@ -185,7 +187,8 @@ fn get_dac_configs() -> DacConfigMap{
DacId::Helios { id: 926298163 },
DacConfig{
name: "Helios#1".into(),
homography: python_cv_h_into_mat3(TMP_PYTHON_LASER_H)
homography: python_cv_h_into_mat3(TMP_PYTHON_LASER_H),
filters: PointFilters::default(),
}
);
dac_configs.insert(
@ -201,7 +204,8 @@ fn get_dac_configs() -> DacConfigMap{
},
DacConfig{
name: "ED - 192.168.8.101".into(),
homography: python_cv_h_into_mat3(TMP_DESK_CLUBMAX)
homography: python_cv_h_into_mat3(TMP_DESK_CLUBMAX),
filters: PointFilters::default(),
}
);
dac_configs.insert(
@ -217,7 +221,8 @@ fn get_dac_configs() -> DacConfigMap{
},
DacConfig{
name: "ED - 192.168.9.101".into(),
homography: python_cv_h_into_mat3(TMP_DESK_CLUBMAX)
homography: python_cv_h_into_mat3(TMP_DESK_CLUBMAX),
filters: PointFilters::default(),
}
);
dac_configs
@ -400,92 +405,102 @@ fn laser_frame_producer(model: &mut LaserModel, frame: &mut laser::Frame){
let points: LaserPoints = (&model.current_lines).into();
let space = &model.current_lines.space;
let pointno = points.len();
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);
}
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 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();
// };
// // 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] ;
// for (id, position ) in projected_positions.iter().enumerate() {
// let point = points[id] ;
let mut new_positions: Vec<[f32;2]> = Vec::new();
// 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);
}
}
}
// // 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 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;
}
// 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);
}
// 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);
}
// if new_points.len() < pointno {
// println!("Cropped Points {} (was: {})", new_points.len(), pointno);
// }
// println!("{:?}", new_points);
frame.add_lines(new_points);
// // println!("{:?}", new_points);
// frame.add_lines(new_points);
}
fn raw_window_event(_app: &App, model: &mut Model, event: &nannou::winit::event::WindowEvent) {
@ -676,11 +691,28 @@ fn update(_app: &App, model: &mut Model, update: Update) {
}
for stream in laser_streams {
let dac = stream
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 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;
}).unwrap();
}
}
//if egui::ComboBox::from_label("Homography")
// .selected_text(format!("{radio:?}"))
// .show_ui(ui, |ui| {

298
src/trap/filters.rs Normal file
View file

@ -0,0 +1,298 @@
use bevy::prelude::*; // for glam::f32::Mat3
use crate::trap::{laser::{apply_homography_matrix, LaserPoints}, tracks::CoordinateSpace};
use nannou_laser::{self as laser, Point};
use serde::{Deserialize, Serialize};
pub trait Filter {
// fn set_config(&self)
// fn set_config(&self)
fn apply(&self, points: &LaserPoints) -> LaserPoints;
}
#[derive(Serialize, Deserialize, Clone)]
pub struct HomographyFilter {
pub homography_matrix: Mat3
}
#[derive(Serialize, Deserialize, Clone)]
pub struct CropFilter {
}
#[derive(Serialize, Deserialize, Clone)]
pub struct DimFilter {
pub intensity: f32
}
#[derive(Serialize, Deserialize, Clone)]
pub struct PincushionFilter {
pub k_x: f32,
pub k_y: f32
}
#[derive(Serialize, Deserialize, Clone)]
// TODO consider moving to struct?
pub enum PointFilter {
Homography(HomographyFilter),
Crop(CropFilter),
Dim(DimFilter),
Pincushion(PincushionFilter),
}
pub struct PointFilterList(Vec<PointFilter>); // deprecated
#[derive(Serialize, Deserialize, Clone)]
pub struct PointFilters{
pub homography: HomographyFilter,
pub dim: DimFilter,
pub pincushion: PincushionFilter,
pub crop: CropFilter,
}
// list of enums deprecated in favour of struct
// impl Default for PointFilterList {
// fn default() -> Self {
// // let crop_filter = CropFilter{};
// Self (
// vec![
// PointFilter::Dim(DimFilter{intensity: 0.5}),
// PointFilter::Pincushion(PincushionFilter{k_x: 0., k_y: 0.}),
// PointFilter::Crop(CropFilter{}),
// ]
// )
// }
// }
impl PointFilters {
pub fn apply(&self, points: &LaserPoints) -> LaserPoints{
let mut p = self.dim.apply(points);
p = self.homography.apply(&p);
p = self.pincushion.apply(&p);
p = self.crop.apply(&p);
p
}
pub fn with_homography(mut self, h: Mat3) -> Self{
self.homography.homography_matrix = h;
self
}
}
impl Default for PointFilters {
fn default() -> Self {
// let crop_filter = CropFilter{};
Self {
homography: HomographyFilter::default(),
dim: DimFilter{intensity: 0.5},
pincushion: PincushionFilter{k_x: 0., k_y: 0.},
crop: CropFilter{},
}
}
}
impl Filter for HomographyFilter {
fn apply(&self, points: &LaserPoints) -> LaserPoints{
let space = points.space;
let projected_positions: Vec<laser::Point> = points.points.iter().map(|point| {
let p = point.position;
let new_position = match space {
CoordinateSpace::World => apply_homography_matrix(self.homography_matrix, &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
let normalised_pos: [f32;2] = [new_position[0]/s - 1., new_position[1]/s - 1.];
laser::Point {
position: normalised_pos,
.. point.clone()
}
}).collect();
LaserPoints{
points: projected_positions,
space: CoordinateSpace::Laser
}
}
}
impl Default for HomographyFilter{
fn default() -> Self {
return Self { homography_matrix: Mat3::IDENTITY }
}
}
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)
}
// 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))
}
impl Filter for CropFilter {
fn apply(&self, points: &LaserPoints) -> LaserPoints {
let space = points.space;
let mut new_points = Vec::new();
for (id, point) in points.points.iter().enumerate() {
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(&point.position) {
let mut either = false;
if id > 0 {
let prev_position = points.points[id-1].position;
if within_laser_bounds(&prev_position) {
either = true;
// interpolate with prev
let clip = clip_line_to_bounds(&prev_position, &point.position);
if let Some((p1, p2)) = clip {
new_positions.push(p1);
new_positions.push(p2);
}
}
}
if id < (points.points.len()-1) {
let next_position = points.points[id+1].position;
if within_laser_bounds(&next_position) {
either = true;
// interpolate with next
let clip = clip_line_to_bounds(&point.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(point.position.clone());
}
for position in new_positions {
// let pos: [f32; 2] = position.clone();
let new_point = laser::Point {
position,
.. point.clone()
};
new_points.push(new_point);
}
}
LaserPoints{
points: new_points,
space
}
}
}
impl Filter for DimFilter {
fn apply(&self, points: &LaserPoints) -> LaserPoints {
let new_points = points.points.iter().map(|point| {
let mut color = point.color.clone();
if self.intensity != 1.0 {
color[0] *= self.intensity;
color[1] *= self.intensity;
color[2] *= self.intensity;
}
Point::new(point.position, 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
// As points in laser space center around 0,0, calculating from the center
// becomes trivial
fn apply(&self, points: &LaserPoints) -> LaserPoints{
let space = points.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))
];
laser::Point {
position: new_position,
.. point.clone()
}
}).collect();
LaserPoints{
points: projected_positions,
space
}
}
}

15
src/trap/laser.rs
View file

@ -2,7 +2,15 @@ use bevy::prelude::*;
use nannou_laser as laser;
use std::time::Instant;
use serde::{Deserialize, Serialize};
use super::tracks::{LaserPoints, RenderableLines};
use crate::trap::{filters::{PointFilter, PointFilters}, tracks::CoordinateSpace};
use super::tracks::{RenderableLines};
pub struct LaserPoints{
pub points: Vec<laser::Point>,
pub space: CoordinateSpace
}
// homography for laserworld in studio
pub const TMP_PYTHON_LASER_H: [[f32;3];3] = [[ 2.47442963e+02, -7.01714050e+01, -9.71749119e+01],
@ -78,7 +86,8 @@ pub struct DacConfig{
// #[serde(with = "DacIdSerializable")]
// id: DacId,
pub name: String,
pub homography: Mat3
pub homography: Mat3,
pub filters: PointFilters
}
const LASER_H: Mat3 = python_cv_h_into_mat3(TMP_PYTHON_LASER_H);
@ -89,6 +98,6 @@ impl Default for DacConfig{
fn default() -> DacConfig{
//DacConfig { name: "Unknown".into(), homography: Mat3::IDENTITY }
// DacConfig { name: "Unknown".into(), homography: LASER_H_CM }
DacConfig { name: "Unknown".into(), homography: LASER_H }
DacConfig { name: "Unknown".into(), homography: LASER_H, filters: PointFilters::default().with_homography(LASER_H) }
}
}

3
src/trap/mod.rs
View file

@ -9,4 +9,5 @@ pub mod tracks;
pub mod shapes;
pub mod laser;
pub mod laser;
pub mod filters;

10
src/trap/tracks.rs
View file

@ -4,6 +4,8 @@ use std::time::Instant;
use nannou_laser as laser;
use serde_repr::*;
use crate::trap::laser::LaserPoints;
#[derive(Serialize,Deserialize)]
pub struct Frame {
pub tracks: std::collections::HashMap<String, Track>
@ -68,7 +70,7 @@ impl RenderableLine {
}
// see also trap/lines.py for matching values
#[derive(Clone, Debug, Serialize_repr, Deserialize_repr)]
#[derive(Clone, Debug, Serialize_repr, Deserialize_repr, Copy)]
#[repr(u8)]
pub enum CoordinateSpace {
Image = 1,
@ -140,7 +142,6 @@ impl From<&Track> for RenderableLines{
}
// TODO migrate to euclid::Point2D<f32, LaserSpace>
pub type LaserPoints = Vec<laser::Point>;
impl From<&RenderableLines> for LaserPoints {
// much like nannou_laser::stream::frame::add_lines()
@ -160,7 +161,10 @@ impl From<&RenderableLines> for LaserPoints {
}
points.extend(line.points.iter().map(|p| laser::Point::from(p)));
}
points
Self{
points,
space: CoordinateSpace::World
}
}
}

2
src/trap/zmqplugin.rs
View file

@ -2,7 +2,7 @@ use zmq::Socket;
use serde_json::Result;
use bevy::{ecs::system::SystemState, prelude::*, render::Render};
use std::num::NonZero;
use super::{laser::{LaserApi, LaserTimer}, tracks::{Frame, LaserPoints, RenderableLines, Track, TrackBundle}};
use super::{laser::{LaserApi, LaserTimer}, tracks::{Frame, RenderableLines, Track, TrackBundle}};
// use trap::{Frame, Track, TrackBundle};