add test for far back specifically, switch to convexhull for more rel…

…iability with detected armor plate shape. tighten constraints for some tests

src/prm_launch/launch/video2detector.py

@@ -15,7 +15,7 @@ def generate_launch_description():
             emulate_tty=True,
             executable='VideoCaptureNode',
             parameters=[{'source': str(video_path),
-                         'fps': 10,
+                         'fps': 1,
                          'frame_id': 'video',
                          }]    
         ), 

src/prm_vision/opencv_armor_detector/src/OpenCVArmorDetector.cpp

@@ -33,6 +33,8 @@ std::vector<_Float32> OpenCVArmorDetector::search(cv::Mat &frame)
         _search_area[1] = 0;
         _search_area[2] = WIDTH;
         _search_area[3] = HEIGHT;
+        _reset_search_area = false;
+        _missed_frames = 0;
     }
     cv::Mat croppedFrame = frame(cv::Range(_search_area[1], _search_area[3]), cv::Range(_search_area[0], _search_area[2])).clone();
 
@@ -62,19 +64,18 @@ std::vector<_Float32> OpenCVArmorDetector::search(cv::Mat &frame)
     cv::setWindowTitle(window_name,
                        "detected: " + std::to_string(_detected_frame) + " / " +
                            std::to_string(_frame_count) + " (" +
-                           std::to_string(_detected_frame * 100 / _frame_count) + "%)");
+                           std::to_string(_detected_frame * 100 / _frame_count) + "%) and missed: " + std::to_string(_missed_frames) + std::string(" frames"));
 
-    cv::waitKey(200);
+    cv::waitKey(1000);
 #endif
 
     // If we didn't find an armor for a few frames (ROS2 param), reset the search area
     if (points.size() == 0)
     {
         _missed_frames++;
-        if (_missed_frames > _max_missed_frames)
+        if (_missed_frames >= _max_missed_frames)
         {
             _reset_search_area = true;
-            _missed_frames = 0;
         }
     }
     else
