提交更新

ros2/Track3DEx.cpp

@@ -18,6 +18,26 @@ void Track3DEx::Init(const std::vector<float>& data)
         tmp[3] += 2 * _PI_;
     BaseTrack::Init(tmp);
 }
+void Track3DEx::Predict()
+{
+    if (kf_ == nullptr)
+        return;
+    point2d pos = {};
+    pos.x = kf_->x_[0];
+    pos.y = kf_->x_[1];
+
+    if(pos.x > 80)
+    {
+        kf_->R_(0, 0) = 10;
+        kf_->R_(1, 1) = 10;
+    }
+    else
+    {
+        kf_->R_(0, 0) = 1;
+        kf_->R_(1, 1) = 1;
+    }
+    Track3D::Predict();
+}
 void Track3DEx::UpdateDataCheck(const std::vector<float>& data, std::vector<float>& out)
 {
     if (kf_ == nullptr)

ros2/Track3DEx.h

@@ -11,6 +11,7 @@ public:
     ~Track3DEx(){}
 
     virtual void Init(const std::vector<float>& data);
+    virtual void Predict();
 
     virtual void UpdateDataCheck(const std::vector<float>& data, std::vector<float>& out);//对于输入数据进行修正