Merge branch 'master' of /home/sjturm/Desktop/RM_auto-aim with conflicts.

armor/src/armor_finder/anti_top/anti_top.cpp

@@ -0,0 +1,68 @@
+//
+// Created by xinyang on 19-7-15.
+//
+
+#include <armor_finder/armor_finder.h>
+#include <log.h>
+
+static double getTimeIntervalms(const timeval& now, const timeval &last){
+    return (now.tv_sec-last.tv_sec)*1000.0 + (now.tv_usec-last.tv_usec)/1000.0;
+}
+
+void ArmorFinder::antiTop() {
+    static double top_periodms = 0;
+    static double last1_top_periodms = 0;
+    static double last2_top_periodms = 0;
+    uint16_t shoot_delay = 0;
+    timeval curr_time;
+//    if(anti_top_state == ANTI_TOP){
+//        cout << "anti top" << endl;
+//    }else if(anti_top_state == NORMAL){
+//        cout << "Normal" << endl;
+//    }
+    gettimeofday(&curr_time, nullptr);
+    auto interval = getTimeIntervalms(curr_time, last_front_time);
+    if(interval > 700){
+        anti_top_state = NORMAL;
+        anti_top_cnt = 0;
+    }
+    
+    ArmorBox::BoxOrientation orientation = armor_box.getOrientation();
+    if(orientation == ArmorBox::UNKNOWN){
+        if(anti_top_state == NORMAL){
+            sendBoxPosition(shoot_delay);
+        }
+        return;
+    }
+
+
+    if(orientation!=last_orient && orientation==ArmorBox::FRONT){
+        last_front_time = curr_time;
+        if(150<interval && interval<700){
+            if(anti_top_state == ANTI_TOP){
+                last2_top_periodms = last1_top_periodms;
+                last1_top_periodms = top_periodms;
+                top_periodms = interval;
+                LOGM(STR_CTR(WORD_LIGHT_GREEN, "top period: %.1lf ms"), top_periodms);
+                shoot_delay = (last1_top_periodms+last1_top_periodms+top_periodms)/3.0-105;
+                last_orient = orientation;
+            }else if(anti_top_state == NORMAL){
+//                LOGM("interval:%.1lf", interval);
+                if(++anti_top_cnt > 4){
+                    anti_top_state = ANTI_TOP;
+                }
+            }
+        }
+    }
+
+    if(anti_top_state == ANTI_TOP){
+        if(orientation == ArmorBox::FRONT){
+            sendBoxPosition(shoot_delay);
+        }
+    }else if(anti_top_state == NORMAL){
+        sendBoxPosition(shoot_delay);
+    }
+
+    last_orient = orientation;
+}
+

armor/src/armor_finder/find/find_light_blobs.cpp

