提交角度修正

BaseTracker/Track3D.cpp

@@ -5,6 +5,7 @@
 #include "Component.h"
 #include "LogBase.h"
 #include <opencv2/opencv.hpp>
+#include <algorithm>
 
 
 Track3D::Track3D():BaseTrack(10, 7)
@@ -215,6 +216,9 @@ int Track3D::GetKFDataOrder(std::vector<int>& order)
     order.push_back(6);
     return 0;
 }
+
+const float ANGLE_CHANGE_MAX = 30 / 180 * _PI_;
+
 void Track3D::UpdateDataCheck(const std::vector<float>& data, std::vector<float>& out)
 {
     if (kf_ == nullptr)
@@ -230,6 +234,10 @@ void Track3D::UpdateDataCheck(const std::vector<float>& data, std::vector<float>
     {
         deta += _PI_ * 2;
     }
+    if (abs(deta) > ANGLE_CHANGE_MAX)
+    {
+        deta = deta > 0 ? ANGLE_CHANGE_MAX : -ANGLE_CHANGE_MAX;
+    }
     angle = x_angle + deta;
     out = data;
     out[3] = angle;