修改检测轨迹阈值

BaseTracker/Track3D.cpp

@@ -180,6 +180,8 @@ void Track3D::Init(const std::vector<float>& data)
         tmp[3] -= 2 * _PI_;
     while (tmp[3] < -_PI_)
         tmp[3] += 2 * _PI_;
+    m_startPoint.x = tmp[0];
+    m_startPoint.y = tmp[1];
     BaseTrack::Init(tmp);
 }
 
@@ -418,24 +420,27 @@ 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];
+    float length_start = sqrt(pow(m_startPoint.x - tmp[0],2) + pow(m_startPoint.y - tmp[1],2));
     if(m_obj && m_points.size() > 1)
     {
         float speed = sqrt(pow(kf_->x_[7],2) + pow(kf_->x_[8],2));
         const int speed_cout_max = 5;
         if(speed > 0.1 && m_last_speed > 0.1)
+        {
             m_dist_thred = DIST_THRED;
+            m_speed_change = 0;
+        }
         else
         {
-            if(speed > m_last_speed && fabs(speed - m_last_speed) > 0.01)
+            if(speed < 0.1)
             {
-                m_speed_change = m_speed_change < speed_cout_max ? m_speed_change+1 : speed_cout_max;
-                m_dist_thred -= 0.1*m_speed_change;
+                m_speed_change = m_speed_change < speed_cout_max ? m_speed_change + 1: speed_cout_max;
             }
-            else if( speed < m_last_speed && fabs(speed - m_last_speed) > 0.01)
+            else
             {
-                m_speed_change = m_speed_change > -speed_cout_max ? m_speed_change - 1 : -speed_cout_max;
-                m_dist_thred -= 0.1*m_speed_change;
+                m_speed_change = m_speed_change > 0 ? m_speed_change -1 : 0;
             }
+            m_dist_thred += 0.3*m_speed_change;
             if(m_dist_thred < DIST_THRED)
                 m_dist_thred = DIST_THRED;
             else if(m_dist_thred > 2.0f)
@@ -451,7 +456,7 @@ void Track3D::UpdateDataCheck(const std::vector<float>& data, std::vector<float>
             );        
         m_last_speed = speed;
     }
-    if (dataSource == 1 && m_points.size() >= 5 && IsClosePoint(m_points,m_dist_thred) == 0)
+    if (dataSource == 1 && m_points.size() >= 5 && length_start > 4.0f && IsClosePoint(m_points,m_dist_thred) == 0)
     {
         double center_rot_y = calc_tangent_direction(m_points);
         m_center_rot_y = center_rot_y;

BaseTracker/Track3D.h

@@ -87,6 +87,8 @@ public:
     float m_dist_thred = 2.0f;
     float m_last_speed = 0.0f;
     int m_speed_change = 0;
+
+    point2d m_startPoint;
 };
 
 double correct_angle(std::vector<point2d>& points);