From 0b24c6a5afab511cde22f57c294332cc1a4c1de1 Mon Sep 17 00:00:00 2001 From: xinyang Date: Sun, 4 Aug 2019 00:37:41 +0800 Subject: [PATCH] fix bug. --- armor/include/armor_finder/armor_finder.h | 2 - armor/src/armor_finder/anti_top/anti_top.cpp | 11 +++-- .../src/armor_finder/find/find_armor_box.cpp | 6 ++- .../searching_state/searching_state.cpp | 21 ++++++++-- .../tracking_state/tracking_state.cpp | 40 ++++++++++--------- main.cpp | 6 +-- 6 files changed, 52 insertions(+), 34 deletions(-) diff --git a/armor/include/armor_finder/armor_finder.h b/armor/include/armor_finder/armor_finder.h index 2a4ad28..eb49254 100644 --- a/armor/include/armor_finder/armor_finder.h +++ b/armor/include/armor_finder/armor_finder.h @@ -124,9 +124,7 @@ private: Serial &serial; // 串口对象，引用外部变量，用于和能量机关共享同一个变量 const uint8_t &use_classifier; // 标记是否启用CNN分类器，引用外部变量，自动变化 RoundQueue top_periodms; // 陀螺周期循环队列 - RoundQueue box_ratioes; // systime last_front_time; // 上一次发生装甲板方向切换的时间 - BoxRatioChangeType last_ratio_type; // int anti_top_cnt; // 满足条件的装甲板方向切换持续次数，用于反陀螺 AntiTopState anti_top_state; // 当前是否识别到陀螺 diff --git a/armor/src/armor_finder/anti_top/anti_top.cpp b/armor/src/armor_finder/anti_top/anti_top.cpp index 985cbed..5e915a0 100644 --- a/armor/src/armor_finder/anti_top/anti_top.cpp +++ b/armor/src/armor_finder/anti_top/anti_top.cpp @@ -81,15 +81,17 @@ void ArmorFinder::antiTop() { void ArmorFinder::antiTop() { if (target_box.rect == cv::Rect2d()) return; + static int fps = 0; uint16_t shoot_delay = 0; auto interval = getTimeIntervalms(frame_time, last_front_time); if (anti_top_state == ANTI_TOP && interval > 700) { anti_top_state = NORMAL; LOGM(STR_CTR(WORD_YELLOW, "switch to normal")); } -// cout << getPointLength(last_box.getCenter() - target_box.getCenter()) << endl; - if (getPointLength(last_box.getCenter() - target_box.getCenter()) > last_box.rect.height * 2.0) { - LOGM("switch!"); + fps++; + if (last_box.rect != cv::Rect2d() && + getPointLength(last_box.getCenter() - target_box.getCenter()) > last_box.rect.height * 1.0) { + LOGM("switch %d! %lf", fps, getPointLength(last_box.getCenter() - target_box.getCenter()) / last_box.rect.height); if (150 < interval && interval < 700) { if (anti_top_state == ANTI_TOP) { top_periodms.push(interval); @@ -106,11 +108,12 @@ void ArmorFinder::antiTop() { } } } + fps = 0; last_front_time = frame_time; } if (anti_top_state == NORMAL) { sendBoxPosition(0); - } else if (interval < top_periodms[-1] * 0.2){ + } else if (interval < top_periodms[-1] * 0.2) { sendBoxPosition(0); } last_box = target_box; diff --git a/armor/src/armor_finder/find/find_armor_box.cpp b/armor/src/armor_finder/find/find_armor_box.cpp index 3a4b053..c4a5194 100644 --- a/armor/src/armor_finder/find/find_armor_box.cpp +++ b/armor/src/armor_finder/find/find_armor_box.cpp @@ -155,8 +155,10 @@ bool ArmorFinder::findArmorBox(const cv::Mat &src, ArmorBox &box) { return a < b; } }); - if(armor_boxes[0].id != 0){ - box = armor_boxes[0]; + for(auto &one_box : armor_boxes){ + if(one_box.id != 0){ + box = one_box; + } } if (save_labelled_boxes) { for (const auto &one_box : armor_boxes) { diff --git a/armor/src/armor_finder/searching_state/searching_state.cpp b/armor/src/armor_finder/searching_state/searching_state.cpp index ac6af41..e7b6195 100644 --- a/armor/src/armor_finder/searching_state/searching_state.cpp +++ b/armor/src/armor_finder/searching_state/searching_state.cpp @@ -8,14 +8,16 @@ #include bool ArmorFinder::stateSearchingTarget(cv::Mat &src) { - if(anti_switch_cnt >= 3){ - last_box = ArmorBox(); - } +// if(anti_switch_cnt >= 3){ +// last_box = ArmorBox(); +// anti_switch_cnt = 0; +// } if (findArmorBox(src, target_box)) { if (last_box.rect != cv::Rect2d() && - (getPointLength(last_box.getCenter() - target_box.getCenter()) > last_box.rect.height * 3.0) && + (getPointLength(last_box.getCenter() - target_box.getCenter()) > last_box.rect.height * 2.0) && anti_switch_cnt++ < 3) { target_box = ArmorBox(); + LOGM("anti-switch!"); return false; } else { anti_switch_cnt = 0; @@ -27,3 +29,14 @@ bool ArmorFinder::stateSearchingTarget(cv::Mat &src) { return false; } } + +/* +bool ArmorFinder::stateSearchingTarget(cv::Mat &src) { + if (findArmorBox(src, target_box)) { + return true; + } else { + target_box = ArmorBox(); + return false; + } +} +*/ \ No newline at end of file diff --git a/armor/src/armor_finder/tracking_state/tracking_state.cpp b/armor/src/armor_finder/tracking_state/tracking_state.cpp index cd9a9b7..53689dc 100644 --- a/armor/src/armor_finder/tracking_state/tracking_state.cpp +++ b/armor/src/armor_finder/tracking_state/tracking_state.cpp @@ -42,25 +42,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(); +// 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; } diff --git a/main.cpp b/main.cpp index 7d6a681..bac5761 100644 --- a/main.cpp +++ b/main.cpp @@ -66,8 +66,8 @@ int main(int argc, char *argv[]) { video_gimbal = new CameraWrapper(ARMOR_CAMERA_GAIN, 2/*, "armor"*/); video_chassis = new CameraWrapper(ENERGY_CAMERA_GAIN, 2/*, "energy"*/); } else { - video_gimbal = new VideoWrapper("/home/sun/项目/energy_video/7.27.avi"); - video_chassis = new VideoWrapper("/home/sun/项目/energy_video/7.27.avi"); + video_gimbal = new VideoWrapper(PROJECT_DIR"/gimbal_video/0.avi"); + video_chassis = new VideoWrapper(PROJECT_DIR"/gimbal_video/0.avi"); } if (video_gimbal->init()) { LOGM("video_gimbal source initialization successfully."); @@ -178,7 +178,7 @@ int main(int argc, char *argv[]) { armorFinder.run(gimbal_src); }); } -// cv::waitKey(0); +// cv::waitKey(0); }); } while (ok); delete video_gimbal;