energy changed

energy/include/energy/energy.h

@@ -25,6 +25,7 @@ class Energy {
 public:
     Energy(Serial &u, uint8_t &color);//构造函数，参数为串口和敌方颜色
     ~Energy();//默认析构函数
+
     int runBig(cv::Mat &gimble_src, cv::Mat &chassis_src);
 
     int runBig(cv::Mat &gimble_src);
@@ -46,14 +47,7 @@ private:
     bool isChassis;//同时具有底盘和云台摄像头时，处于底盘摄像头击打过程
     bool isGuessing;//当前处于发弹到新目标出现的过程，则为true，此时猜测下一个目标
     bool isPredicting;//当前处于新目标出现到发弹的过程，则为true，此时正常击打
-//	int fans_cnt;//图像中的扇叶个数
-//	int armors_cnt;//图像中的装甲板个数
-//	int centerRs_cnt;//图像中可能的风车中心字母R选区个数
-//    int flow_strip_fans_cnt;//图像中的含流动条扇叶个数
-//	int flow_strips_cnt;//图像中的流动条个数
     int last_fans_cnt;//上一帧的扇叶个数
-    int last_flow_strip_fans_cnt;//上一帧的含流动条扇叶个数
-    int last_flow_strips_cnt;//上一帧的流动条个数
     double radius;//大风车半径
     float target_polar_angle;//待击打装甲板的极坐标角度
     float last_target_polar_angle;//上一帧待击打装甲板的极坐标角度
@@ -76,16 +70,14 @@ private:
     float target_cnt;//用于记录寻找到的装甲板总数，该值变化则立即中断主控板发射进程，防止重复击打已点亮的装甲板
     bool save_new_mark;//若操作手进行过手动标定，则为true
     bool shoot;//若为true，则要求主控板发弹
-    int guess_devide;
+    int guess_devide;//刚进入猜测状态时，猜测目标点在极坐标中的分区
-    bool startguessing;
+    bool startguessing;//进入猜测状态的标志
+
     timeval time_start_guess;
+
     std::vector<cv::RotatedRect> fans;//图像中所有扇叶
     std::vector<cv::RotatedRect> armors;//图像中所有可能装甲板（可能存在误识别）
-//    std::vector<cv::RotatedRect> centerRs;//风车中心字母R的可能候选区
+
-//    std::vector<cv::RotatedRect> flow_strips;//图像中所有流动条（理论上只有一个）
-//    std::vector<cv::RotatedRect> flow_strip_fans;//图像中所有流动条所在扇叶（理论上只有一个）
-//    std::vector<cv::RotatedRect> center_ROI;//风车中心候选区
-//    std::vector<cv::RotatedRect> target_armor;//目标装甲板（理论上仅一个）
     cv::RotatedRect centerR;//风车中心字母R的可能候选区
     cv::RotatedRect flow_strip;//图像中所有流动条（理论上只有一个）
     cv::RotatedRect flow_strip_fan;//图像中所有流动条所在扇叶（理论上只有一个）
@@ -140,21 +132,22 @@ private:
     void findTargetByIntersection();//通过面积重合度匹配获取目标装甲板的极坐标角度和装甲板中心坐标
     bool findTargetInFlowStripFan();//在已发现的流动条区域中寻找待击打装甲板
 
-    void rotate();//获取预测点位
+    void rotate(cv::Point target_point);//获取预测点位
     void stretch(cv::Point point_1, cv::Point2f &point_2);//将像素差转换为实际距离差
-    double pointDistance(cv::Point point_1, cv::Point point_2);//计算两点距离
     int devide(float angle);//将极坐标分为五个区域，判断一个角度处于哪个区域
     int linePointX(const cv::Point2f &p1, const cv::Point2f &p2, int y);//计算直线上一点横坐标
-    double nonZeroRateOfRotateRect(const cv::Mat &bin, const cv::RotatedRect &rotatedRect);//计算旋转矩形内亮点占比
+    double pointDistance(cv::Point point_1, cv::Point point_2);//计算两点距离
+    double nonZeroRateOfRotateRect(cv::Mat &bin, const cv::RotatedRect &rotatedRect);//计算旋转矩形内亮点占比
 
     void writeDownMark();//记录操作手标定的云台初始角度
 
     void getTargetPolarAngle();
-    void getPredictPoint();//获取预测点位
+    void getPredictPoint(cv::Point target_point);//获取预测点位
-    void getAimPoint();//通过自瞄逻辑计算点位
+    void getAimPoint(cv::Point target_point);//通过自瞄逻辑计算点位
     bool changeTarget();//判断目标是否改变
     void changeMark();//操作手手动修改标定值
     void gimbleRotation();//计算云台旋转角度
+    void getAllArmorCenters();//记录所有装甲板中心坐标
 
     void judgeBigShoot();//判断大符模式是否可以发弹
     void judgeSmallShoot();//判断小符模式是否可以发弹

energy/src/energy/find/energy_finder.cpp

@@ -119,7 +119,10 @@ bool Energy::findCenterR(const cv::Mat src) {
             continue;
         }
         centerR = cv::minAreaRect(center_R_contour);
+        float target_length =
+                target_armor.size.height > target_armor.size.width ? target_armor.size.height : target_armor.size.width;
         circle_center_point = centerR.center;
+        circle_center_point.y += target_length / 7.5;//实际最小二乘得到的中心在R的下方
         return true;
 
 //        RotatedRect cur_rect = minAreaRect(center_R_contour);
@@ -148,7 +151,7 @@ bool Energy::findFlowStripFan(const cv::Mat src) {
     }
     std::vector<vector<Point> > flow_strip_fan_contours;
     StructingElementErodeDilate(src_bin);//图像膨胀，防止图像断开并更方便寻找
-    imshow("flow strip fan struct", src_bin);
+//    imshow("flow strip fan struct", src_bin);
 
     findContours(src_bin, flow_strip_fan_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
 
@@ -435,10 +438,9 @@ bool Energy::isValidFlowStripContour(const vector<cv::Point> &flow_strip_contour
     if (cur_contour_area / cur_size.area() < energy_part_param_.FLOW_STRIP_CONTOUR_AREA_RATIO_MIN)
         return false;//轮廓对矩形的面积占有率不合适
     std::vector<cv::Point2f> intersection;
-    if (rotatedRectangleIntersection(cur_rect, flow_strip_fan, intersection) != 0) {
+    if (rotatedRectangleIntersection(cur_rect, flow_strip_fan, intersection) == 0 ||
-        if (contourArea(intersection) < energy_part_param_.FLOW_STRIP_CONTOUR_INTERSETION_AREA_MIN) {
+        contourArea(intersection) < energy_part_param_.FLOW_STRIP_CONTOUR_INTERSETION_AREA_MIN) {
         return false;
     }
-    }
     return true;
 }

energy/src/energy/get/aim_point_get.cpp

@@ -11,7 +11,7 @@ using namespace cv;
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数通过自瞄逻辑击打目标点，用于大符的自动对心和小符直接打击
 // ---------------------------------------------------------------------------------------------------------------------
-void Energy::getAimPoint(){
+void Energy::getAimPoint(cv::Point target_point){
     double dx = target_point.x - 320;
     double dy = target_point.y - 240;
     yaw_rotation   = atan(dx / FOCUS_PIXAL) * 180 / PI;

energy/src/energy/get/guess_point_get.cpp

@@ -28,7 +28,7 @@ void Energy::guessTarget() {
     float max_angle = all_fan_angles.at(all_fan_angles.size() - 1);
     float base_angle = min_angle;
     while (base_angle > 72)base_angle -= 72;
-    if (startguessing) {
+    if (startguessing) {cout<<"666"<<endl;
         int i = 0;
         for (i = 1; i < all_fan_angles.size(); ++i) {
             if (abs(min_angle + 72 * i - all_fan_angles.at(i)) > energy_part_param_.TWIN_ANGEL_MAX) {

energy/src/energy/get/polar_angle_get.cpp

@@ -16,3 +16,12 @@ void Energy::getTargetPolarAngle() {
                                                             (target_point.x - circle_center_point.x)));
 }
 
+//----------------------------------------------------------------------------------------------------------------------
+// 此函数用于存储图像中所有装甲板的中心坐标，以便后续最小二乘法计算圆心和半径
+// ---------------------------------------------------------------------------------------------------------------------
+void Energy::getAllArmorCenters()
+{
+    for (const auto &armor : armors) {
+        all_armor_centers.push_back(armor.center);
+    }
+}

energy/src/energy/get/predict_point_get.cpp

@@ -15,8 +15,8 @@ using std::vector;
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数获取预测点坐标
 // ---------------------------------------------------------------------------------------------------------------------
-void Energy::getPredictPoint(){
+void Energy::getPredictPoint(cv::Point target_point){
-    if(energy_rotation_direction==1) rotate();
+    if(energy_rotation_direction==1) rotate(target_point);
-    if(energy_rotation_direction==-1) rotate();
+    if(energy_rotation_direction==-1) rotate(target_point);
 }
 

energy/src/energy/init/clear.cpp

@@ -23,8 +23,8 @@ void Energy::clearAll(){
 // 此函数用于图像预处理
 // ---------------------------------------------------------------------------------------------------------------------
 void Energy::initImage(cv::Mat &src){
-    imagePreprocess(src);
+//    imagePreprocess(src);
-    if(show_process)imshow("img_preprocess", src);
+//    if(show_process)imshow("img_preprocess", src);
     if (src.type() == CV_8UC3)cvtColor(src, src, COLOR_BGR2GRAY);
     threshold(src, src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY);
     if (show_process)imshow("bin", src);

energy/src/energy/init/energy_init.cpp

@@ -17,11 +17,6 @@ void Energy::initEnergy() {
     isMark = false;
     isPredicting = true;
     isGuessing = false;
-//	fans_cnt = 0;
-//	armors_cnt = 0;
-//	centerRs_cnt = 0;
-//    flow_strips_cnt = 0;
-//    flow_strip_fans_cnt = 0;
     circle_center_point = Point(0, 0);
     target_point = Point(0, 0);
     last_target_point = Point(0, 0);
@@ -34,8 +29,6 @@ void Energy::initEnergy() {
     energy_rotation_direction = ANTICLOCKWISE;
     attack_distance = ATTACK_DISTANCE;
     last_fans_cnt = 0;
-    last_flow_strips_cnt = 0;
-    last_flow_strip_fans_cnt = 0;
     send_cnt = 0;
     yaw_rotation = 0;
     pitch_rotation = 0;

energy/src/energy/run.cpp

@@ -10,6 +10,8 @@ using namespace std;
 using namespace cv;
 
 
+
+
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数为大能量机关模式主控制流函数，且步兵需要同时拥有云台摄像头和底盘摄像头
 // ---------------------------------------------------------------------------------------------------------------------
@@ -17,7 +19,7 @@ int Energy::runBig(cv::Mat &gimble_src, cv::Mat &chassis_src) {
     if (chassis_src.empty())
         runBig(gimble_src);//仅拥有云台摄像头则调用单摄像头的run函数
     else if (isGimble) {
-        energy_part_param_ = chassis_energy_part_param_;
+        energy_part_param_ = gimble_energy_part_param_;
         clearAll();
         initImage(gimble_src);
 
@@ -25,9 +27,11 @@ int Energy::runBig(cv::Mat &gimble_src, cv::Mat &chassis_src) {
         if (!findFlowStripFan(gimble_src))return 0;
         if (!findTargetInFlowStripFan()) return 0;
         if (!findCenterROI(gimble_src))return 0;
-        if (show_energy)showFlowStripFanContours("strip", gimble_src);
         if (!findCenterR(gimble_src))return 0;
+
+        if (show_energy)showFlowStripFanContours("strip", gimble_src);
         if (show_energy)showCenterRContours("R", gimble_src);
+
         startChassis();
         getOrigin();
         initEnergy();
@@ -36,19 +40,18 @@ int Energy::runBig(cv::Mat &gimble_src, cv::Mat &chassis_src) {
         energy_part_param_ = chassis_energy_part_param_;
         clearAll();
         initImage(chassis_src);
+
         changeMark();
         if (isMark)return 0;//操作手强制手动标定origin_yaw和origin_pitch
 
-        if (show_process)imshow("bin", chassis_src);
         if (findArmors(chassis_src) < 1)return 0;
         if (!findFlowStripFan(chassis_src))return 0;
         if (!findTargetInFlowStripFan()) return 0;
         if (!findCenterROI(chassis_src))return 0;
-        if (show_energy)showFlowStripFanContours("strip", chassis_src);
         if (!findCenterR(chassis_src))return 0;
-        if (show_energy)showCenterRContours("R", chassis_src);
 
-        findFans(chassis_src);showFanContours("fan",chassis_src);
+        if (show_energy)showFlowStripFanContours("strip", chassis_src);
+        if (show_energy)showCenterRContours("R", chassis_src);
 
         getTargetPolarAngle();
         if (save_mark)writeDownMark();
@@ -57,7 +60,7 @@ int Energy::runBig(cv::Mat &gimble_src, cv::Mat &chassis_src) {
             return 0;
         }
         if (isPredicting) {
-            getPredictPoint();
+            getPredictPoint(target_point);
             gimbleRotation();
             judgeBigShoot();
             sendTarget(serial, yaw_rotation, pitch_rotation, shoot);
@@ -70,8 +73,7 @@ int Energy::runBig(cv::Mat &gimble_src, cv::Mat &chassis_src) {
                 findFans(chassis_src);
                 guessTarget();
                 if (show_energy)showGuessTarget("guess", chassis_src);
-                target_point = guess_point;
+                getPredictPoint(guess_point);
-                getPredictPoint();
                 gimbleRotation();
                 sendTarget(serial, yaw_rotation, pitch_rotation, false);
             }
@@ -81,18 +83,19 @@ int Energy::runBig(cv::Mat &gimble_src, cv::Mat &chassis_src) {
 }
 
 
+
+
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数为大能量机关模式主控制流函数，且步兵仅拥有云台摄像头
 // ---------------------------------------------------------------------------------------------------------------------
 int Energy::runBig(cv::Mat &gimble_src) {
-    energy_part_param_ = chassis_energy_part_param_;
+    energy_part_param_ = gimble_energy_part_param_;
     clearAll();
     initImage(gimble_src);
 
     changeMark();
     if (isMark)return 0;//操作手强制手动标定origin_yaw和origin_pitch
 
-    threshold(gimble_src, gimble_src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY);
     if (show_process)imshow("bin", gimble_src);
     if (findArmors(gimble_src) < 1)return 0;
     if (!findFlowStripFan(gimble_src))return 0;
@@ -108,7 +111,7 @@ int Energy::runBig(cv::Mat &gimble_src) {
         return 0;
     }
     if (isPredicting) {
-        getPredictPoint();
+        getPredictPoint(target_point);
         gimbleRotation();
         judgeBigShoot();
         sendTarget(serial, yaw_rotation, pitch_rotation, shoot);
@@ -121,57 +124,57 @@ int Energy::runBig(cv::Mat &gimble_src) {
             findFans(gimble_src);
             guessTarget();
             if (show_energy)showGuessTarget("guess", gimble_src);
-            target_point = guess_point;
+            getPredictPoint(guess_point);
-            getPredictPoint();
             gimbleRotation();
             sendTarget(serial, yaw_rotation, pitch_rotation, false);
         }
     }
-
     return 0;
 }
 
 
-/*//----------------------------------------------------------------------------------------------------------------------
+
+//----------------------------------------------------------------------------------------------------------------------
 // 此函数为小能量机关模式主控制流函数，击打小符只需要拥有云台摄像头
 // ---------------------------------------------------------------------------------------------------------------------
 int Energy::runSmall(cv::Mat &gimble_src) {
-    if (gimble_src.type() == CV_8UC3)cvtColor(gimble_src, gimble_src, COLOR_BGR2GRAY);
+    energy_part_param_ = chassis_energy_part_param_;
-    energy_part_param_ = gimble_energy_part_param_;
+    clearAll();
-    fans.clear();
+    initImage(gimble_src);
-    armors.clear();
+
-    centerRs.clear();
+    changeMark();
-    flow_strip_fans.clear();
+    if (isMark)return 0;//操作手强制手动标定origin_yaw和origin_pitch
-    flow_strips.clear();
+
-    threshold(gimble_src, gimble_src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY);
+    if (show_process)imshow("bin", gimble_src);
-    if (show_bin)imshow("bin", gimble_src);
+    if (findArmors(gimble_src) < 1)return 0;
-    armors_cnt = findArmor(gimble_src, last_armors_cnt);
+    if (!findFlowStripFan(gimble_src))return 0;
-    flow_strip_fans_cnt = findFlowStripFan(gimble_src, last_flow_strip_fans_cnt);
+    if (!findTargetInFlowStripFan()) return 0;
-    if (flow_strip_fans_cnt == 1 && findTargetInFlowStripFan()) {
+
-        findCenterROI(gimble_src);
+    if (isPredicting) {
-        if (show_strip)showFlowStripFanContours("strip", gimble_src);
+        getAimPoint(target_point);
+        judgeSmallShoot();
+        sendTarget(serial, yaw_rotation, pitch_rotation, shoot);
+    } else if (isGuessing) {
+        if (changeTarget() || is_guessing_timeout()){
+            isPredicting = true;
+            isGuessing = false;
+            LOGM(STR_CTR(WORD_LIGHT_YELLOW, "Start Predicting!"));
         } else {
-        fans_cnt = findFan(gimble_src, last_fans_cnt);
+            if (!findCenterROI(gimble_src))return 0;
-        findTargetByIntersection();
+            if (show_energy)showFlowStripFanContours("strip", gimble_src);
-        if (show_single) {
+            if (!findCenterR(gimble_src))return 0;
-            if (fans_cnt > 0)showFanContours("fan", gimble_src);
+            if (show_energy)showCenterRContours("R", gimble_src);
-            if (armors_cnt > 0)showArmorContours("armor", gimble_src);
+            getTargetPolarAngle();
+            findFans(gimble_src);
+            guessTarget();
+            if (show_energy)showGuessTarget("guess", gimble_src);
+            getAimPoint(guess_point);
+            sendTarget(serial, yaw_rotation, pitch_rotation, false);
         }
-        if (show_both && (fans_cnt > 0 || armors_cnt > 0))showBothContours("both", gimble_src);
     }
-    centerRs_cnt = findCenterR(gimble_src);
-    if (show_center && centerRs_cnt > 0)showCenterRContours("R", gimble_src);
-    if (centerRs.size() != 1)return 0;
-    circle_center_point = centerRs.at(0).center;
-    target_polar_angle = static_cast<float>(180 / PI * atan2(-1 * (target_point.y - circle_center_point.y),
-                                                             (target_point.x - circle_center_point.x)));
-    getAimPoint();
-    changeMode();
-    sendTarget(serial, yaw_rotation, pitch_rotation, target_cnt, small_energy_shoot);
-
     return 0;
-}*/
+
-
+}
 
 
 

energy/src/energy/show/show.cpp

@@ -17,7 +17,7 @@ void Energy::showFanContours(std::string windows_name, const cv::Mat src) {
     if (src.empty())return;
     static Mat image2show;
 
-    if(src.type() == CV_8UC1) // 黑白图像
+    if (src.type() == CV_8UC1) // 黑白图像
     {
         cvtColor(src, image2show, COLOR_GRAY2RGB);
 
@@ -25,8 +25,7 @@ void Energy::showFanContours(std::string windows_name, const cv::Mat src) {
     {
         image2show = src.clone();
     }
-	for (const auto &fan : fans)
+    for (const auto &fan : fans) {
-	{
         Point2f vertices[4];      //定义矩形的4个顶点
         fan.points(vertices);   //计算矩形的4个顶点
         for (int i = 0; i < 4; i++)
@@ -36,7 +35,6 @@ void Energy::showFanContours(std::string windows_name, const cv::Mat src) {
 }
 
 
-
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于显示图像中所有装甲板
 // ---------------------------------------------------------------------------------------------------------------------
@@ -44,7 +42,7 @@ void Energy::showArmorContours(std::string windows_name, const cv::Mat src) {
     if (src.empty())return;
     static Mat image2show;
 
-    if(src.type() == CV_8UC1) // 黑白图像
+    if (src.type() == CV_8UC1) // 黑白图像
     {
         cvtColor(src, image2show, COLOR_GRAY2RGB);
 
@@ -52,8 +50,7 @@ void Energy::showArmorContours(std::string windows_name, const cv::Mat src) {
     {
         image2show = src.clone();
     }
-	for (const auto &armor : armors)
+    for (const auto &armor : armors) {
-	{
         Point2f vertices[4];      //定义矩形的4个顶点
         armor.points(vertices);   //计算矩形的4个顶点
         for (int i = 0; i < 4; i++)
@@ -63,14 +60,13 @@ void Energy::showArmorContours(std::string windows_name, const cv::Mat src) {
 }
 
 
-
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于显示图像中所有扇叶和装甲板,并框出待击打装甲板
 // ---------------------------------------------------------------------------------------------------------------------
 void Energy::showBothContours(std::string windows_name, const cv::Mat src) {
     if (src.empty())return;
     static Mat image2show;
-    if(src.type() == CV_8UC1) // 黑白图像
+    if (src.type() == CV_8UC1) // 黑白图像
     {
         cvtColor(src, image2show, COLOR_GRAY2RGB);
 
@@ -78,19 +74,17 @@ void Energy::showBothContours(std::string windows_name, const cv::Mat src) {
     {
         image2show = src.clone();
     }
-    for (const auto &fan : fans)
+    for (const auto &fan : fans) {
-    {
         Point2f vertices[4];      //定义矩形的4个顶点
         fan.points(vertices);   //计算矩形的4个顶点
         for (int i = 0; i < 4; i++)
             line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(255, 0, 0), 4);
     }
-    for (const auto &armor : armors)
+    for (const auto &armor : armors) {
-    {
         Point2f vertices[4];      //定义矩形的4个顶点
         armor.points(vertices);   //计算矩形的4个顶点
-        for (int i = 0; i < 4; i++){
+        for (int i = 0; i < 4; i++) {
-            if(pointDistance(static_cast<cv::Point2f>(armor.center),target_point) < 5)
+            if (pointDistance(static_cast<cv::Point2f>(armor.center), target_point) < 5)
                 line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(255, 255, 0), 4);
             else
                 line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(0, 0, 255), 4);
@@ -104,7 +98,6 @@ void Energy::showBothContours(std::string windows_name, const cv::Mat src) {
 }
 
 
-
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于显示图像中所有可能的风车中心候选区R
 // ---------------------------------------------------------------------------------------------------------------------
@@ -112,7 +105,7 @@ void Energy::showCenterRContours(std::string windows_name, const cv::Mat src) {
     if (src.empty())return;
     static Mat image2show;
 
-    if(src.type() == CV_8UC1) // 黑白图像
+    if (src.type() == CV_8UC1) // 黑白图像
     {
         cvtColor(src, image2show, COLOR_GRAY2RGB);
 
@@ -125,12 +118,13 @@ void Energy::showCenterRContours(std::string windows_name, const cv::Mat src) {
     centerR.points(vertices);   //计算矩形的4个顶点
     for (int i = 0; i < 4; i++)
         line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(255, 0, 255), 2);
+
+    cv::circle(image2show, circle_center_point, 4, cv::Scalar(0, 0, 255), 2);//在图像中画出特征点，2是圆的半径
+
     imshow(windows_name, image2show);
 }
 
 
-
-
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于显示图像中所有流动条
 // ---------------------------------------------------------------------------------------------------------------------
@@ -138,7 +132,7 @@ void Energy::showFlowStripFanContours(std::string windows_name, const cv::Mat sr
     if (src.empty())return;
     static Mat image2show;
 
-    if(src.type() == CV_8UC1) // 黑白图像
+    if (src.type() == CV_8UC1) // 黑白图像
     {
         cvtColor(src, image2show, COLOR_GRAY2RGB);
 
@@ -158,8 +152,8 @@ void Energy::showFlowStripFanContours(std::string windows_name, const cv::Mat sr
     for (int i = 0; i < 4; i++)
         line(image2show, strip_vertices[i], strip_vertices[(i + 1) % 4], Scalar(0, 255, 0), 2);
 
-    for (const auto &armor : armors){
+    for (const auto &armor : armors) {
-        if(pointDistance(armor.center, target_point) < energy_part_param_.TWIN_POINT_MAX){
+        if (pointDistance(armor.center, target_point) < energy_part_param_.TWIN_POINT_MAX) {
             Point2f vertices[4];      //定义矩形的4个顶点
             armor.points(vertices);   //计算矩形的4个顶点
             for (int i = 0; i < 4; i++)
@@ -175,7 +169,6 @@ void Energy::showFlowStripFanContours(std::string windows_name, const cv::Mat sr
 }
 
 
-
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于显示猜测的下一个目标点
 // ---------------------------------------------------------------------------------------------------------------------
@@ -183,7 +176,7 @@ void Energy::showGuessTarget(std::string windows_name, const cv::Mat src) {
     if (src.empty())return;
     static Mat image2show;
 
-    if(src.type() == CV_8UC1) // 黑白图像
+    if (src.type() == CV_8UC1) // 黑白图像
     {
         cvtColor(src, image2show, COLOR_GRAY2RGB);
 

energy/src/energy/tool/tool.cpp

@@ -27,7 +27,7 @@ void Energy::extract(cv::Mat &src) {
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于计算预测的击打点坐标
 // ---------------------------------------------------------------------------------------------------------------------
-void Energy::rotate() {
+void Energy::rotate(cv::Point target_point) {
     int x1, x2, y1, y2;
     //    为了减小强制转换的误差
     x1 = circle_center_point.x * 100;
@@ -94,8 +94,7 @@ int Energy::linePointX(const cv::Point2f &p1, const cv::Point2f &p2, int y) {
 //----------------------------------------------------------------------------------------------------------------------
 // 计算旋转矩形内的两点占比
 // ---------------------------------------------------------------------------------------------------------------------
-
+double Energy::nonZeroRateOfRotateRect(cv::Mat &bin, const cv::RotatedRect &rotatedRect) {
-double Energy::nonZeroRateOfRotateRect(const cv::Mat &bin, const cv::RotatedRect &rotatedRect) {
     int cnt = 0;
     cv::Point2f corners[4];
     rotatedRect.points(corners);

main.cpp

@@ -27,7 +27,7 @@ using namespace std;
 mcu_data mcuData = {    // 单片机端回传结构体
         0,              // 当前云台yaw角
         0,              // 当前云台pitch角
-        BIG_ENERGY_STATE,    // 当前状态，自瞄-大符-小符
+        SMALL_ENERGY_STATE,    // 当前状态，自瞄-大符-小符
         0,              // 云台角度标记位
         1,              // 是否启用数字识别
         ENEMY_RED,      // 敌方颜色
@@ -61,8 +61,8 @@ int main(int argc, char *argv[]) {
             video_gimble = new CameraWrapper(0/*, "armor"*/);
             video_chassis = new CameraWrapper(1/*, "energy"*/);
         } else {
-            video_gimble = new VideoWrapper("/home/sun/项目/RM2019 能量机关视频/大能量机关（红+开灯）.mov");
+            video_gimble = new VideoWrapper("/home/sun/项目/RM2019 能量机关视频/小能量机关（红+开灯）.mov");
-            video_chassis = new VideoWrapper("/home/sun/项目/RM2019 能量机关视频/大能量机关（红+开灯）.mov");
+            video_chassis = new VideoWrapper("/home/sun/项目/RM2019 能量机关视频/小能量机关（红+开灯）.mov");
         }
         if (video_gimble->init()) {
             LOGM("video_gimble source initialization successfully.");
@@ -100,10 +100,12 @@ int main(int argc, char *argv[]) {
                         cout << "set" << endl;
                     }
                     ok = checkReconnect(video_gimble->read(gimble_src), video_chassis->read(chassis_src));//检查有几个摄像头
-                    resize(gimble_src,gimble_src,cv::Size(853,480));
+
-                    resize(chassis_src,chassis_src,cv::Size(853,480));
+//                    resize(gimble_src,gimble_src,cv::Size(853,480));
-                    gimble_src = gimble_src(Rect(106, 0, 640, 480));
+//                    resize(chassis_src,chassis_src,cv::Size(853,480));
-                    chassis_src = chassis_src(Rect(106, 0, 640, 480));
+//                    gimble_src = gimble_src(Rect(106, 0, 640, 480));
+//                    chassis_src = chassis_src(Rect(106, 0, 640, 480));
+
                     if (save_video) saveVideos(gimble_src, chassis_src);//保存视频
                     if (show_origin) showOrigin(gimble_src, chassis_src);//显示原始图像
                     energy.runBig(gimble_src, chassis_src);//击打大符
@@ -115,9 +117,13 @@ int main(int argc, char *argv[]) {
                     }
                     last_state = mcuData.state;
                     ok = checkReconnect(video_gimble->read(gimble_src));
+
+//                    resize(gimble_src,gimble_src,cv::Size(853,480));
+//                    gimble_src = gimble_src(Rect(106, 0, 640, 480));
+
                     if (save_video) saveVideos(gimble_src);
                     if (show_origin) showOrigin(gimble_src);
-//                    energy.runSmall(gimble_src);
+                    energy.runSmall(gimble_src);
                 } else {                                         // 自瞄模式
                     if(last_state != ARMOR_STATE){
                         ((CameraWrapper*)video_gimble)->changeBrightness(40);