add loss check for easy cases

Author: iBoot32

src/prm_vision/opencv_armor_detector/src/OpenCVArmorDetector.cpp

@@ -64,7 +64,7 @@ std::vector<_Float32> OpenCVArmorDetector::search(cv::Mat &frame)
                            std::to_string(_frame_count) + " (" +
                            std::to_string(_detected_frame * 100 / _frame_count) + "%)");
 
-    cv::waitKey(10);
+    cv::waitKey(200);
 #endif
 
     // If we didn't find an armor for a few frames (ROS2 param), reset the search area

src/prm_vision/opencv_armor_detector/test/resources/close/close.avi renamed to src/prm_vision/opencv_armor_detector/test/resources/close_video/close.avi

@@ -0,0 +1,159 @@
+616,652,620,694,701,649,705,686,
+591,652,591,694,684,651,686,692,
+172,705,169,755,287,699,285,751,
+428,689,432,737,529,683,533,723,
+900,679,902,728,1010,680,1013,728,
+684,682,688,737,800,674,806,726,
+657,669,659,713,755,666,758,709,
+569,674,566,717,663,677,661,724,
+781,653,780,694,880,656,879,700,
+945,694,953,741,1019,688,1026,725,
+520,658,517,701,616,658,614,703,
+600,676,599,721,701,676,701,723,
+673,671,677,718,772,664,777,707,
+758,666,752,709,847,674,840,727,
+611,673,604,710,687,680,680,725,
+600,677,599,721,701,677,701,723,
+600,676,599,721,701,676,701,723,
+233,682,228,723,323,685,318,733,
+600,677,599,721,701,676,701,723,
+752,668,751,721,868,670,867,725,
+526,671,522,706,597,676,591,721,
+501,680,498,724,601,682,599,728,
+696,674,701,718,786,669,791,709,
+556,673,556,722,665,672,666,723,
+600,676,599,721,701,676,701,723,
+1044,657,1054,706,1136,649,1144,691,
+524,672,521,717,624,674,621,725,
+500,663,504,706,585,656,590,695,
+752,662,754,705,844,660,848,698,
+386,653,381,693,474,657,470,701,
+774,652,769,695,864,658,859,710,
+665,659,661,697,752,664,748,707,
+657,690,662,737,750,682,755,725,
+531,656,526,698,623,662,619,706,
+589,680,594,726,684,672,689,715,
+600,676,599,721,701,676,701,723,
+190,678,184,715,265,682,258,730,
+734,678,743,732,834,667,843,713,
+597,676,597,722,703,673,704,720,
+1000,655,1006,702,1105,650,1111,696,
+755,670,752,718,864,675,861,730,
+493,671,488,714,587,676,583,723,
+970,687,977,736,1064,684,1070,726,
+684,682,688,737,800,674,806,726,
+631,686,628,738,744,687,742,743,
+1001,682,1010,737,1104,673,1112,722,
+570,658,569,703,670,659,670,705,
+790,658,787,705,898,662,896,716,
+775,655,770,691,846,662,840,711,
+612,680,608,729,714,686,710,740,
+592,675,591,719,692,677,691,724,
+614,677,615,723,717,676,719,722,
+600,676,599,721,701,677,701,724,
+583,663,581,705,678,664,677,710,
+918,648,920,692,1024,648,1027,693,
+516,669,513,709,606,672,603,713,
+696,674,701,718,786,669,791,709,
+600,677,599,721,701,677,701,723,
+535,676,533,722,638,679,636,726,
+570,658,569,703,670,659,670,705,
+613,678,616,725,711,672,715,715,
+557,678,562,724,649,668,655,711,
+392,673,387,709,465,677,459,718,
+460,686,460,733,564,684,566,727,
+624,651,630,691,694,646,699,677,
+523,671,519,704,590,675,584,720,
+884,665,887,720,1007,663,1011,717,
+617,667,617,711,718,668,718,713,
+582,674,579,718,678,677,677,724,
+556,673,556,722,665,672,666,723,
+0,0,0,0,0,0,0,0,
+545,679,550,726,638,671,644,713,
+755,666,763,710,828,658,834,693,
+748,667,755,711,825,660,831,695,
+428,689,432,737,529,683,533,723,
+283,679,286,729,396,670,400,717,
+555,660,551,697,639,663,634,709,
+523,671,519,704,590,675,584,720,
+526,671,522,706,597,676,591,721,
+413,659,412,703,511,660,509,704,
+549,678,547,724,651,680,650,727,
+548,673,544,712,632,677,628,723,
+708,664,717,712,795,653,802,692,
+600,677,599,721,701,676,701,723,
+748,667,755,711,825,660,831,695,
+498,667,493,707,576,673,570,722,
+190,678,184,715,265,682,258,730,
+559,681,564,728,652,673,657,714,
+597,676,597,722,703,673,704,720,
+536,671,531,709,613,677,608,722,
+680,665,687,708,750,658,755,692,
+415,672,424,719,498,659,506,698,
+172,705,169,755,287,699,285,751,
+596,681,597,729,700,677,702,724,
+752,666,760,710,827,659,833,693,
+380,644,374,676,449,648,442,691,
+616,652,620,694,701,649,705,686,
+612,649,615,695,713,643,717,685,
+493,671,488,714,587,676,583,723,
+749,667,756,711,825,659,831,695,
+884,665,887,720,1007,663,1011,717,
+763,666,771,709,832,659,839,691,
+555,660,551,697,639,663,634,709,
+611,673,604,710,687,680,680,725,
+0,0,0,0,0,0,0,0,
+545,679,550,726,638,671,644,713,
+1044,657,1054,706,1136,649,1144,691,
+547,676,552,723,640,669,645,710,
+600,677,599,721,701,676,701,723,
+668,666,668,709,764,668,765,707,
+646,668,651,718,750,659,757,705,
+448,665,449,708,546,663,547,704,
+600,677,599,721,702,677,701,724,
+560,652,561,696,663,649,664,693,
+758,666,752,709,847,674,840,727,
+566,674,563,717,659,678,656,725,
+600,676,599,721,701,676,701,723,
+530,649,524,685,607,655,600,699,
+392,673,387,709,465,677,459,718,
+543,672,539,711,626,677,621,723,
+688,656,694,701,772,651,778,689,
+596,664,594,711,699,666,697,716,
+665,659,661,697,752,664,748,707,
+612,649,615,695,713,643,717,685,
+806,667,813,720,909,660,915,700,
+806,667,813,720,909,660,915,700,
+758,670,760,723,876,669,879,718,
+502,671,498,706,577,675,572,724,
+900,679,902,728,1010,680,1013,728,
+0,0,0,0,0,0,0,0,
+614,677,615,723,717,676,719,722,
+600,677,599,721,701,676,701,723,
+600,677,599,721,701,676,701,723,
+544,652,539,684,609,659,603,701,
+748,668,755,712,825,660,831,696,
+548,675,543,717,638,680,633,726,
+105,717,98,763,205,716,199,769,
+707,686,715,731,782,677,789,714,
+413,659,412,703,511,660,509,704,
+0,0,0,0,0,0,0,0,
+355,673,362,722,456,660,463,703,
+598,676,597,721,699,676,699,723,
+582,674,579,718,678,677,677,724,
+530,663,537,705,606,656,612,690,
+0,0,0,0,0,0,0,0,
+753,664,747,702,825,673,817,724,
+484,674,479,712,563,676,559,721,
+948,678,954,734,1064,672,1071,724,
+462,666,456,706,543,673,537,720,
+952,682,956,731,1057,681,1062,727,
+734,662,742,709,820,654,827,694,
+597,677,596,721,699,678,699,722,
+605,663,607,708,705,661,707,704,
+707,686,715,731,782,677,789,714,
+87,713,80,751,173,715,164,766,
+532,653,527,685,596,659,590,703,
+768,665,777,708,835,659,841,688,
+490,664,484,703,569,669,563,715,
+0,0,0,0,0,0,0,0,

