From 4582e997f154c04c74f077929f1670824988ac97 Mon Sep 17 00:00:00 2001 From: xinyang <895639507@qq.com> Date: Wed, 10 Jul 2019 19:25:54 +0800 Subject: [PATCH] =?UTF-8?q?=E6=95=B4=E7=90=86=E4=BB=A3=E7=A0=81=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 2 +- armor/include/armor_finder/armor_finder.h | 37 ++- armor/src/armor_finder/armor_finder.cpp | 6 +- .../searching_state/find_armor_boxes.cpp | 105 +++++++ .../searching_state/find_light_blobs.cpp | 133 +++++++++ .../searching_state/image_process.cpp | 65 ----- .../searching_state/image_process.h | 14 - .../searching_state/searching_state.cpp | 272 ++---------------- armor/src/show_images/show_images.cpp | 10 +- 9 files changed, 295 insertions(+), 349 deletions(-) create mode 100644 armor/src/armor_finder/searching_state/find_armor_boxes.cpp create mode 100644 armor/src/armor_finder/searching_state/find_light_blobs.cpp delete mode 100644 armor/src/armor_finder/searching_state/image_process.cpp delete mode 100644 armor/src/armor_finder/searching_state/image_process.h diff --git a/CMakeLists.txt b/CMakeLists.txt index efd23a1..c16f6c7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -27,7 +27,7 @@ INCLUDE_DIRECTORIES(${PROJECT_SOURCE_DIR}/armor/include) INCLUDE_DIRECTORIES(${PROJECT_SOURCE_DIR}/others/include) FILE(GLOB_RECURSE sourcefiles "others/src/*.cpp" "energy/src/*cpp" "armor/src/*.cpp") -ADD_EXECUTABLE(${BIN_NAME} main.cpp ${sourcefiles} others/include/constants.h) +ADD_EXECUTABLE(${BIN_NAME} main.cpp ${sourcefiles} others/include/constants.h armor/src/armor_finder/searching_state/find_light_blobs.cpp armor/src/armor_finder/searching_state/find_armor_boxes.cpp) TARGET_LINK_LIBRARIES(${BIN_NAME} ${CMAKE_THREAD_LIBS_INIT}) TARGET_LINK_LIBRARIES(${BIN_NAME} ${OpenCV_LIBS}) diff --git a/armor/include/armor_finder/armor_finder.h b/armor/include/armor_finder/armor_finder.h index 3e23945..b5d31af 100644 --- a/armor/include/armor_finder/armor_finder.h +++ b/armor/include/armor_finder/armor_finder.h @@ -16,10 +16,27 @@ #define BLOB_RED ENEMY_RED #define BLOB_BLUE ENEMY_BLUE +#define DISTANCE_HEIGHT_5MM (113.0) // 单位: m*pixel +#define DISTANCE_HEIGHT DISTANCE_HEIGHT_5MM + extern std::map id2name; //装甲板id到名称的map extern std::map name2id; //装甲板名称到id的map +/******************* 灯条类定义 ***********************/ +class LightBlob { +public: + cv::RotatedRect rect; //灯条位置 + double length; //灯条长度 + uint8_t BlobColor; //灯条颜色 + + LightBlob(cv::RotatedRect &r) : rect(r) { + length = max(rect.size.height, rect.size.width); + }; +}; + +typedef std::vector LightBlobs; + /********************* 自瞄类定义 **********************/ class ArmorFinder{ public: @@ -33,6 +50,7 @@ private: SEARCHING_STATE, TRACKING_STATE, STANDBY_STATE } State; // 自瞄状态枚举定义 + cv::Mat src_raw; // 当前原图 const uint8_t &enemy_color; // 敌方颜色，引用外部变量，自动变化 State state; // 自瞄状态对象实例 cv::Rect2d armor_box; // 当前目标位置 @@ -41,28 +59,21 @@ private: Classifier classifier; // CNN分类器对象实例，用于数字识别 int contour_area; // 装甲区域亮点个数，用于数字识别未启用时判断是否跟丢（已弃用） int tracking_cnt; // 记录追踪帧数，用于定时退出追踪 - Serial &serial; // 串口对象，引用外部变量，用于和能量机关共享同一个变量 + Serial &serial; // 串口对象，引用外部变量，用于和能量机关共享同一个变量 const uint8_t &use_classifier; // 标记是否启用CNN分类器，引用外部变量，自动变化 bool stateSearchingTarget(cv::Mat &src); // searching state主函数 bool stateTrackingTarget(cv::Mat &src); // tracking state主函数 bool stateStandBy(); // stand by state主函数（已弃用） + + bool findLightBlobs(const cv::Mat &src_bin, // 在二值图上寻找灯条 + LightBlobs &light_blobs); + bool findArmorBoxes(const LightBlobs &light_blobs, // 根据灯条匹配装甲板候选区 + std::vector &armor_boxes); public: void run(cv::Mat &src); // 自瞄主函数 bool sendBoxPosition(); // 和主控板通讯 }; -/******************* 灯条类定义 ***********************/ -class LightBlob { -public: - cv::RotatedRect rect; //灯条位置 - double length; //灯条长度 - uint8_t BlobColor; //灯条颜色 - - LightBlob(cv::RotatedRect &r) : rect(r) { - length = max(rect.size.height, rect.size.width); - }; -}; - #endif /* _ARMOR_FINDER_H_ */ diff --git a/armor/src/armor_finder/armor_finder.cpp b/armor/src/armor_finder/armor_finder.cpp index 7f5bbc6..02beea2 100644 --- a/armor/src/armor_finder/armor_finder.cpp +++ b/armor/src/armor_finder/armor_finder.cpp @@ -46,9 +46,8 @@ ArmorFinder::ArmorFinder(uint8_t &color, Serial &u, const string ¶s_folder, tracking_cnt(0) { } -extern int box_distance; - void ArmorFinder::run(cv::Mat &src) { + src_raw = src; cv::Mat src_use = src.clone(); // 实际参与计算的图像对象 if (show_armor_box) { // 根据条件显示当前目标装甲板 @@ -117,9 +116,6 @@ bool sendTarget(Serial &serial, double x, double y, double z) { return serial.WriteData(buff, sizeof(buff)); } -#define DISTANCE_HEIGHT_5MM (113.0) // 单位: m*pixel -#define DISTANCE_HEIGHT DISTANCE_HEIGHT_5MM - bool ArmorFinder::sendBoxPosition() { auto rect = armor_box; double dx = rect.x + rect.width / 2 - 320; diff --git a/armor/src/armor_finder/searching_state/find_armor_boxes.cpp b/armor/src/armor_finder/searching_state/find_armor_boxes.cpp new file mode 100644 index 0000000..be92840 --- /dev/null +++ b/armor/src/armor_finder/searching_state/find_armor_boxes.cpp @@ -0,0 +1,105 @@ +// +// Created by xinyang on 19-7-10. +// + +#include + +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 : + light_blob_i.rect.angle - 90; + float angle_j = light_blob_j.rect.size.width > light_blob_j.rect.size.height ? light_blob_j.rect.angle : + light_blob_j.rect.angle - 90; + return abs(angle_i - angle_j) < 10; +} + +static bool heightJudge(const LightBlob &light_blob_i, const LightBlob &light_blob_j) { + cv::Point2f centers = light_blob_i.rect.center - light_blob_j.rect.center; + return abs(centers.y) < 30; +} + +static bool lengthJudge(const LightBlob &light_blob_i, const LightBlob &light_blob_j) { + double side_length; + cv::Point2f centers = light_blob_i.rect.center - light_blob_j.rect.center; + side_length = sqrt(centers.ddot(centers)); + return (side_length / light_blob_i.length < 6 && side_length / light_blob_i.length > 0.5); +} + +static bool lengthRatioJudge(const LightBlob &light_blob_i, const LightBlob &light_blob_j) { + return (light_blob_i.length / light_blob_j.length < 2 + && light_blob_i.length / light_blob_j.length > 0.5); +} + +/* 判断两个灯条的错位度，不知道英文是什么！！！ */ +static bool CuoWeiDuJudge(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 : + light_blob_i.rect.angle - 90; + float angle_j = light_blob_j.rect.size.width > light_blob_j.rect.size.height ? light_blob_j.rect.angle : + light_blob_j.rect.angle - 90; + float angle = (angle_i + angle_j) / 2.0 / 180.0 * 3.14159265459; + if (abs(angle_i - angle_j) > 90) { + angle += 3.14159265459 / 2; + } + Vector2f orientation(cos(angle), sin(angle)); + Vector2f p2p(light_blob_j.rect.center.x - light_blob_i.rect.center.x, + light_blob_j.rect.center.y - light_blob_i.rect.center.y); + return abs(orientation.dot(p2p)) < 20; +} + +static bool boxAngleJudge(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 : + light_blob_i.rect.angle - 90; + float angle_j = light_blob_j.rect.size.width > light_blob_j.rect.size.height ? light_blob_j.rect.angle : + light_blob_j.rect.angle - 90; + float angle = (angle_i + angle_j) / 2.0; + if (abs(angle_i - angle_j) > 90) { + angle += 90.0; + } + return (-120.0 < angle && angle < -60.0) || (60.0 < angle && angle < 120.0); +} + +static bool isCoupleLight(const LightBlob &light_blob_i, const LightBlob &light_blob_j, uint8_t enemy_color) { + return light_blob_i.BlobColor == enemy_color && + light_blob_j.BlobColor == enemy_color && + lengthRatioJudge(light_blob_i, light_blob_j) && + lengthJudge(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); + +} + +static double centerDistance(const cv::Rect2d &box) { + double dx = box.x - box.width / 2 - 320; + double dy = box.y - box.height / 2 - 240; + return dx * dx + dy * dy; +} + +bool ArmorFinder::findArmorBoxes(const LightBlobs &light_blobs, std::vector &armor_boxes) { + 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), enemy_color)) { + continue; + } + cv::Rect2d rect_left = light_blobs.at(static_cast(i)).rect.boundingRect(); + cv::Rect2d rect_right = light_blobs.at(static_cast(j)).rect.boundingRect(); + double min_x, min_y, max_x, max_y; + min_x = fmin(rect_left.x, rect_right.x) - 4; + max_x = fmax(rect_left.x + rect_left.width, rect_right.x + rect_right.width) + 4; + min_y = fmin(rect_left.y, rect_right.y) - 0.5 * (rect_left.height + rect_right.height) / 2.0; + 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 > 640 || min_y < 0 || max_y > 480) { + continue; + } + armor_boxes.emplace_back(cv::Rect2d(min_x, min_y, max_x - min_x, max_y - min_y)); + } + } + if (armor_boxes.empty()) { + return false; + } + sort(armor_boxes.begin(), armor_boxes.end(), [](cv::Rect2d box1, cv::Rect2d box2) -> bool { + return centerDistance(box1) < centerDistance(box2); + }); + return true; +} \ No newline at end of file diff --git a/armor/src/armor_finder/searching_state/find_light_blobs.cpp b/armor/src/armor_finder/searching_state/find_light_blobs.cpp new file mode 100644 index 0000000..1f2d798 --- /dev/null +++ b/armor/src/armor_finder/searching_state/find_light_blobs.cpp @@ -0,0 +1,133 @@ +// +// Created by xinyang on 19-7-10. +// + +#include + +static double lw_rate(const cv::RotatedRect &rect) { + return rect.size.height > rect.size.width ? + rect.size.height / rect.size.width : + rect.size.width / rect.size.height; +} + +bool rectangleContainPoint(cv::RotatedRect rectangle, cv::Point2f point) { + //转化为轮廓 + cv::Point2f corners[4]; + rectangle.points(corners); + cv::Point2f *lastItemPointer = (corners + sizeof corners / sizeof corners[0]); + vector contour(corners, lastItemPointer); + //判断 + double indicator = pointPolygonTest(contour, point, true); + return indicator >= 0; +} + +/// Todo: 下面的函数可以有性能优化，暂时未做。 +static double nonZeroRateOfRotateRect(const cv::Mat &bin, const cv::RotatedRect &rotrect) { + auto rect = rotrect.boundingRect(); + if (rect.x < 0 || rect.y < 0 || rect.x + rect.width > bin.cols || rect.y + rect.height > bin.rows) { + return 0; + } + auto roi = bin(rect); + int cnt = 0; + for (int r = 0; r < roi.rows; r++) { + for (int c = 0; c < roi.cols; c++) { + if (rectangleContainPoint(rotrect, cv::Point(c + rect.x, r + rect.y))) { + if (roi.at(r, c)) { + cnt++; + } + } + } + } + return double(cnt) / rotrect.size.area(); +} + +int linePointX(const cv::Point2f &p1, const cv::Point2f &p2, int y) { + return (p2.x - p1.x) / (p2.y - p1.y) * (y - p1.y) + p1.x; +} + +///Todo: 性能优化后的函数。(暂时还有点问题) +static double nonZeroRateOfRotateRect_opt(const cv::Mat &bin, const cv::RotatedRect &rotrect) { + int cnt = 0; + cv::Point2f corners[4]; + rotrect.points(corners); + sort(corners, &corners[4], [](cv::Point2f p1, cv::Point2f p2) { + return p1.y < p2.y; + }); + for (int r = corners[0].y; r < corners[1].y; r++) { + auto start = min(linePointX(corners[0], corners[1], r), linePointX(corners[0], corners[2], r)) - 1; + auto end = max(linePointX(corners[0], corners[1], r), linePointX(corners[0], corners[2], r)) + 1; + if (start < 0 || end > 640) return 0; + for (int c = start; c < end; c++) { + if (bin.at(c, r)) { + cnt++; + } + } + } + for (int r = corners[1].y; r < corners[2].y; r++) { + auto start = min(linePointX(corners[0], corners[2], r), linePointX(corners[1], corners[3], r)) - 1; + auto end = max(linePointX(corners[0], corners[2], r), linePointX(corners[1], corners[3], r)) + 1; + if (start < 0 || end > 640) return 0; + for (int c = start; c < end; c++) { + if (bin.at(r, c)) { + cnt++; + } + } + } + for (int r = corners[2].y; r < corners[3].y; r++) { + auto start = min(linePointX(corners[1], corners[3], r), linePointX(corners[2], corners[3], r)) - 1; + auto end = max(linePointX(corners[1], corners[3], r), linePointX(corners[2], corners[3], r)) + 1; + if (start < 0 || end > 640) return 0; + for (int c = start; c < end; c++) { + if (bin.at(c, r)) { + cnt++; + } + } + } + return double(cnt) / rotrect.size.area(); +} + +static bool isValidLightBlob(const cv::Mat &bin, const cv::RotatedRect &rect) { + return (lw_rate(rect) > 1.5) && + // (rect.size.width*rect.size.height < 3000) && + (rect.size.width * rect.size.height > 1) && + // (nonZeroRateOfRotateRect_opt(bin, rect) > 0.8); + (nonZeroRateOfRotateRect(bin, rect) > 0.8); +} + +void get_blob_color(const cv::Mat &src, std::vector &blobs) { + for (auto &blob : blobs) { + auto region = blob.rect.boundingRect(); + region.x -= fmax(2, region.width * 0.1); + region.y -= fmax(2, region.height * 0.05); + region.width += 2 * fmax(2, region.width * 0.1); + region.height += 2 * fmax(2, region.height * 0.05); + region &= cv::Rect(0, 0, 640, 480); + cv::Mat roi = src(region); + long long int red_cnt = 0, blue_cnt = 0; + for (int row = 0; row < roi.rows; row++) { + for (int col = 0; col < roi.cols; col++) { + red_cnt += roi.at(row, col)[2]; + blue_cnt += roi.at(row, col)[0]; + } + } + if (red_cnt > blue_cnt) { + blob.BlobColor = BLOB_RED; + } else { + blob.BlobColor = BLOB_BLUE; + } + } +} + +bool ArmorFinder::findLightBlobs(const cv::Mat &src_bin, LightBlobs &light_blobs) { + std::vector > light_contours; + cv::findContours(src_bin, light_contours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE); + + for (auto &light_contour : light_contours) { + cv::RotatedRect rect = cv::minAreaRect(light_contour); + if (isValidLightBlob(src_bin, rect)) { + light_blobs.emplace_back(rect); + } + } + get_blob_color(src_raw, light_blobs); + return light_blobs.size() >= 2; +} \ No newline at end of file diff --git a/armor/src/armor_finder/searching_state/image_process.cpp b/armor/src/armor_finder/searching_state/image_process.cpp deleted file mode 100644 index e43bdec..0000000 --- a/armor/src/armor_finder/searching_state/image_process.cpp +++ /dev/null @@ -1,65 +0,0 @@ -// -// Created by xinyang on 19-3-27. -// - -#include "image_process.h" - -static void splitBayerBG(cv::Mat &src, cv::Mat &blue, cv::Mat &red) { - uchar* data; - uchar* bayer_data[2]; - for (int i = 0; i < src.rows; ++i) { - data = src.ptr(i); - bayer_data[0] = blue.ptr(i / 2); - for (int j = 0; j < blue.cols; ++j, data += 2) { - bayer_data[0][j] = *data; - } - data = src.ptr(++i) + 1; - bayer_data[1] = red.ptr(i / 2); - for (int j = 0; j < red.cols; ++j, data += 2) { - bayer_data[1][j] = *data; - } - } -} - -void imageColorSplit(cv::Mat &src_input, cv::Mat &split, uint8_t color) { - cv::Mat blue(240, 320, CV_8UC1), red(240, 320, CV_8UC1); - if(src_input.type() == CV_8UC1){ - splitBayerBG(src_input, blue, red); - if(color == ENEMY_RED){ - split = red - blue; - }else if(color == ENEMY_BLUE){ - split = blue - red; - } - }else if(src_input.type() == CV_8UC3){ - std::vector channels; - cv::split(src_input, channels); - resize(channels.at(0), blue, cv::Size(640, 480)); - resize(channels.at(2), red, cv::Size(640, 480)); - if(color == ENEMY_RED){ - split = red;// - blue; - }else if(color == ENEMY_BLUE){ - split = blue;// - red; - } - } - -} - -void imagePreProcess(cv::Mat &src) { -// cv::medianBlur(src, src, 5); - - static cv::Mat kernel_erode = getStructuringElement(cv::MORPH_RECT, cv::Size(3, 5)); - erode(src, src, kernel_erode); - - static cv::Mat kernel_dilate = getStructuringElement(cv::MORPH_RECT, cv::Size(3, 5)); - dilate(src, src, kernel_dilate); - - static cv::Mat kernel_dilate2 = getStructuringElement(cv::MORPH_RECT, cv::Size(3, 5)); - dilate(src, src, kernel_dilate2); - - static cv::Mat kernel_erode2 = getStructuringElement(cv::MORPH_RECT, cv::Size(3, 5)); - erode(src, src, kernel_erode2); - - float alpha = 1.5; - int beta = 0; - src.convertTo(src, -1, alpha, beta); -} diff --git a/armor/src/armor_finder/searching_state/image_process.h b/armor/src/armor_finder/searching_state/image_process.h deleted file mode 100644 index 61dbbb8..0000000 --- a/armor/src/armor_finder/searching_state/image_process.h +++ /dev/null @@ -1,14 +0,0 @@ -// -// Created by xinyang on 19-3-27. -// - -#ifndef _IMAGE_PROCESS_H_ -#define _IMAGE_PROCESS_H_ - -#include -#include - -void imageColorSplit(cv::Mat &src_input, cv::Mat &split, uint8_t color); -void imagePreProcess(cv::Mat &src); - -#endif /* _IMAGE_PROCESS_H_ */ diff --git a/armor/src/armor_finder/searching_state/searching_state.cpp b/armor/src/armor_finder/searching_state/searching_state.cpp index 72dee06..96b5c2a 100644 --- a/armor/src/armor_finder/searching_state/searching_state.cpp +++ b/armor/src/armor_finder/searching_state/searching_state.cpp @@ -7,242 +7,7 @@ #include #include #include -#include "image_process.h" -typedef std::vector LightBlobs; - -static double lw_rate(const cv::RotatedRect &rect) { - return rect.size.height > rect.size.width ? - rect.size.height / rect.size.width : - rect.size.width / rect.size.height; -} - -bool rectangleContainPoint(cv::RotatedRect rectangle, cv::Point2f point) { - //转化为轮廓 - cv::Point2f corners[4]; - rectangle.points(corners); - cv::Point2f *lastItemPointer = (corners + sizeof corners / sizeof corners[0]); - vector contour(corners, lastItemPointer); - //判断 - double indicator = pointPolygonTest(contour, point, true); - return indicator >= 0; -} - -/// Todo: 下面的函数可以有性能优化，暂时未做。 -static double nonZeroRateOfRotateRect(const cv::Mat &bin, const cv::RotatedRect &rotrect) { - auto rect = rotrect.boundingRect(); - if (rect.x < 0 || rect.y < 0 || rect.x + rect.width > bin.cols || rect.y + rect.height > bin.rows) { - return 0; - } - auto roi = bin(rect); - int cnt = 0; - for (int r = 0; r < roi.rows; r++) { - for (int c = 0; c < roi.cols; c++) { - if (rectangleContainPoint(rotrect, cv::Point(c + rect.x, r + rect.y))) { - if (roi.at(r, c)) { - cnt++; - } - } - } - } - return double(cnt) / rotrect.size.area(); -} - -int linePointX(const cv::Point2f &p1, const cv::Point2f &p2, int y) { - return (p2.x - p1.x) / (p2.y - p1.y) * (y - p1.y) + p1.x; -} - -///Todo: 性能优化后的函数。(暂时还有点问题) -static double nonZeroRateOfRotateRect_opt(const cv::Mat &bin, const cv::RotatedRect &rotrect) { - int cnt = 0; - cv::Point2f corners[4]; - rotrect.points(corners); - sort(corners, &corners[4], [](cv::Point2f p1, cv::Point2f p2) { - return p1.y < p2.y; - }); - for (int r = corners[0].y; r < corners[1].y; r++) { - auto start = min(linePointX(corners[0], corners[1], r), linePointX(corners[0], corners[2], r)) - 1; - auto end = max(linePointX(corners[0], corners[1], r), linePointX(corners[0], corners[2], r)) + 1; - if (start < 0 || end > 640) return 0; - for (int c = start; c < end; c++) { - if (bin.at(c, r)) { - cnt++; - } - } - } - for (int r = corners[1].y; r < corners[2].y; r++) { - auto start = min(linePointX(corners[0], corners[2], r), linePointX(corners[1], corners[3], r)) - 1; - auto end = max(linePointX(corners[0], corners[2], r), linePointX(corners[1], corners[3], r)) + 1; - if (start < 0 || end > 640) return 0; - for (int c = start; c < end; c++) { - if (bin.at(r, c)) { - cnt++; - } - } - } - for (int r = corners[2].y; r < corners[3].y; r++) { - auto start = min(linePointX(corners[1], corners[3], r), linePointX(corners[2], corners[3], r)) - 1; - auto end = max(linePointX(corners[1], corners[3], r), linePointX(corners[2], corners[3], r)) + 1; - if (start < 0 || end > 640) return 0; - for (int c = start; c < end; c++) { - if (bin.at(c, r)) { - cnt++; - } - } - } - return double(cnt) / rotrect.size.area(); -} - -static bool isValidLightBlob(const cv::Mat &bin, const cv::RotatedRect &rect) { - return (lw_rate(rect) > 1.5) && - // (rect.size.width*rect.size.height < 3000) && - (rect.size.width * rect.size.height > 1) && - // (nonZeroRateOfRotateRect_opt(bin, rect) > 0.8); - (nonZeroRateOfRotateRect(bin, rect) > 0.8); -} - -static bool findLightBlobs(const cv::Mat &src, LightBlobs &light_blobs) { - static cv::Mat src_gray; - if (src.type() == CV_8UC3) { - cvtColor(src, src_gray, CV_BGR2GRAY); - } else if (src.type() == CV_8UC1) { - src_gray = src.clone(); - } - std::vector > light_contours; - cv::findContours(src_gray, light_contours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE); - - for (auto &light_contour : light_contours) { - cv::RotatedRect rect = cv::minAreaRect(light_contour); - if (isValidLightBlob(src_gray, rect)) { - light_blobs.emplace_back(rect); - } - } - return light_blobs.size() >= 2; -} - -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 : - light_blob_i.rect.angle - 90; - float angle_j = light_blob_j.rect.size.width > light_blob_j.rect.size.height ? light_blob_j.rect.angle : - light_blob_j.rect.angle - 90; - return abs(angle_i - angle_j) < 10; -} - -bool heightJudge(const LightBlob &light_blob_i, const LightBlob &light_blob_j) { - cv::Point2f centers = light_blob_i.rect.center - light_blob_j.rect.center; - return abs(centers.y) < 30; -} - -bool lengthJudge(const LightBlob &light_blob_i, const LightBlob &light_blob_j) { - double side_length; - cv::Point2f centers = light_blob_i.rect.center - light_blob_j.rect.center; - side_length = sqrt(centers.ddot(centers)); - return (side_length / light_blob_i.length < 6 && side_length / light_blob_i.length > 0.5); -} - -bool lengthRatioJudge(const LightBlob &light_blob_i, const LightBlob &light_blob_j) { - return (light_blob_i.length / light_blob_j.length < 2 - && light_blob_i.length / light_blob_j.length > 0.5); -} - -/* 判断两个灯条的错位度，不知道英文是什么！！！ */ -bool CuoWeiDuJudge(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 : - light_blob_i.rect.angle - 90; - float angle_j = light_blob_j.rect.size.width > light_blob_j.rect.size.height ? light_blob_j.rect.angle : - light_blob_j.rect.angle - 90; - float angle = (angle_i + angle_j) / 2.0 / 180.0 * 3.14159265459; - if (abs(angle_i - angle_j) > 90) { - angle += 3.14159265459 / 2; - } - Vector2f orientation(cos(angle), sin(angle)); - Vector2f p2p(light_blob_j.rect.center.x - light_blob_i.rect.center.x, - light_blob_j.rect.center.y - light_blob_i.rect.center.y); - return abs(orientation.dot(p2p)) < 20; -} - -bool boxAngleJudge(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 : - light_blob_i.rect.angle - 90; - float angle_j = light_blob_j.rect.size.width > light_blob_j.rect.size.height ? light_blob_j.rect.angle : - light_blob_j.rect.angle - 90; - float angle = (angle_i + angle_j) / 2.0; - if (abs(angle_i - angle_j) > 90) { - angle += 90.0; - } - return (-120.0 < angle && angle < -60.0) || (60.0 < angle && angle < 120.0); -} - -bool isCoupleLight(const LightBlob &light_blob_i, const LightBlob &light_blob_j, uint8_t enemy_color) { - return light_blob_i.BlobColor == enemy_color && - light_blob_j.BlobColor == enemy_color && - lengthRatioJudge(light_blob_i, light_blob_j) && - lengthJudge(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); - -} - -double centerDistance(const cv::Rect2d &box) { - double dx = box.x - box.width / 2 - 320; - double dy = box.y - box.height / 2 - 240; - return dx * dx + dy * dy; -} - -static bool findArmorBoxes(LightBlobs &light_blobs, std::vector &armor_boxes, uint8_t enemy_color) { - 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), enemy_color)) { - continue; - } - cv::Rect2d rect_left = light_blobs.at(static_cast(i)).rect.boundingRect(); - cv::Rect2d rect_right = light_blobs.at(static_cast(j)).rect.boundingRect(); - double min_x, min_y, max_x, max_y; - min_x = fmin(rect_left.x, rect_right.x) - 4; - max_x = fmax(rect_left.x + rect_left.width, rect_right.x + rect_right.width) + 4; - min_y = fmin(rect_left.y, rect_right.y) - 0.5 * (rect_left.height + rect_right.height) / 2.0; - 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 > 640 || min_y < 0 || max_y > 480) { - continue; - } - armor_boxes.emplace_back(cv::Rect2d(min_x, min_y, max_x - min_x, max_y - min_y)); - } - } - if (armor_boxes.empty()) { - return false; - } - sort(armor_boxes.begin(), armor_boxes.end(), [](cv::Rect2d box1, cv::Rect2d box2) -> bool { - return centerDistance(box1) < centerDistance(box2); - }); - return true; -} - -void get_blob_color(const cv::Mat &src, std::vector &blobs) { - for (auto &blob : blobs) { - auto region = blob.rect.boundingRect(); - region.x -= fmax(2, region.width * 0.1); - region.y -= fmax(2, region.height * 0.05); - region.width += 2 * fmax(2, region.width * 0.1); - region.height += 2 * fmax(2, region.height * 0.05); - region &= cv::Rect(0, 0, 640, 480); - cv::Mat roi = src(region); - long long int red_cnt = 0, blue_cnt = 0; - for (int row = 0; row < roi.rows; row++) { - for (int col = 0; col < roi.cols; col++) { - red_cnt += roi.at(row, col)[2]; - blue_cnt += roi.at(row, col)[0]; - } - } - if (red_cnt > blue_cnt) { - blob.BlobColor = BLOB_RED; - } else { - blob.BlobColor = BLOB_BLUE; - } - } -} static string prior_blue[] = { "B8", "B1", "B3", "B4", "B5", "B7", "B2", @@ -250,18 +15,36 @@ static string prior_blue[] = { }; static string prior_red[] = { - "B8", "B1", "B3", "B4", "B5", "B7", "B2", "R8", "R1", "R3", "R4", "R5", "R7", "R2", + "B8", "B1", "B3", "B4", "B5", "B7", "B2", }; +void imagePreProcess(cv::Mat &src) { + static cv::Mat kernel_erode = getStructuringElement(cv::MORPH_RECT, cv::Size(3, 5)); + erode(src, src, kernel_erode); + + static cv::Mat kernel_dilate = getStructuringElement(cv::MORPH_RECT, cv::Size(3, 5)); + dilate(src, src, kernel_dilate); + + static cv::Mat kernel_dilate2 = getStructuringElement(cv::MORPH_RECT, cv::Size(3, 5)); + dilate(src, src, kernel_dilate2); + + static cv::Mat kernel_erode2 = getStructuringElement(cv::MORPH_RECT, cv::Size(3, 5)); + erode(src, src, kernel_erode2); + + float alpha = 1.5; + int beta = 0; + src.convertTo(src, -1, alpha, beta); +} + bool ArmorFinder::stateSearchingTarget(cv::Mat &src) { std::vector channels; // 通道拆分 - cv::Mat src_bin, color; // 二值图和颜色通道图 - LightBlobs light_blobs; // 存储所有可能的灯条 + cv::Mat src_bin, color; // 二值图和颜色通道图 + LightBlobs light_blobs; // 存储所有可能的灯条 std::vector armor_boxes; // 装甲板候选区 std::vector boxes_number[15]; // 装甲板候选区放置在对应id位置 - armor_box = cv::Rect2d(0, 0, 0, 0); // 重置目标装甲板位置 + armor_box = cv::Rect2d(0, 0, 0, 0); // 重置目标装甲板位置 boxid = -1; // 重置目标装甲板id cv::split(src, channels); /************************/ @@ -274,24 +57,19 @@ bool ArmorFinder::stateSearchingTarget(cv::Mat &src) { if (!findLightBlobs(src_bin, light_blobs)) { return false; } - if (show_light_blobs) { - showContours("blobs_gray", src_bin, light_blobs); - cv::waitKey(1); - } - - get_blob_color(src, light_blobs); if (show_light_blobs) { showContours("light_blobs", src, light_blobs); cv::waitKey(1); } - if (!findArmorBoxes(light_blobs, armor_boxes, enemy_color)) { + if (!findArmorBoxes(light_blobs, armor_boxes)) { return false; } if (show_armor_boxes) { showArmorBoxVector("boxes", src, armor_boxes); cv::waitKey(1); } + if (classifier && use_classifier) { for (auto box : armor_boxes) { cv::Mat roi = src(box).clone(); @@ -316,7 +94,7 @@ bool ArmorFinder::stateSearchingTarget(cv::Mat &src) { } } } else { - LOGE("enemy_color ERROR!"); + LOGE_INFO("enemy_color ERROR!"); } if (armor_box == cv::Rect2d(0, 0, 0, 0)) { return false; diff --git a/armor/src/show_images/show_images.cpp b/armor/src/show_images/show_images.cpp index 37f1a46..44593ab 100644 --- a/armor/src/show_images/show_images.cpp +++ b/armor/src/show_images/show_images.cpp @@ -53,7 +53,7 @@ void showArmorBoxClass(std::string window_names, const cv::Mat &src, vector