提交更新

BaseTracker/BaseTracker.h

@@ -35,12 +35,14 @@ 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> >& 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>& new_high_ids, std::map<uint64_t, int>& update_low_ids, std::map<uint64_t, int>& new_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>& update_low_ids,std::vector<uint64_t>& lostId);
 
 	std::map<uint64_t, std::shared_ptr<T> >& GetStates();
 
 
-	void AssociateDetectionsToTrackers(const std::vector<std::vector<float> >& detections, int _no/*观测数量*/, int _ns/*状态数量*/, std::map<uint64_t, std::shared_ptr<T> >& tracks, std::map<uint64_t, int>& matched, std::vector<int>& unmatched_det);
+    void AssociateDetectionsToTrackers(const std::vector<std::vector<float> >& detections, int _no/*观测数量*/, int _ns/*状态数量*/, std::map<uint64_t, std::shared_ptr<T> >& tracks, std::map<uint64_t, int>& matched, std::vector<int>& unmatched_det);
+
+    void AssociateDetectionsToTrackersEx(const std::vector<std::vector<float> >& dets_high, const std::vector<std::vector<float> >& dets_low, int _no/*观测数量*/, int _ns/*状态数量*/, std::map<uint64_t, std::shared_ptr<T> >& tracks, std::map<uint64_t, int>& high_matched, std::vector<int>& unmatched_det, std::map<uint64_t, int>& low_matched);
 
 public:
 
@@ -387,8 +389,212 @@ 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>& new_high_ids, std::map<uint64_t, int>& update_low_ids, std::map<uint64_t, int>& new_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>& update_low_ids, std::vector<uint64_t>& lostId)
 {
+#ifdef _USING_NSIGHT_
+    nvtxRangePush("Run Predict");
+#endif
+    for (auto& track : m_tracker)
+    {
+        track.second->Predict();
+    }
+#ifdef _USING_NSIGHT_
+    nvtxRangePop();
+#endif
+    if (dets_high.empty() && dets_low.empty())
+    {
+        /*** Delete lose tracked tracks ***/
+        for (auto it = m_tracker.begin(); it != m_tracker.end();)
+        {
+            if (it->second->IsLost())
+            {
+                lostId.push_back(it->first);
+                it = m_tracker.erase(it);
+            }
+            else
+            {
+                it++;
+            }
+        }
+        return 0;
+    }
+#ifdef _USING_NSIGHT_
+    nvtxRangePush("Run AssociateDetectionsToTrackers");
+#endif
+    // Hash-map between track ID and associated detection bounding box
+    std::map<uint64_t, int> high_matched;
+    std::map<uint64_t, int> low_matched;
+    // vector of unassociated detections
+    std::vector<int> unmatched_det;
 
+    // return values - matched, unmatched_det
+    AssociateDetectionsToTrackersEx(dets_high, dets_low, _no, _ns, m_tracker, high_matched, low_matched, unmatched_det);
+#ifdef _USING_NSIGHT_
+    nvtxRangePop();
+#endif
+    /*** Update tracks with associated bbox ***/
+#ifdef _USING_NSIGHT_
+    nvtxRangePush("Run Update");
+#endif
+    for (const auto& match : high_matched)
+    {
+        const auto& id = match.first;
+        m_tracker[id]->Update(dets_high[match.second]);
+        update_high_ids[id] = match.second;
+    }
+    for (const auto& match : low_matched)
+    {
+        const auto& id = match.first;
+        m_tracker[id]->Update(dets_low[match.second]);
+        update_low_ids[id] = match.second;
+    }
+#ifdef _USING_NSIGHT_
+    nvtxRangePop();
+#endif
+    /*** Create new tracks for unmatched detections ***/
+    for (const auto& det : unmatched_det)
+    {
+        std::shared_ptr<T> trackPtr = std::make_shared<T>();
+        trackPtr->Init(dets_high[det]);
+        trackPtr->SetIouThreshold(m_iou_threshold);
+        trackPtr->SetMaxCoastCycles(m_kMaxCoastCycles);
+        trackPtr->SetValidUpdateCount(m_updateValidCount);
+        trackPtr->SetValues(m_values);
+        // Create new track and generate new ID
+        uint64_t newId = ++m_countId;
+        m_tracker[newId] = trackPtr;
+        update_high_ids[newId] = det;
+    }
+
+    /*** Delete lose tracked tracks ***/
+    for (auto it = m_tracker.begin(); it != m_tracker.end();)
+    {
+        if (it->second->IsLost())
+        {
+            lostId.push_back(it->first);
+            it = m_tracker.erase(it);
+        }
+        else
+        {
+            it++;
+        }
+    }
     return 0;
 }
