修改求5个点轨迹的切线方向的方法

TrackEx/TrackFunc.cpp

@@ -23,39 +23,28 @@
 
 float correct_angle_(std::vector<std::vector<float>>& 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++)
+    if (points.size() < 5) 
     {
-        X(i, 0) = points[i][0];
-        X(i, 1) = points[i][1];
+        return 0;
+    }
+    std::vector<std::vector<float>> 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][0];
+        X(i, 1) = recent_points[i][1];
+    }
+    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][0] - recent_points[0][0], recent_points[4][1] - recent_points[0][1]);
+    Eigen::Vector2d tangent_dir(eigvecs(0, 1), eigvecs(1, 1));
+    double alpha = move_dir.dot(tangent_dir);
+    if (alpha < 0) {
+        tangent_dir *= -1;
     }
-    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();
-    //SDK_LOG(SDK_INFO, "vec = [%f,%f,%f,%f]", vec(0, 0), vec(0, 1), vec(1, 0), vec(1, 1));
-    //SDK_LOG(SDK_INFO, "val = [%f,%f]", val(0), val(1));
-    // 打印
-    //cout << "X_copy: " << endl << X_copy
-    Eigen::Vector2d dX(points[4][0] - points[0][0], points[4][1] - points[0][1]);
-    Eigen::Vector2d orientation(vec(1, 0), vec(1, 1));
-    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]);
-    float center_rot_y = atan2(orientation[1], orientation[0]);
-    return center_rot_y;
+    double angle = atan2(tangent_dir(1), tangent_dir(0));
+    return angle;
 }
\ No newline at end of file

TrackingRosAbs.cpp

@@ -693,8 +693,8 @@ void TrackingRos::ThreadTrackingProcessEx()
                     //     obj.obj_id,obj.v_x,obj.v_y,obj.v_z,obj.loc_x,obj.loc_y,obj.loc_z,obj.Long,obj.Lat,
                     //     objsPtr->array[updateId[iter.first]].x,objsPtr->array[updateId[iter.first]].y,objsPtr->array[updateId[iter.first]].z,objsPtr->array[updateId[iter.first]].rot_y);
                     if(obj.type != 10)
-                        SDK_IDX_LOG(1, SDK_INFO, "%d,%d,%f,%f,%f,%f,%f,%f,%f,%f,%d,%s",
-                            obj.obj_id,m_frameNum,obj.x,obj.y,obj.z,obj.l,obj.w,obj.h,rot_y_angle,obj.score,obj.type,obj.name.c_str());
+                        SDK_IDX_LOG(1, SDK_INFO, "%d,%d,%f,%f,%f,%f,%f,%f,%f,%f,%d,%s,%f,%f",
+                            obj.obj_id,m_frameNum,obj.x,obj.y,obj.z,obj.l,obj.w,obj.h,rot_y_angle,obj.score,obj.type,obj.name.c_str(),obj.v_x,obj.v_y);
 
                     static float UTMX = 0;
                     static float UTMY = 0;