@@ -149,8 +150,27 @@ std::vector<cv::Point2f> OpenCVArmorDetector::detectArmorsInFrame(cv::Mat &frame
     {
         if (contour.size() > 20)
         {
-            // Fit an ellipse to the contour, and check if it's likely a light bar
-            cv::RotatedRect rect = cv::fitEllipseDirect(contour);
+            // Use convex hull to get a convex contour
+            std::vector<cv::Point> hull;
+            cv::convexHull(contour, hull);
+
+            auto rect_bounding = cv::boundingRect(hull);
+            cv::RotatedRect rect = cv::RotatedRect(cv::Point2f(rect_bounding.x + rect_bounding.width / 2, rect_bounding.y + rect_bounding.height / 2), cv::Size2f(rect_bounding.width, rect_bounding.height), 0);
+
+            // draw rotated rectangle
+            cv::Point2f vertices[4];
+            rect.points(vertices);
+            for (int i = 0; i < 4; i++)
+            {
+                cv::line(result, vertices[i], vertices[(i + 1) % 4], cv::Scalar(0, 0, 255), 4, cv::LINE_AA);
+            }
+
+            if (rect.angle > 45)
+            {
+                std::swap(rect.size.width, rect.size.height);
+                rect.angle -= 90;
+            }
+
             if (isLightBar(rect))
             {
                 light_bar_candidates.push_back(rect);

src/prm_vision/opencv_armor_detector/test/resources/far_back/ground_truth.csv

@@ -0,0 +1,54 @@
+531,529,532,541,569,527,570,540,
+527,539,528,554,571,538,571,554,
+0,0,0,0,0,0,0,0,
+529,535,529,550,571,534,571,549,
+527,539,527,556,570,539,571,554,
+498,529,498,540,532,528,530,541,
+518,528,518,541,555,527,554,541,
+555,526,555,538,592,525,593,537,
+525,532,525,547,565,531,565,545,
+566,528,568,541,602,526,603,538,
+532,529,532,541,570,526,570,540,
+572,528,573,541,606,525,607,536,
+558,529,560,541,595,526,596,538,
+493,529,491,539,524,529,522,542,
+549,541,550,558,593,540,594,555,
+550,528,551,542,588,529,588,539,
+544,529,545,542,582,528,583,540,
+529,530,529,543,567,529,567,543,
+533,529,533,541,570,526,570,540,
+529,528,529,541,566,528,566,540,
+535,540,536,556,580,539,580,556,
+529,527,529,537,566,526,566,540,
+538,526,538,540,576,525,576,539,
+529,536,529,552,571,535,571,550,
+514,528,514,541,550,528,549,542,
+529,539,529,553,571,536,572,551,
+532,528,532,540,570,526,569,539,
+0,0,0,0,0,0,0,0,
+514,528,514,539,548,528,547,541,
+523,527,522,538,559,526,559,540,
+530,529,531,542,569,528,569,541,
+527,532,527,544,567,533,566,543,
+533,529,533,541,570,526,570,540,
+533,529,532,540,570,526,570,540,
+527,540,527,556,571,539,571,555,
+524,534,524,548,564,534,565,547,
+518,528,518,538,553,527,552,540,
+544,527,545,540,582,526,582,538,
+516,530,515,542,553,528,552,542,
+502,529,502,541,536,529,535,543,
+529,535,529,550,570,533,570,548,
+520,529,520,542,558,528,558,540,
+509,529,507,541,544,528,544,543,
+0,0,0,0,0,0,0,0,
+528,539,528,554,571,536,571,552,
+526,531,526,543,565,531,566,543,
+0,0,0,0,0,0,0,0,
+579,527,582,541,612,523,613,535,
+526,528,526,541,563,527,563,541,
+508,529,508,540,544,527,542,540,
+530,530,530,542,568,528,568,542,
+523,533,523,548,563,532,563,547,
+0,0,0,0,0,0,0,0,
+536,529,537,542,575,527,575,541,

src/prm_vision/opencv_armor_detector/test/test_OpenCVArmorDetector.cpp

@@ -13,15 +13,20 @@ void readGT(std::string file_path, std::vector<std::vector<cv::Point2f>> &gt);
 void testVideo(std::string video_path, std::string gt_path, OpenCVArmorDetector *detector, float min_detection_rate, float max_loss_pix);
 void testImgs(std::string folder_path, std::string gt_path, OpenCVArmorDetector *detector, float min_detection_rate, float max_loss_pix);
 
-// DETECTOR REQUIREMENTS
-#define MIN_DETECTION_RATE_EASY 0.95                   // 95% detection rate required
+/* DETECTOR REQUIREMENTS */
+// Pre-selected images (tighter requirements since we know an armor is present)
+#define MIN_DETECTION_RATE_EASY 0.95     // 95% detection rate required
+#define MIN_DETECTION_RATE_FAR_BACK 0.90 // 90% detection rate required
+#define MAX_LOSS_EASY 2                  // 2 pixels loss allowed
+#define MAX_LOSS_FAR_BACK 3              // 3 pixels loss allowed
+
+// Realistic videos (looser requirements due to spintop and obstruction)
 #define MIN_DETECTION_RATE_FAR_BACK_SPIN_AND_MOVE 0.70 // 70% detection rate required
 #define MIN_DETECTION_RATE_CLOSE_VIDEO 0.65            // 65% detection rate required
-#define MAX_LOSS_EASY 5                                // 5 pixels loss allowed
-#define MAX_LOSS_FAR_BACK_SPIN_AND_MOVE 5              // 5 pixels loss allowed
-#define MAX_LOSS_CLOSE_VIDEO 5                         // 5 pixels loss allowed
+#define MAX_LOSS_FAR_BACK_SPIN_AND_MOVE 3              // 3 pixels loss allowed
+#define MAX_LOSS_CLOSE_VIDEO 3                         // 3 pixels loss allowed
+/* END DETECTOR REQUIREMENTS */
 
-// Set up the test fixture
 class OpenCVArmorDetectorTest : public ::testing::Test
 {
 protected:
@@ -214,6 +219,24 @@ TEST_F(OpenCVArmorDetectorTest, test_search_armor_easy)
     delete detector_easy;
 }
 
+TEST_F(OpenCVArmorDetectorTest, test_search_armor_far_back)
+{
+    // We install the test resources to the package share directory (done in the CMakeLists.txt)
+    std::string package_share_dir = ament_index_cpp::get_package_share_directory("opencv_armor_detector");
+    OpenCVArmorDetector *detector_far_back = new OpenCVArmorDetector({RED, 30, 100, 150, 1, false});
+
+    // Should be no missed frames or detected frames at the start
+    EXPECT_EQ(detector_far_back->getMissedFrames(), 0);
+    EXPECT_EQ(detector_far_back->_detected_frame, 0);
+    EXPECT_EQ(detector_far_back->_frame_count, 0);
+
+    std::string far_back_path = (std::filesystem::path(package_share_dir) / "resources/far_back").string();
+    std::string gtPath = (std::filesystem::path(package_share_dir) / "resources/far_back/ground_truth.csv").string();
+    testImgs(far_back_path, gtPath, detector_far_back, MIN_DETECTION_RATE_FAR_BACK, MAX_LOSS_FAR_BACK);
+
+    delete detector_far_back;
+}
+
 TEST_F(OpenCVArmorDetectorTest, test_search_armor_close_video)
 {
     // We install the test resources to the package share directory (done in the CMakeLists.txt)
@@ -276,6 +299,13 @@ void testImgs(std::string folder_path, std::string gt_path, OpenCVArmorDetector
         std::vector<_Float32> points = detector->search(frame);
 
         // Loss between the detected points and the ground truth
+        // Skip if no armor detected in gt or in the frame (all zeros) since we want to check loss given a detection. Detection rate is accounted for separately
+        if ((gt.at(frame_idx).at(0) == cv::Point2f(0, 0) && gt.at(frame_idx).at(1) == cv::Point2f(0, 0) && gt.at(frame_idx).at(2) == cv::Point2f(0, 0) && gt.at(frame_idx).at(3) == cv::Point2f(0, 0)) || (points.at(0) == 0 && points.at(1) == 0 && points.at(2) == 0 && points.at(3) == 0 && points.at(4) == 0 && points.at(5) == 0 && points.at(6) == 0 && points.at(7) == 0))
+        {
+            frame_idx++;
+            continue;
+        }
+
         for (int i = 0; i < 4; i++)
         {
             cv::Point2f detected_point(points.at(i * 2), points.at(i * 2 + 1));