From 9e596306b1dca379c5b3d54712058fd4be2e1213 Mon Sep 17 00:00:00 2001 From: SarangaR <59266397+SarangaR@users.noreply.github.com> Date: Wed, 19 Mar 2025 00:58:41 -0400 Subject: [PATCH 1/5] path_cv in progress --- .envrc | 1 + .gitignore | 1 + flake.lock | 48 +++++ flake.nix | 57 ++++++ src/missions/path_align.rs | 1 + src/vision/mod.rs | 1 + src/vision/path_cv.rs | 371 +++++++++++++++++++++++++++++++++++++ 7 files changed, 480 insertions(+) create mode 100644 .envrc create mode 100644 flake.lock create mode 100644 flake.nix create mode 100644 src/vision/path_cv.rs diff --git a/.envrc b/.envrc new file mode 100644 index 00000000000..3550a30f2de --- /dev/null +++ b/.envrc @@ -0,0 +1 @@ +use flake diff --git a/.gitignore b/.gitignore index 3875fd529aa..b925db05b40 100644 --- a/.gitignore +++ b/.gitignore @@ -9,6 +9,7 @@ graphs sysroot-jetson target-jetson console/ +.direnv/ # Ignore all log files generated logging/ diff --git a/flake.lock b/flake.lock new file mode 100644 index 00000000000..3f56904de95 --- /dev/null +++ b/flake.lock @@ -0,0 +1,48 @@ +{ + "nodes": { + "nixpkgs": { + "locked": { + "lastModified": 1725910328, + "narHash": "sha256-n9pCtzGZ0httmTwMuEbi5E78UQ4ZbQMr1pzi5N0LAG8=", + "owner": "NixOS", + "repo": "nixpkgs", + "rev": "5775c2583f1801df7b790bf7f7d710a19bac66f4", + "type": "github" + }, + "original": { + "owner": "NixOS", + "ref": "nixpkgs-unstable", + "repo": "nixpkgs", + "type": "github" + } + }, + "root": { + "inputs": { + "nixpkgs": "nixpkgs", + "rust-overlay": "rust-overlay" + } + }, + "rust-overlay": { + "inputs": { + "nixpkgs": [ + "nixpkgs" + ] + }, + "locked": { + "lastModified": 1721355572, + "narHash": "sha256-I4TQ2guV9jTmZsXeWt5HMojcaqNZHII4zu0xIKZEovM=", + "owner": "oxalica", + "repo": "rust-overlay", + "rev": "d5bc7b1b21cf937fb8ff108ae006f6776bdb163d", + "type": "github" + }, + "original": { + "owner": "oxalica", + "repo": "rust-overlay", + "type": "github" + } + } + }, + "root": "root", + "version": 7 +} diff --git a/flake.nix b/flake.nix new file mode 100644 index 00000000000..655c11ceab7 --- /dev/null +++ b/flake.nix @@ -0,0 +1,57 @@ +{ + description = "A tool for building sysroots for cross compilation"; + + inputs = { + nixpkgs.url = "github:NixOS/nixpkgs/nixpkgs-unstable"; + rust-overlay = { + url = "github:oxalica/rust-overlay"; + inputs.nixpkgs.follows = "nixpkgs"; + }; + }; + + outputs = { self, nixpkgs, rust-overlay }: + let + supportedSystems = [ "x86_64-linux" "aarch64-linux" "x86_64-darwin" "aarch64-darwin" ]; + forEachSupportedSystem = f: nixpkgs.lib.genAttrs supportedSystems (system: f { + pkgs = import nixpkgs { + inherit system; + overlays = [ rust-overlay.overlays.default self.overlays.default ]; + }; + }); + in + { + overlays.default = final: prev: { + rustToolchain = + let + rust = prev.rust-bin; + in + if builtins.pathExists ./rust-toolchain.toml then + rust.fromRustupToolchainFile ./rust-toolchain.toml + else if builtins.pathExists ./rust-toolchain then + rust.fromRustupToolchainFile ./rust-toolchain + else + rust.stable.latest.default.override { + extensions = [ "rust-src" "rustfmt" ]; + }; + }; + + devShells = forEachSupportedSystem ({ pkgs }: { + default = pkgs.mkShell { + packages = with pkgs; [ + rustToolchain + openssl + pkg-config + cargo-deny + cargo-edit + cargo-watch + rust-analyzer + ]; + + env = { + # Required by rust-analyzer + RUST_SRC_PATH = "${pkgs.rustToolchain}/lib/rustlib/src/rust/library"; + }; + }; + }); + }; +} diff --git a/src/missions/path_align.rs b/src/missions/path_align.rs index 77ea658181f..99be36844bc 100644 --- a/src/missions/path_align.rs +++ b/src/missions/path_align.rs @@ -17,6 +17,7 @@ use crate::{ vision::{DetectTarget, ExtractPosition, MidPoint, VisionNormBottom}, }, vision::path::Path, + vision::path_cv::PathCV, }; use super::{ diff --git a/src/vision/mod.rs b/src/vision/mod.rs index dc14d541cfc..67bea5aabcc 100644 --- a/src/vision/mod.rs +++ b/src/vision/mod.rs @@ -22,6 +22,7 @@ pub mod image_prep; pub mod nn_cv2; pub mod octagon; pub mod path; +pub mod path_cv; pub mod pca; pub mod yolo_model; diff --git a/src/vision/path_cv.rs b/src/vision/path_cv.rs new file mode 100644 index 00000000000..f6e5fd3affe --- /dev/null +++ b/src/vision/path_cv.rs @@ -0,0 +1,371 @@ +use std::{fs::create_dir_all, ops::RangeInclusive}; + +use itertools::Itertools; +use opencv::{ + core::{in_range, Size, VecN, Vector}, + imgcodecs::imwrite, + imgproc::{cvt_color, COLOR_RGB2YUV, COLOR_YUV2RGB}, + prelude::{Mat, MatTraitConst, MatTraitConstManual}, +}; +use uuid::Uuid; + +use crate::vision::image_prep::{binary_pca, cvt_binary_to_points}; + +use super::{ + image_prep::{kmeans, resize}, + pca::PosVector, + MatWrapper, VisualDetection, VisualDetector, +}; + +static FORWARD: (f64, f64) = (0.0, -1.0); + +#[derive(Debug, PartialEq)] +pub struct Yuv { + pub y: u8, + pub u: u8, + pub v: u8, +} + +impl From<&VecN> for Yuv { + fn from(value: &VecN) -> Self { + Self { + y: value[0], + u: value[1], + v: value[2], + } + } +} + +impl From<&Yuv> for VecN { + fn from(val: &Yuv) -> Self { + VecN::from_array([val.y, val.u, val.v]) + } +} + +impl Yuv { + fn in_range(&self, range: &RangeInclusive) -> bool { + self.y >= range.start().y + && self.u >= range.start().u + && self.v >= range.start().v + && self.y <= range.end().y + && self.u <= range.end().u + && self.v <= range.end().v + } +} + +#[derive(Debug)] +pub struct PathCV { + color_bounds: RangeInclusive, + width_bounds: RangeInclusive, + num_regions: i32, + size: Size, + attempts: i32, + image: MatWrapper, +} + +impl PathCV { + pub fn image(&self) -> Mat { + (*self.image).clone() + } +} + +impl PathCV { + pub fn new( + color_bounds: RangeInclusive, + width_bounds: RangeInclusive, + num_regions: i32, + size: Size, + attempts: i32, + ) -> Self { + Self { + color_bounds, + width_bounds, + num_regions, + size, + attempts, + image: Mat::default().into(), + } + } +} + +impl Default for PathCV { + fn default() -> Self { + PathCV::new( + (Yuv { y: 0, u: 0, v: 127 })..=(Yuv { + y: 255, + u: 127, + v: 255, + }), + 20.0..=800.0, + 4, + Size::from((400, 300)), + 3, + ) + } +} + +fn compute_angle(v1: (f64, f64), v2: (f64, f64)) -> f64 { + let dot = (v1.0 * v2.0) + (v1.1 * v2.1); + let norm = |vec: (f64, f64)| ((vec.0 * vec.0) + (vec.1 * vec.1)).sqrt(); + let norm_combined = norm(v1) * norm(v2); + (dot / norm_combined).acos() +} + +impl VisualDetector for PathCV { + type ClassEnum = bool; + type Position = PosVector; + + fn detect( + &mut self, + input_image: &Mat, + ) -> anyhow::Result>> { + self.image = resize(input_image, &self.size)?.into(); + let mut yuv_image = Mat::default(); + + cvt_color(&self.image.0, &mut yuv_image, COLOR_RGB2YUV, 0).unwrap(); + yuv_image = kmeans(&yuv_image, self.num_regions, self.attempts); + let image_center = ((yuv_image.cols() / 2) as f64, (yuv_image.rows() / 2) as f64); + + cvt_color(&yuv_image, &mut self.image.0, COLOR_YUV2RGB, 0).unwrap(); + + yuv_image + .iter::>() + .unwrap() + .sorted_by(|(_, val), (_, n_val)| Ord::cmp(val.as_slice(), n_val.as_slice())) + .dedup_by(|(_, val), (_, n_val)| val == n_val) + .map(|(_, val)| { + + // let mut bin_image = Mat::default(); + // in_range(&yuv_image, &val, &val, &mut bin_image).unwrap(); + // let on_points = cvt_binary_to_points(&bin_image.try_into_typed().unwrap()); + // let pca_output = binary_pca(&on_points, 0).unwrap(); + + // let (length_idx, width_idx) = if pca_output.pca_value().get(1).unwrap() + // > pca_output.pca_value().get(0).unwrap() + // { + // (1, 0) + // } else { + // (0, 1) + // }; + // // width bounds have a temp fix -- not sure why output is so large + // let width = pca_output.pca_value().get(width_idx).unwrap() / 100.0; + // let length = pca_output.pca_value().get(length_idx).unwrap(); + // let length_2 = pca_output.pca_vector().get(length_idx + 1).unwrap(); + + // logln!("Testing for valid..."); + // logln!("\tself.width_bounds = {:?}", self.width_bounds); + // logln!("\tself.width = {:?}", width); + // logln!( + // "\tcontained_width = {:?}", + // self.width_bounds.contains(&width) + // ); + // logln!(); + // logln!("\tYUV range = {:?}", self.color_bounds); + // logln!("\tYUV val = {:?}", Yuv::from(&val)); + // logln!( + // "\tcontained_color = {:?}", + // Yuv::from(&val).in_range(&self.color_bounds) + // ); + // logln!(); + + // let valid = self.width_bounds.contains(&width) + // && Yuv::from(&val).in_range(&self.color_bounds); + + // let p_vec = PosVector::new( + // ((pca_output.mean().get(0).unwrap()) - image_center.0) + // + (self.image.size().unwrap().width as f64) / 2.0, + // (pca_output.mean().get(1).unwrap()) - image_center.1 + // + (self.image.size().unwrap().height as f64) / 2.0, + // compute_angle( + // ( + // pca_output.pca_vector().get(length_idx).unwrap(), + // pca_output.pca_vector().get(length_idx + 1).unwrap(), + // ), + // FORWARD, + // ), + // width, + // length / 300.0, + // length_2, + // ); + + // Ok(VisualDetection { + // class: valid, + // position: p_vec, + // }) + }) + .collect() + } + + fn normalize(&mut self, pos: &Self::Position) -> Self::Position { + let img_size = self.image.size().unwrap(); + Self::Position::new( + ((*pos.x() / (img_size.width as f64)) - 0.5) * 2.0, + ((*pos.y() / (img_size.height as f64)) - 0.5) * 2.0, + *pos.angle(), + *pos.width() / (img_size.width as f64), + *pos.length() / (img_size.height as f64), + *pos.length_2() / (img_size.height as f64), + ) + } +} + +impl VisualDetector for PathCV { + type ClassEnum = bool; + type Position = PosVector; + + fn detect( + &mut self, + input_image: &Mat, + ) -> anyhow::Result>> { + let image = resize(input_image, &self.size)?; + let mut yuv_image = Mat::default(); + + cvt_color(&image, &mut yuv_image, COLOR_RGB2YUV, 0).unwrap(); + yuv_image = kmeans(&yuv_image, self.num_regions, self.attempts); + let image_center = ((yuv_image.cols() / 2) as f64, (yuv_image.rows() / 2) as f64); + + cvt_color(&yuv_image, &mut self.image.0, COLOR_YUV2RGB, 0).unwrap(); + + #[cfg(feature = "logging")] + { + create_dir_all("/tmp/path_images").unwrap(); + imwrite( + &("/tmp/path_images/".to_string() + &Uuid::new_v4().to_string() + ".jpeg"), + &self.image.0, + &Vector::default(), + ) + .unwrap(); + } + + yuv_image + .iter::>() + .unwrap() + .sorted_by(|(_, val), (_, n_val)| Ord::cmp(val.as_slice(), n_val.as_slice())) + .dedup_by(|(_, val), (_, n_val)| val == n_val) + .map(|(_, val)| { + let mut bin_image = Mat::default(); + in_range(&yuv_image, &val, &val, &mut bin_image).unwrap(); + let on_points = cvt_binary_to_points(&bin_image.try_into_typed().unwrap()); + let pca_output = binary_pca(&on_points, 0).unwrap(); + + let (length_idx, width_idx) = if pca_output.pca_value().get(1).unwrap() + > pca_output.pca_value().get(0).unwrap() + { + (1, 0) + } else { + (0, 1) + }; + // width bounds have a temp fix -- not sure why output is so large + let width = pca_output.pca_value().get(width_idx).unwrap() / 100.0; + let length = pca_output.pca_value().get(length_idx).unwrap(); + let length_2 = pca_output.pca_vector().get(length_idx + 1).unwrap(); + + logln!("Testing for valid..."); + logln!("\tself.width_bounds = {:?}", self.width_bounds); + logln!("\tself.width = {:?}", width); + logln!( + "\tcontained_width = {:?}", + self.width_bounds.contains(&width) + ); + logln!(); + logln!("\tYUV range = {:?}", self.color_bounds); + logln!("\tYUV val = {:?}", Yuv::from(&val)); + logln!( + "\tcontained_color = {:?}", + Yuv::from(&val).in_range(&self.color_bounds) + ); + logln!(); + + let valid = self.width_bounds.contains(&width) + && Yuv::from(&val).in_range(&self.color_bounds); + + if valid { + logln!("\tself.width_bounds = {:?}", self.width_bounds); + logln!("\tself.width = {:?}", width); + logln!( + "\tcontained_width = {:?}", + self.width_bounds.contains(&width) + ); + logln!(); + logln!("\tYUV range = {:?}", self.color_bounds); + logln!("\tYUV val = {:?}", Yuv::from(&val)); + logln!( + "\tcontained_color = {:?}", + Yuv::from(&val).in_range(&self.color_bounds) + ); + }; + + let p_vec = PosVector::new( + ((pca_output.mean().get(0).unwrap()) - image_center.0) + + (self.image.size().unwrap().width as f64) / 2.0, + (pca_output.mean().get(1).unwrap()) - image_center.1 + + (self.image.size().unwrap().height as f64) / 2.0, + compute_angle( + ( + pca_output.pca_vector().get(length_idx).unwrap(), + pca_output.pca_vector().get(length_idx + 1).unwrap(), + ), + FORWARD, + ), + width, + length / 300.0, + length_2, + ); + + Ok(VisualDetection { + class: valid, + position: p_vec, + }) + }) + .collect() + } + + fn normalize(&mut self, pos: &Self::Position) -> Self::Position { + let img_size = self.image.size().unwrap(); + Self::Position::new( + ((*pos.x() / (img_size.width as f64)) - 0.5) * 2.0, + ((*pos.y() / (img_size.height as f64)) - 0.5) * 2.0, + *pos.angle(), + *pos.width() / (img_size.width as f64), + *pos.length() / (img_size.height as f64), + *pos.length_2() / (img_size.height as f64), + ) + } +}: + +#[cfg(test)] +mod tests { + use std::fs::create_dir_all; + + use opencv::{ + core::Vector, + imgcodecs::{imread, imwrite, IMREAD_COLOR}, + }; + + use crate::{logln, vision::Draw}; + + use super::*; + + #[test] + fn detect_single() { + let image = imread("tests/vision/resources/path_images/1.jpeg", IMREAD_COLOR).unwrap(); + let mut path = PathCV::default(); + let detections = >::detect(&mut path, &image).unwrap(); + let mut shrunk_image = path.image().clone(); + + detections.iter().for_each(|result| { + as Draw>::draw(result, &mut shrunk_image).unwrap() + }); + + logln!("Detections: {:#?}", detections); + + create_dir_all("tests/vision/output/path_images").unwrap(); + imwrite( + "tests/vision/output/path_images/1.jpeg", + &shrunk_image, + &Vector::default(), + ) + .unwrap(); + } +} + From 9a25b34fd1074e8fc2b1110fd34d0a8d11558c05 Mon Sep 17 00:00:00 2001 From: SarangaR <59266397+SarangaR@users.noreply.github.com> Date: Sat, 22 Mar 2025 00:19:06 -0400 Subject: [PATCH 2/5] Complete pathCV implementation (path detection using pure cv and contour detection). Compiles for jetson and ready to test for pool test 3/22/25. --- src/missions/path_align.rs | 2 +- src/vision/path_cv.rs | 412 +++++++++++++++++++++---------------- 2 files changed, 231 insertions(+), 183 deletions(-) diff --git a/src/missions/path_align.rs b/src/missions/path_align.rs index 99be36844bc..729df686a4d 100644 --- a/src/missions/path_align.rs +++ b/src/missions/path_align.rs @@ -42,7 +42,7 @@ pub fn path_align< ZeroMovement::new(context, DEPTH), DelayAction::new(2.0), ActionWhile::new(ActionChain::new( - VisionNormBottom::::new(context, Path::default()), + VisionNormBottom::::new(context, PathCV::default()), TupleSecond::new(ActionConcurrent::new( act_nest!( ActionChain::new, diff --git a/src/vision/path_cv.rs b/src/vision/path_cv.rs index f6e5fd3affe..5f63bce1aea 100644 --- a/src/vision/path_cv.rs +++ b/src/vision/path_cv.rs @@ -1,24 +1,105 @@ -use std::{fs::create_dir_all, ops::RangeInclusive}; +use std::{fs::create_dir_all, ops::Mul, ops::RangeInclusive}; +use derive_getters::Getters; use itertools::Itertools; use opencv::{ - core::{in_range, Size, VecN, Vector}, + core::{in_range, Point, Scalar, Size, VecN, Vector}, imgcodecs::imwrite, - imgproc::{cvt_color, COLOR_RGB2YUV, COLOR_YUV2RGB}, + imgproc::{ + self, box_points, circle, contour_area_def, cvt_color_def, find_contours_def, + min_area_rect, CHAIN_APPROX_SIMPLE, COLOR_BGR2YUV, COLOR_YUV2BGR, LINE_8, RETR_EXTERNAL, + }, prelude::{Mat, MatTraitConst, MatTraitConstManual}, }; use uuid::Uuid; use crate::vision::image_prep::{binary_pca, cvt_binary_to_points}; +use crate::vision::{Angle2D, Draw, Offset2D, RelPosAngle}; use super::{ image_prep::{kmeans, resize}, - pca::PosVector, MatWrapper, VisualDetection, VisualDetector, }; static FORWARD: (f64, f64) = (0.0, -1.0); +#[derive(Debug, Clone, Getters, PartialEq)] +pub struct PosVector { + x: f64, + y: f64, + z: f64, + angle: f64, +} + +impl PosVector { + fn new(x: f64, y: f64, z: f64, angle: f64) -> Self { + Self { x, y, z, angle } + } +} + +impl RelPosAngle for PosVector { + type Number = f64; + + fn offset_angle(&self) -> Angle2D { + Angle2D { + x: self.x, + y: self.y, + angle: self.angle, + } + } +} + +impl Mul<&Mat> for PosVector { + type Output = Self; + + fn mul(self, rhs: &Mat) -> Self::Output { + let size = rhs.size().unwrap(); + Self { + x: (self.x + 0.5) * (size.width as f64), + y: (self.y + 0.5) * (size.height as f64), + z: 0., + angle: self.angle, + } + } +} + +impl Draw for VisualDetection { + fn draw(&self, canvas: &mut Mat) -> anyhow::Result<()> { + let color = if self.class { + logln!("Drawing true: {:#?}", self.position()); + Scalar::from((0.0, 255.0, 0.0)) + } else { + Scalar::from((0.0, 0.0, 255.0)) + }; + + imgproc::circle( + canvas, + Point::new(*self.position.x() as i32, *self.position.y() as i32), + 10, + color, + 2, + LINE_8, + 0, + )?; + + // imgproc::arrowed_line( + // canvas, + // Point::new(*self.position.x() as i32, *self.position.y() as i32), + // Point::new( + // (self.position.x() + 0.02 * self.position.length() * self.position.length()) as i32, + // (self.position.y() + 0.4 * self.position.length_2() * self.position.length()) + // as i32, + // ), + // color, + // 2, + // LINE_8, + // 0, + // 0.1, + // )?; + Ok(()) + } +} + #[derive(Debug, PartialEq)] pub struct Yuv { pub y: u8, @@ -91,7 +172,7 @@ impl PathCV { impl Default for PathCV { fn default() -> Self { PathCV::new( - (Yuv { y: 0, u: 0, v: 127 })..=(Yuv { + (Yuv { y: 0, u: 0, v: 175 })..=(Yuv { y: 255, u: 127, v: 255, @@ -122,78 +203,77 @@ impl VisualDetector for PathCV { self.image = resize(input_image, &self.size)?.into(); let mut yuv_image = Mat::default(); - cvt_color(&self.image.0, &mut yuv_image, COLOR_RGB2YUV, 0).unwrap(); - yuv_image = kmeans(&yuv_image, self.num_regions, self.attempts); - let image_center = ((yuv_image.cols() / 2) as f64, (yuv_image.rows() / 2) as f64); - - cvt_color(&yuv_image, &mut self.image.0, COLOR_YUV2RGB, 0).unwrap(); - - yuv_image - .iter::>() - .unwrap() - .sorted_by(|(_, val), (_, n_val)| Ord::cmp(val.as_slice(), n_val.as_slice())) - .dedup_by(|(_, val), (_, n_val)| val == n_val) - .map(|(_, val)| { - - // let mut bin_image = Mat::default(); - // in_range(&yuv_image, &val, &val, &mut bin_image).unwrap(); - // let on_points = cvt_binary_to_points(&bin_image.try_into_typed().unwrap()); - // let pca_output = binary_pca(&on_points, 0).unwrap(); - - // let (length_idx, width_idx) = if pca_output.pca_value().get(1).unwrap() - // > pca_output.pca_value().get(0).unwrap() - // { - // (1, 0) - // } else { - // (0, 1) - // }; - // // width bounds have a temp fix -- not sure why output is so large - // let width = pca_output.pca_value().get(width_idx).unwrap() / 100.0; - // let length = pca_output.pca_value().get(length_idx).unwrap(); - // let length_2 = pca_output.pca_vector().get(length_idx + 1).unwrap(); - - // logln!("Testing for valid..."); - // logln!("\tself.width_bounds = {:?}", self.width_bounds); - // logln!("\tself.width = {:?}", width); - // logln!( - // "\tcontained_width = {:?}", - // self.width_bounds.contains(&width) - // ); - // logln!(); - // logln!("\tYUV range = {:?}", self.color_bounds); - // logln!("\tYUV val = {:?}", Yuv::from(&val)); - // logln!( - // "\tcontained_color = {:?}", - // Yuv::from(&val).in_range(&self.color_bounds) - // ); - // logln!(); - - // let valid = self.width_bounds.contains(&width) - // && Yuv::from(&val).in_range(&self.color_bounds); - - // let p_vec = PosVector::new( - // ((pca_output.mean().get(0).unwrap()) - image_center.0) - // + (self.image.size().unwrap().width as f64) / 2.0, - // (pca_output.mean().get(1).unwrap()) - image_center.1 - // + (self.image.size().unwrap().height as f64) / 2.0, - // compute_angle( - // ( - // pca_output.pca_vector().get(length_idx).unwrap(), - // pca_output.pca_vector().get(length_idx + 1).unwrap(), - // ), - // FORWARD, - // ), - // width, - // length / 300.0, - // length_2, - // ); - - // Ok(VisualDetection { - // class: valid, - // position: p_vec, - // }) - }) - .collect() + cvt_color_def(&self.image.0, &mut yuv_image, COLOR_BGR2YUV)?; + + let color_start = self.color_bounds.start(); + let color_end = self.color_bounds.end(); + let lower_orange = Scalar::new( + color_start.y as f64, + color_start.u as f64, + color_start.v as f64, + 0., + ); + let upper_orange = Scalar::new( + color_end.y as f64, + color_end.u as f64, + color_end.v as f64, + 0., + ); + + let mut mask = Mat::default(); + in_range(&yuv_image, &lower_orange, &upper_orange, &mut mask); + + let mut contours = Vector::>::new(); + find_contours_def(&mask, &mut contours, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE)?; + + let max_contour = contours.iter().max_by(|x, y| { + contour_area_def(&x) + .unwrap() + .partial_cmp(&contour_area_def(&y).unwrap()) + .unwrap() + }); + + if let Some(contour) = max_contour { + let area = contour_area_def(&contour)?; + if area > 1000.0 { + let rect = min_area_rect(&contour)?; + let mut angle = rect.angle as f64; + let width = rect.size.width; + let height = rect.size.height; + if width > height { + angle = rect.angle as f64; + } else { + angle = (rect.angle as f64) + 90.0; + } + angle -= 90.0; + let mut box_rect = Mat::default(); + box_points(rect, &mut box_rect)?; + + println!("{:?}", angle); + // DRAW STUFF + // SEND TO RTSP SERVER + + let center_adjusted_x = + (rect.center.x as f64) - ((self.image.size()?.width as f64) / 2.0); + let center_adjusted_y = + ((self.image.size()?.height as f64) / 2.0) - (rect.center.y as f64); + + Ok(vec![VisualDetection { + class: true, + position: PosVector::new(center_adjusted_x, center_adjusted_y, 0., angle), + }]) + } else { + Ok(vec![VisualDetection { + class: false, + position: PosVector::new(0., 0., 0., 0.), + }]) + } + } else { + Ok(vec![VisualDetection { + class: false, + position: PosVector::new(0., 0., 0., 0.), + }]) + } } fn normalize(&mut self, pos: &Self::Position) -> Self::Position { @@ -201,10 +281,8 @@ impl VisualDetector for PathCV { Self::Position::new( ((*pos.x() / (img_size.width as f64)) - 0.5) * 2.0, ((*pos.y() / (img_size.height as f64)) - 0.5) * 2.0, + 0., *pos.angle(), - *pos.width() / (img_size.width as f64), - *pos.length() / (img_size.height as f64), - *pos.length_2() / (img_size.height as f64), ) } } @@ -217,107 +295,80 @@ impl VisualDetector for PathCV { &mut self, input_image: &Mat, ) -> anyhow::Result>> { - let image = resize(input_image, &self.size)?; + self.image = resize(input_image, &self.size)?.into(); let mut yuv_image = Mat::default(); - cvt_color(&image, &mut yuv_image, COLOR_RGB2YUV, 0).unwrap(); - yuv_image = kmeans(&yuv_image, self.num_regions, self.attempts); - let image_center = ((yuv_image.cols() / 2) as f64, (yuv_image.rows() / 2) as f64); - - cvt_color(&yuv_image, &mut self.image.0, COLOR_YUV2RGB, 0).unwrap(); - - #[cfg(feature = "logging")] - { - create_dir_all("/tmp/path_images").unwrap(); - imwrite( - &("/tmp/path_images/".to_string() + &Uuid::new_v4().to_string() + ".jpeg"), - &self.image.0, - &Vector::default(), - ) - .unwrap(); - } + cvt_color_def(&self.image.0, &mut yuv_image, COLOR_BGR2YUV)?; + + let color_start = self.color_bounds.start(); + let color_end = self.color_bounds.end(); + let lower_orange = Scalar::new( + color_start.y as f64, + color_start.u as f64, + color_start.v as f64, + 0., + ); + let upper_orange = Scalar::new( + color_end.y as f64, + color_end.u as f64, + color_end.v as f64, + 0., + ); + + let mut mask = Mat::default(); + in_range(&yuv_image, &lower_orange, &upper_orange, &mut mask); + + let mut contours = Vector::>::new(); + find_contours_def(&mask, &mut contours, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE)?; + + let max_contour = contours.iter().max_by(|x, y| { + contour_area_def(&x) + .unwrap() + .partial_cmp(&contour_area_def(&y).unwrap()) + .unwrap() + }); - yuv_image - .iter::>() - .unwrap() - .sorted_by(|(_, val), (_, n_val)| Ord::cmp(val.as_slice(), n_val.as_slice())) - .dedup_by(|(_, val), (_, n_val)| val == n_val) - .map(|(_, val)| { - let mut bin_image = Mat::default(); - in_range(&yuv_image, &val, &val, &mut bin_image).unwrap(); - let on_points = cvt_binary_to_points(&bin_image.try_into_typed().unwrap()); - let pca_output = binary_pca(&on_points, 0).unwrap(); - - let (length_idx, width_idx) = if pca_output.pca_value().get(1).unwrap() - > pca_output.pca_value().get(0).unwrap() - { - (1, 0) + if let Some(contour) = max_contour { + let area = contour_area_def(&contour)?; + if area > 1000.0 { + let rect = min_area_rect(&contour)?; + let mut angle = rect.angle as f64; + let width = rect.size.width; + let height = rect.size.height; + if width > height { + angle = rect.angle as f64; } else { - (0, 1) - }; - // width bounds have a temp fix -- not sure why output is so large - let width = pca_output.pca_value().get(width_idx).unwrap() / 100.0; - let length = pca_output.pca_value().get(length_idx).unwrap(); - let length_2 = pca_output.pca_vector().get(length_idx + 1).unwrap(); - - logln!("Testing for valid..."); - logln!("\tself.width_bounds = {:?}", self.width_bounds); - logln!("\tself.width = {:?}", width); - logln!( - "\tcontained_width = {:?}", - self.width_bounds.contains(&width) - ); - logln!(); - logln!("\tYUV range = {:?}", self.color_bounds); - logln!("\tYUV val = {:?}", Yuv::from(&val)); - logln!( - "\tcontained_color = {:?}", - Yuv::from(&val).in_range(&self.color_bounds) - ); - logln!(); - - let valid = self.width_bounds.contains(&width) - && Yuv::from(&val).in_range(&self.color_bounds); - - if valid { - logln!("\tself.width_bounds = {:?}", self.width_bounds); - logln!("\tself.width = {:?}", width); - logln!( - "\tcontained_width = {:?}", - self.width_bounds.contains(&width) - ); - logln!(); - logln!("\tYUV range = {:?}", self.color_bounds); - logln!("\tYUV val = {:?}", Yuv::from(&val)); - logln!( - "\tcontained_color = {:?}", - Yuv::from(&val).in_range(&self.color_bounds) - ); - }; - - let p_vec = PosVector::new( - ((pca_output.mean().get(0).unwrap()) - image_center.0) - + (self.image.size().unwrap().width as f64) / 2.0, - (pca_output.mean().get(1).unwrap()) - image_center.1 - + (self.image.size().unwrap().height as f64) / 2.0, - compute_angle( - ( - pca_output.pca_vector().get(length_idx).unwrap(), - pca_output.pca_vector().get(length_idx + 1).unwrap(), - ), - FORWARD, - ), - width, - length / 300.0, - length_2, - ); - - Ok(VisualDetection { - class: valid, - position: p_vec, - }) - }) - .collect() + angle = (rect.angle as f64) + 90.0; + } + angle -= 90.0; + let mut box_rect = Mat::default(); + box_points(rect, &mut box_rect)?; + + println!("{:?}", angle); + // DRAW STUFF + // SEND TO RTSP SERVER + + let center_adjusted_x = + (rect.center.x as f64) - ((self.image.size()?.width as f64) / 2.0); + let center_adjusted_y = + ((self.image.size()?.height as f64) / 2.0) - (rect.center.y as f64); + + Ok(vec![VisualDetection { + class: true, + position: PosVector::new(center_adjusted_x, center_adjusted_y, 0., angle), + }]) + } else { + Ok(vec![VisualDetection { + class: false, + position: PosVector::new(0., 0., 0., 0.), + }]) + } + } else { + Ok(vec![VisualDetection { + class: false, + position: PosVector::new(0., 0., 0., 0.), + }]) + } } fn normalize(&mut self, pos: &Self::Position) -> Self::Position { @@ -325,13 +376,11 @@ impl VisualDetector for PathCV { Self::Position::new( ((*pos.x() / (img_size.width as f64)) - 0.5) * 2.0, ((*pos.y() / (img_size.height as f64)) - 0.5) * 2.0, + 0., *pos.angle(), - *pos.width() / (img_size.width as f64), - *pos.length() / (img_size.height as f64), - *pos.length_2() / (img_size.height as f64), ) } -}: +} #[cfg(test)] mod tests { @@ -368,4 +417,3 @@ mod tests { .unwrap(); } } - From d8cb1fd9415de0358f1a429a32999d88a92824b3 Mon Sep 17 00:00:00 2001 From: SarangaR <59266397+SarangaR@users.noreply.github.com> Date: Sat, 22 Mar 2025 00:24:16 -0400 Subject: [PATCH 3/5] fix: remove nix flakes --- .envrc | 1 - flake.lock | 48 --------------------------------------------- flake.nix | 57 ------------------------------------------------------ 3 files changed, 106 deletions(-) delete mode 100644 .envrc delete mode 100644 flake.lock delete mode 100644 flake.nix diff --git a/.envrc b/.envrc deleted file mode 100644 index 3550a30f2de..00000000000 --- a/.envrc +++ /dev/null @@ -1 +0,0 @@ -use flake diff --git a/flake.lock b/flake.lock deleted file mode 100644 index 3f56904de95..00000000000 --- a/flake.lock +++ /dev/null @@ -1,48 +0,0 @@ -{ - "nodes": { - "nixpkgs": { - "locked": { - "lastModified": 1725910328, - "narHash": "sha256-n9pCtzGZ0httmTwMuEbi5E78UQ4ZbQMr1pzi5N0LAG8=", - "owner": "NixOS", - "repo": "nixpkgs", - "rev": "5775c2583f1801df7b790bf7f7d710a19bac66f4", - "type": "github" - }, - "original": { - "owner": "NixOS", - "ref": "nixpkgs-unstable", - "repo": "nixpkgs", - "type": "github" - } - }, - "root": { - "inputs": { - "nixpkgs": "nixpkgs", - "rust-overlay": "rust-overlay" - } - }, - "rust-overlay": { - "inputs": { - "nixpkgs": [ - "nixpkgs" - ] - }, - "locked": { - "lastModified": 1721355572, - "narHash": "sha256-I4TQ2guV9jTmZsXeWt5HMojcaqNZHII4zu0xIKZEovM=", - "owner": "oxalica", - "repo": "rust-overlay", - "rev": "d5bc7b1b21cf937fb8ff108ae006f6776bdb163d", - "type": "github" - }, - "original": { - "owner": "oxalica", - "repo": "rust-overlay", - "type": "github" - } - } - }, - "root": "root", - "version": 7 -} diff --git a/flake.nix b/flake.nix deleted file mode 100644 index 655c11ceab7..00000000000 --- a/flake.nix +++ /dev/null @@ -1,57 +0,0 @@ -{ - description = "A tool for building sysroots for cross compilation"; - - inputs = { - nixpkgs.url = "github:NixOS/nixpkgs/nixpkgs-unstable"; - rust-overlay = { - url = "github:oxalica/rust-overlay"; - inputs.nixpkgs.follows = "nixpkgs"; - }; - }; - - outputs = { self, nixpkgs, rust-overlay }: - let - supportedSystems = [ "x86_64-linux" "aarch64-linux" "x86_64-darwin" "aarch64-darwin" ]; - forEachSupportedSystem = f: nixpkgs.lib.genAttrs supportedSystems (system: f { - pkgs = import nixpkgs { - inherit system; - overlays = [ rust-overlay.overlays.default self.overlays.default ]; - }; - }); - in - { - overlays.default = final: prev: { - rustToolchain = - let - rust = prev.rust-bin; - in - if builtins.pathExists ./rust-toolchain.toml then - rust.fromRustupToolchainFile ./rust-toolchain.toml - else if builtins.pathExists ./rust-toolchain then - rust.fromRustupToolchainFile ./rust-toolchain - else - rust.stable.latest.default.override { - extensions = [ "rust-src" "rustfmt" ]; - }; - }; - - devShells = forEachSupportedSystem ({ pkgs }: { - default = pkgs.mkShell { - packages = with pkgs; [ - rustToolchain - openssl - pkg-config - cargo-deny - cargo-edit - cargo-watch - rust-analyzer - ]; - - env = { - # Required by rust-analyzer - RUST_SRC_PATH = "${pkgs.rustToolchain}/lib/rustlib/src/rust/library"; - }; - }; - }); - }; -} From 68fc221afdf0cde55f7261431168008fc1b7693a Mon Sep 17 00:00:00 2001 From: SarangaR <59266397+SarangaR@users.noreply.github.com> Date: Sat, 22 Mar 2025 00:25:35 -0400 Subject: [PATCH 4/5] fix: update gitignore --- .gitignore | 1 - 1 file changed, 1 deletion(-) diff --git a/.gitignore b/.gitignore index b925db05b40..3875fd529aa 100644 --- a/.gitignore +++ b/.gitignore @@ -9,7 +9,6 @@ graphs sysroot-jetson target-jetson console/ -.direnv/ # Ignore all log files generated logging/ From 343b26ffa239093df4e5875f3ae99dd5a2aa5eca Mon Sep 17 00:00:00 2001 From: Cowboylaserkittenjetshark <82691052+Cowboylaserkittenjetshark@users.noreply.github.com> Date: Sun, 30 Mar 2025 14:26:17 -0400 Subject: [PATCH 5/5] feat: changes from March 29th, 2025 pool test - Tune depths for dive well - Replace path align PCA implementation with CV implementation --- src/missions/coinflip.rs | 4 +- src/missions/gate.rs | 2 +- src/missions/path_align.rs | 106 ++++++++++++++++++++++++++++++++++--- src/vision/path_cv.rs | 24 +++++---- 4 files changed, 116 insertions(+), 20 deletions(-) diff --git a/src/missions/coinflip.rs b/src/missions/coinflip.rs index 276d96eab46..e8fcfb122a5 100644 --- a/src/missions/coinflip.rs +++ b/src/missions/coinflip.rs @@ -40,10 +40,10 @@ pub fn coinflip< const TRUE_COUNT: u32 = 4; const DELAY_TIME: f32 = 3.0; - const DEPTH: f32 = -1.15; + const DEPTH: f32 = -1.75; const ALIGN_X_SPEED: f32 = 0.0; const ALIGN_Y_SPEED: f32 = 0.0; - const ALIGN_YAW_SPEED: f32 = -6.0; + const ALIGN_YAW_SPEED: f32 = -3.0; const ALIGN_YAW_CORRECTION_SPEED: f32 = 0.0; act_nest!( diff --git a/src/missions/gate.rs b/src/missions/gate.rs index 93c46ba5bb7..329a9514e76 100644 --- a/src/missions/gate.rs +++ b/src/missions/gate.rs @@ -121,7 +121,7 @@ pub fn gate_run_coinflip< ) -> impl ActionExec> + '_ { const TIMEOUT: f32 = 30.0; - let depth: f32 = -1.0; + let depth: f32 = -1.6; act_nest!( ActionSequence::new, diff --git a/src/missions/path_align.rs b/src/missions/path_align.rs index 729df686a4d..298c3e04000 100644 --- a/src/missions/path_align.rs +++ b/src/missions/path_align.rs @@ -9,10 +9,10 @@ use crate::{ TupleSecond, }, basic::DelayAction, - extra::{CountTrue, OutputType, Terminal, ToVec}, + extra::{CountFalse, CountTrue, OutputType, Terminal, ToVec}, movement::{ - LinearYawFromX, OffsetToPose, Stability2Adjust, Stability2Movement, Stability2Pos, - ZeroMovement, + AdjustType, ClampX, FlipX, LinearYawFromX, OffsetToPose, SetY, Stability2Adjust, + Stability2Movement, Stability2Pos, ZeroMovement, }, vision::{DetectTarget, ExtractPosition, MidPoint, VisionNormBottom}, }, @@ -34,13 +34,48 @@ pub fn path_align< >( context: &Con, ) -> impl ActionExec<()> + '_ { - const DEPTH: f32 = -1.25; + const DEPTH: f32 = -1.5; const PATH_ALIGN_SPEED: f32 = 0.3; act_nest!( ActionSequence::new, ZeroMovement::new(context, DEPTH), DelayAction::new(2.0), + ActionWhile::new(ActionChain::new( + VisionNormBottom::::new(context, PathCV::default()), + TupleSecond::new(ActionConcurrent::new( + act_nest!( + ActionChain::new, + ActionDataConditional::new( + DetectTarget::new(true), + act_nest!( + ActionChain::new, + ExtractPosition::new(), + MidPoint::new(), + OffsetToPose::default(), + // LinearYawFromX::::new(0.0), + ClampX::new(1.0), + // SetY::::new(AdjustType::Adjust(0.02)), + // FlipX::default(), + Stability2Movement::new( + context, + Stability2Pos::new(0.0, 0.0, 0.0, 0.0, None, DEPTH), + ), + ), + act_nest!( + ActionSequence::new, + Terminal::new(), + Stability2Movement::new( + context, + Stability2Pos::new(0.0, PATH_ALIGN_SPEED, 0.0, 0.0, None, DEPTH), + ), + ), + ), + OutputType::<()>::new(), + ), + ActionChain::new(DetectTarget::new(true), CountTrue::new(10)), + )), + )), ActionWhile::new(ActionChain::new( VisionNormBottom::::new(context, PathCV::default()), TupleSecond::new(ActionConcurrent::new( @@ -56,7 +91,7 @@ pub fn path_align< LinearYawFromX::::default(), Stability2Movement::new( context, - Stability2Pos::new(0.0, PATH_ALIGN_SPEED, 30.0, 0.0, None, DEPTH), + Stability2Pos::new(0.0, PATH_ALIGN_SPEED, 0.0, 0.0, None, DEPTH), ), ), act_nest!( @@ -64,13 +99,70 @@ pub fn path_align< Terminal::new(), Stability2Movement::new( context, - Stability2Pos::new(0.0, PATH_ALIGN_SPEED, 30.0, 0.0, None, DEPTH), + Stability2Pos::new(0.0, PATH_ALIGN_SPEED, 0.0, 0.0, None, DEPTH), ), ), ), OutputType::<()>::new(), ), - ActionChain::new(DetectTarget::new(true), CountTrue::new(5)), + ActionChain::new(DetectTarget::new(true), CountTrue::new(10)), + )), + )), + ActionWhile::new(ActionChain::new( + VisionNormBottom::::new(context, PathCV::default()), + TupleSecond::new(ActionConcurrent::new( + act_nest!( + ActionChain::new, + // act_nest!( + // ActionChain::new, + // ActionDataConditional::new( + // DetectTarget::new(true), + // act_nest!( + // ActionChain::new, + // ExtractPosition::new(), + // MidPoint::new(), + // OffsetToPose::default(), + // LinearYawFromX::::default(), + // Stability2Movement::new( + // context, + // Stability2Pos::new( + // 0.0, + // PATH_ALIGN_SPEED, + // 0.0, + // 0.0, + // None, + // DEPTH + // ), + // ), + // ), + // act_nest!( + // ActionSequence::new, + // Terminal::new(), + // Stability2Movement::new( + // context, + // Stability2Pos::new( + // 0.0, + // PATH_ALIGN_SPEED, + // 0.0, + // 0.0, + // None, + // DEPTH + // ), + // ), + // ), + // OutputType::<()>::new(), + // ), + act_nest!( + ActionSequence::new, + Terminal::new(), + Stability2Movement::new( + context, + Stability2Pos::new(0.0, PATH_ALIGN_SPEED / 1.5, 0.0, 0.0, None, DEPTH), + ), + ), + OutputType::<()>::new(), + ), + ActionChain::new(DetectTarget::new(true), CountFalse::new(5)), )), )), Terminal::new(), diff --git a/src/vision/path_cv.rs b/src/vision/path_cv.rs index 5f63bce1aea..e89d4fb58bc 100644 --- a/src/vision/path_cv.rs +++ b/src/vision/path_cv.rs @@ -235,7 +235,7 @@ impl VisualDetector for PathCV { if let Some(contour) = max_contour { let area = contour_area_def(&contour)?; - if area > 1000.0 { + if area > 5000.0 { let rect = min_area_rect(&contour)?; let mut angle = rect.angle as f64; let width = rect.size.width; @@ -253,10 +253,12 @@ impl VisualDetector for PathCV { // DRAW STUFF // SEND TO RTSP SERVER - let center_adjusted_x = - (rect.center.x as f64) - ((self.image.size()?.width as f64) / 2.0); - let center_adjusted_y = - ((self.image.size()?.height as f64) / 2.0) - (rect.center.y as f64); + // let center_adjusted_x = + // (rect.center.x as f64) - ((self.image.size()?.width as f64) / 2.0); + // let center_adjusted_y = + // ((self.image.size()?.height as f64) / 2.0) - (rect.center.y as f64); + let center_adjusted_x = rect.center.x as f64; + let center_adjusted_y = rect.center.y as f64; Ok(vec![VisualDetection { class: true, @@ -330,7 +332,7 @@ impl VisualDetector for PathCV { if let Some(contour) = max_contour { let area = contour_area_def(&contour)?; - if area > 1000.0 { + if area > 500.0 { let rect = min_area_rect(&contour)?; let mut angle = rect.angle as f64; let width = rect.size.width; @@ -348,10 +350,12 @@ impl VisualDetector for PathCV { // DRAW STUFF // SEND TO RTSP SERVER - let center_adjusted_x = - (rect.center.x as f64) - ((self.image.size()?.width as f64) / 2.0); - let center_adjusted_y = - ((self.image.size()?.height as f64) / 2.0) - (rect.center.y as f64); + // let center_adjusted_x = + // (rect.center.x as f64) - ((self.image.size()?.width as f64) / 2.0); + // let center_adjusted_y = + // ((self.image.size()?.height as f64) / 2.0) - (rect.center.y as f64); + let center_adjusted_x = rect.center.x as f64; + let center_adjusted_y = rect.center.y as f64; Ok(vec![VisualDetection { class: true,