commit edfdc46172c473bfa8361258dec4a8788422c10b Author: Lapin Raving Date: Tue Sep 12 23:35:29 2023 +0200 first push pour l'instant le programe detecte chacun des bord puis recadre le trapeze en carrer avec une marge en cas de depassement. diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..b2706ed --- /dev/null +++ b/.gitignore @@ -0,0 +1,3 @@ +*.png +image +target diff --git a/Cargo.toml b/Cargo.toml new file mode 100644 index 0000000..349db38 --- /dev/null +++ b/Cargo.toml @@ -0,0 +1,19 @@ +[package] +name = "lj_qualibration" +version = "0.1.0" +edition = "2021" + +# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html + + +[dependencies] +chrono = "0.4.26" +config = "0.13.3" +ctrlc = "3.4.0" +env_logger = "0.10.0" +log = "0.4.18" +redis = "0.23.0" +toml = "0.7.4" +serde = { version = "1.0.163", features = ["derive"] } +opencv = "0.84.5" +enum-iterator = "1.4.1" diff --git a/copyme.settings.toml b/copyme.settings.toml new file mode 100644 index 0000000..4637283 --- /dev/null +++ b/copyme.settings.toml @@ -0,0 +1,17 @@ +# file: settings.toml +# Rename me ! + +# The main key of your laser in LJ +laser_id = 0 + +# Your client id in LJ +client_id = 0 + +# Activate for more debug +debug = true + +# How many Frames Per Second +framerate = 20 + +# Redis URL as redis://IP:port/ +redis_url = "redis://127.0.0.1:6379/" diff --git a/src/conf.rs b/src/conf.rs new file mode 100644 index 0000000..71b1a8d --- /dev/null +++ b/src/conf.rs @@ -0,0 +1,22 @@ +use config::Config; +use serde::{Deserialize, Serialize}; +use std::error::Error; + +#[derive(Serialize, Deserialize, Debug, Clone)] +pub struct Conf { + pub debug: bool, + pub laser_id: u8, + pub client_id: u8, + pub redis_url: String, + pub framerate: u8, +} + +impl Conf { + pub fn new(path: &str) -> Result> { + let settings = Config::builder() + .add_source(config::File::with_name(path)) + .build()?; + let conf: Conf = settings.try_deserialize()?; + Ok(conf) + } +} diff --git a/src/draw.rs b/src/draw.rs new file mode 100644 index 0000000..d231035 --- /dev/null +++ b/src/draw.rs @@ -0,0 +1,53 @@ +// This is where you can put your custom code + +use crate::{Color, Point}; + +const RADIUS: f32 = 2000.0; +const X_CENTER: f32 = 2047.0; +const Y_CENTER: f32 = 2047.0; + +pub fn draw_line( + p1: &Point, + p2: &Point, + nb1: usize, + nb2: usize, +) -> Result, Box> { + let mut pl = vec![]; + let black = Color { r: 0, g: 0, b: 0 }; + + for _ in 0..nb1 { + pl.push(Point { + color: black, + ..*p1 + }); + } + for _ in 0..nb2 { + pl.push(*p2); + } + for _ in 0..(nb1 - nb2) { + pl.push(Point { + color: black, + ..*p2 + }); + } + + Ok(pl) +} + +#[allow(dead_code)] +pub fn draw(_time: f64) -> Result, Box> { + let mut v: Vec = vec![]; + for i in 0..128 { + let a = i as f32 / 128.0 * std::f32::consts::PI * 2.0; + v.push(Point { + x: (X_CENTER + a.cos() * RADIUS) as f32, + y: (Y_CENTER + a.sin() * RADIUS) as f32, + color: Color { + r: 255, + g: 255, + b: 255, + }, + }); + } + Ok(v) +} diff --git a/src/framerate.rs b/src/framerate.rs new file mode 100644 index 0000000..cfca9f6 --- /dev/null +++ b/src/framerate.rs @@ -0,0 +1,44 @@ +use log::{debug, warn}; +use std::thread; +use std::time::{Duration, Instant}; + +/// Converts helios Geometry to Helios +#[derive(Debug, Clone, Copy)] +pub struct Framerate { + start: Instant, + prev_trace_time: Instant, + framerate: u8, +} + +impl Framerate { + pub fn new(framerate: u8) -> Result> { + Ok(Framerate { + start: Instant::now(), + prev_trace_time: Instant::now(), + framerate, + }) + } + pub fn handle_time(&mut self) -> Result<(f64), Box> { + let frame_time = 1000000000 / self.framerate as u128; + let now = Instant::now(); + // How long since last loop ? + let nanotime_spent = self.prev_trace_time.elapsed().as_nanos(); + // Diw it go too fast? If so : sleep a bit + if frame_time > nanotime_spent { + let nanotime_towait = frame_time - nanotime_spent; + let dur = Duration::new(0, (nanotime_towait as f32 * 0.9) as u32); + //debug!( "{:?} - {:?} : {:?}", nanotime_towait, self.prev_trace_time, now); + thread::sleep(dur); + //debug!("Framerate OK"); + } else { + //warn!( + // "Frame slower than expected {:?} > {:?}", + // nanotime_spent, frame_time, + //); + } + + let time_from_begin = self.start.elapsed().as_nanos() as f64 / 1000000000.; + self.prev_trace_time = now; + Ok(time_from_begin) + } +} diff --git a/src/logs.rs b/src/logs.rs new file mode 100644 index 0000000..b31be09 --- /dev/null +++ b/src/logs.rs @@ -0,0 +1,17 @@ +use crate::Conf; +use env_logger::Builder; +use log::LevelFilter; + +pub fn init_logging(config: &Result>) { + if let Ok(ref config) = config { + let level = if config.debug { + LevelFilter::Debug + } else { + LevelFilter::Info + }; + let mut builder = Builder::from_default_env(); + builder.filter(None, level).init(); + return; + } + env_logger::init(); +} diff --git a/src/main.rs b/src/main.rs new file mode 100644 index 0000000..d375fca --- /dev/null +++ b/src/main.rs @@ -0,0 +1,123 @@ +/// +/// Configure udev: +/// https://github.com/Grix/helios_dac/blob/master/docs/udev_rules_for_linux.md +/// +mod conf; +mod draw; +mod framerate; +mod logs; +mod point; + +mod utils; +mod qualibration; + +use qualibration::{Qualibration, annalyse::adding_trackbar}; + +use conf::Conf; +use log::{/*debug, warn, */ error, info}; +use logs::init_logging; +use point::{Color, Point}; +use redis::{Client, Commands, Connection}; +use std::sync::{Arc, atomic::{AtomicBool, Ordering}}; + + +use opencv::Result; +use opencv::highgui; + +opencv::opencv_branch_4! { + #[allow(unused_imports)] + use opencv::imgproc::LINE_AA; +} +opencv::not_opencv_branch_4! { + #[allow(unused_imports)] + use opencv::core::LINE_AA; +} + + +const DEFAULT_CONF_FILE: &str = "settings.toml"; + +pub fn main() { + match run_all() { + Ok(()) => {} + Err(err) => { + error!("Error: {}", err); + } + } +} + +fn run_all() -> Result<(), Box> { + // Setup configuration file and set up logs + let filename = DEFAULT_CONF_FILE.to_string(); + let config = Conf::new(&filename); + init_logging(&config); + let config = config?; + let conf2 = config.clone(); + + let client = Client::open(config.redis_url.clone())?; + let mut con: Connection = client.get_connection()?; + + let mut framerate_handler = framerate::Framerate::new(config.framerate)?; + + // Setup handler for interrupt Signals + let running = Arc::new(AtomicBool::new(true)); + let r = running.clone(); + ctrlc::set_handler(move || { + let client = Client::open(conf2.redis_url.clone()).unwrap(); + let mut con: Connection = client.get_connection().unwrap(); + let _a: () = con + .set( + format!("/pl/{}/{}", conf2.client_id, conf2.laser_id), + format!("[]"), + ) + .unwrap(); + r.store(false, Ordering::SeqCst); + })?; + + info!("*** Starting up ***"); + info!("{:?}", config); + + let mut qualibration = Qualibration::new()?; + adding_trackbar(&mut qualibration, "histogram: 0")?; + + let _a: () = con.set(format!("/kpps/{}", conf2.laser_id), 65535)?; + let _a: () = con.set( + format!("/order/{}", conf2.laser_id), + 7, // order to change kpps + )?; + + while running.load(Ordering::SeqCst) { + let _t = framerate_handler.handle_time()?; + ///////////////// + qualibration.run_step()?; + let key = highgui::wait_key(1)?; + if key != -1 { + qualibration.key = key; + } + if key == 27 { // esc in my case + break; + } + + let points_list: Vec = qualibration.draw_sequence()?; //draw::draw(_t)?; + //qualibration.id = next_id; + let v: Vec<(f32, f32, u32)> = points_list + .iter() + .map(|pt| (pt.x, pt.y, u32::from(pt.color))) + .collect(); + // println!("{:?}", v); + let _ = con.set( + format!("/pl/{}/{}", config.client_id, config.laser_id), + format!("{:?}", v), + )?; + + if qualibration.capture_mode { + let millis = std::time::Duration::from_millis(300); // TODO: find solution to know when change has been done + std::thread::sleep(millis); + } + } + + let _ = con.set( + format!("/pl/{}/{}", config.client_id, config.laser_id), + format!("[]"), + )?; + Ok(()) +} diff --git a/src/point.rs b/src/point.rs new file mode 100644 index 0000000..36f13b8 --- /dev/null +++ b/src/point.rs @@ -0,0 +1,43 @@ +pub type Line = Vec<(f32, f32, u32)>; + +#[derive(Debug, Clone, Copy, Default, PartialEq)] +pub struct Point { + pub x: f32, + pub y: f32, + pub color: Color, +} + +#[derive(Debug, Clone, Copy, Default, PartialEq)] +pub struct Color { + pub r: u8, + pub g: u8, + pub b: u8, +} + +impl From for u32 { + fn from(value: Color) -> Self { + let r = value.r as u32; + let g = value.g as u32; + let b = (value.b) as u32; + (r << 16) + (g << 8) + b + } +} + +impl From<(f32, f32, u32)> for Point { + fn from((x, y, color): (f32, f32, u32)) -> Point { + let r = (color >> 16) as u8; + let g = ((color >> 8) & 255) as u8; + let b = (color & 255) as u8; + Point { + x, + y, + color: Color { r, g, b }, + } + } +} + +impl From for (f32, f32, u32) { + fn from(pt: Point) -> (f32, f32, u32) { + (pt.x, pt.y, pt.color.into()) + } +} diff --git a/src/qualibration.rs b/src/qualibration.rs new file mode 100644 index 0000000..e63d8ae --- /dev/null +++ b/src/qualibration.rs @@ -0,0 +1,514 @@ +pub mod annalyse; +pub mod borders; + +use annalyse::{image_diff, is_same_frame}; +use borders::{bord_mult, get_extermities, get_intersection, mix_borders, probabilistic_hough}; +use std::env::args; + +use crate::draw; +use crate::point::{Color, Point}; + +use enum_iterator::{next, Sequence as Seq}; +use opencv::core::Mat; +use opencv::Result; +use std::f64::consts::PI; + +use opencv::core::{bitwise_and, find_file, in_range, Point as OcvPoint, Scalar}; +use opencv::core::{VecN, Vector}; +use opencv::imgcodecs::imwrite; +use opencv::imgcodecs::{imread, IMREAD_COLOR}; +use opencv::imgproc::{canny, cvt_color, line, COLOR_BGR2GRAY}; +use opencv::prelude::*; +use opencv::{ + highgui, + videoio::{self, VideoCapture}, +}; +use std::fs::create_dir; +use std::fs::read_dir; + +use opencv::{ + calib3d, + core::{self, Size}, + imgproc, +}; + +opencv::opencv_branch_4! { + use opencv::imgproc::LINE_AA; +} +opencv::not_opencv_branch_4! { + use opencv::core::LINE_AA; +} + +const DEBUG: bool = true; + +#[derive(Debug, PartialEq, Seq, Copy, Clone)] +pub enum Sequence { +//TODO: avoir le meme nombre d'image en mode capture ET en mode replay + FirstState, + ReadDir, + ComputeArea, + WaitSpace, + Finish, + + LinearConstSpeed, // [multiple test] + JumpFromTo, + + AdaptLineSeg(u16), // [multiple test] find the correct distense + AdaptLineLum, // [multiple test] try minimu, medium, maximum. + // + TestRedSpeed, + TestGreenSpeed, + TestBlueSpeed, + SelectSpeedestColor, // on pourait mettre a jour les valeur a chaque passage + + + BackGround, + UpBorder, + LeftBorder, + DownBorder, + RightBorder, + Vertical(u16), + Horizontal(u16), +} + +#[derive(Debug)] +pub struct Qualibration { + pub cam: VideoCapture, + pub capture_mode: bool, + pub frame: Mat, + pub frame_prev: Mat, + pub img: Vec, + pub id: Option, + pub nb_pt_1: usize, + pub nb_pt_2: usize, + pub nb_liss: i32, + pub tresh: Treshold, + pub dir_name: String, + pub key: i32, + pub canny_v1: i32, + pub canny_v2: i32, + pub hough_param: HoughLine, + pub border_pt: Vec<(f64, f64)>, +} + +impl Qualibration { + pub fn new() -> Result { + let mut dir_name = "".to_string(); //"building.jpg".to_string(); // by default + if let Some(dir_name_arg) = args().nth(1) { + dir_name = dir_name_arg; + } + + let mut cam = videoio::VideoCapture::new(0, videoio::CAP_ANY)?; // 0 is the default camera ; + let opened_cam = videoio::VideoCapture::is_opened(&cam)?; + if !opened_cam { + panic!("Unable to open default camera!"); + } + + let mut frame = Mat::default(); + cam.read(&mut frame)?; + + Ok(Qualibration { + cam, + capture_mode: dir_name.len() == 0, + img: vec![], + frame: Mat::default(), // TODO: init with frame from cam + frame_prev: Mat::default(), + id: Some(Sequence::FirstState), + nb_pt_1: 130, + nb_pt_2: 40, + nb_liss: 10, + tresh: Treshold::new("histogram: 0", 0, 255)?, + dir_name: dir_name.clone(), + key: 10, + canny_v1: 150, + canny_v2: 255, + hough_param: HoughLine { + rho: 100, + theta: 100, + treshold: 30, + min_length: 0, + max_line_gap: 50000, + }, + border_pt: vec![], + }) + } + + pub fn run_step(&mut self) -> Result<(), Box> { + if self.capture_mode { + //println!("pouette: >{}<\n:self{:?}", self.dir_name, self); + self.cam.read(&mut self.frame)?; + } + if self.frame.size()?.width > 0 && self.frame_prev.size()?.width > 0 || !self.capture_mode { + if self.id.is_some() { + self.id = if !self.capture_mode || is_same_frame(&self.frame, &self.frame_prev)? { + if self.id != Some(Sequence::WaitSpace) && self.capture_mode { + self.img.push(self.frame.clone()); + } + self.compute_sequence()?; + self.get_next_id_seq() + } else { + self.id + }; + } + } + println!("sequence: {:?}", self.id); + self.frame_prev = self.frame.clone(); + Ok(()) + } + + pub fn draw_sequence(&self) -> Result, Box> { + let seq = self.id; + let mut pl = vec![]; + //let color = Color { r: 0, g: 30, b: 0 }; + let color = Color { r: 60, g: 0, b: 0 }; + //let color = Color { r: 0, g: 0, b: 50 }; + let p0 = Point { + x: 0., + y: 0., + color, + }; + let p1 = Point { + x: 4095., + y: 0., + color, + }; + let p2 = Point { + x: 4095., + y: 4095., + color, + }; + let p3 = Point { + x: 0., + y: 4095., + color, + }; + let nb1 = self.nb_pt_1; + let nb2 = self.nb_pt_2; + + if seq.is_some() { + match seq.unwrap() { + Sequence::WaitSpace => { + ////let l1 = draw::draw_line(&p0, &p1, nb1, nb2)?; + //let l0 = draw::draw_line(&p0, &p1, nb1, nb2)?; + //let l1 = draw::draw_line(&p1, &p2, nb1, nb2)?; + //let l2 = draw::draw_line(&p3, &p0, nb1, nb2)?; + //let l3 = draw::draw_line(&p2, &p3, nb1, nb2)?; + //pl.extend(l0); + //pl.extend(l1); + //pl.extend(l2); + //pl.extend(l3); + } + Sequence::UpBorder => { + pl = draw::draw_line(&p0, &p1, nb1, nb2)?; + } + Sequence::RightBorder => { + pl = draw::draw_line(&p1, &p2, nb1, nb2)?; + } + Sequence::DownBorder => { + pl = draw::draw_line(&p2, &p3, nb1, nb2)?; + } + Sequence::LeftBorder => { + pl = draw::draw_line(&p3, &p0, nb1, nb2)?; + } + Sequence::Vertical(n) => { + let p1 = Point { + x: n as f32, + y: 0., + color, + }; + let p2 = Point { + x: n as f32, + y: 4095., + color, + }; + pl = draw::draw_line(&p1, &p2, nb1, nb2)?; + } + Sequence::Horizontal(n) => { + let p1 = Point { + x: 0., + y: n as f32, + color, + }; + let p2 = Point { + x: 4095., + y: n as f32, + color, + }; + pl = draw::draw_line(&p1, &p2, nb1, nb2)?; + } + _ => (), + } + } + + Ok(pl) + } + + pub fn get_next_id_seq(&self) -> Option { + let line_max = 4095; + let line_add = 100; + + if self.id.is_none() { + return None; + } + + match self.id.unwrap() { + //Sequence::Finish => Some(Sequence::Finish), + Sequence::Finish => None, + Sequence::WaitSpace => { + //println!("key: {}", self.key); + if self.key == 32 { + next(&Sequence::WaitSpace) + } else { + Some(Sequence::WaitSpace) + } + } + Sequence::Vertical(n) => { + let after = if n > line_max { u16::MAX } else { n + line_add }; + next(&Sequence::Vertical(after)) + } + Sequence::Horizontal(n) => { + let after = if n > line_max { u16::MAX } else { n + line_add }; + next(&Sequence::Horizontal(after)) + } + Sequence::ComputeArea => Some(Sequence::ComputeArea), // + id => next(&id), + } + } + + pub fn compute_sequence(&mut self) -> Result<(), Box> { + if self.id.is_some() { + match self.id.unwrap() { + Sequence::ComputeArea => { + let background: Mat; + let borders: Vec; + + // on recupere les image en fonction de mode: capture/dossier + if !self.capture_mode { + background = self.img[1].clone(); + borders = self.img[2..6].into(); + } else { + background = self.img[0].clone(); + borders = self.img[1..].into(); + } + + // on recupere chaqu'un des 4 bord + let mut bords_pts = vec![]; + for (i, bord) in borders.iter().enumerate() { + let bord_pt = + self.get_one_border(&background, &bord, i)?; + bords_pts.push(bord_pt); + } + + // on calcul le cadre + let border_pt = get_intersection(&bords_pts); + self.border_pt = bord_mult(border_pt, 1.1); + let color: VecN = VecN::new(255., 128., 0., 255.); + let mut mixed = mix_borders(&background, borders)?; + let b = &self.border_pt; + for i in 0..b.len() { + let j = (i + 1) % self.border_pt.len(); + let pa = VecN::from_array([b[i].0 as i32, b[i].1 as i32]); + let pb = VecN::from_array([b[j].0 as i32, b[j].1 as i32]); + let a = OcvPoint::from_vec2(pa); + let b = OcvPoint::from_vec2(pb); + line(&mut mixed, a, b, color, 1, LINE_AA, 0)?; + } + highgui::imshow("mixed bored", &mixed)?; + + // ici on va requadrer la partie de la projection laser de l'image + let warped_image_size = Size::new(1024, 1024); + let roi_corners: Vec = self + .border_pt + .iter() + .map(|(x, y)| OcvPoint::new(*x as i32, *y as i32)) + .collect(); + let dst = [(0, 0), (0, 1024), (1024, 1024), (1024, 0)]; + let dst_corners: Vec = + dst.iter().map(|(x, y)| OcvPoint::new(*x, *y)).collect(); + let roi_corners_mat = Mat::from_slice(&roi_corners[..])?; + let dst_corners_mat = Mat::from_slice(&dst_corners)?; + let h = calib3d::find_homography( + &roi_corners_mat, + &dst_corners_mat, + &mut Mat::default(), + 0, + 3., + )?; //get homography + let mut warped_image = Mat::default(); + imgproc::warp_perspective( + &mixed, + &mut warped_image, + &h, + warped_image_size, + imgproc::INTER_LINEAR, // I dont see difference with INTER_CUBIC + core::BORDER_CONSTANT, + Scalar::default(), + )?; // do perspective transformation + highgui::imshow("Warped Image", &warped_image)?; + } + Sequence::ReadDir => { + if !self.capture_mode { + self.load_image()?; + } + } + Sequence::Finish => { + if self.capture_mode { + self.save_image()? + } + } + _ => (), + } + } + Ok(()) + } + + fn save_image(&self) -> Result<()> { + // on fait le nom de dossier general au cas ou + // on fait un nom de dossier avec le temps + // on sauvgarde toutes les image + let now = std::time::Instant::now(); + let mut name = "image/".to_owned(); + create_dir(&name).unwrap_or(()); + name.push_str(format!("testouille_{now:?}/").as_str()); + let name = format!("testouille_{now:?}"); + + // + for (i, img) in self.img.iter().enumerate() { + let mut name_img = name.clone(); + name_img.push_str(&format!("img_{i}.png")); + imwrite(&name_img, img, &Vector::from_slice(&[6, 6, 6, 0]))?; + } + + Ok(()) + } + + //use std::cmp::Ordering; + fn load_image(&mut self) -> Result<(), Box> { + let mut imgs = vec![]; + let paths = read_dir(&self.dir_name)?; + //let len = paths.size_hint(); + for entry in paths { + let dir = entry?; + let path = dir.path(); + let a: Vec<_> = path.to_str().unwrap().to_string().chars().collect(); + let b: String = a[21..(a.len() - 4)].iter().collect(); + let img_id: i32 = b.parse()?; + let path = format!("{path:?}"); + let path = path[1..(path.len() - 1)].to_owned(); + let img: Mat = imread(&find_file(&path, false, false)?, IMREAD_COLOR)?; + // highgui::imshow(&path, &img)?; + imgs.push((img_id, img)); + } + + imgs.sort_by(|v1, v2| { + if v1.0 > v2.0 { + std::cmp::Ordering::Greater + } else if v1.0 == v2.0 { + std::cmp::Ordering::Equal + } else { + std::cmp::Ordering::Less + } + }); + + for m in imgs { + self.img.push(m.1); + } + + Ok(()) + } + + pub fn get_one_border( + &self, + background: &Mat, + bord: &Mat, + id: usize, + ) -> Result<((f64, f64), (f64, f64))> { + let (t1, s1, l1) = (self.tresh.min_0 as f64, self.tresh.min_1 as f64, self.tresh.min_2 as f64); + let (t2, s2, l2) = (self.tresh.max_0 as f64, self.tresh.max_1 as f64, self.tresh.max_2 as f64); + let min = Mat::from_slice(&[t1, s1, l1])?; + let max = Mat::from_slice(&[t2, s2, l2])?; + let mut color_selected = Mat::default(); + + let diff: Mat = image_diff(bord, background)?; + let _ = in_range(&diff, &min, &max, &mut color_selected); + //highgui::imshow(format!("mask: {id}").as_str(), &color_selected)?; + + let mut bord_treshed = Mat::default(); + bitwise_and(&diff, &diff, &mut bord_treshed, &color_selected)?; + + // Pass the image to gray + let mut diff_gray = Mat::default(); + cvt_color(&diff, &mut diff_gray, COLOR_BGR2GRAY, 0)?; + // Apply Canny edge detector + let mut edges = Mat::default(); + canny( + &diff_gray, + &mut edges, + self.canny_v1 as f64, + self.canny_v2 as f64, + 3, + false, + )?; + let lines = probabilistic_hough(&edges, &self.hough_param, id)?; + //let ((x1, y1), (x2, y2)) = get_extermities(&lines, id); + Ok(get_extermities(&lines, id)) + } +} + +// ca c'est les donner manipuler par les slider +#[derive(Debug, Clone)] +pub struct HoughLine { + pub rho: i32, + pub theta: i32, + pub treshold: i32, + pub min_length: i32, + pub max_line_gap: i32, +} + +// ca c'est les donner qu'on envoie a la fonction +pub struct HoughLineValue { + pub rho: f64, + pub theta: f64, + pub treshold: i32, + pub min_length: f64, + pub max_line_gap: f64, +} + +impl HoughLine { + pub fn get_param(&self) -> HoughLineValue { + HoughLineValue { + rho: self.rho as f64 / 100., + theta: self.theta as f64 / 100. * PI / 180., + treshold: self.treshold, + min_length: self.min_length as f64 / 100., + max_line_gap: self.max_line_gap as f64 / 100., + } + } +} + +#[derive(Clone, Debug)] +pub struct Treshold { + pub win_name: String, + pub min_0: i32, + pub min_1: i32, + pub min_2: i32, + pub max_0: i32, + pub max_1: i32, + pub max_2: i32, +} + +impl Treshold { + pub fn new(name: &str, min: i32, max: i32) -> Result { + let tresh = Treshold { + win_name: name.to_owned(), + min_0: min, + min_1: min, + min_2: min, + max_0: max, + max_1: max, + max_2: max, + }; + + Ok(tresh) + } +} diff --git a/src/qualibration/annalyse.rs b/src/qualibration/annalyse.rs new file mode 100644 index 0000000..ffebf1e --- /dev/null +++ b/src/qualibration/annalyse.rs @@ -0,0 +1,465 @@ +use super::Qualibration; +use super::DEBUG; +//use opencv::prelude::MatTraitConst; +use opencv::prelude::*; //MatTraitConst; + +use opencv::core::{add, subtract, Mat, Point as OcvPoint, Point3_, VecN, CV_8UC3}; +use opencv::highgui::{self, create_trackbar, named_window, WINDOW_AUTOSIZE}; +use opencv::imgproc::{cvt_color, line, COLOR_BGR2GRAY}; +use opencv::Result; + +opencv::opencv_branch_4! { + use opencv::imgproc::LINE_AA; +} +opencv::not_opencv_branch_4! { + use opencv::core::LINE_AA; +} + +use super::Treshold; +const MAX_TRACKBAR: i32 = 255; + +fn draw_histograme_dbg( + window_name: &str, + histo: &Vec, + (from, to): (usize, usize), +) -> Result<()> { + let v: VecN = VecN::new(0., 0., 0., 255.); + let c1: VecN = VecN::new(128., 128., 128., 255.); + let c2: VecN = VecN::new(255., 255., 255., 255.); + //let color: VecN = VecN::new(255., 255., 255., 255.); + let mut img = Mat::new_rows_cols_with_default(256 * 2, 256 * 2, CV_8UC3, v)?; + + let mut max = 0.; + for i in 0..256 { + if histo[i] > max { + max = histo[i]; + } + } + + let v_log = 10.; + + for i in 0..255 { + let x1 = ((i + 0) * 2) as i32; + let x2 = ((i + 1) * 2) as i32; + let y1 = + ((histo[i + 0] as f64 + 1.).log(v_log) / (max as f64).log(v_log) * 2. * 256.) as i32; + let y2 = + ((histo[i + 1] as f64 + 1.).log(v_log) / (max as f64).log(v_log) * 2. * 256.) as i32; + let color = if i >= from && i <= to { c2 } else { c1 }; + let pt1 = OcvPoint::new(x1, y1); + let pt2 = OcvPoint::new(x2, y2); + line(&mut img, pt1, pt2, color, 1, LINE_AA, 0)?; + } + + highgui::imshow(window_name, &img)?; + + Ok(()) +} + +fn draw_histograme(window_name: &str, histo: &Vec) -> Result<()> { + let v: VecN = VecN::new(0., 0., 0., 255.); + let color: VecN = VecN::new(255., 255., 255., 255.); + let mut img = Mat::new_rows_cols_with_default(256 * 2, 256 * 2, CV_8UC3, v)?; + + let mut max = 0.; + for i in 0..256 { + if histo[i] > max { + max = histo[i]; + } + } + + let v_log = 10.; + + for i in 0..255 { + let x1 = ((i + 0) * 2) as i32; + let x2 = ((i + 1) * 2) as i32; + let y1 = + ((histo[i + 0] as f64 + 1.).log(v_log) / (max as f64).log(v_log) * 2. * 256.) as i32; + let y2 = + ((histo[i + 1] as f64 + 1.).log(v_log) / (max as f64).log(v_log) * 2. * 256.) as i32; + let pt1 = OcvPoint::new(x1, y1); + let pt2 = OcvPoint::new(x2, y2); + line(&mut img, pt1, pt2, color, 1, LINE_AA, 0)?; + } + + highgui::imshow(window_name, &img)?; + + Ok(()) +} + +fn draw_histograme_bgr(window_name: &str, histo: &Vec>) -> Result<()> { + let v: VecN = VecN::new(0., 0., 0., 255.); + let b: VecN = VecN::new(255., 0., 0., 255.); + let g: VecN = VecN::new(0., 255., 0., 255.); + let r: VecN = VecN::new(0., 0., 255., 255.); + let color = vec![b, g, r]; + let mut img = Mat::new_rows_cols_with_default(256 * 2, 256 * 2, CV_8UC3, v)?; + + let mut range = vec![vec![f64::MAX, f64::MIN]; 3]; + for j in 0..3 { + for i in 0..256 { + if histo[j][i] > range[j][1] { + range[j][1] = histo[j][i]; + } + if histo[j][i] < range[j][0] { + range[j][0] = histo[j][i]; + } + } + } + + //let v_log = 10.; + + for j in 0..3 { + for i in 0..255 { + let x1 = ((i + 0) * 2) as i32; + let x2 = ((i + 1) * 2) as i32; + let y1 = ((histo[j][i + 0] + 1.).log10() / range[j][1].log10() * 2. * 256.) as i32; + let y2 = ((histo[j][i + 1] + 1.).log10() / range[j][1].log10() * 2. * 256.) as i32; + let pt1 = OcvPoint::new(x1, y1); + let pt2 = OcvPoint::new(x2, y2); + line(&mut img, pt1, pt2, color[j], 1, LINE_AA, 0)?; + } + } + + highgui::imshow(window_name, &img)?; + + Ok(()) +} + +fn draw_histograme_bgr_tresh( + window_name: &str, + histo: &Vec>, + tresh: &Treshold, +) -> Result<()> { + let v: VecN = VecN::new(0., 0., 0., 255.); + let b: VecN = VecN::new(255., 0., 0., 255.); + let g: VecN = VecN::new(0., 255., 0., 255.); + let r: VecN = VecN::new(0., 0., 255., 255.); + let color1 = vec![b, g, r]; + let color2 = vec![b / 2., g / 2., r / 2.]; + let mut img = Mat::new_rows_cols_with_default(256 * 2, 256 * 2, CV_8UC3, v)?; + + let mut vmax = vec![f64::MIN; 3]; + for j in 0..histo.len() { + for i in 0..histo[j].len() { + if histo[j][i] > vmax[j] { + vmax[j] = histo[j][i]; + } + } + } + + //let v_log = 10.; + let max: Vec = [tresh.max_0 as f64, tresh.max_1 as f64, tresh.max_2 as f64].into(); + let min: Vec = [tresh.min_0 as f64, tresh.min_1 as f64, tresh.min_2 as f64].into(); + + //println!("min: {min:?}\tmax: {max:?}"); + + for j in 0..3 { + for i in 0..255 { + let x1 = ((i + 0) * 2) as i32; + let x2 = ((i + 1) * 2) as i32; + let y1 = ((histo[j][i + 0] + 1.).log10() / vmax[j].log10() * 2. * 256.) as i32; + let y2 = ((histo[j][i + 1] + 1.).log10() / vmax[j].log10() * 2. * 256.) as i32; + let pt1 = OcvPoint::new(x1, y1); + let pt2 = OcvPoint::new(x2, y2); + + //let val = (histo[j][i] + 1.).log10() / max[j].log10(); + let (color, thickness) = if i as f64 >= min[j] && i as f64 <= max[j] { + (color1[j], 2) + } else { + (color2[j], 1) + }; + line(&mut img, pt1, pt2, color, thickness, LINE_AA, 0)?; + } + } + + highgui::imshow(window_name, &img)?; + + Ok(()) +} + +// limit = 0.35 c'est bien +pub fn is_same_frame(frame: &Mat, frame_prev: &Mat) -> Result { + let nb_liss: i32 = 50; // plus on lisse la courbe plus on attein la limite facilement + let limit = 0.45; // plus c'est haut, plus on tolere de changement entre 2 image + + let d_bgr = image_diff(frame, frame_prev)?; + let histo = histogram_1d(&d_bgr, nb_liss)?; + let ((_id1, v1), (_id2, v2)) = first_invert(&histo); + + if DEBUG { + // on affiche l'image de la cam + highgui::imshow("cam image", frame)?; + // on affiche l'image de la cam + highgui::imshow("prev image", frame_prev)?; + // on affiche la difference + highgui::imshow("diff image", &d_bgr)?; + // on affiche l'histograme + let ids = ((128 - _id2), (128 + _id1)); + draw_histograme_dbg("histograme", &histo, ids)?; + // -- pour chaque image enregistrer on l'affiche ma ca se fait autre part + } + + if DEBUG { + println!("v1[{_id1}]:{v1}\tv2[{_id1}:{v2}"); + } + + if v1 >= limit || v2 >= limit { + println!("\t XXX DIFFERENT XXX"); + Ok(false) + } else { + println!("\t :) Same (: "); + Ok(true) + } +} + +pub fn image_diff(frame: &Mat, frame_prev: &Mat) -> Result { + let mut diff_bgr = Mat::default(); + let mut diff_bgr_2 = Mat::default(); + let mut d_bgr = Mat::default(); + let (row, col) = (frame.rows(), frame.cols()); + let mask = Mat::default(); + let v: VecN = VecN::new(128., 128., 128., 128.); + let mid: Mat = Mat::new_rows_cols_with_default(row, col, CV_8UC3, v)?; + + // ca parait etonant d'enlever la difference dans l'autre sens mais paradoxalement, ca permet + // d'avoir toutes les valeur, pck a chaque fois les valeur negative sont mise a 0 dans + // l'operation de soustraction + subtract(frame, frame_prev, &mut diff_bgr, &mask, -1)?; + add(&diff_bgr, &mid, &mut diff_bgr_2, &mask, -1)?; + subtract(frame_prev, frame, &mut diff_bgr, &mask, -1)?; + subtract(&diff_bgr_2, &diff_bgr, &mut d_bgr, &mask, -1)?; + + Ok(d_bgr) +} + +fn histogram_3d(m: &Mat, nb_liss: i32) -> Result>> { + let (cols, rows) = (m.cols(), m.rows()); + let mut histo = vec![vec![0.; 256]; 3]; + + // on calcule l'histograme + for j in 0..rows { + for i in 0..cols { + let v: &Point3_ = m.at_2d(j, i)?; + let (b, g, r) = (v.x as usize, v.y as usize, v.z as usize); + histo[2][r] += 1.; + histo[1][g] += 1.; + histo[0][b] += 1.; + } + } + + // on lisse l'histograme + for j in 0..3 { + let mut tmp = histo[j].clone(); + for _ in 0..nb_liss { + for i in 1..(tmp.len() - 1) { + histo[j][i] = (tmp[i - 1] + 1. * tmp[i] + tmp[i + 1]) / 3.; + } + tmp = histo[j].clone(); + } + } + + Ok(histo) +} + +fn histogram_1d(m: &Mat, nb_liss: i32) -> Result> { + let (cols, rows) = (m.cols(), m.rows()); + let mut histo = vec![0; 256]; + let mut m_gray = Mat::default(); + + // on convertie en gris + cvt_color(m, &mut m_gray, COLOR_BGR2GRAY, 0)?; + // on calcule l'histograme + for j in 0..rows { + for i in 0..cols { + let v: &u8 = m_gray.at_2d(j, i)?; + let id = *v as usize; + histo[id] += 1; + } + } + + // on lisse l'histograme + let mut histo: Vec = histo.iter().map(|x| *x as f64).collect(); + let mut tmp = histo.clone(); + for _ in 0..nb_liss { + for i in 1..(histo.len() - 1) { + histo[i] = (tmp[i - 1] + 2. * tmp[i] + tmp[i + 1]) / 4.; + } + tmp = histo.clone(); + } + + Ok(histo) +} + +fn first_invert(histo: &Vec) -> ((usize, f64), (usize, f64)) { + // on applique un log puis on normalise mar le log du max + let mut normalised = vec![0.; histo.len()]; + let mut p1 = vec![0.; histo.len() / 2]; + let mut p2 = vec![0.; histo.len() / 2]; + let mut dp1 = vec![0.; histo.len() / 2]; + let mut dp2 = vec![0.; histo.len() / 2]; + let mid = (histo.len() + 1) / 2; + let max = (histo[mid] as f64).log10(); // on par du principe que le max est au centre + + for i in 0..histo.len() { + normalised[i] = (histo[i] as f64 + 1.).log10() / max; + } + for i in (mid)..(histo.len() - 1) { + p1[i - mid] = mid as f64 * ((normalised[mid] - normalised[i + 1]) / (i - mid + 2) as f64); + } + for i in (1..mid).rev() { + p2[mid - i - 1] = mid as f64 * ((normalised[mid] - normalised[i]) / (mid - i) as f64); + } + for i in 0..(mid - 1) { + dp1[i] = p1[i + 1] - p1[i]; + dp2[i] = p2[i + 1] - p2[i]; + } + + let mut dist_1 = 0; + for (i, v) in dp1.iter().enumerate() { + if v < &0. { + dist_1 = i; + break; + } + } + let mut dist_2 = 0; + for (i, v) in dp2.iter().enumerate() { + if v < &0. { + dist_2 = i; + break; + } + } + + ( + (dist_1, normalised[mid + dist_1]), + (dist_2, normalised[mid - dist_2]), + ) +} + +pub fn adding_trackbar(mem: &mut Qualibration, winname: &str) -> Result<()> { + //println!("winname: {winname}"); + named_window(winname, WINDOW_AUTOSIZE)?; + associate_trackbar(winname, &mut mem.tresh)?; + create_trackbar( + "nb_liss", + winname, + Some(&mut mem.nb_liss), + MAX_TRACKBAR, + None, + )?; + + //highgui + let winname = format!("{}: {}", winname, 0); //"bord selected: 0"; + named_window(winname.as_str(), WINDOW_AUTOSIZE)?; + highgui::move_window(winname.as_str(), 20, 20)?; + // + create_trackbar( + "canny min", + winname.as_str(), + Some(&mut mem.canny_v1), + MAX_TRACKBAR, + None, + )?; + create_trackbar( + "canny max", + winname.as_str(), + Some(&mut mem.canny_v2), + MAX_TRACKBAR, + None, + )?; + + create_trackbar( + "rho : ", + winname.as_str(), + Some(&mut mem.hough_param.rho), + 1000, + None, + )?; + create_trackbar( + "theta : ", + winname.as_str(), + Some(&mut mem.hough_param.theta), + 1000, + None, + )?; + create_trackbar( + "treshold: ", + winname.as_str(), + Some(&mut mem.hough_param.treshold), + 255, + None, + )?; + create_trackbar( + "min_leng: ", + winname.as_str(), + Some(&mut mem.hough_param.min_length), + 1000, + None, + )?; + create_trackbar( + "max_gap : ", + winname.as_str(), + Some(&mut mem.hough_param.max_line_gap), + 500000, + None, + )?; + + //let winname = "bord selected: 0"; + //create_trackbar("scale : ", winname, Some(&mut mem.lsd_param.scale ), 1000, None)?; + //create_trackbar("sigma_scal", winname, Some(&mut mem.lsd_param.sigma_scale), 1000, None)?; + //create_trackbar("quant : ", winname, Some(&mut mem.lsd_param.quant ), 1000, None)?; + //create_trackbar("ang_th : ", winname, Some(&mut mem.lsd_param.ang_th ), 1000, None)?; + //create_trackbar("log_eps : ", winname, Some(&mut mem.lsd_param.log_eps ), 1000, None)?; + //create_trackbar("density_th", winname, Some(&mut mem.lsd_param.density_th ), 1000, None)?; + //create_trackbar("n_bins : ", winname, Some(&mut mem.lsd_param.n_bins ), 1000, None)?; + Ok(()) +} + +fn associate_trackbar(winname: &str, tresh: &mut Treshold) -> Result<()> { + create_trackbar( + "blue min: ", + winname, + Some(&mut tresh.min_0), + MAX_TRACKBAR, + None, + )?; + create_trackbar( + "blue max: ", + winname, + Some(&mut tresh.max_0), + MAX_TRACKBAR, + None, + )?; + + create_trackbar( + "green min: ", + winname, + Some(&mut tresh.min_1), + MAX_TRACKBAR, + None, + )?; + create_trackbar( + "green max: ", + winname, + Some(&mut tresh.max_1), + MAX_TRACKBAR, + None, + )?; + + create_trackbar( + "red min: ", + winname, + Some(&mut tresh.min_2), + MAX_TRACKBAR, + None, + )?; + create_trackbar( + "red max: ", + winname, + Some(&mut tresh.max_2), + MAX_TRACKBAR, + None, + )?; + + Ok(()) +} diff --git a/src/qualibration/borders.rs b/src/qualibration/borders.rs new file mode 100644 index 0000000..1302317 --- /dev/null +++ b/src/qualibration/borders.rs @@ -0,0 +1,182 @@ +use super::HoughLine; +use crate::utils::{CartesianEquation, EqAffine, Pt}; +use opencv::core::{add, subtract, Mat, VecN, Vector, CV_8UC3}; +//use opencv::prelude::MatTraitConst; +use opencv::imgproc::{cvt_color, hough_lines_p, COLOR_GRAY2BGR}; +use opencv::prelude::*; //MatTraitConst; +use opencv::types::VectorOfVec4i; +use opencv::Result; + +pub fn mix_borders(background: &Mat, borders: Vec) -> Result { + let (row, col) = (background.rows(), background.cols()); + //let mask = Mat::default(); + let v: VecN = VecN::new(0., 0., 0., 0.); + let mut sum_diff_0 = Mat::new_rows_cols_with_default(row, col, CV_8UC3, v)?; + let mut sum_diff_1 = Mat::new_rows_cols_with_default(row, col, CV_8UC3, v)?; + let mask = Mat::default(); + let mut tmp = Mat::default(); + + // on va faire la somme des difference + // on va les ajouter a l'image du fond + for bord in borders { + //let diff = image_diff(&bord, background)?; + //add(&diff, &mid, &mut diff_bgr_2, &mask, -1)?; + let mut diff = Mat::default(); + subtract(&bord, background, &mut diff, &mask, -1)?; + add(&diff, &sum_diff_0, &mut tmp, &mask, -1)?; + sum_diff_0 = tmp.clone(); + + subtract(background, &bord, &mut diff, &mask, -1)?; + add(&diff, &sum_diff_1, &mut tmp, &mask, -1)?; + sum_diff_1 = tmp.clone(); + } + //let v: VecN = VecN::new(128., 128., 128., 128.); + //let mid = Mat::new_rows_cols_with_default(row, col, CV_8UC3, v)?; + let mut tmp = Mat::default(); + //let mut tmp2 = Mat::default(); + let mut mix = Mat::default(); + add(&sum_diff_0, &background, &mut tmp, &mask, -1)?; + subtract(&tmp, &sum_diff_1, &mut mix, &mask, -1)?; + + Ok(mix) +} + +//impl Add for (f64, f64) { +//} +pub fn bord_mult(pt: Vec<(f64, f64)>, factor: f64) -> Vec<(f64, f64)> { + let pt: Vec = pt.iter().map(|p| Pt::from(p)).collect(); + + let mut pa = vec![]; + let mut pb = vec![]; + for i in 0..pt.len() { + let k = (i + pt.len() - 1) % pt.len(); + let j = (i + 1) % pt.len(); + pa.push((pt[i] - pt[j]) * factor + pt[j]); + pb.push((pt[i] - pt[k]) * factor + pt[k]); + } + + let mut eq = vec![]; + for i in 0..pt.len() { + let j = (i + 1) % pt.len(); + eq.push(CartesianEquation::new_from_pt(&pb[i], &pa[j])); + } + + let mut p_out = vec![]; + for i in 0..pt.len() { + let k = (i + pt.len() - 1) % pt.len(); + p_out.push(eq[i].intersection(&eq[k]).unwrap()); // TODO: faire un truc pour le unwrap... + // normalement c'est un gars sur ta + // compris ;)... mais bon... un alignement + // malencontreux ca arrive vite + } + + p_out.iter().map(|p| (p.x, p.y)).collect() +} + +// en fait ca marche pas dutout...next time +pub fn bord_mult_v2(pt: Vec<(f64, f64)>, factor: f64) -> Vec<(f64, f64)> { + let mut pt: Vec = pt.iter().map(|p| Pt::from(p)).collect(); + + let mut pn = vec![]; + for i in 0..pt.len() { + let j = (i + 2) % pt.len(); + pn.push((pt[i] - pt[j]) * factor + pt[j]); + } + + pn.iter().map(|p| (p.x, p.y)).collect() +} + +pub fn get_intersection(pts: &[((f64, f64), (f64, f64))]) -> Vec<(f64, f64)> { + let mut eq_cart = vec![]; + for i in 0..pts.len() { + eq_cart.push(CartesianEquation::new_from_tuple(pts[i])); + } + + let mut points = vec![]; + for i in 0..eq_cart.len() { + let id_next = (i + 1) % eq_cart.len(); + let pt = eq_cart[i].intersection(&eq_cart[id_next]).unwrap(); // TODO verifier quand meme la sortie au lieu de unwrap salement... xD + points.push((pt.x, pt.y)); + } + + points +} + +pub fn get_extermities(lines: &Vector>, id: usize) -> ((f64, f64), (f64, f64)) { + let mut p0: (f64, f64) = (0., 0.); + let mut p1: (f64, f64) = (0., 0.); + let (mut min, mut max): (f64, f64) = (f64::MAX, f64::MIN); + //let mut eq: (f64, f64, f64) = (0., 0., 0.); + let mut dst_sum = 0.; + let mut v_eq = vec![]; + + // on cherche les extremite + for l in lines { + // rename value and switch x and y if necessery + let (mut a0, mut b0, mut a1, mut b1) = if id % 2 == 0 { + (l[0] as f64, l[1] as f64, l[2] as f64, l[3] as f64) + } else { + (l[1] as f64, l[0] as f64, l[3] as f64, l[2] as f64) // switch x <-> y + }; + + // reorder if not + if a0 > a1 { + (a0, b0, a1, b1) = (a1, b1, a0, b0); + } + + // update min/max + min = min.min(a0).min(a1); + max = max.max(a0).max(a1); + + // cancel computation if devide by zero + if a1 - a0 == 0. { + continue; + } + + let eq = EqAffine::new(a0, b0, a1, b1); + dst_sum += eq.dst; + v_eq.push(eq); + } + + p0.0 = min; + p1.0 = max; + for eq in v_eq { + p0.1 += eq.get_val_dst(min); + p1.1 += eq.get_val_dst(max); + } + p0.1 /= dst_sum; + p1.1 /= dst_sum; + + // revert x-y if already reverted previously + if id % 2 != 0 { + p0 = (p0.1, p0.0); + p1 = (p1.1, p1.0); + } + + (p0, p1) +} + +pub fn probabilistic_hough( + edges: &Mat, + hough_param: &HoughLine, + id: usize, +) -> Result>> { + let mut p_lines = VectorOfVec4i::new(); + let mut probabalistic_hough = Mat::default(); + + cvt_color(edges, &mut probabalistic_hough, COLOR_GRAY2BGR, 0)?; + + // 2. Use Probabilistic Hough Transform + let p = hough_param.get_param(); + hough_lines_p( + edges, + &mut p_lines, + p.rho, + p.theta, + p.treshold, + p.min_length, + p.max_line_gap, + )?; + + Ok(p_lines) +} diff --git a/src/utils.rs b/src/utils.rs new file mode 100644 index 0000000..65f339b --- /dev/null +++ b/src/utils.rs @@ -0,0 +1,417 @@ +use crate::point::Point; +static NEAR_ZERO: f64 = 0.000001; + +use std::ops::Add; +use std::ops::Mul; +use std::ops::MulAssign; +use std::ops::Sub; + +impl Add for Pt { + type Output = Self; + + fn add(self, other: Self) -> Self { + Self { + x: self.x + other.x, + y: self.y + other.y, + } + } +} + +impl Sub for Pt { + type Output = Self; + + fn sub(self, other: Self) -> Self { + Self { + x: self.x - other.x, + y: self.y - other.y, + } + } +} + +impl MulAssign for Pt { + fn mul_assign(&mut self, rhs: f64) { + self.x *= rhs; + self.y *= rhs; + } +} + +impl Mul for Pt { + type Output = Self; + + fn mul(self, rhs: f64) -> Self { + Pt { + x: self.x * rhs, + y: self.y * rhs, + } + } +} + +#[derive(Debug, Clone, Copy, PartialEq, PartialOrd)] +pub struct Pt { + pub x: f64, + pub y: f64, +} + +impl From for (f64, f64) { + fn from(pt: Pt) -> Self { + (pt.x, pt.y) + } +} + +impl From<(f64, f64)> for Pt { + fn from((x, y): (f64, f64)) -> Self { + Pt { x, y } + } +} + +impl From<&(f64, f64)> for Pt { + fn from((x, y): &(f64, f64)) -> Self { + Pt { x: *x, y: *y } + } +} + +impl Pt { + pub fn new(x: f64, y: f64) -> Self { + Self { x, y } + } + + pub fn mul_assign(&mut self, rhs: f64) { + self.x *= rhs; + self.y *= rhs; + } +} + +#[derive(Debug, Clone, Copy)] +pub struct CartesianEquation { + pub a: f64, + pub b: f64, + pub c: f64, +} + +#[allow(dead_code)] +impl CartesianEquation { + pub fn new_from_tuple(((x0, y0), (x1, y1)): ((f64, f64), (f64, f64))) -> Self { + let p0 = Pt { x: x0, y: y0 }; + let p1 = Pt { x: x1, y: y1 }; + + CartesianEquation::new_from_pt(&p0, &p1) + } + + pub fn new_from_pt(p0: &Pt, p1: &Pt) -> Self { + let a = p0.y - p1.y; + let b = p1.x - p0.x; + let c = -(a * p0.x + b * p0.y); + + Self { a, b, c } + } + + pub fn from_parametric_eq(pt: &Pt, dir: &Pt) -> Self { + let p0 = pt.clone(); + let p1 = Pt { + x: pt.x + dir.x, + y: pt.y + dir.y, + }; + CartesianEquation::new_from_pt(&p0, &p1) + } + + pub fn new(a: f64, b: f64, c: f64) -> Self { + Self { a, b, c } + } + + pub fn is_similar(&self, other: &Self) -> bool { + if self.b.abs() > NEAR_ZERO { + let y0 = -self.a / self.b; + let y1 = -other.a / other.b; + + return (y0 - y1).abs() <= NEAR_ZERO; + } else { + let x0 = -self.b / self.a; + let x1 = -other.b / other.a; + + return (x0 - x1).abs() <= NEAR_ZERO; + } + } + + pub fn intersection(&self, other: &Self) -> Option { + let (a0, b0, c0) = (self.a, self.b, self.c); + let (a1, b1, c1) = (other.a, other.b, other.c); + + if self.is_similar(other) { + return None; + } + + let x = (b0 * (c0 - c1) - c0 * (b0 - b1)) / (b0 * (a1 - a0) + a0 * (b0 - b1)); + let y = (a0 * (c0 - c1) - c0 * (a0 - a1)) / (a0 * (b1 - b0) + b0 * (a0 - a1)); + + Some(Pt { x, y }) + } +} + +pub struct EqAffine { + pub a: f64, + pub b: f64, + pub dst: f64, +} + +impl EqAffine { + pub fn new(a0: f64, b0: f64, a1: f64, b1: f64) -> Self { + let dx = a1 - a0; + let dy = b1 - b0; + let dst = (dx * dx + dy * dy).sqrt(); + let a = dy / dx; + let b = b0 - a * a0; + EqAffine { a, b, dst } + } + + pub fn get_val(&self, var: f64) -> f64 { + self.a * var + self.b + } + + pub fn get_val_dst(&self, var: f64) -> f64 { + self.dst * self.get_val(var) + } +} + +//#[derive(Serialize,Deserialize,Debug,Clone,Copy)] +#[derive(Debug, Clone, Copy)] +pub struct Keystone { + pub p0: Pt, + pub p1: Pt, + pub p2: Pt, + pub p3: Pt, + pub fx: Option, + pub fy: Option, + pub factor: f64, + pub translate: f64, +} + +#[allow(dead_code)] +impl Keystone { + pub fn new( + pt0: (f64, f64), + pt1: (f64, f64), + pt2: (f64, f64), + pt3: (f64, f64), + factor: f64, + translate: f64, + ) -> Self { + let eqx1 = CartesianEquation::new_from_pt(&Pt::from(pt0), &Pt::from(pt1)); + let eqx2 = CartesianEquation::new_from_pt(&Pt::from(pt3), &Pt::from(pt2)); + + let eqy1 = CartesianEquation::new_from_pt(&Pt::from(pt0), &Pt::from(pt3)); + let eqy2 = CartesianEquation::new_from_pt(&Pt::from(pt1), &Pt::from(pt2)); + + let fx = eqx1.intersection(&eqx2); + let fy = eqy1.intersection(&eqy2); + + Keystone { + p0: Pt::from(pt0), + p1: Pt::from(pt1), + p2: Pt::from(pt2), + p3: Pt::from(pt3), + fx, + fy, + factor, + translate, + } + } + + pub fn transform(&self, pt: &Point) -> Point { + let x = (pt.x as f64 + self.translate) / self.factor; + let y = (pt.y as f64 + self.translate) / self.factor; + let ptx: Pt = Pt { + x: self.p0.x + x * (self.p1.x - self.p0.x), + y: self.p0.y + x * (self.p1.y - self.p0.y), + }; + + let pty: Pt = Pt { + x: self.p0.x + y * (self.p3.x - self.p0.x), + y: self.p0.y + y * (self.p3.y - self.p0.y), + }; + + let eqx = if self.fx.is_some() { + CartesianEquation::new_from_pt(&(self.fx.unwrap()), &pty) + } else { + let dir = Pt { + x: self.p1.x - self.p0.x, + y: self.p1.y - self.p0.y, + }; + CartesianEquation::from_parametric_eq(&pty, &dir) + }; + + let eqy = if self.fy.is_some() { + CartesianEquation::new_from_pt(&(self.fy.unwrap()), &ptx) + } else { + let dir = Pt { + x: self.p3.x - self.p0.x, + y: self.p3.y - self.p0.y, + }; + CartesianEquation::from_parametric_eq(&ptx, &dir) + }; + + let intersection = eqx.intersection(&eqy).unwrap(); + + Point { + x: intersection.x as f32, + y: intersection.y as f32, + ..*pt + } + } +} + +//impl Transformers for Keystone { +// fn apply(&self, point_list: &[Point]) -> Vec { +// point_list.iter() +// .map(| pt | { +// self.transform(pt) +// }).collect() +// } +//} + +#[cfg(test)] +mod test_keystone_correction { + use super::*; + + #[test] + fn simple_y_focal() { + let keystone = Keystone::new((0.1, 0.0), (0.9, 0.0), (1.0, 1.0), (0.0, 1.0), 1.0, 0.); + let must_be = Pt::new(0.5, -4.0); + + let x_are_similar = (must_be.x - keystone.fy.unwrap().x).abs() < NEAR_ZERO; + let y_are_similar = (must_be.y - keystone.fy.unwrap().y).abs() < NEAR_ZERO; + + assert!(x_are_similar); + assert!(y_are_similar); + assert_eq!(None, keystone.fx); + } + + #[test] + fn simple_x_focal() { + let keystone = Keystone::new((0.0, 0.0), (1.0, 0.1), (1.0, 0.9), (0.0, 1.0), 1., 0.); + let must_be = Pt::new(5., 0.5); + + let x_are_similar = (must_be.x - keystone.fx.unwrap().x).abs() < NEAR_ZERO; + let y_are_similar = (must_be.y - keystone.fx.unwrap().y).abs() < NEAR_ZERO; + + println!("keystone:{:?}", keystone); + assert!(x_are_similar); + assert!(y_are_similar); + assert_eq!(None, keystone.fy); + } + + #[test] + fn zero_correction() { + // on change rien + let keystone = Keystone::new((0.0, 0.0), (1.0, 0.0), (1.0, 1.0), (0.0, 1.0), 1., 0.); + let pt = Point::from((0.34, 0.2, 0)); + let res = keystone.transform(&pt); + + assert!(((pt.x - res.x).abs() as f64) < NEAR_ZERO); + assert!(((pt.y - res.y).abs() as f64) < NEAR_ZERO); + } + + #[test] + fn x_correction_1() { + let keystone = Keystone::new((0.1, 0.0), (0.9, 0.0), (1.0, 1.0), (0.0, 1.0), 1.0, 0.); + let pt = Point::from((0.1, 0., 0)); + let res = keystone.transform(&pt); + let must_be = Point::from((0.18, 0., 0)); + + assert!(((must_be.x - res.x).abs() as f64) < NEAR_ZERO); + assert!(((must_be.y - res.y).abs() as f64) < NEAR_ZERO); + } + + #[test] + fn x_correction_2() { + let keystone = Keystone::new((0.0, 0.0), (0.9, 0.0), (1.0, 1.0), (0.0, 1.0), 1.0, 0.); + let pt = Point::from((0.1, 0.1, 0)); + let res = keystone.transform(&pt); + let must_be = Point::from((0.091, 0.1, 0)); + + assert!(((must_be.x - res.x).abs() as f64) < NEAR_ZERO); + assert!(((must_be.y - res.y).abs() as f64) < NEAR_ZERO); + } + + #[test] + fn y_correction_1() { + let keystone = Keystone::new((0.0, 0.0), (1.0, 0.1), (1.0, 0.9), (0.0, 1.0), 1.0, 0.); + let pt = Point::from((0., 0.1, 0)); + let res = keystone.transform(&pt); + let must_be = Point::from((0., 0.1, 0)); + + assert!(((must_be.x - res.x).abs() as f64) < NEAR_ZERO); + assert!(((must_be.y - res.y).abs() as f64) < NEAR_ZERO); + } + + #[test] + fn y_correction_2() { + let keystone = Keystone::new((0.0, 0.0), (1.0, 0.0), (1.0, 0.9), (0.0, 1.0), 1.0, 0.); + let pt = Point::from((0.1, 0.1, 0)); + let res = keystone.transform(&pt); + let must_be = Point::from((0.1, 0.099, 0)); + + assert!(((must_be.x - res.x).abs() as f64) < NEAR_ZERO); + assert!(((must_be.y - res.y).abs() as f64) < NEAR_ZERO); + } +} + +#[cfg(test)] +mod test_cartesian_equation { + use super::*; + + #[test] + fn similar_eq_1() { + let p0a = Pt::new(0.1, 0.0); + let p1a = Pt::new(0.9, 0.0); + let eq0 = CartesianEquation::new_from_pt(&p0a, &p1a); + + let p0b = Pt::new(0.0, 1.0); + let p1b = Pt::new(1.0, 1.0); + let eq1 = CartesianEquation::new_from_pt(&p0b, &p1b); + + assert!(eq0.is_similar(&eq1)); + } + + #[test] + fn similar_eq_2() { + let p0a = Pt::new(0.0, 0.0); + let p1a = Pt::new(0.0, 1.0); + let eq0 = CartesianEquation::new_from_pt(&p0a, &p1a); + + let p0b = Pt::new(1.0, 0.1); + let p1b = Pt::new(1.0, 0.9); + let eq1 = CartesianEquation::new_from_pt(&p0b, &p1b); + + assert!(eq0.is_similar(&eq1)); + } + + #[test] + fn paralle_value() { + let (p0, p1) = (Pt { x: 0., y: 0. }, Pt { x: 1., y: 4. }); + let (p2, p3) = (Pt { x: 1., y: 3. }, Pt { x: 0., y: -1. }); + let eq0 = CartesianEquation::new_from_pt(&p0, &p1); + let eq1 = CartesianEquation::new_from_pt(&p2, &p3); + + let res = eq0.intersection(&eq1); + + assert_eq!(res, None); + } + + #[test] + fn test_intersection() { + let d0 = Pt { x: 1., y: 1. }; + let p0 = Pt { x: 0., y: 0. }; + + let d1 = Pt { x: -1., y: 1. }; + let p1 = Pt { x: 4., y: 0. }; + + let must_be = Pt { x: 2., y: 2. }; + + let eq0 = CartesianEquation::from_parametric_eq(&p0, &d0); + let eq1 = CartesianEquation::from_parametric_eq(&p1, &d1); + + let intersect = eq0.intersection(&eq1).unwrap(); + + assert_eq!(must_be, intersect); + } +}