+
+template<class T>
+void BaseTracker<T>::AssociateDetectionsToTrackersEx(const std::vector<std::vector<float> >& dets_high, const std::vector<std::vector<float> >& dets_low, int _no/*观测数量*/, int _ns/*状态数量*/, std::map<uint64_t, std::shared_ptr<T> >& tracks, std::map<uint64_t, int>& high_matched, std::vector<int>& unmatched_det, std::map<uint64_t, int>& low_matched)
+{
+    if (tracks.empty())
+    {
+        //不做匹配
+        for (int i = 0; i < dets_high.size(); i++)
+        {
+            unmatched_det.push_back(i);
+        }
+        return;
+    }
+
+    std::vector<std::vector<float>> iou_matrix_high;
+    // resize IOU matrix based on number of detection and tracks
+    iou_matrix_high.resize(dets_high.size(), std::vector<float>(tracks.size()));
+
+    std::vector<std::vector<float>> association_high;
+    // resize association matrix based on number of detection and tracks
+    association_high.resize(dets_high.size(), std::vector<float>(tracks.size()));
+
+    for (size_t i = 0; i < dets_high.size(); i++)
+    {
+        size_t j = 0;
+        for (const auto& trk : tracks)
+        {
+            iou_matrix_high[i][j] = trk.second->CalculateIou(dets_high[i]);
+            j++;
+        }
+    }
+
+    // Find association
+    //std::string str = GetMatrixStr(iou_matrix, detections.size(), tracks.size());
+    //SDK_LOG(SDK_INFO, "iou_matrix = [%s]",str.c_str());
+    HungarianMatching(iou_matrix_high, dets_high.size(), tracks.size(), association_high);
+
+    for (size_t i = 0; i < dets_high.size(); i++)
+    {
+        bool matched_flag = false;
+        size_t j = 0;
+        for (auto& trk : tracks)
+        {
+            if (0 == association_high[i][j])
+            {
+                // Filter out matched with low IOU
+                //SDK_LOG(SDK_INFO, "match info i = %d,j = %d, iou_matrix = %f, m_iou_threshold = %f",i,j, iou_matrix[i][j], trk.second->m_iou_threshold);
+                if (iou_matrix_high[i][j] >= trk.second->m_iou_threshold)
+                {
+                    high_matched[trk.first] = i;
+                    trk.second->m_prob = iou_matrix_high[i][j];
+                    matched_flag = true;
+                }
+                // It builds 1 to 1 association, so we can break from here
+                break;
+            }
+            j++;
+        }
+        // if detection cannot match with any tracks
+        if (!matched_flag) {
+            unmatched_det.push_back(i);
+        }
+    }
+
+    std::vector<std::vector<float>> iou_matrix_low;
+    // resize IOU matrix based on number of detection and tracks
+    iou_matrix_low.resize(dets_low.size(), std::vector<float>(tracks.size() - high_matched.size()));
+
+    std::vector<std::vector<float>> association_low;
+    // resize association matrix based on number of detection and tracks
+    association_low.resize(dets_low.size(), std::vector<float>(tracks.size() - high_matched.size()));
+
+    for (size_t i = 0; i < dets_low.size(); i++)
+    {
+        size_t j = 0;
+        for (const auto& trk : tracks)
+        {
+            if (high_matched.find(trk.first) == high_matched.end())
+            {
+                iou_matrix_low[i][j] = trk.second->CalculateIou(dets_low[i]);
+                j++;
+            }
+        }
+    }
+
+    // Find association
+    //std::string str = GetMatrixStr(iou_matrix, detections.size(), tracks.size());
+    //SDK_LOG(SDK_INFO, "iou_matrix = [%s]",str.c_str());
+    HungarianMatching(iou_matrix_low, dets_low.size(), tracks.size() - high_matched.size(), association_low);
+
+    for (size_t i = 0; i < dets_low.size(); i++)
+    {
+        bool matched_flag = false;
+        size_t j = 0;
+        for (auto& trk : tracks)
+        {
+            if (high_matched.find(trk.first) == high_matched.end())
+            {
+                if (0 == association_low[i][j])
+                {
+                    // Filter out matched with low IOU
+                    //SDK_LOG(SDK_INFO, "match info i = %d,j = %d, iou_matrix = %f, m_iou_threshold = %f",i,j, iou_matrix[i][j], trk.second->m_iou_threshold);
+                    if (iou_matrix_low[i][j] >= trk.second->m_iou_threshold)
+                    {
+                        low_matched[trk.first] = i;
+                        trk.second->m_prob = iou_matrix_high[i][j];
+                        matched_flag = true;
+                    }
+                    // It builds 1 to 1 association, so we can break from here
+                    break;
+                }
+                j++;
+            }
+        }
+    }
+
+}