回滚到kf增加置信度和累计次数朝向判断修改之前的一个commit id=3ad91fc5

BaseTracker/Track3D.cpp

@@ -107,7 +107,7 @@ Track3D::Track3D():BaseTrack(10, 7)
 }
 void Track3D::Init(const std::vector<float>& data)
 {
-    if (data.size() != 10)
+    if (data.size() != 9)
         return;
     std::vector<float> tmp(data.begin() + 1, data.begin() + 8);
     //修正角度在-PI到PI
@@ -155,7 +155,6 @@ void Track3D::Update(const std::vector<float>& data)
 {
     std::vector<float> out;
     UpdateDataCheck(data, out);
-    SetQ(data);
     BaseTrack::Update(out);
 }
 
@@ -201,9 +200,9 @@ float calcIOU(cv::RotatedRect rect1, cv::RotatedRect rect2) {
 
 double Track3D::CalculateIou(const std::vector<float>& data)
 {
-    if (data.size() != 10)//add 置信度9->10
+    if (data.size() != 9)
     {
-        SDK_LOG(SDK_INFO, "CalculateIou data size != 10");
+        SDK_LOG(SDK_INFO, "CalculateIou data size != 9");
         return 0.0f;
     }
     if (m_obj == nullptr)
@@ -342,15 +341,15 @@ void Track3D::UpdateDataCheck(const std::vector<float>& data, std::vector<float>
 {
     if (kf_ == nullptr)
         return;
-    if (data.size() != 10)
+    if (data.size() != 9)
     {
-        SDK_LOG(SDK_INFO, "UpdateDataCheck data size is not 10");
+        SDK_LOG(SDK_INFO, "UpdateDataCheck data size is not 8");
         return;
     }
     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);
@@ -365,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 )
     {
@@ -373,45 +373,52 @@ 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;
 
-    //检测朝向是否需要转向
-    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)
@@ -453,9 +460,9 @@ int Track3D::GetIouData(std::vector<float>& data, int& obj_type)
 
 void Track3D::MeasureIouData(const std::vector<float>& input, std::vector<float>& out, int& obj_type)
 {
-    if (input.size() != 10)
+    if (input.size() != 9)
     {
-        SDK_LOG(SDK_INFO, "input is not 10");
+        SDK_LOG(SDK_INFO, "input is not 9");
         return;
     }
     obj_type = input[0];
@@ -520,53 +527,4 @@ 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

@@ -50,8 +50,6 @@ public:
 
     virtual void SetValues(std::vector<float>& data);
 
-    void SetQ(const std::vector<float>& data);//lyc 2.13
-
     virtual int GetIouData(std::vector<float>& data, int& obj_type);
     static void MeasureIouData(const std::vector<float>& input, std::vector<float>& out, int& obj_type);
 
@@ -71,11 +69,8 @@ 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

@@ -520,7 +520,6 @@ void TrackingRos::ThreadTrackingProcess()
                 data.push_back(obj.w);
                 data.push_back(obj.h);
                 data.push_back(obj.dataSource);
-                data.push_back(obj.score);//置信度
                 input.emplace_back(data);
             }
             std::vector<uint64_t> detectionsId;
@@ -594,7 +593,7 @@ void TrackingRos::ThreadTrackingProcess()
                         obj.x = data[0];
                         obj.y = data[1];
                         obj.z = data[2];
-                        obj.rot_y = iter.second->GetRotY(data[3]);
+                        obj.rot_y = data[3];
                         obj.l = data[4];
                         obj.w = data[5];
                         obj.h = data[6];
@@ -677,7 +676,7 @@ void TrackingRos::ThreadTrackingProcess()
                         obj.x = data[0];
                         obj.y = data[1];
                         obj.z = data[2];
-                        obj.rot_y = iter.second->GetRotY(data[3]);
+                        obj.rot_y = data[3];
                         obj.l = data[4];
                         obj.w = data[5];
                         obj.h = data[6];