提交更新

BaseTracker/Track3D.cpp

@@ -219,6 +219,37 @@ int Track3D::GetKFDataOrder(std::vector<int>& order)
 
 const float ANGLE_CHANGE_MAX = 30 / 180 * _PI_;
 
+double correct_angle(std::queue<point2d>& m_points)
+{
+    if (m_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++)
+    {
+        point2d pos = m_points.pop();
+        X(i, 0) = pos.x;
+        X(i, 1) = pos.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);
+    // 计算特征值和特征向量
+    SelfAdjointEigenSolver<MatrixXd> eig(C); // 产生的vec和val按照特征值升序排列
+    vec = eig.eigenvectors();
+    val = eig.eigenvalues();
+    // 打印
+    //cout << "X_copy: " << endl << X_copy
+    return 0;
+}
+
 void Track3D::UpdateDataCheck(const std::vector<float>& data, std::vector<float>& out)
 {
     if (kf_ == nullptr)

BaseTracker/Track3D.h

 #pragma once
 
 #include <vector>
+#include <queue>
 #include "BaseTrack.h"
 #include "kalman_filter.h"
 #ifdef _QICHECHENG_
@@ -12,6 +13,12 @@
 
 using trackOjbPtr = std::shared_ptr< jfx_common_msgs::det_tracking>;
 
+struct point2d
+{
+    double x;
+    double y;
+};
+
 class Track3D : public BaseTrack
 {
 public:
@@ -27,4 +34,6 @@ public:
     virtual void UpdateDataCheck(const std::vector<float>& data, std::vector<float>& out);//对于输入数据进行修正
 
     trackOjbPtr m_obj = nullptr;
+
+    std::queue<point2d> m_points;//保存最近的5个移动的点位
 };