加入速度小的时候轨迹计算距离加大

BaseTracker/Track3D.cpp

@@ -75,27 +75,43 @@ double correct_angle(std::vector<point2d>& points)
     return center_rot_y;
 }
 
-int IsClosePoint(std::vector<point2d>& points)
+int IsClosePoint(std::vector<point2d>& points, float dist_thred)
 {
     if (points.size() != 5) 
         return 1;
-    float len01 = sqrt(pow(points[0].x - points[1].x,2) + pow(points[0].y - points[1].y,2));
-    float len02 = sqrt(pow(points[0].x - points[2].x,2) + pow(points[0].y - points[2].y,2));
-    float len03 = sqrt(pow(points[0].x - points[3].x,2) + pow(points[0].y - points[3].y,2));
-    float len04 = sqrt(pow(points[0].x - points[4].x,2) + pow(points[0].y - points[4].y,2));
-    float lenAll = len01 + len02 + len03 + len04;
     int isFar = 0;
-    if(len04 > DIST_THRED)
-    {
-        float rateX = lenAll/len04;
-        if(rateX > 1.5 && rateX < 2.5)
-            isFar = 1;
-    }
+    int sortX = 0;
+    if(points[0].x > points[4].x && points[0].x >= points[1].x && points[1].x >= points[2].x && points[3].x >= points[4].x)
+        sortX = 1;
+    else if(points[0].x < points[4].x && points[0].x <= points[1].x && points[1].x <= points[2].x && points[3].x <= points[4].x)
+        sortX = 1;
+    int sortY = 0;
+    if(points[0].y > points[4].y && points[0].y >= points[1].y && points[1].y >= points[2].y && points[3].y >= points[4].y)
+        sortY = 1;
+    else if(points[0].y < points[4].y && points[0].y <= points[1].y && points[1].y <= points[2].y && points[3].y <= points[4].y)
+        sortY = 1;
+    if(sortX == 1 || sortY == 1)
+        isFar = 1; 
     if(isFar == 1)
     {
-        return 0;
+        float len01 = sqrt(pow(points[0].x - points[1].x,2) + pow(points[0].y - points[1].y,2));
+        float len02 = sqrt(pow(points[0].x - points[2].x,2) + pow(points[0].y - points[2].y,2));
+        float len03 = sqrt(pow(points[0].x - points[3].x,2) + pow(points[0].y - points[3].y,2));
+        float len04 = sqrt(pow(points[0].x - points[4].x,2) + pow(points[0].y - points[4].y,2));
+        float lenAll = len01 + len02 + len03 + len04;
+        if(len04 > dist_thred)
+        {
+            float rateX = lenAll/len04;
+            if(rateX > 1.75 && rateX < 3.25)
+                return 0;
+            else
+                return 4;
+        }
+        else
+            return 3;
     }
-    return 1;
+    else
+        return 2;
 }
 
 Track3D::Track3D():BaseTrack(10, 7)
@@ -402,7 +418,40 @@ 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 && IsClosePoint(m_points) == 0)
+    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;
+        else
+        {
+            if(speed > m_last_speed && fabs(speed - m_last_speed) > 0.01)
+            {
+                m_speed_change = m_speed_change < speed_cout_max ? m_speed_change+1 : speed_cout_max;
+                m_dist_thred -= 0.1*m_speed_change;
+            }
+            else if( speed < m_last_speed && fabs(speed - m_last_speed) > 0.01)
+            {
+                m_speed_change = m_speed_change > -speed_cout_max ? m_speed_change - 1 : -speed_cout_max;
+                m_dist_thred -= 0.1*m_speed_change;
+            }
+            if(m_dist_thred < DIST_THRED)
+                m_dist_thred = DIST_THRED;
+            else if(m_dist_thred > 2.0f)
+                m_dist_thred = 2.0f;
+        }
+        SDK_LOG(SDK_INFO, "IsClosePoint objId = %d, m_last_speed = %f, speed = %f,m_speed_change = %d, m_dist_thred = %f, IsClosePoint = %d,points = [%f,%f][%f,%f][%f.%f][%f,%f][%f,%f]", 
+            m_obj->obj_id,m_last_speed,speed,m_speed_change,m_dist_thred,IsClosePoint(m_points,m_dist_thred),
+            m_points.size() > 0 ? m_points[0].x : 0,m_points.size() > 0 ? m_points[0].y : 0,
+            m_points.size() > 1 ? m_points[1].x : 0,m_points.size() > 0 ? m_points[1].y : 0,
+            m_points.size() > 2 ? m_points[2].x : 0,m_points.size() > 0 ? m_points[2].y : 0,
+            m_points.size() > 3 ? m_points[3].x : 0,m_points.size() > 0 ? m_points[3].y : 0,
+            m_points.size() > 4 ? m_points[4].x : 0,m_points.size() > 0 ? m_points[4].y : 0
+            );        
+        m_last_speed = speed;
+    }
+    if (dataSource == 1 && m_points.size() >= 5 && IsClosePoint(m_points,m_dist_thred) == 0)
     {
         double center_rot_y = calc_tangent_direction(m_points);
         m_center_rot_y = center_rot_y;
@@ -419,7 +468,7 @@ void Track3D::UpdateDataCheck(const std::vector<float>& data, std::vector<float>
     }
     else if (dataSource > 1 && m_points.size() >= 5 )
     {
-        if (IsClosePoint(m_points) == 0)//5个点首尾有距离差才计算方向
+        if (IsClosePoint(m_points,m_dist_thred) == 0)//5个点首尾有距离差才计算方向
         {
             double center_rot_y = calc_tangent_direction(m_points);
             m_center_rot_y = center_rot_y;

BaseTracker/Track3D.h

@@ -83,6 +83,10 @@ public:
         std::string name;
     };
     std::map<int,TypeNameInfo> m_typeNameList;
+
+    float m_dist_thred = 2.0f;
+    float m_last_speed = 0.0f;
+    int m_speed_change = 0;
 };
 
 double correct_angle(std::vector<point2d>& points);