测试

BaseTracker/Track3D.cpp

@@ -289,7 +289,7 @@ void Track3D::UpdateDataCheck(const std::vector<float>& data, std::vector<float>
     if (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);
-        SDK_LOG(SDK_INFO, "center_rot_y = %f,rot_y = %f", center_rot_y,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_)
             rotate -= 2 * _PI_;
@@ -302,6 +302,7 @@ void Track3D::UpdateDataCheck(const std::vector<float>& data, std::vector<float>
     }
     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;
@@ -310,7 +311,16 @@ void Track3D::UpdateDataCheck(const std::vector<float>& data, std::vector<float>
     {
         deta += _PI_ * 2;
     }
-    rot_y = x_angle + deta;
+    while (deta2 >= _PI_)
+    {
+        deta2 -= _PI_ * 2;
+    }
+    while (deta2 < -_PI_)
+    {
+        deta2 += _PI_ * 2;
+    }
+    float detaT = abs(deat) > abs(deta2) ? deta2:deta;
+    rot_y = x_angle + detaT;
     out = data;
     out[3] = rot_y;