提交更新

BaseTracker/Track3D.cpp

@@ -219,17 +219,19 @@ int Track3D::GetKFDataOrder(std::vector<int>& order)
 
 const float ANGLE_CHANGE_MAX = 30 / 180 * _PI_;
 
-double correct_angle(std::queue<point2d>& m_points)
+double correct_angle(std::queue<point2d>& points)
 {
     if (m_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 = m_points.pop();
+        point2d pos = tmppoints.front();
         X(i, 0) = pos.x;
         X(i, 1) = pos.y;
+        tmppoints.pop();
     }
     Eigen::MatrixXd X_copy = X; // 拷贝副本，因为X取均值后会被改变
     Eigen::MatrixXd C(2, 2); // 方差
@@ -242,7 +244,7 @@ double correct_angle(std::queue<point2d>& m_points)
     C = X.adjoint() * X;
     C = C.array() / (X.rows() - 1);
     // 计算特征值和特征向量
-    SelfAdjointEigenSolver<MatrixXd> eig(C); // 产生的vec和val按照特征值升序排列
+    Eigen::SelfAdjointEigenSolver<MatrixXd> eig(C); // 产生的vec和val按照特征值升序排列
     vec = eig.eigenvectors();
     val = eig.eigenvalues();
     // 打印