提交检测5个点是运动点的方法。

BaseTracker/Track3D.cpp

@@ -7,7 +7,7 @@
 #include <opencv2/opencv.hpp>
 #include <algorithm>
 
-#define DIST_THRED  0.5
+#define DIST_THRED  1
 
 double calc_tangent_direction(const std::vector<point2d>& points) {
     if (points.size() < 5) {
@@ -75,6 +75,28 @@ double correct_angle(std::vector<point2d>& points)
     return center_rot_y;
 }
 
+int IsClosePoint(std::vector<point2d>& points)
+{
+    if (points.size() != 5) 
+        return 1;
+    float lenAllX = fabs(points[0].x - points[1].x) + fabs(points[0].x - points[2].x) + fabs(points[0].x - points[3].x) + fabs(points[0].x - points[4].x);
+    float lenX = fabs(points[0].x - points[4].x);
+    float lenAllY = fabs(points[0].y - points[1].y) + fabs(points[0].y - points[2].y) + fabs(points[0].y - points[3].y) + fabs(points[0].y - points[4].y);
+    float lenY = fabs(points[0].y - points[4].y);
+    if(lenX > DIST_THRED)
+    {
+        float rateX = lenAllX/lenX;
+        if(rateX > 1.75 && rateX < 2.25)
+            return 0;
+    }
+    if(lenY > DIST_THRED)
+    {
+        float rateY = lenAllY/lenY;
+        if(rateY > 1.75 && rateY < 2.25)
+            return 0;
+    }
+    return 1;
+}
 
 Track3D::Track3D():BaseTrack(10, 7)
 {
@@ -380,7 +402,7 @@ void Track3D::UpdateDataCheck(const std::vector<float>& data, std::vector<float>
     double rot_y = tmp[3];
     double rot_y_detect = rot_y;//检测到的朝向
     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))
+    if (dataSource == 1 && m_points.size() >= 5 && IsClosePoint(m_points) == 0)
     {
         double center_rot_y = calc_tangent_direction(m_points);
         m_center_rot_y = center_rot_y;
@@ -397,7 +419,7 @@ void Track3D::UpdateDataCheck(const std::vector<float>& data, std::vector<float>
     }
     else if (dataSource > 1 && m_points.size() >= 5 )
     {
-        if ((abs(m_points[4].x - m_points[0].x) > DIST_THRED || abs(m_points[4].y - m_points[0].y) > DIST_THRED))//5个点首尾有距离差才计算方向
+        if (IsClosePoint(m_points) == 0)//5个点首尾有距离差才计算方向
         {
             double center_rot_y = calc_tangent_direction(m_points);
             m_center_rot_y = center_rot_y;