energy changed

energy/src/energy/calculate/circle_calculate.cpp

@@ -0,0 +1,63 @@
+//
+// Created by xixiliadorabarry on 1/24/19.
+//
+#include "energy/energy.h"
+
+using namespace cv;
+using std::cout;
+using std::endl;
+using std::vector;
+
+
+
+
+//----------------------------------------------------------------------------------------------------------------------
+// 此函数通过最小二乘法计算大风车圆心和半径
+// ---------------------------------------------------------------------------------------------------------------------
+void Energy::circleLeastFit()
+{
+    circle_center_point.x = 0;
+    circle_center_point.y = 0;
+    radius = 0.0f;
+    if (all_armor_centers.size() < 3)
+    {
+//        cout<<"Cannot calculate a circle"<<endl;
+        return;
+    }
+    double sum_x = 0.0f, sum_y = 0.0f;
+    double sum_x2 = 0.0f, sum_y2 = 0.0f;
+    double sum_x3 = 0.0f, sum_y3 = 0.0f;
+    double sum_xy = 0.0f, sum_x1y2 = 0.0f, sum_x2y1 = 0.0f;
+    int N = static_cast<int>(all_armor_centers.size());
+    for (int i = 0; i < N; i++)
+    {
+        double x = all_armor_centers.at(i).x;
+        double y = all_armor_centers.at(i).y;
+        double x2 = x * x;
+        double y2 = y * y;
+        sum_x += x;
+        sum_y += y;
+        sum_x2 += x2;
+        sum_y2 += y2;
+        sum_x3 += x2 * x;
+        sum_y3 += y2 * y;
+        sum_xy += x * y;
+        sum_x1y2 += x * y2;
+        sum_x2y1 += x2 * y;
+    }
+    double C, D, E, G, H;
+    double a, b, c;
+    C = N * sum_x2 - sum_x * sum_x;
+    D = N * sum_xy - sum_x * sum_y;
+    E = N * sum_x3 + N * sum_x1y2 - (sum_x2 + sum_y2) * sum_x;
+    G = N * sum_y2 - sum_y * sum_y;
+    H = N * sum_x2y1 + N * sum_y3 - (sum_x2 + sum_y2) * sum_y;
+    a = (H * D - E * G) / (C * G - D * D);
+    b = (H * C - E * D) / (D * D - G * C);
+    c = -(a * sum_x + b * sum_y + sum_x2 + sum_y2) / N;
+    circle_center_point.x = static_cast<int>(a / (-2));
+    circle_center_point.y = static_cast<int>(b / (-2));
+    radius = sqrt(a * a + b * b - 4 * c) / 2;
+//    cout << "The cycle center is: " << cycle_center << endl;
+//    cout << "The radius is: " << radius << endl;
+}

energy/src/energy/get/polar_angle_get.cpp

@@ -0,0 +1,57 @@
+//
+// Created by xixiliadorabarry on 1/24/19.
+//
+#include "energy/energy.h"
+
+using namespace cv;
+using std::cout;
+using std::endl;
+using std::vector;
+
+
+
+
+//----------------------------------------------------------------------------------------------------------------------
+// 此函数用于获得图像中所有扇叶的当前极坐标角度
+// ---------------------------------------------------------------------------------------------------------------------
+void Energy::getFanPolarAngle() {
+	if (radius == 0)return;
+	for (const auto &fan : fans)
+	{
+		float angle = static_cast<float>(180 / PI * atan2(-1 * (fan.rect.center.y - circle_center_point.y),
+		        (fan.rect.center.x - circle_center_point.x)));
+		fan_polar_angle.push_back(angle);
+	}
+//	cout << "fanPosition.size() = " << fanPosition.size() << '\t' << endl;
+}
+
+
+
+
+//----------------------------------------------------------------------------------------------------------------------
+// 此函数用于获得图像中所有装甲板的当前极坐标角度
+// ---------------------------------------------------------------------------------------------------------------------
+void Energy::getArmorPolarAngle() {
+	if (radius == 0)return;
+	for (const auto &armor : armors)
+	{
+		float angle = static_cast<float>(180 / PI * atan2(-1 * (armor.rect.center.y - circle_center_point.y),
+		        (armor.rect.center.x - circle_center_point.x)));
+		armor_polar_angle.push_back(angle);
+
+	}
+//	cout << "armorPosition.size() = " << armorPosition.size() << '\t' << endl;
+}
+
+
+
+
+//----------------------------------------------------------------------------------------------------------------------
+// 此函数用于存储图像中所有装甲板的中心坐标，以便后续最小二乘法计算圆心和半径
+// ---------------------------------------------------------------------------------------------------------------------
+void Energy::getAllArmorCenters()
+{
+	for (const auto &armor : armors) {
+		all_armor_centers.push_back(armor.rect.center);
+	}
+}

energy/src/energy/get/predict_point_get.cpp

@@ -0,0 +1,37 @@
+//
+// Created by xixiliadorabarry on 1/24/19.
+//
+
+#include "energy/energy.h"
+#include "energy/constant.h"
+
+using namespace cv;
+using std::cout;
+using std::endl;
+using std::vector;
+
+
+
+//----------------------------------------------------------------------------------------------------------------------
+// 此函数获取预测点坐标
+// ---------------------------------------------------------------------------------------------------------------------
+void Energy::getPredictPoint(){
+    if(energy_rotation_direction==1) rotate();
+    if(energy_rotation_direction==-1) rotate();
+}
+
+
+
+//----------------------------------------------------------------------------------------------------------------------
+// 此函数用于操作手手动标定
+// ---------------------------------------------------------------------------------------------------------------------
+bool Energy::changeTarget(){
+    if(fabs(target_polar_angle - last_target_polar_angle) < 30||fabs(target_polar_angle - last_target_polar_angle) > 330){
+        last_target_polar_angle = target_polar_angle;
+        return false;
+    }
+    else{
+        last_target_polar_angle = target_polar_angle;
+        return true;
+    }
+}