修改计算方向的逻辑

BaseTracker/Track3D.cpp

@@ -340,6 +340,7 @@ void Track3D::UpdateDataCheck(const std::vector<float>& data, std::vector<float>
     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))
     {
         double center_rot_y = correct_angle(m_points);
+        m_center_rot_y = center_rot_y;
         //SDK_LOG(SDK_INFO, "center_rot_y = %f,rot_y = %f", center_rot_y,rot_y);
         double rotate = abs(center_rot_y - rot_y);
         while (rotate > _PI_)
@@ -351,10 +352,16 @@ void Track3D::UpdateDataCheck(const std::vector<float>& data, std::vector<float>
             rot_y += _PI_;
         }
     }
-    else 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))
+    else if (dataSource > 1 && m_points.size() >= 5 )
     {
-        double center_rot_y = correct_angle(m_points);
-        rot_y = center_rot_y;
+        if ((abs(m_points[4].x - m_points[0].x) > DIST_THRED || abs(m_points[4].y - m_points[0].y) > DIST_THRED))//5个点首尾有距离差才计算方向
+        {
+            double center_rot_y = correct_angle(m_points);
+            m_center_rot_y = center_rot_y;
+            rot_y = center_rot_y;
+        }
+        else//没有距离差就使用最后一次的方向
+            rot_y = center_rot_y;
     }
     double x_angle = kf_->x_[3];
     double deta = rot_y - x_angle;

BaseTracker/Track3D.h

@@ -53,6 +53,8 @@ public:
     std::vector<point2d> m_points;//保存最近的5个移动的点位
 
     uint64_t m_lastLastTimestamp = 0;//记录上上一次的时间戳
+
+    double m_center_rot_y = 0;//通过计算轨迹上的五个点算出来的方向
 };
 
 double correct_angle(std::vector<point2d>& points);