energy changed

energy/include/energy/energy.h

@@ -48,6 +48,7 @@ private:
 	bool isMark;
 	int fans_cnt;
 	int armors_cnt;
+	int centerRs_cnt;
 	int count;
 	int last_fans_cnt;
 	int last_armors_cnt;
@@ -82,6 +83,7 @@ private:
 
 	std::vector<EnergyPart> fans;
 	std::vector<EnergyPart> armors;
+    std::vector<EnergyPart> centerRs;
  //   std::vector<EnergyPart> gimble_zero_points;
 
 	cv::Point cycle_center;
@@ -102,15 +104,17 @@ private:
 
 	int findFan(const cv::Mat &src, vector<EnergyPart> &fans, int &last_fans_cnt);
 	int findArmor(const cv::Mat &src, vector<EnergyPart> &armors, int &last_armors_cnt);
-    int findGimbleZeroPoint(const cv::Mat &src, vector<EnergyPart> &gimble_zero_point);
+    int findCenterR(const cv::Mat src);
 
 	void showFanContours(std::string windows_name, const cv::Mat &src, const std::vector<EnergyPart> &fans);
 	void showArmorContours(std::string windows_name, const cv::Mat &src, const std::vector<EnergyPart> &armors);
 	void showBothContours(std::string windows_name, const cv::Mat &src, const std::vector<EnergyPart> &fans,
 		const std::vector<EnergyPart> &armors);
+    void showCenterRContours(std::string windows_name, const cv::Mat src);
 
 	bool isValidFanContour(const vector<cv::Point> &fan_contour);
 	bool isValidArmorContour(const vector<cv::Point> &armor_contour);
+    bool isValidCenterRContour(const vector<cv::Point> center_R_contour);
 
     void getFanPosition(std::vector<float> &fanPosition, const std::vector<EnergyPart> &fans, cv::Point cycle_center, double radius);
 	void getArmorPosition(std::vector<float> &armorPosition, const std::vector<EnergyPart> &armors, cv::Point cycle_center, double radius);

energy/include/energy/param_struct_define.h

@@ -48,6 +48,15 @@ struct EnergyPartParam {
 	float ARMOR_CONTOUR_HW_RATIO_MAX;
 	float ARMOR_CONTOUR_HW_RATIO_MIN;
 
+    long CENTER_R_CONTOUR_AREA_MAX;
+    long CENTER_R_CONTOUR_AREA_MIN;
+    long CENTER_R_CONTOUR_LENGTH_MIN;
+    long CENTER_R_CONTOUR_WIDTH_MIN;
+    long CENTER_R_CONTOUR_LENGTH_MAX;
+    long CENTER_R_CONTOUR_WIDTH_MAX;
+    float CENTER_R_CONTOUR_HW_RATIO_MAX;
+    float CENTER_R_CONTOUR_HW_RATIO_MIN;
+
 	float TWIN_ANGEL_MAX;
 };
 

energy/src/energy/find/energy_finder.cpp

@@ -121,38 +121,42 @@ int Energy::findArmor(const cv::Mat &src, vector<EnergyPart> &armors, int &last_
 	return static_cast<int>(armors.size());
 }
 