src/prm_vision/opencv_armor_detector/test/resources/close/ground_truth.csv renamed to src/prm_vision/opencv_armor_detector/test/resources/close_video/ground_truth.csv

@@ -9,17 +9,17 @@
 #include <sstream>
 
 // Helpers
-void iterateThroughFolder(std::string folder_path, OpenCVArmorDetector *detector);
 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_EASY_DETECTION_RATE 0.95                   // 95% detection rate required
-#define MAX_EASY_LOSS_PIX 5                            // 5 pixels loss allowed
-#define MIN_FAR_BACK_SPIN_AND_MOVE_DETECTION_RATE 0.70 // 70% detection rate required
-#define MAX_FAR_BACK_SPIN_AND_MOVE_LOSS_PIX 5          // 5 pixels loss allowed
-#define MIN_CLOSE_DETECTION_RATE 0.65                  // 65% detection rate required
-#define MAX_CLOSE_LOSS_PIX 5                           // 5 pixels loss allowed
+#define MIN_DETECTION_RATE_EASY 0.95                   // 95% detection rate required
+#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
 
 // Set up the test fixture
 class OpenCVArmorDetectorTest : public ::testing::Test
@@ -194,6 +194,7 @@ TEST_F(OpenCVArmorDetectorTest, test_search_no_armor)
 // This is the key test section for the armor detection. We will test the search method with frames containing armor
 // We must meet a detection rate and loss threshold to pass
 //
