处理抖动优化kf

BaseTracker/Track3D.cpp

@@ -155,6 +155,7 @@ void Track3D::Update(const std::vector<float>& data)
 {
     std::vector<float> out;
     UpdateDataCheck(data, out);
+    SetQ(data);
     BaseTrack::Update(out);
 }
 
@@ -527,4 +528,34 @@ int Track3D::GetColorInfo(std::string& color)
         }
     }
     return 0;    
+}
+
+void Track3D::SetQ(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

@@ -49,6 +49,8 @@ public:
     virtual void UpdateDataCheck(const std::vector<float>& data, std::vector<float>& out);//对于输入数据进行修正
 
     virtual void SetValues(std::vector<float>& data);
+    
+    void SetQ(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);

TrackingRos.cpp

@@ -520,6 +520,7 @@ 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;