@@ -0,0 +1,157 @@
+//
+// Created by xinyang on 19-7-10.
+//
+
+#include <armor_finder/armor_finder.h>
+#include <options/options.h>
+#include <opencv2/highgui.hpp>
+
+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;
+}
+
+
+static double areaRatio(const std::vector<cv::Point> &contour, const cv::RotatedRect &rect){
+    return cv::contourArea(contour) / rect.size.area();
+}
+
+static bool isValidLightBlob(const std::vector<cv::Point> &contour, const cv::RotatedRect &rect) {
+    return (1.5 < lw_rate(rect) && lw_rate(rect) < 10) &&
+//           (rect.size.area() < 3000) &&
+            ((rect.size.area() < 50 && areaRatio(contour, rect) > 0.4) ||
+            (rect.size.area() >= 50 && areaRatio(contour, rect) > 0.6));
+}
+
+// 此函数可以有性能优化.
+static uint8_t get_blob_color(const cv::Mat &src, const cv::RotatedRect &blobPos) {
+    auto region = blobPos.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, src.cols, src.rows);
+    cv::Mat roi = src(region);
+    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) {
+        return BLOB_RED;
+    } else {
+        return BLOB_BLUE;
+    }
+}
+
+static float 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 get_blob_color_opt(const cv::Mat &src, cv::RotatedRect blobPos) {
+    int blue_cnt=0, red_cnt=0;
+    blobPos.size.height *= 1.05;
+    blobPos.size.width *= 1.1;
+    cv::Point2f corners[4];
+    blobPos.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++) {
+            red_cnt += src.at<cv::Vec3b>(r, c)[2];
+            blue_cnt += src.at<cv::Vec3b>(r, c)[0];
+        }
+    }
+    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++) {
+            red_cnt += src.at<cv::Vec3b>(r, c)[2];
+            blue_cnt += src.at<cv::Vec3b>(r, c)[0];
+        }
+    }
+    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++) {
+            red_cnt += src.at<cv::Vec3b>(r, c)[2];
+            blue_cnt += src.at<cv::Vec3b>(r, c)[0];
+        }
+    }
+    if (red_cnt > blue_cnt) {
+        return BLOB_RED;
+    } else {
+        return BLOB_BLUE;
+    }
+}
+
+static 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::findLightBlobs(const cv::Mat &src, LightBlobs &light_blobs) {
+    cv::Mat color_channel;
+    cv::Mat src_bin_light, src_bin_dim;
+    std::vector<cv::Mat> channels;       // 通道拆分
+
+    cv::split(src, channels);               /************************/
+    if (enemy_color == ENEMY_BLUE){         /*                      */
+        color_channel = channels[0];        /* 根据目标颜色进行通道提取 */
+    }else if (enemy_color == ENEMY_RED){    /*                      */
+        color_channel = channels[2];        /************************/
+    }
+
+
+
+    cv::threshold(color_channel, src_bin_light, 200, 255, CV_THRESH_BINARY); // 二值化对应通道
+    imagePreProcess(src_bin_light);                                  // 开闭运算
+
+    cv::threshold(color_channel, src_bin_dim, 160, 255, CV_THRESH_BINARY); // 二值化对应通道
+    imagePreProcess(src_bin_dim);                                  // 开闭运算
+
+    if(src_bin_light.size() == cv::Size(640, 480) && show_light_blobs) {
+        imshow("bin_light", src_bin_light);
+        imshow("bin_dim", src_bin_dim);
+    }
+
+    std::vector<std::vector<cv::Point> > light_contours_light, light_contours_dim;
+    cv::findContours(src_bin_light, light_contours_light, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);
+    cv::findContours(src_bin_dim, light_contours_dim, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);
+    for (auto &light_contour : light_contours_light) {
+        cv::RotatedRect rect = cv::minAreaRect(light_contour);
+        if (isValidLightBlob(light_contour, rect)) {
+            light_blobs.emplace_back(rect, get_blob_color(src, rect));
+        }
+    }
+    for (auto &light_contour : light_contours_dim) {
+        cv::RotatedRect rect = cv::minAreaRect(light_contour);
+        if (isValidLightBlob(light_contour, rect)) {
+            light_blobs.emplace_back(rect, get_blob_color(src, rect));
+        }
+    }
+    return light_blobs.size() >= 2;
+}

main.cpp

@@ -30,10 +30,10 @@ using namespace std;
 mcu_data mcuData = {    // 单片机端回传结构体
         0,              // 当前云台yaw角
         0,              // 当前云台pitch角
-        BIG_ENERGY_STATE,    // 当前状态，自瞄-大符-小符
+        ARMOR_STATE,    // 当前状态，自瞄-大符-小符
         0,              // 云台角度标记位
         1,              // 是否启用数字识别
-        ENEMY_RED,      // 敌方颜色
+        ENEMY_BLUE,      // 敌方颜色
 };
 
 WrapperHead *video_gimbal = nullptr;    // 云台摄像头视频源
