energy changed

energy/include/energy/energy.h

@@ -26,7 +26,6 @@ public:
 	~Energy();//默认析构函数
 	int runBig(cv::Mat &gimble_src, cv::Mat &chassis_src);
     int runBig(cv::Mat &gimble_src);
-    int runSmall(cv::Mat &gimble_src, cv::Mat &chassis_src);
     int runSmall(cv::Mat &gimble_src);
 	Serial &serial;//串口
 	void setEnergyRotationInit();//判断顺逆时针函数
@@ -69,6 +68,7 @@ private:
 
 	cv::Point circle_center_point;//风车圆心坐标
 	cv::Point target_point;//目标装甲板中心坐标
+	cv::Point last_target_point;//上一帧目标装甲板中心坐标
 	cv::Point predict_point;//预测的击打点坐标
 	cv::Point former_point;//之前预测的圆心坐标
 	std::vector<float>fan_polar_angle;//当前帧所有扇叶的极坐标角度
@@ -99,7 +99,8 @@ private:
 
     void circleLeastFit();//利用所有记录的装甲板中心最小二乘法计算圆心和半径
 
-	void findTarget();//获取目标装甲板的极坐标角度和装甲板中心坐标
+	void findTargetByPolar();//通过极坐标角度匹配获取目标装甲板的极坐标角度和装甲板中心坐标
+    void findTargetByIntersection();//通过面积重合度匹配获取目标装甲板的极坐标角度和装甲板中心坐标
 
     void rotate();//获取预测点位
 	void stretch(cv::Point point_1, cv::Point2f &point_2);//将像素差转换为实际距离差
@@ -108,6 +109,7 @@ private:
 	void writeDownMark();//记录操作手标定的云台初始角度
 
     void getPredictPoint();//获取预测点位
+    void getAimPoint();//通过自瞄逻辑计算点位
 	bool changeTarget();//判断目标是否改变
 	void changeMark();//操作手手动修改标定值
     void gimbleRotation();//计算云台旋转角度

energy/include/energy/param_struct_define.h

@@ -65,6 +65,9 @@ struct EnergyPartParam {
     float CENTER_R_CONTOUR_HW_RATIO_MIN;//风车中心R长宽比最小值
 
 	float TWIN_ANGEL_MAX;//扇叶和装甲板匹配时极坐标角度差的最大值
+	long INTERSETION_CONTOUR_AREA_MIN;//扇叶与装甲板匹配时的最小重合面积
+
+	long TARGET_CHANGE_DISTANCE_MAX;//目标未更改时，目标装甲板中心与原目标装甲板中心的距离变化最大值
 };
 
 

energy/src/energy/find/target_finder.cpp

@@ -11,9 +11,9 @@ using std::vector;
 
 
 //----------------------------------------------------------------------------------------------------------------------
-// 此函数用于匹配扇叶和装甲板，找到目标装甲板,计算其极坐标角度和中心坐标
+// 此函数通过极坐标角度匹配扇叶和装甲板，找到目标装甲板,计算其极坐标角度和中心坐标
 // ---------------------------------------------------------------------------------------------------------------------
