提交测试

BaseTracker/Track3D.cpp

@@ -9,6 +9,43 @@
 
 #define DIST_THRED  0.5
 
+double correct_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();
+    // 打印
+    //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(0, 0), vec(0, 1));
+    double alpha = dX.transpose() * orientation;
+    if (alpha < 0)
+        orientation = -orientation;
+    double center_rot_y = atan2(orientation[1], orientation[0]);
+    return center_rot_y;
+}
+
+
 Track3D::Track3D():BaseTrack(10, 7)
 {
     std::shared_ptr<mytracker::KalmanFilter> kf = std::make_shared<mytracker::KalmanFilter>(10, 7);
@@ -65,6 +102,15 @@ Track3D::Track3D():BaseTrack(10, 7)
     kf_ = kf;
 
     m_iou_threshold = 0.01;
+
+    std::vector<point2d> tests;
+    tests.push_back(point2d(1, 1));
+    tests.push_back(point2d(2, 2));
+    tests.push_back(point2d(3, 3));
+    tests.push_back(point2d(4, 4));
+    tests.push_back(point2d(5,5));
+    double angle = correct_angle(tests);
+    SDK_LOG(SDK_INFO, "test correct_angle = %f",angle);
 }
 void Track3D::Init(const std::vector<float>& data)
 {
@@ -245,41 +291,7 @@ int Track3D::GetKFDataOrder(std::vector<int>& order)
 
 const float ANGLE_CHANGE_MAX = 30 / 180 * _PI_;
 
-double correct_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();
-    // 打印
-    //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(0,0), vec(0,1));
-    double alpha = dX.transpose()* orientation;
-    if (alpha < 0)
-        orientation = -orientation;
-    double center_rot_y = atan2(orientation[1], orientation[0]);
-    return center_rot_y;
-}
+
 
 void Track3D::UpdateDataCheck(const std::vector<float>& data, std::vector<float>& out)
 {