From 4075352a705bb04ffb73d0735ae59573f6274d20 Mon Sep 17 00:00:00 2001 From: wanpiqiu123 <779893878@qq.com> Date: Mon, 22 Jul 2019 11:53:40 +0800 Subject: [PATCH] update priority. --- armor/include/armor_finder/armor_finder.h | 14 +++- armor/include/show_images/show_images.h | 2 +- .../src/armor_finder/armor_box/armor_box.cpp | 57 +++++++++------- armor/src/armor_finder/armor_finder.cpp | 12 ++++ .../src/armor_finder/find/find_armor_box.cpp | 67 +++++-------------- .../armor_finder/send_target/send_target.cpp | 4 +- armor/src/show_images/show_images.cpp | 39 ++++++----- main.cpp | 2 +- 8 files changed, 95 insertions(+), 102 deletions(-) diff --git a/armor/include/armor_finder/armor_finder.h b/armor/include/armor_finder/armor_finder.h index 4d1b6ae..5f4f042 100644 --- a/armor/include/armor_finder/armor_finder.h +++ b/armor/include/armor_finder/armor_finder.h @@ -20,6 +20,9 @@ #define BOX_RED ENEMY_RED #define BOX_BLUE ENEMY_BLUE +#define IMAGE_CENTER_X (320) +#define IMAGE_CENTER_Y (240-20) + #define DISTANCE_HEIGHT_5MM (107.0) // 单位: m*pixel #define DISTANCE_HEIGHT DISTANCE_HEIGHT_5MM @@ -40,6 +43,9 @@ extern std::map id2name; //装甲板id到名称的map extern std::map name2id; //装甲板名称到id的map +extern std::map prior_blue; +extern std::map prior_red; + /******************* 灯条类定义 ***********************/ class LightBlob { @@ -72,11 +78,13 @@ public: explicit ArmorBox(const cv::Rect &pos=cv::Rect2d(), const LightBlobs &blobs=LightBlobs(), uint8_t color=0, int i=0); - double blobsDistance() const; - double lengthRatio() const; + double getBlobsDistance() const; double lengthDistanceRatio() const; - double getDistance() const; + double getBoxDistance() const; BoxOrientation getOrientation() const; +// double + + bool operator<(const ArmorBox &box) const; }; typedef std::vector ArmorBoxes; diff --git a/armor/include/show_images/show_images.h b/armor/include/show_images/show_images.h index 457c1c5..37e8ab1 100644 --- a/armor/include/show_images/show_images.h +++ b/armor/include/show_images/show_images.h @@ -12,7 +12,7 @@ void showArmorBoxes(std::string windows_name, const cv::Mat &src, const ArmorBoxes &armor_boxes); void showArmorBox(std::string windows_name, const cv::Mat &src, const ArmorBox &armor_box); void showLightBlobs(std::string windows_name, const cv::Mat &src, const LightBlobs &light_blobs); -void showArmorBoxesClass(std::string window_names, const cv::Mat &src, const ArmorBoxes boxes[15]); +void showArmorBoxesClass(std::string window_names, const cv::Mat &src, const ArmorBoxes &boxes); void showTrackSearchingPos(std::string window_names, const cv::Mat &src, const cv::Rect2d pos); #endif /* _SHOW_IMAGES_H_ */ diff --git a/armor/src/armor_finder/armor_box/armor_box.cpp b/armor/src/armor_finder/armor_box/armor_box.cpp index d669d61..cfd13ab 100644 --- a/armor/src/armor_finder/armor_box/armor_box.cpp +++ b/armor/src/armor_finder/armor_box/armor_box.cpp @@ -6,48 +6,38 @@ #include ArmorBox::ArmorBox(const cv::Rect &pos, const LightBlobs &blobs, uint8_t color, int i) : -rect(pos), light_blobs(blobs), box_color(color), id(i){}; + rect(pos), light_blobs(blobs), box_color(color), id(i) {}; -double ArmorBox::blobsDistance() const{ - if(light_blobs.size() == 2){ +double ArmorBox::getBlobsDistance() const { + if (light_blobs.size() == 2) { auto &x = light_blobs[0].rect.center; auto &y = light_blobs[1].rect.center; - return sqrt((x.x-y.x)*(x.x-y.x) + (x.y-y.y)*(x.y-y.y)); - }else{ - return 0; - } -} - -double ArmorBox::lengthRatio() const{ - if(light_blobs.size() == 2){ - return light_blobs[0].length / light_blobs[1].length < 1 ? - (light_blobs[0].length / light_blobs[1].length) : - (light_blobs[1].length / light_blobs[0].length); - }else{ + return sqrt((x.x - y.x) * (x.x - y.x) + (x.y - y.y) * (x.y - y.y)); + } else { return 0; } } double ArmorBox::lengthDistanceRatio() const { - if(light_blobs.size() == 2){ + if (light_blobs.size() == 2) { return max(light_blobs[0].length, light_blobs[1].length) - / blobsDistance(); - }else{ + / getBlobsDistance(); + } else { return 100; } } -double ArmorBox::getDistance() const{ - if(light_blobs.size() == 2 ){ +double ArmorBox::getBoxDistance() const { + if (light_blobs.size() == 2) { return DISTANCE_HEIGHT / 2 / max(light_blobs[0].length, light_blobs[1].length); } else { return DISTANCE_HEIGHT / rect.height; } } -ArmorBox::BoxOrientation ArmorBox::getOrientation() const{ +ArmorBox::BoxOrientation ArmorBox::getOrientation() const { // cout << lengthDistanceRatio() << endl; - if(light_blobs.size() != 2){ + if (light_blobs.size() != 2) { return UNKNOWN; } switch (id) { @@ -57,9 +47,9 @@ ArmorBox::BoxOrientation ArmorBox::getOrientation() const{ case B1: case B7: case B8: - if(lengthDistanceRatio() < 0.24){ + if (lengthDistanceRatio() < 0.24) { return FRONT; - }else{ + } else { return SIDE; } case R2: @@ -72,10 +62,27 @@ ArmorBox::BoxOrientation ArmorBox::getOrientation() const{ case B5: if (lengthDistanceRatio() < 0.48) { return FRONT; - }else{ + } else { return SIDE; } default: return UNKNOWN; } } + +bool ArmorBox::operator<(const ArmorBox &box) const { + if (id != box.id) { + if (box_color == BOX_BLUE) { + return prior_blue[id2name[id]] < prior_blue[id2name[box.id]]; + } else { + return prior_red[id2name[id]] < prior_red[id2name[box.id]]; + } + } else { + auto d1 = (rect.x-IMAGE_CENTER_X)*(rect.x-IMAGE_CENTER_X) + + (rect.y-IMAGE_CENTER_Y)*(rect.y-IMAGE_CENTER_Y); + auto d2 = (box.rect.x-IMAGE_CENTER_X)*(box.rect.x-IMAGE_CENTER_X) + + (box.rect.y-IMAGE_CENTER_Y)*(box.rect.y-IMAGE_CENTER_Y); + return d1 < d2; + } +} + diff --git a/armor/src/armor_finder/armor_finder.cpp b/armor/src/armor_finder/armor_finder.cpp index b25efeb..06c23fc 100644 --- a/armor/src/armor_finder/armor_finder.cpp +++ b/armor/src/armor_finder/armor_finder.cpp @@ -35,6 +35,18 @@ std::map name2id = { //装甲板名 {"R1", 8},{"R2", 9},{"R3", 10},{"R4", 11},{"R5", 12},{"R7", 13},{"R8", 14}, }; +std::map prior_blue = { + {"B8", 0}, {"B1", 1}, {"B3", 2}, {"B4", 2}, {"B5", 2}, {"B7", 3}, {"B2", 4}, + {"R8", 5}, {"R1", 6}, {"R3", 7}, {"R4", 7}, {"R5", 7}, {"R7", 8}, {"R2", 9}, + {"NO", 10}, +}; + +std::map prior_red = { + {"R8", 0}, {"R1", 1}, {"R3", 2}, {"R4", 2}, {"R5", 2}, {"R7", 3}, {"R2", 4}, + {"B8", 5}, {"B1", 6}, {"B3", 7}, {"B4", 7}, {"B5", 7}, {"B7", 8}, {"B2", 9}, + {"NO", 10}, +}; + ArmorFinder::ArmorFinder(uint8_t &color, Serial &u, const string ¶s_folder, const uint8_t &use) : serial(u), enemy_color(color), diff --git a/armor/src/armor_finder/find/find_armor_box.cpp b/armor/src/armor_finder/find/find_armor_box.cpp index cb40a83..82802f0 100644 --- a/armor/src/armor_finder/find/find_armor_box.cpp +++ b/armor/src/armor_finder/find/find_armor_box.cpp @@ -8,15 +8,7 @@ #include #include -static string prior_blue[] = { - "B8", "B1", "B3", "B4", "B5", "B7", "B2", - "R8", "R1", "R3", "R4", "R5", "R7", "R2", -}; -static string prior_red[] = { - "R8", "R1", "R3", "R4", "R5", "R7", "R2", - "B8", "B1", "B3", "B4", "B5", "B7", "B2", -}; static bool angelJudge(const LightBlob &light_blob_i, const LightBlob &light_blob_j) { float angle_i = light_blob_i.rect.size.width > light_blob_i.rect.size.height ? light_blob_i.rect.angle : @@ -76,7 +68,7 @@ static bool isCoupleLight(const LightBlob &light_blob_i, const LightBlob &light_ light_blob_j.blob_color == enemy_color && lengthRatioJudge(light_blob_i, light_blob_j) && lengthJudge(light_blob_i, light_blob_j) && -// heightJudge(light_blob_i, light_blob_j) && + // heightJudge(light_blob_i, light_blob_j) && angelJudge(light_blob_i, light_blob_j) && boxAngleJudge(light_blob_i, light_blob_j) && CuoWeiDuJudge(light_blob_i, light_blob_j); @@ -94,7 +86,6 @@ bool matchArmorBoxes(const cv::Mat &src, const LightBlobs &light_blobs, ArmorBox 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)) { -// cout << "match fail" << endl; continue; } cv::Rect2d rect_left = light_blobs.at(static_cast(i)).rect.boundingRect(); @@ -106,7 +97,6 @@ bool matchArmorBoxes(const cv::Mat &src, const LightBlobs &light_blobs, ArmorBox max_y = fmax(rect_left.y + rect_left.height, rect_right.y + rect_right.height) + 0.5 * (rect_left.height + rect_right.height) / 2.0; if (min_x < 0 || max_x > src.cols || min_y < 0 || max_y > src.rows) { -// cout << "out of range" << endl; continue; } LightBlobs pair_blobs = {light_blobs.at(i), light_blobs.at(j)}; @@ -117,27 +107,20 @@ bool matchArmorBoxes(const cv::Mat &src, const LightBlobs &light_blobs, ArmorBox ); } } - if (armor_boxes.empty()) { - return false; - } - sort(armor_boxes.begin(), armor_boxes.end(), [](ArmorBox box1, ArmorBox box2) -> bool { - return centerDistance(box1.rect) < centerDistance(box2.rect); - }); - return true; + return !armor_boxes.empty(); } -bool ArmorFinder::findArmorBox(const cv::Mat &src, ArmorBox &box){ +bool ArmorFinder::findArmorBox(const cv::Mat &src, ArmorBox &box) { LightBlobs light_blobs; // 存储所有可能的灯条 ArmorBoxes armor_boxes; // 装甲板候选区 - ArmorBoxes boxes_number[15]; // 装甲板候选区放置在对应id位置 - box.rect = cv::Rect2d(0,0,0,0); - box.id = -1; + box.rect = cv::Rect2d(0, 0, 0, 0); + box.id = -1; if (!findLightBlobs(src, light_blobs)) { return false; } - if (show_light_blobs && src.size()==cv::Size(640, 480)) { + if (show_light_blobs && src.size() == cv::Size(640, 480)) { showLightBlobs("light_blobs", src, light_blobs); cv::waitKey(1); } @@ -146,7 +129,7 @@ bool ArmorFinder::findArmorBox(const cv::Mat &src, ArmorBox &box){ // cout << "Box fail!" << endl; return false; } - if (show_armor_boxes && src.size()==cv::Size(640, 480)) { + if (show_armor_boxes && src.size() == cv::Size(640, 480)) { showArmorBoxes("boxes", src, armor_boxes); cv::waitKey(1); } @@ -157,40 +140,24 @@ bool ArmorFinder::findArmorBox(const cv::Mat &src, ArmorBox &box){ cv::resize(roi, roi, cv::Size(48, 36)); int c = classifier(roi); armor_box.id = c; - boxes_number[c].emplace_back(armor_box); } - if (enemy_color == ENEMY_BLUE) { - for (auto &name : prior_blue) { - if (!boxes_number[name2id[name]].empty()) { - box = boxes_number[name2id[name]][0]; - break; - } - } - } else if (enemy_color == ENEMY_RED) { - for (auto &name : prior_red) { - if (!boxes_number[name2id[name]].empty()) { - box = boxes_number[name2id[name]][0]; - break; - } - } - } else { - LOGE_INFO("enemy_color ERROR!"); + sort(armor_boxes.begin(), armor_boxes.end()); + if(armor_boxes[0].id != 0){ + box = armor_boxes[0]; } if (save_labelled_boxes) { - for (int i = 0; i < sizeof(boxes_number) / sizeof(boxes_number[0]); i++) { - for (auto &armor_box : boxes_number[i]) { - char filename[100]; - sprintf(filename, PROJECT_DIR"/armor_box_photo/%s_%d.jpg", id2name[i].data(), - time(nullptr) + clock()); - cv::imwrite(filename, src(armor_box.rect)); - } + for (const auto &one_box : armor_boxes) { + char filename[100]; + sprintf(filename, PROJECT_DIR"/armor_box_photo/%s_%d.jpg", id2name[one_box.id].data(), + time(nullptr) + clock()); + cv::imwrite(filename, src(armor_box.rect)); } } if (box.rect == cv::Rect2d(0, 0, 0, 0)) { return false; } - if (show_armor_boxes && src.size()==cv::Size(640, 480)) { - showArmorBoxesClass("class", src, boxes_number); + if (show_armor_boxes && src.size() == cv::Size(640, 480)) { + showArmorBoxesClass("class", src, armor_boxes); } } else { box = armor_boxes[0]; diff --git a/armor/src/armor_finder/send_target/send_target.cpp b/armor/src/armor_finder/send_target/send_target.cpp index 3cd3e1d..e2c8729 100644 --- a/armor/src/armor_finder/send_target/send_target.cpp +++ b/armor/src/armor_finder/send_target/send_target.cpp @@ -47,8 +47,8 @@ bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) { LOGM(STR_CTR(WORD_BLUE, "shoot after %dms"), shoot_delay); } auto rect = armor_box.rect; - double dx = rect.x + rect.width / 2 - 320; - double dy = rect.y + rect.height / 2 - 240 - 20; + double dx = rect.x + rect.width / 2 - IMAGE_CENTER_X; + double dy = rect.y + rect.height / 2 - IMAGE_CENTER_Y; double yaw = atan(dx / FOCUS_PIXAL) * 180 / PI; double pitch = atan(dy / FOCUS_PIXAL) * 180 / PI; double dist = DISTANCE_HEIGHT / rect.height; diff --git a/armor/src/show_images/show_images.cpp b/armor/src/show_images/show_images.cpp index b48e870..8392e15 100644 --- a/armor/src/show_images/show_images.cpp +++ b/armor/src/show_images/show_images.cpp @@ -48,33 +48,32 @@ void showArmorBoxes(std::string windows_name, const cv::Mat &src, const ArmorBox /************************** * 显示多个装甲板区域及其类别 * **************************/ -void showArmorBoxesClass(std::string window_names, const cv::Mat &src, const ArmorBoxes boxes[15]) { +void showArmorBoxesClass(std::string window_names, const cv::Mat &src, const ArmorBoxes &boxes) { static Mat image2show; if (src.type() == CV_8UC1) { // 黑白图像 cvtColor(src, image2show, COLOR_GRAY2RGB); } else if (src.type() == CV_8UC3) { //RGB 彩色 image2show = src.clone(); } - for (int i = 1; i < 15; i++) { - if (!boxes[i].empty()) { - for (auto box : boxes[i]) { - rectangle(image2show, box.rect, Scalar(0, 255, 0), 1); - drawLightBlobs(image2show, box.light_blobs); - if (i == -1) - putText(image2show, id2name[i], Point(box.rect.x + 2, box.rect.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1, - Scalar(0, 255, 0)); - else if (1 <= i && i < 8) - putText(image2show, id2name[i], Point(box.rect.x + 2, box.rect.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1, - Scalar(255, 0, 0)); - else if (8 <= i && i < 15) - putText(image2show, id2name[i], Point(box.rect.x + 2, box.rect.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1, - Scalar(0, 0, 255)); - else if (i != 0) - LOGE_INFO("Invalid box id:%d!", i); - } + for (const auto &box : boxes) { + if(box.id != 0) { + rectangle(image2show, box.rect, Scalar(0, 255, 0), 1); + drawLightBlobs(image2show, box.light_blobs); + if (box.id == -1) + putText(image2show, id2name[box.id], Point(box.rect.x + 2, box.rect.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1, + Scalar(0, 255, 0)); + else if (1 <= box.id && box.id < 8) + putText(image2show, id2name[box.id], Point(box.rect.x + 2, box.rect.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1, + Scalar(255, 0, 0)); + else if (8 <= box.id && box.id < 15) + putText(image2show, id2name[box.id], Point(box.rect.x + 2, box.rect.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1, + Scalar(0, 0, 255)); + else if (box.id != 0) + LOGE_INFO("Invalid box id:%d!", box.id); } } imshow(window_names, image2show); + } /************************** @@ -93,7 +92,7 @@ void showArmorBox(std::string windows_name, const cv::Mat &src, const ArmorBox & drawLightBlobs(image2show, box.light_blobs); // static FILE *fp = fopen(PROJECT_DIR"/ratio.txt", "w"); // if(box.light_blobs.size() == 2) -// fprintf(fp, "%lf %lf %lf\n", box.light_blobs[0].length, box.light_blobs[1].length, box.blobsDistance()) +// fprintf(fp, "%lf %lf %lf\n", box.light_blobs[0].length, box.light_blobs[1].length, box.getBlobsDistance()) // cout << box.lengthDistanceRatio() << endl; if(box.getOrientation() == ArmorBox::FRONT){ @@ -103,7 +102,7 @@ void showArmorBox(std::string windows_name, const cv::Mat &src, const ArmorBox & }; char dist[5]; - sprintf(dist, "%.1f", box.getDistance()); + sprintf(dist, "%.1f", box.getBoxDistance()); if (box.id == -1) putText(image2show, id2name[box.id]+" "+dist, Point(box.rect.x + 2, box.rect.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1, Scalar(0, 255, 0)); diff --git a/main.cpp b/main.cpp index fe1b3f4..82b8de3 100644 --- a/main.cpp +++ b/main.cpp @@ -30,7 +30,7 @@ using namespace std; mcu_data mcuData = { // 单片机端回传结构体 0, // 当前云台yaw角 0, // 当前云台pitch角 - BIG_ENERGY_STATE, // 当前状态，自瞄-大符-小符 + ARMOR_STATE, // 当前状态，自瞄-大符-小符 0, // 云台角度标记位 1, // 是否启用数字识别 ENEMY_RED, // 敌方颜色