@@ -64,10 +64,8 @@ int main(int argc, char *argv[]) {
             video_gimbal = new CameraWrapper(0/*, "armor"*/);
             video_chassis = new CameraWrapper(1/*, "energy"*/);
         } else {
-//            video_gimbal = new VideoWrapper("/home/sjturm/Desktop/videos/147.avi");
+            video_gimbal = new VideoWrapper("/home/sun/项目/energy_video/gimble3.avi");
-//            video_chassis = new VideoWrapper("/home/sjturm/Desktop/videos/147.avi");
+            video_chassis = new VideoWrapper("/home/sun/项目/energy_video/gimble3.avi");
-            video_gimbal = new VideoWrapper("/home/sjturm/Desktop/dafu/gimble3.avi");
-            video_chassis = new VideoWrapper("/home/sjturm/Desktop/dafu/chassis3.avi");
         }
         if (video_gimbal->init()) {
             LOGM("video_gimbal source initialization successfully.");
@@ -93,19 +91,18 @@ int main(int argc, char *argv[]) {
         bool ok = true;
         cout << "start running" << endl;
         do {
-            CNT_TIME("Total", {
+//            CNT_TIME("Total", {
-                if (mcuData.state != ARMOR_STATE) {//能量机关模式
+                if (mcuData.state == BIG_ENERGY_STATE) {//大能量机关模式
-                    if (last_state == ARMOR_STATE) {//若上一帧是自瞄模式，即刚往完成切换，则需要初始化
+                    if (last_state != BIG_ENERGY_STATE) {//若上一帧不是大能量机关模式，即刚往完成切换，则需要初始化
                         destroyAllWindows();
                         ((CameraWrapper *) video_gimbal)->changeBrightness(ENERGY_CAMERA_GAIN);
-                        energy.setEnergyInit();
+                        energy.setBigEnergyInit();
                         checkReconnect(video_chassis->read(chassis_src));
 #ifdef CHASSIS_FLIP_MODE
                         flip(chassis_src, chassis_src, CHASSIS_FLIP_MODE);
 #endif
                     }
                     ok = checkReconnect(video_gimbal->read(gimbal_src));
-                    video_gimbal->read(gimbal_src);
                     video_chassis->read(chassis_src);
 #ifdef GIMBAL_FLIP_MODE
                     flip(gimbal_src, gimbal_src, GIMBAL_FLIP_MODE);
@@ -113,8 +110,22 @@ int main(int argc, char *argv[]) {
                     if (!from_camera) extract(gimbal_src, chassis_src);
                     if (save_video) saveVideos(gimbal_src, chassis_src);//保存视频
                     if (show_origin) showOrigin(gimbal_src, chassis_src);//显示原始图像
-                    energy.run(gimbal_src, chassis_src);
+                    energy.runBig(gimbal_src, chassis_src);
-//                    energy.run(gimbal_src);
+                    last_state = mcuData.state;//更新上一帧状态
+                } else if (mcuData.state == SMALL_ENERGY_STATE) {
+                    if (mcuData.state != SMALL_ENERGY_STATE) {
+                        destroyAllWindows();
+                        ((CameraWrapper *) video_gimbal)->changeBrightness(ENERGY_CAMERA_GAIN);
+                        energy.setSmallEnergyInit();
+                    }
+                    ok = checkReconnect(video_gimbal->read(gimbal_src));
+#ifdef GIMBAL_FLIP_MODE
+                    flip(gimbal_src, gimbal_src, GIMBAL_FLIP_MODE);
+#endif
+                    if (!from_camera) extract(gimbal_src);
+                    if (save_video) saveVideos(gimbal_src);//保存视频
+                    if (show_origin) showOrigin(gimbal_src);//显示原始图像
+                    energy.runSmall(gimbal_src);
                     last_state = mcuData.state;//更新上一帧状态
                 } else {                                         // 自瞄模式
                     if (last_state != ARMOR_STATE) {
@@ -133,8 +144,8 @@ int main(int argc, char *argv[]) {
                             armorFinder.run(gimbal_src);
                     });
                 }
-                cv::waitKey(1);
+//                cv::waitKey(1);
-            });
+//            });
         } while (ok);
         delete video_gimbal;
         video_gimbal = nullptr;