修改方向的修正，如果已经有轨迹了，那么就按照轨迹的方向为真正朝向，否则使用偏差最小的朝向。

BaseTracker/Track3D.cpp

@@ -349,6 +349,7 @@ void Track3D::UpdateDataCheck(const std::vector<float>& data, std::vector<float>
     std::vector<float> tmp(data.begin() + 1, data.begin() + 8);
     double rot_y = tmp[3];
     int dataSource = data[8];
+    int isCheck = 0;//是否检查了方向
     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);
@@ -363,6 +364,7 @@ void Track3D::UpdateDataCheck(const std::vector<float>& data, std::vector<float>
         {
             rot_y += _PI_;
         }
+        isCheck = 1;
     }
     else if (dataSource > 1 && m_points.size() >= 5 )
     {
@@ -371,31 +373,49 @@ void Track3D::UpdateDataCheck(const std::vector<float>& data, std::vector<float>
             double center_rot_y = correct_angle(m_points);
             m_center_rot_y = center_rot_y;
             rot_y = center_rot_y;
+            isCheck = 1;
         }
         else//没有距离差就使用最后一次的方向
             rot_y = m_center_rot_y;
     }
-    double x_angle = kf_->x_[3];
-    double deta = rot_y - x_angle;
-    double deta2 = rot_y + _PI_ - x_angle;
-    while (deta >= _PI_)
+    if(isCheck == 0)//如果没有检查方向，那么就使用偏差最小的作为方向
     {
-        deta -= _PI_ * 2;
-    }
-    while (deta < -_PI_)
-    {
-        deta += _PI_ * 2;
-    }
-    while (deta2 >= _PI_)
-    {
-        deta2 -= _PI_ * 2;
+        double x_angle = kf_->x_[3];
+        double deta = rot_y - x_angle;
+        double deta2 = rot_y + _PI_ - x_angle;
+        while (deta >= _PI_)
+        {
+            deta -= _PI_ * 2;
+        }
+        while (deta < -_PI_)
+        {
+            deta += _PI_ * 2;
+        }
+        while (deta2 >= _PI_)
+        {
+            deta2 -= _PI_ * 2;
+        }
+        while (deta2 < -_PI_)
+        {
+            deta2 += _PI_ * 2;
+        }
+        float detaT = abs(deta) > abs(deta2) ? deta2:deta;
+        rot_y = x_angle + detaT;
     }
-    while (deta2 < -_PI_)
+    else //已经检查了方向，那么就直接使用这个方向
     {
-        deta2 += _PI_ * 2;
+        double x_angle = kf_->x_[3];
+        double deta = rot_y - x_angle;
+        while (deta >= _PI_)
+        {
+            deta -= _PI_ * 2;
+        }
+        while (deta < -_PI_)
+        {
+            deta += _PI_ * 2;
+        }
+        rot_y = x_angle + deta;
     }
-    float detaT = abs(deta) > abs(deta2) ? deta2:deta;
-    rot_y = x_angle + detaT;
     out = tmp;
     out[3] = rot_y;