+
 TEST_F(OpenCVArmorDetectorTest, test_search_armor_easy)
 {
     // We install the test resources to the package share directory (done in the CMakeLists.txt)
@@ -206,13 +207,9 @@ TEST_F(OpenCVArmorDetectorTest, test_search_armor_easy)
     EXPECT_EQ(detector_easy->_frame_count, 0);
 
     // TEST THE EASY CASES
-    std::filesystem::path easy_path = std::filesystem::path(package_share_dir) / "resources/easy";
-    iterateThroughFolder(easy_path.string(), detector_easy);
-    EXPECT_EQ(detector_easy->_frame_count, 159); // 159 frames in the easy folder
-    double detection_rate = static_cast<double>(detector_easy->_detected_frame) / static_cast<double>(detector_easy->_frame_count);
-    EXPECT_GE(detection_rate, MIN_EASY_DETECTION_RATE);
-
-    // TODO: Check loss against ground truths
+    std::string easy_path = (std::filesystem::path(package_share_dir) / "resources/easy").string();
+    std::string gtPath = (std::filesystem::path(package_share_dir) / "resources/easy/ground_truth.csv").string();
+    testImgs(easy_path, gtPath, detector_easy, MIN_DETECTION_RATE_EASY, MAX_LOSS_EASY);
 
     delete detector_easy;
 }
@@ -228,30 +225,77 @@ TEST_F(OpenCVArmorDetectorTest, test_search_armor_close_video)
     EXPECT_EQ(detector_close->_detected_frame, 0);
     EXPECT_EQ(detector_close->_frame_count, 0);
 
-    std::string videoPath = (std::filesystem::path(package_share_dir) / "resources/close/close.avi").string();
-    std::string gtPath = (std::filesystem::path(package_share_dir) / "resources/close/ground_truth.csv").string();
-    testVideo(videoPath, gtPath, detector_close, MIN_CLOSE_DETECTION_RATE, MAX_CLOSE_LOSS_PIX);
+    std::string videoPath = (std::filesystem::path(package_share_dir) / "resources/close_video/close.avi").string();
+    std::string gtPath = (std::filesystem::path(package_share_dir) / "resources/close_video/ground_truth.csv").string();
+    testVideo(videoPath, gtPath, detector_close, MIN_DETECTION_RATE_CLOSE_VIDEO, MAX_LOSS_CLOSE_VIDEO);
 }
 
-// This an overall test, does up-close, far, spin, and hidden. So target accuracy is lower
+// This an overall test. Contains up-close, far, spin, and hidden. So target accuracy is lower
 TEST_F(OpenCVArmorDetectorTest, test_search_armor_far_back_spin_and_move_video)
 {
     // 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_spin_and_move = new OpenCVArmorDetector({RED, 30, 100, 150, 1, true});
+
     // Should be no missed frames or detected frames at the start
     EXPECT_EQ(detector_far_back_spin_and_move->getMissedFrames(), 0);
     EXPECT_EQ(detector_far_back_spin_and_move->_detected_frame, 0);
     EXPECT_EQ(detector_far_back_spin_and_move->_frame_count, 0);
 
     std::string videoPath = (std::filesystem::path(package_share_dir) / "resources/far_back_spin_and_move/far.avi").string();
     std::string gtPath = (std::filesystem::path(package_share_dir) / "resources/far_back_spin_and_move/ground_truth.csv").string();
-    testVideo(videoPath, gtPath, detector_far_back_spin_and_move, MIN_FAR_BACK_SPIN_AND_MOVE_DETECTION_RATE, MAX_FAR_BACK_SPIN_AND_MOVE_LOSS_PIX);
+    testVideo(videoPath, gtPath, detector_far_back_spin_and_move, MIN_DETECTION_RATE_FAR_BACK_SPIN_AND_MOVE, MAX_LOSS_FAR_BACK_SPIN_AND_MOVE);
 }
 
 //////////////////////
 // HELPER FUNCTIONS //
 //////////////////////
