提交计算角度的通用函数。

common/Component.cpp

@@ -2,6 +2,8 @@
 #include "Component.h"
 #include <sys/time.h>
 #include <unistd.h>
+#include <eigen3/Eigen/Dense>
+#include <algorithm>
 
 #ifndef _IGNORE_GETCURTIME_
 uint64_t GetCurTime()
@@ -115,3 +117,42 @@ float calcIntersectionRate(cv::RotatedRect rect1, cv::RotatedRect rect2)
     float iou_2d = area / (rect1.size.width * rect1.size.height + rect2.size.width * rect2.size.height);
     return iou_2d;
 }
+
+double calculate_angle(std::vector<point2d>& points)
+{
+    if (points.size() < 5)
+        return 0;
+    Eigen::MatrixXd X(5, 2); // 输入直线 y=x 上的三个点 
+    // 坐标 (1, 1), (2, 2), (3, 3)
+    for (int i = 0; i < 5; i++)
+    {
+        X(i, 0) = points[i].x;
+        X(i, 1) = points[i].y;
+    }
+    Eigen::MatrixXd X_copy = X; // 拷贝副本，因为X取均值后会被改变
+    Eigen::MatrixXd C(2, 2); // 方差
+    Eigen::MatrixXd vec, val;
+    // 坐标去均值
+    Eigen::RowVectorXd meanvecRow = X.colwise().mean(); // 求每一列的均值，相当于在向量的每个维度上求均值
+    X.rowwise() -= meanvecRow; // 样本减去各自维度的均值
+    //计算协方差矩阵C = X^t*X / (n-1); 
+    // 其中，n是一个向量的维度，这里向量只有x,y2个维度
+    C = X.adjoint() * X;
+    C = C.array() / (X.rows() - 1);
+    // 计算特征值和特征向量
+    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eig(C); // 产生的vec和val按照特征值升序排列
+    vec = eig.eigenvectors();
+    val = eig.eigenvalues();
+    //SDK_LOG(SDK_INFO, "vec = [%f,%f,%f,%f]", vec(0, 0), vec(0, 1), vec(1, 0), vec(1, 1));
+    //SDK_LOG(SDK_INFO, "val = [%f,%f]", val(0), val(1));
+    // 打印
+    //cout << "X_copy: " << endl << X_copy
+    Eigen::Vector2d dX(points[4].x - points[0].x, points[4].y - points[0].y);
+    Eigen::Vector2d orientation(vec(1, 0), vec(1, 1));
+    double alpha = dX.transpose() * orientation;
+    if (alpha < 0)
+        orientation = -orientation;
+    //SDK_LOG(SDK_INFO, "correct_angle alpha = %f, orientation[0] = %f, orientation[1] = %f", alpha, orientation[0], orientation[1]);
+    double center_rot_y = atan2(orientation[1], orientation[0]);
+    return center_rot_y;
+}
\ No newline at end of file

common/Component.h

@@ -19,3 +19,5 @@ std::string GetMatrixStr(float** data_ptr, int col, int row);
 double calcIntersectionArea(cv::RotatedRect rect1, cv::RotatedRect rect2);
 
 float calcIntersectionRate(cv::RotatedRect rect1, cv::RotatedRect rect2);
+
+double calculate_angle(std::vector<point2d>& points)