From 41de64dbb6242553da1f127e300256db37f8b10e Mon Sep 17 00:00:00 2001
From: xinyang <895639507@qq.com>
Date: Wed, 7 Aug 2019 02:59:34 +0800
Subject: [PATCH] fix bug.

---
 armor/include/armor_finder/armor_finder.h     |  1 +
 .../src/armor_finder/find/find_armor_box.cpp  | 11 ++---
 .../tracking_state/tracking_state.cpp         | 46 ++++++++++---------
 3 files changed, 30 insertions(+), 28 deletions(-)

diff --git a/armor/include/armor_finder/armor_finder.h b/armor/include/armor_finder/armor_finder.h
index a886655..789a578 100644
--- a/armor/include/armor_finder/armor_finder.h
+++ b/armor/include/armor_finder/armor_finder.h
@@ -124,6 +124,7 @@ private:
 
     bool findLightBlobs(const cv::Mat &src, LightBlobs &light_blobs);
     bool findArmorBox(const cv::Mat &src, ArmorBox &box);
+    bool matchArmorBoxes(const cv::Mat &src, const LightBlobs &light_blobs, ArmorBoxes &armor_boxes);
 
     bool stateSearchingTarget(cv::Mat &src);            // searching state主函数
     bool stateTrackingTarget(cv::Mat &src);             // tracking state主函数
diff --git a/armor/src/armor_finder/find/find_armor_box.cpp b/armor/src/armor_finder/find/find_armor_box.cpp
index 25f6364..3dfcfcf 100644
--- a/armor/src/armor_finder/find/find_armor_box.cpp
+++ b/armor/src/armor_finder/find/find_armor_box.cpp
@@ -83,11 +83,11 @@ static double centerDistance(const cv::Rect2d &box) {
     return dx * dx + dy * dy;
 }
 
-bool matchArmorBoxes(const cv::Mat &src, const LightBlobs &light_blobs, ArmorBoxes &armor_boxes, uint8_t color) {
+bool ArmorFinder::matchArmorBoxes(const cv::Mat &src, const LightBlobs &light_blobs, ArmorBoxes &armor_boxes) {
     armor_boxes.clear();
     for (int i = 0; i < light_blobs.size() - 1; ++i) {
         for (int j = i + 1; j < light_blobs.size(); ++j) {
-            if (!isCoupleLight(light_blobs.at(i), light_blobs.at(j), color)) {
+            if (!isCoupleLight(light_blobs.at(i), light_blobs.at(j), enemy_color)) {
                 continue;
             }
             cv::Rect2d rect_left = light_blobs.at(static_cast<unsigned long>(i)).rect.boundingRect();
@@ -101,13 +101,13 @@ bool matchArmorBoxes(const cv::Mat &src, const LightBlobs &light_blobs, ArmorBox
             if (min_x < 0 || max_x > src.cols || min_y < 0 || max_y > src.rows) {
                 continue;
             }
-            if ((max_y + min_y) / 2 < 120) continue;
+            if (state == SEARCHING_STATE && (max_y + min_y) / 2 < 120) continue;
             if ((max_x - min_x) / (max_y - min_y) < 0.8) continue;
             LightBlobs pair_blobs = {light_blobs.at(i), light_blobs.at(j)};
             armor_boxes.emplace_back(
                     cv::Rect2d(min_x, min_y, max_x - min_x, max_y - min_y),
                     pair_blobs,
-                    color
+                    enemy_color
             );
         }
     }
@@ -130,8 +130,7 @@ bool ArmorFinder::findArmorBox(const cv::Mat &src, ArmorBox &box) {
         cv::waitKey(1);
     }
     CNT_TIME("boxes", {
-        if (!matchArmorBoxes(src, light_blobs, armor_boxes, enemy_color)) {
-            //        cout << "Box fail!" << endl;
+        if (!matchArmorBoxes(src, light_blobs, armor_boxes)) {
             return false;
         }
     });
diff --git a/armor/src/armor_finder/tracking_state/tracking_state.cpp b/armor/src/armor_finder/tracking_state/tracking_state.cpp
index be3d0b0..97a52b7 100644
--- a/armor/src/armor_finder/tracking_state/tracking_state.cpp
+++ b/armor/src/armor_finder/tracking_state/tracking_state.cpp
@@ -10,10 +10,12 @@ bool ArmorFinder::stateTrackingTarget(cv::Mat &src) {
     auto pos = target_box.rect;
     if(!tracker->update(src, pos)){
         target_box = ArmorBox();
+        LOGW("Track fail!");
         return false;
     }
     if((pos & cv::Rect2d(0, 0, 640, 480)) != pos){
         target_box = ArmorBox();
+        LOGW("Track out range!");
         return false;
     }
 
@@ -25,8 +27,8 @@ bool ArmorFinder::stateTrackingTarget(cv::Mat &src) {
     bigger_rect.width  = pos.width * 2;
     bigger_rect &= cv::Rect2d(0, 0, 640, 480);
 
-//    if(show_armor_box)
-//        showTrackSearchingPos("track", src, bigger_rect);
+    if(show_armor_box)
+        showTrackSearchingPos("track", src, bigger_rect);
 
     cv::Mat roi = src(bigger_rect).clone();
 
@@ -42,27 +44,27 @@ bool ArmorFinder::stateTrackingTarget(cv::Mat &src) {
         tracker = TrackerToUse::create();
         tracker->init(src, target_box.rect);
     }else{
-//        roi = src(pos).clone();
-//        if(classifier){
-//            cv::resize(roi, roi, cv::Size(48, 36));
-//            if(classifier(roi) == 0){
-//                target_box = ArmorBox();
-//                return false;
-//            }
-//        }else{
-//            cv::Mat roi_gray;
-//            cv::cvtColor(roi, roi_gray, CV_RGB2GRAY);
-//            cv::threshold(roi_gray, roi_gray, 180, 255, cv::THRESH_BINARY);
-//            contour_area = cv::countNonZero(roi_gray);
-//            if(abs(cv::countNonZero(roi_gray) - contour_area) > contour_area * 0.3){
-//                target_box = ArmorBox();
-//                return false;
-//            }
-//        }
-//        target_box.rect = pos;
-//        target_box.light_blobs.clear();
+        roi = src(pos).clone();
+        if(classifier){
+            cv::resize(roi, roi, cv::Size(48, 36));
+            if(classifier(roi) == 0){
+                target_box = ArmorBox();
+                LOGW("Track classify fail range!");
+                return false;
+            }
+        }else{
+            cv::Mat roi_gray;
+            cv::cvtColor(roi, roi_gray, CV_RGB2GRAY);
+            cv::threshold(roi_gray, roi_gray, 180, 255, cv::THRESH_BINARY);
+            contour_area = cv::countNonZero(roi_gray);
+            if(abs(cv::countNonZero(roi_gray) - contour_area) > contour_area * 0.3){
+                target_box = ArmorBox();
+                return false;
+            }
+        }
+        target_box.rect = pos;
+        target_box.light_blobs.clear();
         target_box = ArmorBox();
-        return false;
     }
     return true;
 }