+void testImgs(std::string folder_path, std::string gt_path, OpenCVArmorDetector *detector, float min_detection_rate, float max_loss_pix)
+{
+    // Read ground truths
+    std::vector<std::vector<cv::Point2f>> gt;
+    readGT(gt_path, gt);
+
+    // Loss is the sum of the L2 norm between the detected points and the ground truth
+    float total_loss = 0;
+    int frame_idx = 0;
+
+    // Loop through all images in the folder
+    for (const auto &entry : std::filesystem::directory_iterator(folder_path))
+    {
+        // if not png or jpg, skip
+        if (entry.path().extension() != ".png" && entry.path().extension() != ".jpg")
+        {
+            continue;
+        }
+
+        // Read the image and run the detector
+        std::string img_path = entry.path().string();
+        cv::Mat frame = cv::imread(img_path);
+        cv::resize(frame, frame, cv::Size(WIDTH, HEIGHT));
+        std::vector<_Float32> points = detector->search(frame);
+
+        // Loss between the detected points and the ground truth
+        for (int i = 0; i < 4; i++)
+        {
+            cv::Point2f detected_point(points.at(i * 2), points.at(i * 2 + 1));
+            cv::Point2f gt_point = gt.at(frame_idx).at(i);
+
+            total_loss += 0.25 * cv::norm(detected_point - gt_point);
+        }
+
+        frame_idx++;
+    }
+
+    // Calculate the average loss across all frames
+    double loss = total_loss / detector->_frame_count;
+
+    // Check detection rate and loss
+    double detection_rate = static_cast<double>(detector->_detected_frame) / static_cast<double>(detector->_frame_count);
+    EXPECT_GE(detection_rate, min_detection_rate);
+    EXPECT_LT(loss / detector->_frame_count, max_loss_pix);
+}
+
 void testVideo(std::string video_path, std::string gt_path, OpenCVArmorDetector *detector, float min_detection_rate, float max_loss_pix)
 {
     // Read ground truths
@@ -271,64 +315,43 @@ void testVideo(std::string video_path, std::string gt_path, OpenCVArmorDetector
 
     // Read the video frame by frame
     cv::Mat frame;
-    int idx = 0;
-    float loss = 0;
 
+    float total_loss = 0;
+    int frame_idx = 0;
     while (cap.read(frame))
     {
         cv::resize(frame, frame, cv::Size(WIDTH, HEIGHT));
         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(idx).at(0) == cv::Point2f(0, 0) && gt.at(idx).at(1) == cv::Point2f(0, 0) && gt.at(idx).at(2) == cv::Point2f(0, 0) && gt.at(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))
+        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))
         {
-            idx++;
+            frame_idx++;
             continue;
         }
 
         // Loss is the sum of the L2 norm between the detected points and the ground truth
         for (int i = 0; i < 4; i++)
         {
-            loss += cv::norm(points.at(i * 2) - gt.at(idx).at(i).x) + cv::norm(points.at(i * 2 + 1) - gt.at(idx).at(i).y);
+            cv::Point2f detected_point(points.at(i * 2), points.at(i * 2 + 1));
+            cv::Point2f gt_point = gt.at(frame_idx).at(i);
+
+            total_loss += 0.25 * cv::norm(detected_point - gt_point);
         }
 
-        idx++;
+        frame_idx++;
     }
 
+    // Calculate the average loss across all frames
+    double loss = total_loss / detector->_frame_count;
+
     // Check detection rate and loss
     double detection_rate = static_cast<double>(detector->_detected_frame) / static_cast<double>(detector->_frame_count);
     EXPECT_GE(detection_rate, min_detection_rate);
     EXPECT_LT(loss / detector->_frame_count, max_loss_pix);
 }
 
-void iterateThroughFolder(std::string folder_path, OpenCVArmorDetector *detector)
-{
-    namespace fs = std::filesystem;
-
-    // Iterate through all .png files in the easy tests folder
-    for (const auto &entry : fs::directory_iterator(folder_path))
-    {
-        if (entry.is_regular_file() && entry.path().extension() == ".png")
-        {
-            std::string file_path = entry.path().string();
-
-            // Load the image
-            cv::Mat frame = cv::imread(file_path, cv::IMREAD_COLOR);
-            if (frame.empty())
-            {
-                std::cerr << "Failed to read image: " << file_path << std::endl;
-                continue;
-            }
-
-            cv::resize(frame, frame, cv::Size(WIDTH, HEIGHT));
-
-            // Call the detector's search function
-            std::vector<_Float32> points = detector->search(frame);
-        }
-    }
-}
-
 void readGT(std::string file_path, std::vector<std::vector<cv::Point2f>> &gt)
 {
     std::ifstream file(file_path);