提交被覆盖的朝向判断和修改的恢复。

BaseTracker/Track3D.cpp

@@ -348,8 +348,8 @@ 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];
-    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);
@@ -364,7 +364,6 @@ void Track3D::UpdateDataCheck(const std::vector<float>& data, std::vector<float>
         {
             rot_y += _PI_;
         }
-        // isCheck = 1;
     }
     else if (dataSource > 1 && m_points.size() >= 5 )
     {
@@ -373,52 +372,45 @@ 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;
     }
-    if(isCheck == 0)//如果没有检查方向，那么就使用偏差最小的作为方向
+    double x_angle = kf_->x_[3];
+    double deta = rot_y - x_angle;
+    double deta2 = rot_y + _PI_ - x_angle;
+    while (deta >= _PI_)
     {
-        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;
+        deta -= _PI_ * 2;
     }
-    else //已经检查了方向，那么就直接使用这个方向
+    while (deta < -_PI_)
     {
-        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;
+        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;
     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)
@@ -527,4 +519,53 @@ 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)
+{
+    if(data.size()<10)
+    {
+        return;
+    }
+        
+    if(kf_ == nullptr)
+    {
+        return;
+    }
+    
+    float score = data[9];//置信度
+    float ratio = 1;
+    if(score<0.4)
+    {
+        ratio = 10;
+    }
+    else if(score >0.7)
+    {
+        ratio = 1;
+    }
+    else
+    {
+        ratio = pow(10,(0.7-score)*33);
+    }
+    kf_->R_ = kf_->R_ * ratio;
+    return;
 }
\ No newline at end of file

BaseTracker/Track3D.h

@@ -69,8 +69,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

@@ -593,7 +593,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];
@@ -676,7 +676,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];