Skip to content

Commit

Permalink
Repair OpenCV runtime errors
Browse files Browse the repository at this point in the history
  • Loading branch information
Bennett-Petzold committed Aug 11, 2024
1 parent b358abc commit ff51729
Show file tree
Hide file tree
Showing 4 changed files with 111 additions and 65 deletions.
39 changes: 11 additions & 28 deletions src/missions/octagon.rs
Original file line number Diff line number Diff line change
Expand Up @@ -20,10 +20,7 @@ use crate::{
},
vision::{DetectTarget, ExtractPosition, MidPoint, Norm, Vision},
},
vision::{
path::{Path, Yuv},
Offset2D,
},
vision::{octagon::Octagon, path::Yuv, Offset2D},
POOL_YAW_SIGN,
};

Expand All @@ -32,22 +29,8 @@ use super::{
action_context::{GetControlBoard, GetFrontCamMat, GetMainElectronicsBoard},
};

pub fn octagon_path_model() -> Path {
Path::new(
(Yuv {
y: 64,
u: 127,
v: 127,
})..=(Yuv {
y: 180,
u: 224,
v: 224,
}),
5.0..=200.0,
4,
Size::from((400, 300)),
3,
)
pub fn octagon_path_model() -> Octagon {
Octagon::default()
}

pub fn octagon<
Expand Down Expand Up @@ -141,12 +124,12 @@ pub fn octagon<
),
act_nest!(
ActionChain::new,
Vision::<Con, Path, f64>::new(context, octagon_path_model()),
Vision::<Con, Octagon, f64>::new(context, octagon_path_model()),
TupleSecond::new(ActionConcurrent::new(
act_nest!(
ActionChain::new,
ToVec::new(),
Norm::new(Path::default()),
Norm::new(Octagon::default()),
ExtractPosition::new(),
MidPoint::new(),
OffsetToPose::<Offset2D<f64>>::default(),
Expand Down Expand Up @@ -174,13 +157,13 @@ pub fn octagon<
)),
ActionWhile::new(act_nest!(
ActionChain::new,
Vision::<Con, Path, f64>::new(context, octagon_path_model()),
Vision::<Con, Octagon, f64>::new(context, octagon_path_model()),
ActionDataConditional::new(
DetectTarget::new(true),
ActionSequence::new(
act_nest!(
ActionChain::new,
Norm::new(Path::default()),
Norm::new(Octagon::default()),
ExtractPosition::new(),
MidPoint::new(),
OffsetToPose::<Offset2D<f64>>::default(),
Expand Down Expand Up @@ -263,7 +246,7 @@ mod tests {
)
.unwrap();

let output: Vec<_> = <Path as VisualDetector<f64>>::detect(&mut model, &image)
let output: Vec<_> = <Octagon as VisualDetector<f64>>::detect(&mut model, &image)
.unwrap()
.into_iter()
.filter(|x| *x.class())
Expand All @@ -289,7 +272,7 @@ mod tests {
)
.unwrap();

let output: Vec<_> = <Path as VisualDetector<f64>>::detect(&mut model, &image)
let output: Vec<_> = <Octagon as VisualDetector<f64>>::detect(&mut model, &image)
.unwrap()
.into_iter()
.filter(|x| *x.class())
Expand Down Expand Up @@ -325,7 +308,7 @@ mod tests {
let mut model = octagon_path_model();
let image = imread(f.unwrap().path().to_str().unwrap(), IMREAD_COLOR).unwrap();

let output: Vec<_> = <Path as VisualDetector<f64>>::detect(&mut model, &image)
let output: Vec<_> = <Octagon as VisualDetector<f64>>::detect(&mut model, &image)
.unwrap()
.into_iter()
.filter(|x| *x.class())
Expand Down Expand Up @@ -374,7 +357,7 @@ mod tests {
let image = imread(f.unwrap().path().to_str().unwrap(), IMREAD_COLOR).unwrap();

let output_prefilter: Vec<_> =
<Path as VisualDetector<f64>>::detect(&mut model, &image).unwrap();
<Octagon as VisualDetector<f64>>::detect(&mut model, &image).unwrap();

let output: Vec<_> = output_prefilter
.clone()
Expand Down
22 changes: 20 additions & 2 deletions src/vision/mod.rs
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@ use derive_getters::Getters;
use itertools::Itertools;
use num_traits::{zero, FromPrimitive, Num};
use opencv::{
core::{MatTraitConst, Rect2d, Scalar, Vector},
core::{MatTraitConst, Point, Rect, Rect2d, Scalar, Vector},
imgproc::{self, LINE_8},
prelude::Mat,
};
Expand All @@ -20,10 +20,10 @@ pub mod gate;
pub mod gate_poles;
pub mod image_prep;
pub mod nn_cv2;
pub mod octagon;
pub mod path;
pub mod pca;
pub mod yolo_model;
pub mod octagon;

pub trait Draw {
/// Draws self on top of `canvas`
Expand Down Expand Up @@ -90,6 +90,24 @@ impl<T: Num + FromPrimitive> Div<usize> for Offset2D<T> {
}
}

impl<T: Num + Clone + TryInto<i32, Error: Debug>> Draw for Offset2D<T> {
fn draw(&self, canvas: &mut Mat) -> Result<()> {
imgproc::circle(
canvas,
Point::new(
self.x().clone().try_into().unwrap(),
self.y().clone().try_into().unwrap(),
),
10,
Scalar::from((0.0, 255.0, 0.0)),
2,
LINE_8,
0,
)?;
Ok(())
}
}

/// Holds x, y, and angle offset of object in frame
#[derive(Debug, Getters)]
pub struct Angle2D<T: Num> {
Expand Down
88 changes: 55 additions & 33 deletions src/vision/octagon.rs
Original file line number Diff line number Diff line change
Expand Up @@ -9,8 +9,8 @@ use opencv::{
},
imgcodecs::imwrite,
imgproc::{
bounding_rect, contour_area, cvt_color, filter_2d, find_contours, COLOR_RGB2YUV,
COLOR_YUV2RGB,
bounding_rect, contour_area, cvt_color, filter_2d, find_contours, find_contours_def,
CHAIN_APPROX_SIMPLE, COLOR_RGB2YUV, COLOR_YUV2RGB, RETR_TREE,
},
prelude::{Mat, MatTraitConst, MatTraitConstManual},
};
Expand Down Expand Up @@ -45,9 +45,16 @@ impl From<&Rgb> for VecN<u8, 3> {
}
}

impl Into<(u8, u8, u8)> for Rgb {
fn into(self) -> (u8, u8, u8) {
(self.r, self.g, self.b)
impl From<Rgb> for (u8, u8, u8) {
fn from(val: Rgb) -> Self {
(val.r, val.g, val.b)
}
}

impl From<Rgb> for (f64, f64, f64) {
fn from(val: Rgb) -> Self {
let (x, y, z): (u8, u8, u8) = val.into();
(x as f64, y as f64, z as f64)
}
}

Expand All @@ -66,6 +73,10 @@ impl Octagon {
image: Mat::default().into(),
}
}

pub fn image(&self) -> Mat {
self.image.0.clone()
}
}

impl Default for Octagon {
Expand All @@ -77,7 +88,7 @@ impl Default for Octagon {
b: 210,
})..=(Rgb {
r: 255,
g: 220,
g: 180,
b: 255,
}),
Size::from((400, 300)),
Expand All @@ -94,45 +105,57 @@ impl VisualDetector<f64> for Octagon {
input_image: &Mat,
) -> anyhow::Result<Vec<VisualDetection<Self::ClassEnum, Self::Position>>> {
let image = resize(input_image, &self.size)?;
let mut rgb_image = Mat::default();
self.image.0 = image.clone();

cvt_color(&image, &mut rgb_image, COLOR_RGB2YUV, 0).unwrap();
let image_center = ((rgb_image.cols() / 2) as f64, (rgb_image.rows() / 2) as f64);
let mut mask = Mat::default();
// let Rgb { upper_r: , g, b } = self.color_bounds.start();
let lower: VecN<u8, 3> = self.color_bounds.start().into();
let upper: VecN<u8, 3> = self.color_bounds.end().into();
in_range(&image, &lower, &upper, &mut mask).unwrap();

cvt_color(&rgb_image, &mut self.image.0, COLOR_YUV2RGB, 0).unwrap();
#[cfg(feature = "logging")]
{
create_dir_all("/tmp/octagon_images").unwrap();
imwrite(
&("/tmp/octagon_images/".to_string() + &Uuid::new_v4().to_string() + ".jpeg"),
&image,
&Vector::default(),
)
.unwrap();
}

#[cfg(feature = "logging")]
{
create_dir_all("/tmp/path_images").unwrap();
create_dir_all("/tmp/masks").unwrap();
imwrite(
&("/tmp/path_images/".to_string() + &Uuid::new_v4().to_string() + ".jpeg"),
&self.image.0,
&("/tmp/masks/".to_string() + &Uuid::new_v4().to_string() + ".jpeg"),
&mask,
&Vector::default(),
)
.unwrap();
}

let mut mask = Mat::default();
// let Rgb { upper_r: , g, b } = self.color_bounds.start();
let lower: (u8, u8, u8) = self.color_bounds.start().clone().into();
let upper: (u8, u8, u8) = self.color_bounds.end().clone().into();
in_range(
&rgb_image,
&opencv::core::Scalar_::<u8>::from(lower),
&opencv::core::Scalar_::<u8>::from(upper),
&mut mask,
println!("MASK: {:#?}", mask);

let mut contours_out: Vector<Vector<Point>> = Vector::new();
find_contours(
&mask,
&mut contours_out,
RETR_TREE,
CHAIN_APPROX_SIMPLE,
Point::new(0, 0),
)
.unwrap();
let mut contours_out = Mat::default();
find_contours(&mask, &mut contours_out, 3, 2, Point::new(0, 0)).unwrap();

let contour_vec: Vec<Point> = contours_out.iter().unwrap().map(|x| x.1).collect();
if (contour_vec.len() > 2) {
let selected_contour = contour_vec.get(contour_vec.len() - 2).unwrap();
Ok(vec![VisualDetection {
position: Offset2D::new(selected_contour.x as f64, selected_contour.y as f64),
class: true,
}])

if contours_out.len() > 2 {
let selected_contour_set = contours_out.get(contours_out.len() - 2).unwrap();
Ok(selected_contour_set
.iter()
.map(|contour| VisualDetection {
position: Offset2D::new(contour.x as f64, contour.y as f64),
class: true,
})
.collect())
} else {
Ok(vec![])
}
Expand All @@ -146,4 +169,3 @@ impl VisualDetector<f64> for Octagon {
)
}
}

27 changes: 25 additions & 2 deletions src/vision/pca.rs
Original file line number Diff line number Diff line change
@@ -1,6 +1,8 @@
use std::ops::Mul;
use std::{fmt::Debug, ops::Mul};

use anyhow::anyhow;
use derive_getters::Getters;
use num_traits::Num;
use opencv::{
core::{MatTraitConst, Point, Scalar},
imgproc::{self, LINE_8},
Expand All @@ -9,7 +11,7 @@ use opencv::{

use crate::logln;

use super::{Angle2D, Draw, RelPosAngle, VisualDetection};
use super::{Angle2D, Draw, Offset2D, RelPosAngle, VisualDetection};

#[derive(Debug, Clone, Getters)]
pub struct PosVector {
Expand Down Expand Up @@ -98,3 +100,24 @@ impl Draw for VisualDetection<bool, PosVector> {
Ok(())
}
}

impl Draw for VisualDetection<bool, Offset2D<f64>> {
fn draw(&self, canvas: &mut Mat) -> anyhow::Result<()> {
let color = if self.class {
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,
)?;
Ok(())
}
}

0 comments on commit ff51729

Please sign in to comment.