提交测试

BaseTracker/BaseTrack.cpp

@@ -67,6 +67,16 @@ int BaseTrack::GetStateData(std::vector<float>& data)
         data.push_back(kf_->x_[i]);
     return 0;
 }
+int BaseTrack::GetPredictData(std::vector<float>& data)
+{
+    if (kf_ == nullptr || m_num_obs > m_num_states)
+        return -1;
+    data.clear();
+    for (int i = 0; i < m_num_obs; i++)
+        data.push_back(kf_->x_predict_[i]);
+    return 0;
+}
+
 float BaseTrack::GetNIS() const
 {
     if (kf_ == nullptr)

BaseTracker/BaseTrack.h

@@ -20,6 +20,7 @@ public:
     virtual void Update(const std::vector<float>& data);
     virtual void UpdateHit();
     virtual int GetStateData(std::vector<float>& data);
+    virtual int GetPredictData(std::vector<float>& data);
     virtual float GetNIS() const;
     virtual float GetProb() const;
     virtual bool IsLost();

TrackingRos.cpp

@@ -222,10 +222,15 @@ void TrackingRos::ThreadTrackingProcess()
                         obj.v_y = data[8];
                         obj.v_z = data[9];
                         obj.obj_id = iter.first;
-                        SDK_LOG(SDK_INFO, "frame = %llu, id = %llu, input(%f,%f,%f,%f,%f,%f,%f) output(%f,%f,%f,%f,%f,%f,%f) prob = %f",
-                            obj.frame,iter.first, objsPtr->array[updateId[iter.first]].x, objsPtr->array[updateId[iter.first]].y, objsPtr->array[updateId[iter.first]].z,
-                            objsPtr->array[updateId[iter.first]].l, objsPtr->array[updateId[iter.first]].w, objsPtr->array[updateId[iter.first]].h, objsPtr->array[updateId[iter.first]].rot_y,
-                            obj.x,obj.y,obj.z,obj.l,obj.w,obj.h,obj.rot_y, iter.second->GetProb());
+                        std::vector<float> predict;
+                        if (iter.second->GetProb(predict) == 0)
+                        {
+                            SDK_LOG(SDK_INFO, "frame = %llu, id = %llu, input(%f,%f,%f,%f,%f,%f,%f) predict(%f,%f,%f,%f,%f,%f,%f) output(%f,%f,%f,%f,%f,%f,%f) prob = %f",
+                                obj.frame, iter.first, objsPtr->array[updateId[iter.first]].x, objsPtr->array[updateId[iter.first]].y, objsPtr->array[updateId[iter.first]].z,
+                                objsPtr->array[updateId[iter.first]].l, objsPtr->array[updateId[iter.first]].w, objsPtr->array[updateId[iter.first]].h, objsPtr->array[updateId[iter.first]].rot_y,
+                                predict[0], predict[1], predict[2], predict[3], predict[4], predict[5], predict[6],
+                                obj.x, obj.y, obj.z, obj.l, obj.w, obj.h, obj.rot_y, iter.second->GetProb());
+                        }
                         float gx = 0.0f;
                         float gy = 0.0f;
                         CalculateGuassPos(obj.x, obj.y, obj.z, m_trans, gx, gy);