-int Energy::findGimbleZeroPoint(const cv::Mat &src, vector<EnergyPart> &gimble_zero_points) {
+int Energy::findCenterR(const cv::Mat src) {
     if (src.empty())return 0;
     static Mat src_bin;
     src_bin = src.clone();
-//    threshold(src, src_bin, energy_part_param_.FAN_GRAY_THRESH, 255, THRESH_BINARY);
+//    threshold(src, src_bin, energy_part_param_.ARMOR_GRAY_THRESH, 255, THRESH_BINARY);
     if(src.type() == CV_8UC3){
         cvtColor(src_bin, src_bin, CV_BGR2GRAY);
     }
-    std::vector<vector<Point> > zero_point_contours;
+    std::vector<vector<Point> > center_R_contours;
 
-    findContours(src_bin, zero_point_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
+    StructingElementErodeDilate(src_bin);
+//    imshow("R struct",src_bin);
 
-    for (auto &zero_point_contour : zero_point_contours) {
+    findContours(src_bin, center_R_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
 
-        double cur_contour_area = contourArea(zero_point_contour);
+    for (auto &center_R_contour : center_R_contours) {
-        RotatedRect cur_rect = minAreaRect(zero_point_contour);
+        if (!isValidCenterRContour(center_R_contour))
+        {
+            continue;
+        }
+
+        RotatedRect cur_rect = minAreaRect(center_R_contour);
         Size2f cur_size = cur_rect.size;
-
-//        cout<<"cur_contour_area: "<<cur_contour_area<<'\t'<<"rect_area: "<<cur_size.area()<<'\t'<<"ratio: "<<cur_contour_area/cur_size.area()<<endl;
-
         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;
 
-		if(length<10&&width<10&&length>1&&width>1){
+//        if(length>10 && width>5){
-			cout<<"zero point center: "<<cur_rect.center<<endl;
+//            centerRs.emplace_back(center_R_contour);
-			cout<<"zero point area: "<<length<<'\t'<<width<<endl;
+//            cout<<"center R area: "<<length<<'\t'<<width<<'\t'<<cur_rect.center<<endl;
-			gimble_zero_points.emplace_back(zero_point_contour);
+//        }
-		}
+        centerRs.emplace_back(center_R_contour);
 
+//        cout<<"armor area: "<<length<<'\t'<<width<<endl;
     }
 
-    return static_cast<int>(fans.size());
+    return static_cast<int>(centerRs.size());
 }
 
 bool Energy::isValidFanContour(const vector<cv::Point> &fan_contour) {
@@ -226,3 +230,39 @@ bool Energy::isValidArmorContour(const vector<cv::Point> &armor_contour) {
 	return true;
 }
 
+bool Energy::isValidCenterRContour(const vector<cv::Point> center_R_contour) {
+    double cur_contour_area = contourArea(center_R_contour);
+//	if (cur_contour_area > energy_part_param_.ARMOR_CONTOUR_AREA_MAX ||
+//		cur_contour_area < energy_part_param_.ARMOR_CONTOUR_AREA_MIN)
+//	{
+//		//cout<<cur_contour_area<<" "<<energy_fan_param_.CONTOUR_AREA_MIN<<" "<<energy_fan_param_.CONTOUR_AREA_MAX<<endl;
+//		//cout<<"area fail."<<endl;
+//		return false;
+//	}
+    RotatedRect cur_rect = minAreaRect(center_R_contour);
+    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;
+    if (length < energy_part_param_.CENTER_R_CONTOUR_LENGTH_MIN || width < energy_part_param_.CENTER_R_CONTOUR_WIDTH_MIN)
+    {
+        //cout<<"length width min fail."<<endl;
+        return false;
+    }
+
+    if (length > energy_part_param_.CENTER_R_CONTOUR_LENGTH_MAX||width>energy_part_param_.CENTER_R_CONTOUR_WIDTH_MAX)
+    {
+        //cout<<"length width max fail."<<endl;
+        return false;
+    }
+
+    float length_width_ratio = length / width;
+    if (length_width_ratio > energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MAX ||
+        length_width_ratio < energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MIN)
+    {
+        //cout<<"length width ratio fail."<<endl;
+        return false;
+    }
+    if (cur_contour_area / cur_size.area() < 0.7) return false;
+
+    return true;
+}

energy/src/energy/param_init.cpp

@@ -15,6 +15,7 @@ void Energy::initEnergy() {
 
 	fans_cnt = 0;
 	armors_cnt = 0;
+	centerRs_cnt = 0;
 	cycle_center = Point(0, 0);
 	target_center = Point(0, 0);
 	last_target_center = Point(0, 0);
@@ -66,6 +67,7 @@ void Energy::initEnergy() {
 
 	fans.clear();
 	armors.clear();
+    centerRs.clear();
 	fanPosition.clear();
 	armorPosition.clear();
 	Armor_center.clear();
@@ -104,6 +106,15 @@ void Energy::initEnergyPartParam() {
 	energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MAX = 3;
 	energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MIN = 1;
 
+    energy_part_param_.CENTER_R_CONTOUR_AREA_MAX = 100000;
+    energy_part_param_.CENTER_R_CONTOUR_AREA_MIN = 0;
+    energy_part_param_.CENTER_R_CONTOUR_LENGTH_MIN = 10;
+    energy_part_param_.CENTER_R_CONTOUR_WIDTH_MIN = 10;
+    energy_part_param_.CENTER_R_CONTOUR_LENGTH_MAX = 30;
+    energy_part_param_.CENTER_R_CONTOUR_WIDTH_MAX = 30;
+    energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MAX = 3;
+    energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MIN = 1;
+
 	energy_part_param_.TWIN_ANGEL_MAX = 10;
 
 	lift_height_.LIFT_0 = 0;

energy/src/energy/run.cpp

@@ -17,6 +17,7 @@ int Energy::run(cv::Mat &src){
 //    imshow("src",src);
     fans.clear();
     armors.clear();
+    centerRs.clear();
     fanPosition.clear();
     armorPosition.clear();
 //    gimble_zero_points.clear();
@@ -24,12 +25,6 @@ int Energy::run(cv::Mat &src){
 
 	changeMark();
 	if (isMark)return 0;
-//	cout<<"yaw"<<origin_yaw<<endl;
-
-//    if(all_armor_centers.size()>200)all_armor_centers.clear();
-//    if(first_armor_centers.size()>200)first_armor_centers.clear();
-//    cout<<"first_armor_centers.size(): "<<first_armor_centers.size()<<endl;
-
 //    imagePreprocess(src);
 //    imshow("img_preprocess",src);
 
@@ -37,6 +32,7 @@ int Energy::run(cv::Mat &src){
 //    imshow("bin",src);
 
 
+
     fans_cnt = findFan(src, fans, last_fans_cnt);
 //    cout<<"fans_cnt: "<<fans_cnt<<endl;
     if(fans_cnt==-1) return 0;//滤去漏判的帧
@@ -50,8 +46,10 @@ int Energy::run(cv::Mat &src){
 
     if(armors_cnt>0||fans_cnt>0) showBothContours("Both",src, fans, armors);
 
+    centerRs_cnt = findCenterR(src);
+    if(centerRs_cnt>0)showCenterRContours("R",src);
 
-    if(armors_cnt>=4 && fans_cnt>=3) {
+        if(armors_cnt>=4 && fans_cnt>=3) {
         FILE *fp = fopen(PROJECT_DIR"/Mark/mark.txt", "w");
         if (fp) {
             fprintf(fp, "yaw: %f, pitch: %f\n", origin_yaw, origin_pitch);
@@ -112,6 +110,8 @@ int Energy::run(cv::Mat &src){
 //    cout<<"position mode: "<<position_mode<<endl;
 
     gimbleRotation();
+
+
     if(changeTarget())target_cnt++;
 
 //	if (!isSendTarget)return 0;

energy/src/energy/show/show.cpp

@@ -58,6 +58,31 @@ void Energy::showArmorContours(std::string windows_name, const cv::Mat &src, con
 	imshow(windows_name, image2show);
 }
 
+void Energy::showCenterRContours(std::string windows_name, const cv::Mat src) {
+    if (src.empty())return;
+    static Mat image2show;
+
+    if(src.type() == CV_8UC1) // 黑白图像
+    {
+        cvtColor(src, image2show, COLOR_GRAY2RGB);
+
+    } else if (src.type() == CV_8UC3) //RGB 彩色
+    {
+        image2show = src.clone();
+    }
+    //cvtColor(image2show, image2show, COLOR_GRAY2RGB);
+    for (const auto &center_R : centerRs)
+    {
+        Point2f vertices[4];      //定义矩形的4个顶点
+        center_R.rect.points(vertices);   //计算矩形的4个顶点
+        for (int i = 0; i < 4; i++)
+            line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(255, 0, 255), 2);
+        //cout << armor.rect.center << '\t' << armor.rect.angle << '\t';
+        //cout << endl;
+    }
+    imshow(windows_name, image2show);
+}
+
 void Energy::showBothContours(std::string windows_name, const cv::Mat &src, const std::vector<EnergyPart> &fans,
 	const std::vector<EnergyPart> &armors) {
 	if (src.empty())return;

main.cpp

@@ -29,7 +29,7 @@ using namespace std;
 mcu_data mcuData = {
         0,
         0,
-        ARMOR_STATE,
+        ENERGY_STATE,
         0,
         1,
         ENEMY_RED,
@@ -65,8 +65,8 @@ int main(int argc, char *argv[]) {
 //            video_armor = new VideoWrapper(armor_video);
 //            lastVideo(energy_video, PROJECT_DIR"/energy_video/");
 //            video_energy = new VideoWrapper(energy_video);
-            video_armor = new VideoWrapper("/home/sjturm/Desktop/valid_video/armor/65.avi");
+            video_armor = new VideoWrapper("/home/sun/项目/energy_video/energy_test.avi");
-            video_energy = new VideoWrapper("/home/sjturm/Desktop/valid_video/energy/121.avi");
+            video_energy = new VideoWrapper("/home/sun/项目/energy_video/energy_test.avi");
         }
         if (video_armor->init()) {
             LOGM("video_armor source initialization successfully.");
@@ -169,7 +169,7 @@ int main(int argc, char *argv[]) {
                     }
                 }
 //                cout<<last_state<<endl;
-                waitKey(1);
+                waitKey(3);
             });
         } while (ok);