-void Energy::findTarget() {
+void Energy::findTargetByPolar() {
 	if (fan_polar_angle.size() >= armor_polar_angle.size()) return;//扇叶多于装甲板，识别错误
 	if (armor_polar_angle.empty())return;//找不到扇叶，识别错误
 	if (fan_polar_angle.empty()) {
@@ -26,7 +26,7 @@ void Energy::findTarget() {
 	}
 	sort(fan_polar_angle.begin(), fan_polar_angle.end());//对扇叶的极坐标角度进行排序
 	sort(armor_polar_angle.begin(), armor_polar_angle.end());//对装甲板的极坐标角度进行排序
-	int i, j = 0;
+	int i = 0, j = 0;
 	for (i = 0; i < fan_polar_angle.size(); ++i) {
 		if (armor_polar_angle.at(i) - fan_polar_angle.at(j) < energy_part_param_.TWIN_ANGEL_MAX
 		    && armor_polar_angle.at(i) - fan_polar_angle.at(j) > -1 * energy_part_param_.TWIN_ANGEL_MAX) {
@@ -55,6 +55,40 @@ void Energy::findTarget() {
 			target_point = armor.rect.center;//根据已经确定的目标装甲板极坐标角度，获得该装甲板的中心坐标
 		}
 	}
-
 }
 
+
+
+//----------------------------------------------------------------------------------------------------------------------
+// 此函数根据矩形重合面积匹配扇叶与装甲板
+// ---------------------------------------------------------------------------------------------------------------------
+void Energy::findTargetByIntersection() {
+    if (fans.size() >= armors.size()) return;//扇叶多于装甲板，识别错误
+    if (armors.empty())return;//找不到扇叶，识别错误
+    if (fans.empty()) {
+        target_point = armors.at(0).rect.center;
+        return;
+    }
+    int i = 0, j = 0;
+    while (i < armors.size()) {
+        for (j = 0; j < fans.size(); ++j) {
+            std::vector<cv::Point2f> intersection;
+            if(rotatedRectangleIntersection(armors.at(i).rect, fans.at(j).rect, intersection) == 0)//返回0表示没有重合面积
+                continue;
+            else
+                rotatedRectangleIntersection(armors.at(i).rect, fans.at(j).rect, intersection);
+                double cur_contour_area = contourArea(intersection);
+                if (cur_contour_area > energy_part_param_.INTERSETION_CONTOUR_AREA_MIN) {
+//                    cout << endl;
+//                    cout << "NO. " << i << " armor and No. " << j << "fans are matched, the intersection area is"
+//                         << cur_contour_area << endl;
+                    break;
+                }
+        }
+        if(j == fans.size()){
+            target_point = armors.at(i).rect.center;
+            break;
+        }
+        i++;
+    }
+}

energy/src/energy/get/aim_point_get.cpp

@@ -0,0 +1,23 @@
+//
+// Created by sun on 19-7-6.
+//
+
+#include "energy/energy.h"
+#include "energy/constant.h"
+
+using namespace std;
+using namespace cv;
+
+#define FOCUS_PIXAL_5MM  (917)
+#define FOCUS_PIXAL      FOCUS_PIXAL_5MM
+
+
+//----------------------------------------------------------------------------------------------------------------------
+// 此函数通过自瞄逻辑击打目标点，用于大符的自动对心和小符直接打击
+// ---------------------------------------------------------------------------------------------------------------------
+void Energy::getAimPoint(){
+    double dx = target_point.x - 320;
+    double dy = target_point.y - 240;
+    yaw_rotation   = atan(dx / FOCUS_PIXAL) * 180 / PI;
+    pitch_rotation = atan(dy / FOCUS_PIXAL) * 180 / PI;
+}

energy/src/energy/get/predict_point_get.cpp

@@ -26,12 +26,12 @@ void Energy::getPredictPoint(){
 // 此函数用于操作手手动标定
 // ---------------------------------------------------------------------------------------------------------------------
 bool Energy::changeTarget(){
-    if(fabs(target_polar_angle - last_target_polar_angle) < 30||fabs(target_polar_angle - last_target_polar_angle) > 330){
+    if(pointDistance(target_point,last_target_point) < energy_part_param_.TARGET_CHANGE_DISTANCE_MAX){
-        last_target_polar_angle = target_polar_angle;
+        last_target_point = target_point;
         return false;
     }
     else{
-        last_target_polar_angle = target_polar_angle;
+        last_target_point= target_point;
         return true;
     }
 }

energy/src/energy/param_init.cpp

@@ -23,6 +23,7 @@ void Energy::initEnergy() {
 	gimble_cnt = 0;
     circle_center_point = Point(0, 0);
 	target_point = Point(0, 0);
+    last_target_point = Point(0, 0);
     predict_point = Point(0, 0);
 	former_point = Point(0,0);
     target_polar_angle = -1000;
@@ -98,6 +99,9 @@ void Energy::initEnergyPartParam() {
     energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MIN = 1;
 
 	energy_part_param_.TWIN_ANGEL_MAX = 10;
+	energy_part_param_.INTERSETION_CONTOUR_AREA_MIN = 60;
+
+	energy_part_param_.TARGET_CHANGE_DISTANCE_MAX = 20;
 }
 
 

energy/src/energy/run.cpp

@@ -102,13 +102,16 @@ int Energy::runBig(cv::Mat &gimble_src){
     centerRs_cnt = findCenterR(gimble_src);
 //    if(centerRs_cnt>0)showCenterRContours("R", gimble_src);
 
+    writeDownMark();
+
     getAllArmorCenters();
     circleLeastFit();
-    attack_distance = ATTACK_DISTANCE;
+//    attack_distance = ATTACK_DISTANCE;
 
     getFanPolarAngle();
     getArmorPolarAngle();
-    findTarget();
+    findTargetByPolar();
+//    findTargetByIntersection();
 
     if(armors_cnt>0||fans_cnt>0) showBothContours("Both", gimble_src);
 
@@ -129,24 +132,21 @@ int Energy::runBig(cv::Mat &gimble_src){
 
 
 
-
 //----------------------------------------------------------------------------------------------------------------------
-// 此函数为小能量机关模式主控制流函数，且步兵需要同时拥有云台摄像头和底盘摄像头
+// 此函数为小能量机关模式主控制流函数，击打小符只需要拥有云台摄像头
-// ---------------------------------------------------------------------------------------------------------------------
-int Energy::runSmall(cv::Mat &gimble_src, cv::Mat &chassis_src){
-    if(chassis_src.empty())runSmall(gimble_src);//仅拥有云台摄像头则调用单摄像头的run函数
-    else return 0;
-}
-
-
-
-
-
-//----------------------------------------------------------------------------------------------------------------------
-// 此函数为小能量机关模式主控制流函数，且步兵仅拥有云台摄像头
 // ---------------------------------------------------------------------------------------------------------------------
 int Energy::runSmall(cv::Mat &gimble_src){
-
+    imshow("gimble src", gimble_src);
+    fans.clear();
+    armors.clear();
+    threshold(gimble_src, gimble_src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY);
+    fans_cnt = findFan(gimble_src, last_fans_cnt);
+    armors_cnt = findArmor(gimble_src, last_armors_cnt);
+    if(fans_cnt==-1 || armors_cnt==-1 || armors_cnt != fans_cnt+1) return 0;
+    findTargetByIntersection();
+    if(armors_cnt>0||fans_cnt>0) showBothContours("Both", gimble_src);
+    getAimPoint();
+    sendTargetByUart(yaw_rotation, pitch_rotation, target_cnt);
 }
 
 

energy/src/energy/tool/tool.cpp

@@ -70,4 +70,6 @@ double Energy::pointDistance(cv::Point point_1, cv::Point point_2){
     distance = sqrt(pow(static_cast<double>(point_1.x - point_2.x),2)
                     + pow(static_cast<double>(point_1.y - point_2.y),2));
     return distance;
-}
+}
+
+

main.cpp

@@ -26,7 +26,7 @@ using namespace std;
 mcu_data mcuData = {
         0,
         0,
-        BIG_ENERGY_STATE,
+        SMALL_ENERGY_STATE,
         0,
         1,
         ENEMY_RED,
@@ -53,23 +53,23 @@ int main(int argc, char *argv[]) {
 
     while (true) {
         if (from_camera) {
-            video_gimble = new CameraWrapper(0, "armor");
+            video_gimble = new CameraWrapper(0/*, "armor"*/);
-            video_chassis = new CameraWrapper(1, "energy");
+            video_chassis = new CameraWrapper(1/*, "energy"*/);
         } else {
-            video_gimble = new VideoWrapper("/Users/leo/Desktop/videos/170.avi");
+            video_gimble = new VideoWrapper("/home/sun/项目/energy_video/energy_test.avi");
-            video_chassis = new VideoWrapper("/Users/leo/Desktop/videos/170.avi");
+            video_chassis = new VideoWrapper("/home/sun/项目/energy_video/energy_test.avi");
         }
         if (video_gimble->init()) {
-            LOGM("video_armor source initialization successfully.");
+            LOGM("video_gimble source initialization successfully.");
         } else {
-            LOGW("video_armor source unavailable!");
+            LOGW("video_gimble source unavailable!");
             delete video_gimble;
             video_gimble = nullptr;
         }
         if (video_chassis->init()) {
-            LOGM("video_energy source initialization successfully.");
+            LOGM("video_chassis source initialization successfully.");
         } else {
-            LOGW("video_energy source unavailable!");
+            LOGW("video_chassis source unavailable!");
             delete video_chassis;
             video_chassis = nullptr;
         }
@@ -87,37 +87,35 @@ int main(int argc, char *argv[]) {
         cout<<"start running"<<endl;
         do {
             CNT_TIME("Total", {
-                if (mcuData.state != ARMOR_STATE) {//能量机关模式
+                if (mcuData.state == BIG_ENERGY_STATE) {//大符模式
                     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 (from_camera == 0) {
-                    if (mcuData.state == BIG_ENERGY_STATE) {//大符模式
+//                        cv::resize(chassis_src, chassis_src, cv::Size(640, 480), 2);
-//                        if (from_camera == 0) {
+//                        imshow("resize", chassis_src);
-//                            cv::resize(energy_src, energy_src, cv::Size(640, 480), 2);
+//                        energy.extract(chassis_src);
-//                            imshow("resize", energy_src);
+//                    }
-//                            energy.extract(energy_src);
+                    if (last_state != BIG_ENERGY_STATE) {//若上一帧不是大符模式，即刚往完成切换，则需要初始化
-//                        }
+                        energy.setEnergyRotationInit();
-                        if (last_state != BIG_ENERGY_STATE) {//若上一帧不是大符模式，即刚往完成切换，则需要初始化
+                        cout << "set" << endl;
-                            energy.setEnergyRotationInit();
-                            cout << "set" << endl;
-                        }
-                        energy.runBig(gimble_src, chassis_src);//击打大符
-                    }
-                    else if (mcuData.state == SMALL_ENERGY_STATE) {
-                        energy.runSmall(gimble_src, chassis_src);//击打小符
                     }
+                    energy.runBig(gimble_src, chassis_src);//击打大符
                     last_state = mcuData.state;//更新上一帧状态
                 }
-                else if (mcuData.state == ARMOR_STATE) {//自瞄模式
+                else if (mcuData.state != BIG_ENERGY_STATE) {//自瞄或小符模式
                     last_state = mcuData.state;
                     ok = checkReconnect(video_gimble->read(gimble_src));
                     if (save_video) saveVideos(gimble_src);
                     if (show_origin) showOrigin(gimble_src);
-                    CNT_TIME("Armor Time", {
+                    if (mcuData.state == ARMOR_STATE){
-                            armorFinder.run(gimble_src);
+                        CNT_TIME("Armor Time", {
-                    });
+                                armorFinder.run(gimble_src);
-
+                        });
+                    }
+                    else if(mcuData.state == SMALL_ENERGY_STATE){
+                        energy.runSmall(gimble_src);
+                    }
                 }
                 cv::waitKey(1);
             });