更新了计算5个点的运动方向的函数

BaseTracker/Track3D.cpp

@@ -9,6 +9,33 @@
 
 #define DIST_THRED  0.5
 
+double calc_tangent_direction(const std::vector<point2d>& points) {
+    if (points.size() < 5) {
+        std::cerr << "Error: The number of points is less than 5." << std::endl;
+        return 0;
+    }
+    std::vector<point2d> recent_points(points.end()- 5, points.end());
+    Eigen::MatrixXd X(5, 2);
+    for (int i = 0; i < 5; i++) {
+        X(i, 0) = recent_points[i].x;
+        X(i, 1) = recent_points[i].y;
+    }
+    Eigen::RowVectorXd meanvec = X.colwise().mean();
+    X.rowwise() -= meanvec;
+    Eigen::MatrixXd covar = X.transpose() * X / (X.rows() - 1);
+    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eig(covar);
+    Eigen::MatrixXd eigvecs = eig.eigenvectors();
+    Eigen::MatrixXd eigvals = eig.eigenvalues();
+    Eigen::Vector2d move_dir(recent_points[4].x - recent_points[0].x, recent_points[4].y - recent_points[0].y);
+    Eigen::Vector2d tangent_dir(eigvecs(0, 1), eigvecs(1, 1));
+    double alpha = move_dir.dot(tangent_dir);
+    if (alpha < 0) {
+        tangent_dir *= -1;
+    }
+    double angle = atan2(tangent_dir(1), tangent_dir(0));
+    return angle;
+}
+
 double correct_angle(std::vector<point2d>& points)
 {
     if (points.size() < 5)
@@ -43,7 +70,7 @@ double correct_angle(std::vector<point2d>& points)
     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]);
+    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;
 }
@@ -355,9 +382,9 @@ void Track3D::UpdateDataCheck(const std::vector<float>& data, std::vector<float>
     int dataSource = data[8];
     if (dataSource == 1 && m_points.size() >= 5 && (abs(m_points[4].x - m_points[0].x) > DIST_THRED || abs(m_points[4].y - m_points[0].y) > DIST_THRED))
     {
-        double center_rot_y = correct_angle(m_points);
+        double center_rot_y = calc_tangent_direction(m_points);
         m_center_rot_y = center_rot_y;
-        //SDK_LOG(SDK_INFO, "center_rot_y = %f,rot_y = %f", center_rot_y,rot_y);
+        SDK_LOG(SDK_INFO, "center_rot_y = %f,rot_y = %f", center_rot_y,rot_y);
         double rotate = abs(center_rot_y - rot_y);
         while (rotate > _PI_)
             rotate -= 2 * _PI_;
@@ -372,7 +399,7 @@ void Track3D::UpdateDataCheck(const std::vector<float>& data, std::vector<float>
     {
         if ((abs(m_points[4].x - m_points[0].x) > DIST_THRED || abs(m_points[4].y - m_points[0].y) > DIST_THRED))//5个点首尾有距离差才计算方向
         {
-            double center_rot_y = correct_angle(m_points);
+            double center_rot_y = calc_tangent_direction(m_points);
             m_center_rot_y = center_rot_y;
             rot_y = center_rot_y;
         }