提交朝向的判断和修改

BaseTracker/Track3D.cpp

@@ -107,7 +107,7 @@ Track3D::Track3D():BaseTrack(10, 7)
 }
 void Track3D::Init(const std::vector<float>& data)
 {
-    if (data.size() != 9)
+    if (data.size() != 10)
         return;
     std::vector<float> tmp(data.begin() + 1, data.begin() + 8);
     //修正角度在-PI到PI
@@ -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];
+    double rot_y_detect = rot_y;//检测到的朝向
     int dataSource = data[8];
     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))
     {
@@ -400,6 +401,17 @@ void Track3D::UpdateDataCheck(const std::vector<float>& data, std::vector<float>
     out = tmp;
     out[3] = rot_y;
 
+    //检测朝向是否需要转向
+    double rot_y_correct = rot_y;//修正后的朝向
+    double rotation = rot_y_correct - rot_y_detect;
+    while(rotation < -_PI_) rotation += _PI_ * 2;
+    while(rotation >= _PI_) rotation -= _PI_ * 2;
+    int isRotation = 0;
+    if(abs(rotation) > _PI_ / 2) //钝角说明转向了
+        isRotation = 1;
+    if(m_rot_y_diret.size() >= 10)
+        m_rot_y_diret.erase(m_rot_y_diret.begin());
+    m_rot_y_diret.push_back(isRotation);
 }
 
 void Track3D::SetValues(std::vector<float>& data)
@@ -509,6 +521,25 @@ int Track3D::GetColorInfo(std::string& color)
     }
     return 0;    
 }
+float Track3D::GetRotY(float rot_y)
+{
+    float rot_y_real = rot_y;
+    int size = m_rot_y_diret.size();
+    int count = 0;
+    for(auto iter: m_rot_y_diret)
+    {
+        if(iter == 1)
+            count++;
+    }
+    if(count > size/2)
+    {
+        rot_y_real += _PI_;
+    }
+    while(rot_y_real < 0) rot_y_real += _PI_ * 2;
+    while(rot_y_real >= _PI_ * 2) rot_y_real -= _PI_ * 2;
+    return rot_y_real;
+}
+
 
 void Track3D::SetQ(const std::vector<float>& data)
 {

BaseTracker/Track3D.h

@@ -71,8 +71,11 @@ public:
 
     std::map<std::string,int> m_colorInfos;///< 记录车辆的颜色信息
 
+    std::vector<int> m_rot_y_diret;///< 每帧检测到是否转向
+
     int UpdateColorInfo(std::string color);
     int GetColorInfo(std::string& color);
+    float GetRotY(float rot_y);
 };
 
 double correct_angle(std::vector<point2d>& points);

TrackingRos.cpp

@@ -594,7 +594,7 @@ void TrackingRos::ThreadTrackingProcess()
                         obj.x = data[0];
                         obj.y = data[1];
                         obj.z = data[2];
-                        obj.rot_y = data[3];
+                        obj.rot_y = iter.second->GetRotY(data[3]);
                         obj.l = data[4];
                         obj.w = data[5];
                         obj.h = data[6];
@@ -677,7 +677,7 @@ void TrackingRos::ThreadTrackingProcess()
                         obj.x = data[0];
                         obj.y = data[1];
                         obj.z = data[2];
-                        obj.rot_y = data[3];
+                        obj.rot_y = iter.second->GetRotY(data[3]);
                         obj.l = data[4];
                         obj.w = data[5];
                         obj.h = data[6];