diff --git a/simcontroller/src/main.rs b/simcontroller/src/main.rs index 86fc275..a2b5791 100644 --- a/simcontroller/src/main.rs +++ b/simcontroller/src/main.rs @@ -1,5 +1,5 @@ use camera::Camera; -use opencv::{core::{VecN, CV_8UC4}, highgui, imgcodecs::ImreadModes, imgproc}; +use opencv::{core::{Ptr, VecN, CV_8UC4}, features2d::ORB, highgui, imgcodecs::ImreadModes, imgproc, prelude::Feature2DTrait}; use webots::prelude::*; use std::{f64::consts::TAU, time::Duration}; mod camera; @@ -12,6 +12,7 @@ struct MyRobot { left_motor: Motor, right_motor: Motor, window: &'static str, + orb_detector: Ptr, } impl Robot for MyRobot { @@ -33,11 +34,14 @@ impl Robot for MyRobot { let window = "processed"; //highgui::named_window(window, 1).unwrap(); + let orb_detector = ORB::create_def().unwrap(); + Self { camera, left_motor, right_motor, window, + orb_detector, } } @@ -49,7 +53,15 @@ impl Robot for MyRobot { let img = opencv::core::Mat::new_rows_cols_with_bytes::>(240, 320, &img).unwrap(); //opencv::imgcodecs::imwrite_def("cam.png", &img).unwrap(); - highgui::imshow(self.window, &img).unwrap(); + //highgui::imshow(self.window, &img).unwrap(); + + let mask = opencv::core::Mat::default(); + let mut debugout = opencv::core::Mat::default(); + let mut keypoints = opencv::core::Vector::default(); + let mut descriptors = opencv::core::Mat::default(); + self.orb_detector.detect_and_compute_def(&img, &mask, &mut keypoints, &mut descriptors).unwrap(); + opencv::features2d::draw_keypoints(&img, &keypoints, &mut debugout, opencv::core::VecN([0.,255.,255.,255.]), opencv::features2d::DrawMatchesFlags::DEFAULT).unwrap(); + highgui::imshow("keypoints", &debugout).unwrap(); highgui::poll_key().unwrap(); }