update

BaseTracker/BaseTracker.h

@@ -36,7 +36,7 @@ public:
     int Run(const std::vector<std::vector<float> >& detections, int _no/*观测数量*/, int _ns/*状态数量*/, std::vector<uint64_t>& detectionsId, std::map<uint64_t, int>& updateId, std::vector<uint64_t>& lostId);
     int Run(const std::vector<std::vector<float> >& detections, int _no/*观测数量*/, int _ns/*状态数量*/, std::vector<uint64_t>& detectionsId, std::map<uint64_t, int>& updateId, std::map<uint64_t, int>& addId,std::vector<uint64_t>& lostId);
 
-    int Run(const std::vector<std::vector<float> >& dets_high, const std::vector<std::vector<float> >& dets_low, int _no/*观测数量*/, int _ns/*状态数量*/, std::map<uint64_t, int>& update_high_ids, std::map<uint64_t, int>& update_low_ids, std::vector<uint64_t>& lostId);
+    int Run(const std::vector<std::vector<float> >& dets_high, const std::vector<std::vector<float> >& dets_low, int _no/*观测数量*/, int _ns/*状态数量*/, std::map<uint64_t, int>& update_high_ids, std::map<uint64_t, int>& addId, std::map<uint64_t, int>& update_low_ids, std::vector<uint64_t>& lostId);
 
 	std::map<uint64_t, std::shared_ptr<T> >& GetStates();
 
@@ -390,7 +390,7 @@ void BaseTracker<T>::AssociateDetectionsToTrackers(const std::vector<std::vector
 }
 
 template<class T>
-int BaseTracker<T>::Run(const std::vector<std::vector<float> >& dets_high, const std::vector<std::vector<float> >& dets_low, int _no/*观测数量*/, int _ns/*状态数量*/, std::map<uint64_t, int>& update_high_ids, std::map<uint64_t, int>& update_low_ids, std::vector<uint64_t>& lostId)
+int BaseTracker<T>::Run(const std::vector<std::vector<float> >& dets_high, const std::vector<std::vector<float> >& dets_low, int _no/*观测数量*/, int _ns/*状态数量*/, std::map<uint64_t, int>& update_high_ids, std::map<uint64_t, int>& addId, std::map<uint64_t, int>& update_low_ids, std::vector<uint64_t>& lostId)
 {
 #ifdef _USING_NSIGHT_
     nvtxRangePush("Run Predict");
@@ -465,6 +465,7 @@ int BaseTracker<T>::Run(const std::vector<std::vector<float> >& dets_high, const
         uint64_t newId = ++m_countId;
         m_tracker[newId] = trackPtr;
         update_high_ids[newId] = det;
+        addId[newId] = det;
     }
 
     /*** Delete lose tracked tracks ***/

Tracking/TrackingObj.cpp

@@ -41,6 +41,8 @@ namespace juefx_tracking
         if(m_config["matrix_angle_r_value"])
             matrix_angle_r_value = m_config["matrix_angle_r_value"].as<float>();
         SDK_LOG(SDK_INFO, "matrix_angle_r_value = %f", matrix_angle_r_value);
+        if(m_config["using_high_low_ekf"])
+            m_using_high_low_ekf = m_config["using_high_low_ekf"].as<int32_t>();
         std::vector<float> values;
         values.push_back(matrix_angle_r_value);
         m_tracker.SetValues(values);

Tracking/TrackingObj.h

@@ -44,6 +44,7 @@ namespace juefx_tracking
         std::vector<std::vector<double>> m_kitti2origin;  
         
         float m_lidar_x_angle = 0.0f;//修改rot_y的参数，从配置里读取
+        float m_using_high_low_ekf = 0;//是否使用高低配置逻辑
 
         YAML::Node m_config;
         TrackingProcessCallback m_cb= nullptr;

ros2/Track3DEx.cpp

@@ -125,13 +125,13 @@ double Track3DEx::CalculateIou(const std::vector<float>& data)
             return 1.0f;
         }
     }
-    // else if(obj_cameraId > 0 && obj_trackingId > 0)
-    // {
-    //     if(obj_cameraId == input_cameraId && input_trackingId == obj_trackingId)
-    //     {
-    //         return 0.1f;
-    //     }        
-    // }
+    else if(obj_cameraId > 0 && obj_trackingId > 0)
+    {
+        if(obj_cameraId == input_cameraId && input_trackingId == obj_trackingId)
+        {
+            return 0.1f;
+        }        
+    }
     int input_type = data[0];
     int obj_type = m_obj->type;
     // if (input_type != obj_type)