提交更新

BaseTracker/Track3D.cpp

@@ -219,19 +219,16 @@ int Track3D::GetKFDataOrder(std::vector<int>& order)
 
 const float ANGLE_CHANGE_MAX = 30 / 180 * _PI_;
 
-double correct_angle(std::queue<point2d>& points)
+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)
-    std::queue<point2d> tmppoints = points;
     for (int i = 0; i < 5; i++)
     {
-        point2d pos = tmppoints.front();
-        X(i, 0) = pos.x;
-        X(i, 1) = pos.y;
-        tmppoints.pop();
+        X(i, 0) = points[i].x;
+        X(i, 1) = points[y].y;
     }
     Eigen::MatrixXd X_copy = X; // 拷贝副本，因为X取均值后会被改变
     Eigen::MatrixXd C(2, 2); // 方差
@@ -249,7 +246,13 @@ double correct_angle(std::queue<point2d>& points)
     val = eig.eigenvalues();
     // 打印
     //cout << "X_copy: " << endl << X_copy
-    return 0;
+    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)