energy changed

energy/include/energy/energy.h

@@ -65,6 +65,7 @@ private:
 
     double radius;//大风车半径
 
+    int curr_fps;//帧率
     int send_cnt;//向主控板发送的数据总次数
     int camera_cnt;//摄像头数量
     int fans_cnt;//扇叶个数
@@ -131,6 +132,7 @@ private:
     int findArmors(const cv::Mat src);//寻找图中所有装甲板
     bool findCenterR(const cv::Mat src);//寻找图中可能的风车中心字母R
     bool findFlowStrip(const cv::Mat src);//寻找图中的流动条
+    bool findFlowStripSub(const cv::Mat src);//寻找图中的流动条
     bool findCenterROI(const cv::Mat src);//框取中心R候选区
     bool findFlowStripFan(const cv::Mat src);//寻找图中的流动条所在扇叶
     bool findFlowStripWeak(const cv::Mat src);//弱识别寻找图中的流动条
@@ -191,6 +193,7 @@ private:
     void ArmorStruct(cv::Mat &src);//腐蚀和膨胀
     void FlowStripFanStruct(cv::Mat &src);//腐蚀和膨胀
     void FlowStripStruct(cv::Mat &src);//腐蚀和膨胀
+    void FlowStripStructSub(cv::Mat &src);//腐蚀和膨胀
     void CenterRStruct(cv::Mat &src);//腐蚀和膨胀
 };
 

energy/include/energy/param_struct_define.h

