From b67ef4b475d0a489383092457ade533cea0e960c Mon Sep 17 00:00:00 2001 From: xinyang Date: Sat, 3 Aug 2019 22:49:46 +0800 Subject: [PATCH 1/7] fix bug. --- armor/src/armor_finder/send_target/send_target.cpp | 2 +- armor/src/armor_finder/tracking_state/tracking_state.cpp | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/armor/src/armor_finder/send_target/send_target.cpp b/armor/src/armor_finder/send_target/send_target.cpp index 06f53f2..8e45ad0 100644 --- a/armor/src/armor_finder/send_target/send_target.cpp +++ b/armor/src/armor_finder/send_target/send_target.cpp @@ -44,7 +44,7 @@ static bool sendTarget(Serial &serial, double x, double y, double z, uint16_t sh bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) { if (target_box.rect == cv::Rect2d()) return false; if (shoot_delay) { - LOGM(STR_CTR(WORD_BLUE, "shoot after %dms"), shoot_delay); + LOGM(STR_CTR(WORD_BLUE, "next box %dms"), shoot_delay); } auto rect = target_box.rect; double dx = rect.x + rect.width / 2 - IMAGE_CENTER_X; diff --git a/armor/src/armor_finder/tracking_state/tracking_state.cpp b/armor/src/armor_finder/tracking_state/tracking_state.cpp index cd9a9b7..46aaf8b 100644 --- a/armor/src/armor_finder/tracking_state/tracking_state.cpp +++ b/armor/src/armor_finder/tracking_state/tracking_state.cpp @@ -62,5 +62,6 @@ bool ArmorFinder::stateTrackingTarget(cv::Mat &src) { target_box.rect = pos; target_box.light_blobs.clear(); } + last_box = target_box; return true; } From 04496ae99b7965d6f2d6ea7e1d63200fc0e208da Mon Sep 17 00:00:00 2001 From: xinyang Date: Sat, 3 Aug 2019 22:53:27 +0800 Subject: [PATCH 2/7] fix bug. --- armor/src/armor_finder/anti_top/anti_top.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/armor/src/armor_finder/anti_top/anti_top.cpp b/armor/src/armor_finder/anti_top/anti_top.cpp index 6d99005..8a61cd9 100644 --- a/armor/src/armor_finder/anti_top/anti_top.cpp +++ b/armor/src/armor_finder/anti_top/anti_top.cpp @@ -99,7 +99,7 @@ void ArmorFinder::antiTop() { } else { if (++anti_top_cnt > 4) { - anti_top_state == ANTI_TOP; + anti_top_state = ANTI_TOP; LOGM(STR_CTR(WORD_CYAN, "switch to anti-top")); } } From 81ab17afa51fe4f7046a1ba1dfc81d0ef5da74c1 Mon Sep 17 00:00:00 2001 From: xinyang Date: Sat, 3 Aug 2019 23:11:42 +0800 Subject: [PATCH 3/7] fix bug. --- armor/src/armor_finder/anti_top/anti_top.cpp | 7 +++++-- armor/src/armor_finder/searching_state/searching_state.cpp | 1 - armor/src/armor_finder/tracking_state/tracking_state.cpp | 1 - 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/armor/src/armor_finder/anti_top/anti_top.cpp b/armor/src/armor_finder/anti_top/anti_top.cpp index 8a61cd9..c3463f0 100644 --- a/armor/src/armor_finder/anti_top/anti_top.cpp +++ b/armor/src/armor_finder/anti_top/anti_top.cpp @@ -87,7 +87,9 @@ void ArmorFinder::antiTop() { anti_top_state = NORMAL; LOGM(STR_CTR(WORD_YELLOW, "switch to normal")); } - if (getPointLength(last_box.getCenter() - target_box.getCenter()) > last_box.rect.height * 3.0) { +// cout << getPointLength(last_box.getCenter() - target_box.getCenter()) << endl; + if (getPointLength(last_box.getCenter() - target_box.getCenter()) > last_box.rect.height * 1.0) { + LOGM("switch!"); if (150 < interval && interval < 700) { if (anti_top_state == ANTI_TOP) { top_periodms.push(interval); @@ -96,7 +98,6 @@ void ArmorFinder::antiTop() { getsystime(curr_time); auto calculate_time = getTimeIntervalms(curr_time, frame_time); shoot_delay = mean(top_periodms) - calculate_time; - } else { if (++anti_top_cnt > 4) { anti_top_state = ANTI_TOP; @@ -104,11 +105,13 @@ void ArmorFinder::antiTop() { } } } + last_front_time = frame_time; } if (anti_top_state == NORMAL) { sendBoxPosition(0); } else if (interval < top_periodms[-1] * 0.2){ sendBoxPosition(shoot_delay); } + last_box = target_box; } diff --git a/armor/src/armor_finder/searching_state/searching_state.cpp b/armor/src/armor_finder/searching_state/searching_state.cpp index be99312..ac6af41 100644 --- a/armor/src/armor_finder/searching_state/searching_state.cpp +++ b/armor/src/armor_finder/searching_state/searching_state.cpp @@ -18,7 +18,6 @@ bool ArmorFinder::stateSearchingTarget(cv::Mat &src) { target_box = ArmorBox(); return false; } else { - last_box = target_box; anti_switch_cnt = 0; return true; } diff --git a/armor/src/armor_finder/tracking_state/tracking_state.cpp b/armor/src/armor_finder/tracking_state/tracking_state.cpp index 46aaf8b..cd9a9b7 100644 --- a/armor/src/armor_finder/tracking_state/tracking_state.cpp +++ b/armor/src/armor_finder/tracking_state/tracking_state.cpp @@ -62,6 +62,5 @@ bool ArmorFinder::stateTrackingTarget(cv::Mat &src) { target_box.rect = pos; target_box.light_blobs.clear(); } - last_box = target_box; return true; } From 2b83305eb9e24d10dbead20365146813b2c82006 Mon Sep 17 00:00:00 2001 From: xinyang Date: Sat, 3 Aug 2019 23:16:32 +0800 Subject: [PATCH 4/7] fix bug. --- armor/src/armor_finder/anti_top/anti_top.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/armor/src/armor_finder/anti_top/anti_top.cpp b/armor/src/armor_finder/anti_top/anti_top.cpp index c3463f0..985cbed 100644 --- a/armor/src/armor_finder/anti_top/anti_top.cpp +++ b/armor/src/armor_finder/anti_top/anti_top.cpp @@ -88,7 +88,7 @@ void ArmorFinder::antiTop() { 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 * 1.0) { + if (getPointLength(last_box.getCenter() - target_box.getCenter()) > last_box.rect.height * 2.0) { LOGM("switch!"); if (150 < interval && interval < 700) { if (anti_top_state == ANTI_TOP) { @@ -98,6 +98,7 @@ void ArmorFinder::antiTop() { getsystime(curr_time); auto calculate_time = getTimeIntervalms(curr_time, frame_time); shoot_delay = mean(top_periodms) - calculate_time; + sendBoxPosition(shoot_delay); } else { if (++anti_top_cnt > 4) { anti_top_state = ANTI_TOP; @@ -110,7 +111,7 @@ void ArmorFinder::antiTop() { if (anti_top_state == NORMAL) { sendBoxPosition(0); } else if (interval < top_periodms[-1] * 0.2){ - sendBoxPosition(shoot_delay); + sendBoxPosition(0); } last_box = target_box; } From 0b24c6a5afab511cde22f57c294332cc1a4c1de1 Mon Sep 17 00:00:00 2001 From: xinyang Date: Sun, 4 Aug 2019 00:37:41 +0800 Subject: [PATCH 5/7] 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; From bbe05a7db8d12b92250ea5e90435b6721af9485d Mon Sep 17 00:00:00 2001 From: xinyang <895639507@qq.com> Date: Sun, 4 Aug 2019 01:45:55 +0800 Subject: [PATCH 6/7] small change --- armor/src/armor_finder/anti_top/anti_top.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/armor/src/armor_finder/anti_top/anti_top.cpp b/armor/src/armor_finder/anti_top/anti_top.cpp index 5e915a0..677538d 100644 --- a/armor/src/armor_finder/anti_top/anti_top.cpp +++ b/armor/src/armor_finder/anti_top/anti_top.cpp @@ -81,18 +81,16 @@ 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) { + if (anti_top_state == ANTI_TOP && interval > 500) { anti_top_state = NORMAL; LOGM(STR_CTR(WORD_YELLOW, "switch to normal")); } - 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) { + LOGM("switch! %lf", getPointLength(last_box.getCenter() - target_box.getCenter()) / last_box.rect.height); + if (150 < interval && interval < 500) { if (anti_top_state == ANTI_TOP) { top_periodms.push(interval); LOGM(STR_CTR(WORD_LIGHT_GREEN, "top period: %.1lf ms"), interval); @@ -113,7 +111,7 @@ void ArmorFinder::antiTop() { } if (anti_top_state == NORMAL) { sendBoxPosition(0); - } else if (interval < top_periodms[-1] * 0.2) { + } else if (interval < top_periodms[-1] * 0.15) { sendBoxPosition(0); } last_box = target_box; From 4e8adbb6712142717e06b5e6c066a718f14336e9 Mon Sep 17 00:00:00 2001 From: xinyang <895639507@qq.com> Date: Sun, 4 Aug 2019 01:46:37 +0800 Subject: [PATCH 7/7] small change --- armor/src/armor_finder/anti_top/anti_top.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/armor/src/armor_finder/anti_top/anti_top.cpp b/armor/src/armor_finder/anti_top/anti_top.cpp index 677538d..7e13307 100644 --- a/armor/src/armor_finder/anti_top/anti_top.cpp +++ b/armor/src/armor_finder/anti_top/anti_top.cpp @@ -106,7 +106,6 @@ void ArmorFinder::antiTop() { } } } - fps = 0; last_front_time = frame_time; } if (anti_top_state == NORMAL) {