Merge branch 'master' of https://github.com/lloi7/RM_auto-aim

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})

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<int, string> id2name;   //装甲板id到名称的map
 extern std::map<string, int> 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<LightBlob> 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;                               // 当前目标位置
@@ -48,21 +66,14 @@ private:
     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<cv::Rect2d> &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_ */

armor/src/armor_finder/armor_finder.cpp

@@ -15,13 +15,13 @@
 /*                                                                           */
 /*===========================================================================*/
 
-//#define LOG_LEVEL LOG_NONE
+#define LOG_LEVEL LOG_NONE
-
 #include <log.h>
 #include <options/options.h>
 #include <show_images/show_images.h>
 #include <opencv2/highgui.hpp>
 #include <armor_finder/armor_finder.h>
+#include <sys/time.h>
 
 std::map<int, string> id2name = {                               //装甲板id到名称的map
         {-1, "OO"},{ 0, "NO"},
@@ -46,9 +46,8 @@ ArmorFinder::ArmorFinder(uint8_t &color, Serial &u, const string &paras_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) {                 // 根据条件显示当前目标装甲板
@@ -71,14 +70,14 @@ void ArmorFinder::run(cv::Mat &src) {
                     tracker->init(src_use, armor_box);
                     state = TRACKING_STATE;
                     tracking_cnt = 0;
-//                    LOGM(STR_CTR(WORD_LIGHT_CYAN, "into track"));
+                    LOGM(STR_CTR(WORD_LIGHT_CYAN, "into track"));
                 }
             }
             break;
         case TRACKING_STATE:
             if (!stateTrackingTarget(src_use) || ++tracking_cnt > 100) {    // 最多追踪100帧图像
                 state = SEARCHING_STATE;
-//                LOGM(STR_CTR(WORD_LIGHT_YELLOW, "into search!"));
+                LOGM(STR_CTR(WORD_LIGHT_YELLOW, "into search!"));
             }
             break;
         case STANDBY_STATE:
@@ -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;

armor/src/armor_finder/searching_state/find_armor_boxes.cpp

@@ -0,0 +1,105 @@
+//
+// Created by xinyang on 19-7-10.
+//
+
+#include <armor_finder/armor_finder.h>
+
+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<cv::Rect2d> &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<unsigned long>(i)).rect.boundingRect();
+            cv::Rect2d rect_right = light_blobs.at(static_cast<unsigned long>(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;
+}

armor/src/armor_finder/searching_state/find_light_blobs.cpp

@@ -0,0 +1,133 @@
+//
+// Created by xinyang on 19-7-10.
+//
+
+#include <armor_finder/armor_finder.h>
+
+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<cv::Point2f> 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<uint8_t>(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<uint8_t>(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<uint8_t>(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<uint8_t>(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<LightBlob> &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<cv::Vec3b>(row, col)[2];
+                blue_cnt += roi.at<cv::Vec3b>(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<std::vector<cv::Point> > 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;
+}

armor/src/armor_finder/searching_state/image_process.cpp

@@ -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<uchar>(i);
-        bayer_data[0] = blue.ptr<uchar>(i / 2);
-        for (int j = 0; j < blue.cols; ++j, data += 2) {
-            bayer_data[0][j] = *data;
-        }
-        data = src.ptr<uchar>(++i) + 1;
-        bayer_data[1] = red.ptr<uchar>(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<cv::Mat> 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);
-}

armor/src/armor_finder/searching_state/image_process.h

@@ -1,14 +0,0 @@
-//
-// Created by xinyang on 19-3-27.
-//
-
-#ifndef _IMAGE_PROCESS_H_
-#define _IMAGE_PROCESS_H_
-
-#include <opencv2/core.hpp>
-#include <armor_finder/armor_finder.h>
-
-void imageColorSplit(cv::Mat &src_input, cv::Mat &split, uint8_t color);
-void imagePreProcess(cv::Mat &src);
-
-#endif /* _IMAGE_PROCESS_H_ */

armor/src/armor_finder/searching_state/searching_state.cpp

@@ -7,242 +7,7 @@
 #include <show_images/show_images.h>
 #include <options/options.h>
 #include <log.h>
-#include "image_process.h"
 
-typedef std::vector<LightBlob> 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<cv::Point2f> 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<uint8_t>(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<uint8_t>(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<uint8_t>(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<uint8_t>(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<std::vector<cv::Point> > 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<cv::Rect2d> &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<unsigned long>(i)).rect.boundingRect();
-            cv::Rect2d rect_right = light_blobs.at(static_cast<unsigned long>(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<LightBlob> &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<cv::Vec3b>(row, col)[2];
-                blue_cnt += roi.at<cv::Vec3b>(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,10 +15,28 @@ 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<cv::Mat> channels;      // 通道拆分
     cv::Mat src_bin, color;             // 二值图和颜色通道图
@@ -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;

armor/src/show_images/show_images.cpp

@@ -53,7 +53,7 @@ void showArmorBoxClass(std::string window_names, const cv::Mat &src, vector<cv::
 }
 
 /**************************
- * 显示多个装甲板区域及其类别 *
+ * 显示单个装甲板区域及其类别 *
  **************************/
 void showArmorBox(std::string windows_name, const cv::Mat &src, cv::Rect2d armor_box, int boxid) {
     static Mat image2show;
@@ -63,14 +63,16 @@ void showArmorBox(std::string windows_name, const cv::Mat &src, cv::Rect2d armor
         image2show = src.clone();
     }
     rectangle(image2show, armor_box, Scalar(0, 255, 0), 1);
+    char dist[5];
+//    sprintf(dist, "%.1f", distance);
     if (boxid == -1)
-        putText(image2show, id2name[boxid], Point(armor_box.x + 2, armor_box.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1,
+        putText(image2show, id2name[boxid]+" "+dist, Point(armor_box.x + 2, armor_box.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1,
                 Scalar(0, 255, 0));
     else if (1 <= boxid && boxid < 8)
-        putText(image2show, id2name[boxid], Point(armor_box.x + 2, armor_box.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1,
+        putText(image2show, id2name[boxid]+" "+dist, Point(armor_box.x + 2, armor_box.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1,
                 Scalar(255, 0, 0));
     else if (8 <= boxid && boxid < 15)
-        putText(image2show, id2name[boxid], Point(armor_box.x + 2, armor_box.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1,
+        putText(image2show, id2name[boxid]+" "+dist, Point(armor_box.x + 2, armor_box.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1,
                 Scalar(0, 0, 255));
     else if (boxid != 0)
         LOGE_INFO("Invalid box id:%d!", boxid);

main.cpp

@@ -27,7 +27,7 @@ using namespace std;
 mcu_data mcuData = {    // 单片机端回传结构体
         0,              // 当前云台yaw角
         0,              // 当前云台pitch角
-        BIG_ENERGY_STATE,    // 当前状态，自瞄-大符-小符
+        ARMOR_STATE,    // 当前状态，自瞄-大符-小符
         0,              // 云台角度标记位
         1,              // 是否启用数字识别
         ENEMY_RED,      // 敌方颜色
@@ -94,28 +94,37 @@ int main(int argc, char *argv[]) {
         do {
             CNT_TIME("Total", {
                 if (mcuData.state == BIG_ENERGY_STATE) {//大符模式
+                    if (last_state != BIG_ENERGY_STATE) {//若上一帧不是大符模式，即刚往完成切换，则需要初始化
+                        energy.setEnergyRotationInit();
+                        ((CameraWrapper*)video_gimble)->changeBrightness(20);
+                        cout << "set" << endl;
+                    }
                     ok = checkReconnect(video_gimble->read(gimble_src), video_chassis->read(chassis_src));//检查有几个摄像头
                     if (save_video) saveVideos(gimble_src, chassis_src);//保存视频
                     if (show_origin) showOrigin(gimble_src, chassis_src);//显示原始图像
-                    if (last_state != BIG_ENERGY_STATE) {//若上一帧不是大符模式，即刚往完成切换，则需要初始化
-                        energy.setEnergyRotationInit();
-                        cout << "set" << endl;
-                    }
                     energy.runBig(gimble_src, chassis_src);//击打大符
 //                    energy.runBig(gimble_src);
                     last_state = mcuData.state;//更新上一帧状态
-                } else if (mcuData.state != BIG_ENERGY_STATE) {//自瞄或小符模式
+                } else if (mcuData.state == SMALL_ENERGY_STATE) {// 小符模式
+                    if(last_state == ARMOR_STATE){
+                        ((CameraWrapper*)video_gimble)->changeBrightness(20);
+                    }
+                    last_state = mcuData.state;
+                    ok = checkReconnect(video_gimble->read(gimble_src));
+                    if (save_video) saveVideos(gimble_src);
+                    if (show_origin) showOrigin(gimble_src);
+                    energy.runSmall(gimble_src);
+                } else {                                         // 自瞄模式
+                    if(last_state != ARMOR_STATE){
+                        ((CameraWrapper*)video_gimble)->changeBrightness(40);
+                    }
                     last_state = mcuData.state;
                     ok = checkReconnect(video_gimble->read(gimble_src));
                     if (save_video) saveVideos(gimble_src);
                     if (show_origin) showOrigin(gimble_src);
-                    if (mcuData.state == ARMOR_STATE) {
                     CNT_TIME("Armor Time", {
                             armorFinder.run(gimble_src);
                     });
-                    } else if (mcuData.state == SMALL_ENERGY_STATE) {
-                        energy.runSmall(gimble_src);
-                    }
                 }
 //                cv::waitKey(0);
             });

others/include/camera/camera_wrapper.h

@@ -47,8 +47,7 @@ public:
     bool read(cv::Mat& src) final;
     bool readRaw(cv::Mat& src);
     bool readProcessed(cv::Mat& src);
-
+    bool changeBrightness(int brightness);
-
 };
 
 

others/src/camera/camera_wrapper.cpp

@@ -109,6 +109,9 @@ bool CameraWrapper::init() {
     return true;
 }
 
+bool CameraWrapper::changeBrightness(int brightness) {
+    CameraSetAnalogGain(h_camera, brightness);
+}
 
 bool CameraWrapper::read(cv::Mat& src) {
     if(mode==0)return readProcessed(src);