@@ -20,8 +20,6 @@ struct EnergyPartParam {
 	int RED_GRAY_THRESH;//红方二值化阈值
 	int BLUE_GRAY_THRESH;//蓝方二值化阈值
 	int SPLIT_GRAY_THRESH;//通道分离二值化阈值
-	int FAN_GRAY_THRESH;//扇叶识别二值化阈值
-	int ARMOR_GRAY_THRESH;//装甲板识别二值化阈值
 
 	long FAN_CONTOUR_AREA_MAX;//扇叶面积最大值
 	long FAN_CONTOUR_AREA_MIN;//扇叶面积最小值

energy/src/energy/calibrate/structing.cpp

@@ -99,26 +99,51 @@ void Energy::FlowStripFanStruct(cv::Mat &src) {
 // ---------------------------------------------------------------------------------------------------------------------
 void Energy::FlowStripStruct(cv::Mat &src) {
     Mat element_dilate_1 = getStructuringElement(MORPH_RECT, Size(5, 5));
-    Mat element_erode_1 = getStructuringElement(MORPH_RECT, Size(5, 5));
+    Mat element_erode_1 = getStructuringElement(MORPH_RECT, Size(2, 2));
-    Mat element_dilate_2 = getStructuringElement(MORPH_RECT, Size(2, 2));
+    Mat element_dilate_2 = getStructuringElement(MORPH_RECT, Size(3, 3));
     Mat element_erode_2 = getStructuringElement(MORPH_RECT, Size(2 , 2));
+    Mat element_dilate_3 = getStructuringElement(MORPH_RECT, Size(3, 3));
+    Mat element_erode_3 = getStructuringElement(MORPH_RECT, Size(1 , 1));
+
+    dilate(src, src, element_dilate_1);
+//    imshow("dilate_1", src);
+    erode(src,src, element_erode_1);
+//    imshow("erode_1", src);
+    erode(src,src, element_erode_2);
+//    imshow("erode_2", src);
+    erode(src,src, element_erode_3);
+//    imshow("erode_3", src);
+//    dilate(src, src, element_dilate_2);
+//    imshow("dilate_2", src);
+//    dilate(src, src, element_dilate_3);
+//    imshow("dilate_3", src);
+}
+
+
+//----------------------------------------------------------------------------------------------------------------------
+// 此函数对图像进行腐蚀与膨胀操作
+// ---------------------------------------------------------------------------------------------------------------------
+void Energy::FlowStripStructSub(cv::Mat &src) {
+    Mat element_dilate_1 = getStructuringElement(MORPH_RECT, Size(5, 5));
+    Mat element_erode_1 = getStructuringElement(MORPH_RECT, Size(4, 4));
+    Mat element_dilate_2 = getStructuringElement(MORPH_RECT, Size(3, 3));
+    Mat element_erode_2 = getStructuringElement(MORPH_RECT, Size(3 , 3));
     Mat element_dilate_3 = getStructuringElement(MORPH_RECT, Size(2, 2));
     Mat element_erode_3 = getStructuringElement(MORPH_RECT, Size(2 , 2));
     Mat element_dilate_4 = getStructuringElement(MORPH_RECT, Size(2, 2));
-    Mat element_erode_4 = getStructuringElement(MORPH_RECT, Size(1 , 1));
 
-//    dilate(src, src, element_dilate_1);
-//    imshow("dilate_1", src);
     erode(src,src, element_erode_1);
-    imshow("erode_1", src);
+//    imshow("sub_erode_1", src);
 //    erode(src,src, element_erode_2);
 //    imshow("erode_2", src);
-//    erode(src,src, element_erode_3);
+    erode(src,src, element_erode_3);
-//    imshow("erode_3", src);
+//    imshow("sub_erode_3", src);
+//    erode(src,src, element_erode_4);
+//    imshow("erode_4", src);
     dilate(src, src, element_dilate_1);
-//    imshow("dilate_1", src);
+//    imshow("sub_dilate_1", src);
     dilate(src, src, element_dilate_2);
-    imshow("dilate_2", src);
+//    imshow("sub_dilate_2", src);
 //    dilate(src, src, element_dilate_3);
 //    imshow("dilate_3", src);
 //    dilate(src, src, element_dilate_4);
@@ -126,7 +151,6 @@ void Energy::FlowStripStruct(cv::Mat &src) {
 }
 
 
-
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数对图像进行腐蚀与膨胀操作
 // ---------------------------------------------------------------------------------------------------------------------

energy/src/energy/clear/clear.cpp

@@ -28,12 +28,16 @@ void Energy::clearAll() {
 void Energy::initImage(cv::Mat &src) {
 //    imagePreprocess(src);
 //    if(show_process)imshow("img_preprocess", src);
-    if (src.type() == CV_8UC3)cvtColor(src, src, COLOR_BGR2GRAY);
+    if (src.type() == CV_8UC3){
+        cvtColor(src, src, COLOR_BGR2GRAY);
+    }
     if (mcu_data.enemy_color == ENEMY_BLUE){
         threshold(src, src, energy_part_param_.RED_GRAY_THRESH, 255, THRESH_BINARY);
     } else if(mcu_data.enemy_color == ENEMY_RED){
         threshold(src, src, energy_part_param_.BLUE_GRAY_THRESH, 255, THRESH_BINARY);
     }
-    if (show_process)imshow("bin", src);
+    if (show_process){
+        imshow("bin", src);
+    }
     if (show_energy || show_process)waitKey(1);
 }

energy/src/energy/clear/energy_init.cpp

@@ -29,6 +29,7 @@ void Energy::initEnergy() {
 
     radius = 0;
 
+    curr_fps = 0;
     send_cnt = 0;
     camera_cnt = 1;
     fans_cnt = 0;
@@ -83,11 +84,9 @@ void Energy::initEnergy() {
 void Energy::initEnergyPartParam() {
 //    gimbal_energy_part_param_.GRAY_THRESH = 120;//home
 //    gimbal_energy_part_param_.GRAY_THRESH = 200;//official
-    gimbal_energy_part_param_.RED_GRAY_THRESH = 150;//game
+    gimbal_energy_part_param_.RED_GRAY_THRESH = 180;//game
     gimbal_energy_part_param_.BLUE_GRAY_THRESH = 100;//game
     gimbal_energy_part_param_.SPLIT_GRAY_THRESH = 180;
-    gimbal_energy_part_param_.FAN_GRAY_THRESH = 75;
-    gimbal_energy_part_param_.ARMOR_GRAY_THRESH = 80;
 
     gimbal_energy_part_param_.FAN_CONTOUR_AREA_MAX = 5000;
     gimbal_energy_part_param_.FAN_CONTOUR_AREA_MIN = 1500;
@@ -143,11 +142,9 @@ void Energy::initEnergyPartParam() {
     gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_LENGTH_MIN = 32;
     gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_LENGTH_MAX = 55;
     gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MIN = 4;
-//    gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MAX = 20;
+    gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MAX = 20;
-    gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MAX = 40;
     gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MAX = 7;
-//    gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN = 3;
+    gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN = 3;
-    gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN = 1;
     gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_AREA_RATIO_MIN = 0.6;
     gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_INTERSETION_AREA_MIN = 100;
 
@@ -165,8 +162,6 @@ void Energy::initEnergyPartParam() {
 //    chassis_energy_part_param_.GRAY_THRESH = 200;//official
 //    chassis_energy_part_param_.GRAY_THRESH = 225;
     chassis_energy_part_param_.SPLIT_GRAY_THRESH = 230;
-    chassis_energy_part_param_.FAN_GRAY_THRESH = 75;
-    chassis_energy_part_param_.ARMOR_GRAY_THRESH = 80;
 
     chassis_energy_part_param_.FAN_CONTOUR_AREA_MAX = 17000;
     chassis_energy_part_param_.FAN_CONTOUR_AREA_MIN = 0;

energy/src/energy/find/energy_finder.cpp

@@ -315,6 +315,105 @@ bool Energy::findFlowStrip(const cv::Mat src) {
 }
 
 
+//----------------------------------------------------------------------------------------------------------------------
+// 此函数用于寻找流动条
+// ---------------------------------------------------------------------------------------------------------------------
+bool Energy::findFlowStripSub(const cv::Mat src) {
+    if (src.empty()) {
+        if (show_info) cout << "empty!" << endl;
+        return false;
+    }
+    cv::Mat src_bin;
+    src_bin = src.clone();
+
+    if (src_bin.type() == CV_8UC1) // 黑白图像
+    {
+        cvtColor(src_bin, src_bin, COLOR_GRAY2RGB);
+
+    }
+    std::vector<cv::RotatedRect> candidate_target_armors = target_armors;
+    for (auto &candidate_target_armor: candidate_target_armors) {
+        Point2f vertices[4];
+        candidate_target_armor.size.height *= 1.3;
+        candidate_target_armor.size.width *= 1.3;
+        candidate_target_armor.points(vertices);   //计算矩形的4个顶点
+        for (int i = 0; i < 4; i++) {
+            line(src_bin, vertices[i], vertices[(i + 1) % 4], Scalar(0, 0, 0), 20);
+        }
+    }
+
+    cvtColor(src_bin, src_bin, CV_BGR2GRAY);//若读取三通道视频文件，需转换为单通道
+
+    FlowStripStructSub(src_bin);//图像膨胀，防止图像断开并更方便寻找
+    if (show_process)imshow("flow strip struct", src_bin);
+
+    std::vector<vector<Point> > flow_strip_contours;
+    findContours(src_bin, flow_strip_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
+
+    for (auto candidate_flow_strip_fan: flow_strip_fans) {
+        for (auto &flow_strip_contour : flow_strip_contours) {
+            if (!isValidFlowStripContour(flow_strip_contour)) {
+                continue;
+            }
+
+            std::vector<cv::Point2f> intersection;
+            RotatedRect cur_rect = minAreaRect(flow_strip_contour);
+
+            if (rotatedRectangleIntersection(cur_rect, candidate_flow_strip_fan, intersection) == 0) {
+                continue;
+            } else if (contourArea(intersection) > energy_part_param_.FLOW_STRIP_CONTOUR_INTERSETION_AREA_MIN) {
+                flow_strips.emplace_back(cv::minAreaRect(flow_strip_contour));
+//                cout << "intersection: " << contourArea(intersection) << '\t' << cur_rect.center << endl;
+            } else {
+                continue;
+            }
+
+//            Size2f cur_size = cur_rect.size;
+//            float length = cur_size.height > cur_size.width ? cur_size.height : cur_size.width;
+//            float width = cur_size.height < cur_size.width ? cur_size.height : cur_size.width;
+//            double cur_contour_area = contourArea(flow_strip_contour);
+//            float length_width_ratio = length / width;
+//            cout << "area: " << cur_contour_area << '\t' << endl;
+//            cout << "length: " << length << '\t' << "width: " << width << '\t' << cur_rect.center << endl;
+//            cout << "HW: " << length_width_ratio << '\t' << cur_rect.center << endl;
+//            cout << "area ratio: " << cur_contour_area / cur_size.area() << '\t' << cur_rect.center << endl;
+//            cout<<endl;
+        }
+    }
+    if (flow_strips.empty()) {
+        if (show_info)cout << "flow strip false!" << endl;
+//        waitKey(0);
+        return false;
+    } else if (flow_strips.size() > 1) {
+        if (show_info)cout << "Too many flow strips!" << endl;
+//        waitKey(0);
+        return false;
+    } else {
+        flow_strip = flow_strips.at(0);
+        for (auto &candidate_flow_strip_fan: flow_strip_fans) {
+            std::vector<cv::Point2f> intersection;
+            if (rotatedRectangleIntersection(flow_strip, candidate_flow_strip_fan, intersection) == 0) {
+                continue;
+            } else if (contourArea(intersection) > energy_part_param_.FLOW_STRIP_CONTOUR_INTERSETION_AREA_MIN) {
+                flow_strip_fan = candidate_flow_strip_fan;
+            }
+        }
+        int i = 0;
+        for (i = 0; i < target_armors.size(); ++i) {
+            std::vector<cv::Point2f> intersection;
+            if (rotatedRectangleIntersection(target_armors.at(i), flow_strip_fan, intersection) == 0)
+                continue;//返回0表示没有重合面积
+            double cur_contour_area = contourArea(intersection);
+            if (cur_contour_area > energy_part_param_.TARGET_INTERSETION_CONTOUR_AREA_MIN) {
+                target_armor = target_armors.at(i);
+                target_point = target_armor.center;
+            }
+        }
+    }
+    return true;
+}
+
+
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于弱识别寻找流动条
 // ---------------------------------------------------------------------------------------------------------------------

energy/src/energy/mark/mark.cpp

@@ -7,12 +7,14 @@
 using namespace std;
 using namespace cv;
 
+extern McuData mcu_data;
+
 
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于记录操作手的微调dx和dy
 // ---------------------------------------------------------------------------------------------------------------------
 void Energy::writeDownSlightChange(cv::Mat &src) {
-    if (fans_cnt >= 4) {
+    if (fans_cnt >= 2) {
         FILE *fp_delta = fopen(PROJECT_DIR"/Mark/delta.txt", "w");
         if (fp_delta) {
             fprintf(fp_delta, "delta_x: %d, delta_y: %d\n", mcu_data.delta_x + manual_delta_x,
@@ -20,6 +22,27 @@ void Energy::writeDownSlightChange(cv::Mat &src) {
             fclose(fp_delta);
         }
     }
+    if(fans_cnt>=1){
+        FILE *fp_data = fopen(PROJECT_DIR"/Mark/data.txt", "a");
+        if (fp_data) {
+            if(mcu_data.mark == 1){
+                fprintf(fp_data, "PID: %s\t", "new");
+            } else {
+                fprintf(fp_data, "PID: %s\t", "default");
+            }
+            if(is_big){
+                fprintf(fp_data, "state: %s\t", "big");
+            } else if(is_small){
+                fprintf(fp_data, "state: %s\t", "small");
+            }
+            fprintf(fp_data, "fps: %d\t", curr_fps);
+            fprintf(fp_data, "fans_cnt: %d\t", fans_cnt);
+            fprintf(fp_data, "yaw: %lf , pitch: %lf\t", yaw_rotation, pitch_rotation);
+            fprintf(fp_data, "delta_x: %d, delta_y: %d\n", mcu_data.delta_x + manual_delta_x,
+                    mcu_data.delta_y + manual_delta_y);
+            fclose(fp_data);
+        }
+    }
 }
 
 

energy/src/energy/run.cpp

@@ -83,7 +83,11 @@ void Energy::runBig(cv::Mat &gimbal_src) {
     } else {
         if (show_energy)showFlowStripFan("strip fan", gimbal_src);
         if (!findTargetInFlowStripFan()) return;
-        if (!findFlowStrip(gimbal_src))return;
+            if(!findFlowStrip(gimbal_src)){
+                if(!findFlowStripSub(gimbal_src)) {
+                    return;
+                }
+            }
     }
     if (!findCenterROI(gimbal_src))return;
     if (show_energy)showFlowStrip("strip", gimbal_src);

energy/src/energy/send/send.cpp

@@ -112,6 +112,7 @@ void Energy::sendTarget(Serial &serial, float x, float y, float z) {
     if (last_time != t) {
         last_time = t;
         cout << "Energy: fps:" << fps << ", (" << x << "," << y << "," << z << ")" << endl;
+        curr_fps = fps;
         fps = 0;
     }
     fps += 1;
@@ -148,6 +149,7 @@ void Energy::sendTarget(Serial &serial, float x, float y, float z, uint16_t u) {
     if (last_time != t) {
         last_time = t;
         cout << "Energy: fps:" << fps << ", (" << x << "," << y << "," << z << "," << u << ")" << endl;
+        curr_fps = fps;
         fps = 0;
     }
     fps += 1;

main.cpp

@@ -32,7 +32,7 @@ McuData mcu_data = {    // 单片机端回传结构体
         0,              // 当前云台pitch角
         ARMOR_STATE,    // 当前状态，自瞄-大符-小符
         0,              // 云台角度标记位
-        0,              // 是否为反陀螺模式
+        1,              // 是否启用数字识别
         ENEMY_RED,      // 敌方颜色
         0,              // 能量机关x轴补偿量
         0,              // 能量机关y轴补偿量
@@ -66,8 +66,8 @@ int main(int argc, char *argv[]) {
             video_gimbal = new CameraWrapper(ARMOR_CAMERA_EXPOSURE, ARMOR_CAMERA_GAIN, 2/*, "armor"*/);
             video_chassis = new CameraWrapper(ENERGY_CAMERA_EXPOSURE, ENERGY_CAMERA_GAIN, 2/*, "energy"*/);
         } else {
-            video_gimbal = new VideoWrapper(PROJECT_DIR"/8-7-NO7.avi");
+            video_gimbal = new VideoWrapper(PROJECT_DIR"/test_video/blue_big.avi");
-            video_chassis = new VideoWrapper(PROJECT_DIR"/8-7-NO7.avi");
+            video_chassis = new VideoWrapper(PROJECT_DIR"/test_video/blue_big.avi");
         }
         if (video_gimbal->init()) {
             LOGM("video_gimbal source initialization successfully.");