提交更新

src/BaseTracker/Track2D.h

@@ -8,6 +8,7 @@
 #else
 #include "jfx_common_msgs/det_tracking.h"
 #endif
+#include "track.h"
 
 using trackOjbPtr = std::shared_ptr< jfx_common_msgs::det_tracking>;
 
@@ -26,4 +27,7 @@ public:
     virtual double CalculateIou(const std::vector<float>& data);
 
     static void MeasureIouData(const std::vector<float>& input, std::vector<float>& out, int& obj_type) {}
+
+
+    TrackObj m_obj;
 };

src/track.h

@@ -17,6 +17,8 @@
 #include <stdlib.h>
 #include <thread>
 #include <vector>
+#include "BaseTracker.h"
+#include "Track2D.h"
 
 using namespace jfx_vision;
 
@@ -193,6 +195,13 @@ public:
         std::vector<uint64_t> lost_detection;
         uint64_t trackFrmIdx = 0;
 
+        BaseTracker<Track2D> _tracker;
+        _tracker.SetGPU(0);
+        _tracker.SetIouThreshold(0.1);
+        _tracker.SetMaxCoastCycles(3);
+        _tracker.SetValidUpdateCount(2);
+        float high_low_rate = 0.4;
+
         initTransUV(trackResPtr->transUV, cameraIndex);
         initCoordCVT(trackResPtr->coordinatCvt, cameraIndex);
 
@@ -218,18 +227,123 @@ public:
             add_detection.clear();
             lost_detection.clear();
 
+            std::vector< std::vector<float> > inputH;
+            std::vector< std::vector<float> > inputL;
+            std::vector<int> indexH;
+            std::vector<int> indexL;
+
+            for (int i = 0; i < trackItem.detections_info.size(); i++)
+            {
+                TrackObj& item = trackItem.detections_info[i];
+                std::vector<float> data;
+                auto width = static_cast<float>(trackItem.detections[i].width);
+                auto height = static_cast<float>(trackItem.detections[i].height);
+                data.push_back(trackItem.detections[i].x + trackItem.detections[i].width/2);
+                data.push_back(trackItem.detections[i].y + trackItem.detections[i].height/2);
+                data.push_back(width);
+                data.push_back(height);
+                if (item.prob > high_low_rate)
+                {
+                    inputH.push_back(data);
+                    indexH.push_back(i);
+                }
+                else
+                {
+                    inputL.push_back(data);
+                    indexL.push_back(i);
+                }
+            }
+            std::map<uint64_t, int> updateHId;
+            std::map<uint64_t, int> updateLId;
+            std::vector<uint64_t> lostId;
+            _tracker.Run(inputH, inputL, 4, 8, updateHId, updateLId, lostId);
+
+            std::map<uint64_t, std::shared_ptr<Track2D> >& trackers = _tracker.GetStates();
+
+            jfx_common_msgs::det_tracking_array objArray;
+            for (auto& iter : trackers)
+            {
+                jfx_common_msgs::det_tracking obj = {};
+                TrackObj info = {};
+                obj.frame = trackItem.batckIndex;
+                obj.create_stamp = trackItem.ts;
+                int is_need_send = 0;//是否需要发送
+                if (updateHId.find(iter.first) != updateHId.end() || updateLId.find(iter.first) != updateLId.end())
+                {
+                    if (updateHId.find(iter.first) != updateHId.end())
+                        info = trackItem.detections_info[indexH[updateHId[iter.first]]];   //从检测结果里得到初始高分obj
+                    if (updateLId.find(iter.first) != updateLId.end())
+                        info = trackItem.detections_info[indexL[updateLId[iter.first]]];   //从检测结果里得到初始低分obj
+                }
+                else
+                {
+                    info = iter.second->m_obj;
+                }
+                obj.obj_id = iter.first;
+                std::vector<float> data;
+                if (iter.second->IsValid() && iter.second->GetStateData(data) == 0)   //初始obj更新跟踪的推理结果
+                {
+                    is_need_send = 1;
+                    int bcenter_x = data[0] + data[2] / 2;
+                    int bcenter_y = data[1] + data[3];
+                    vector<float> r;
+                    double lat = 0, lon = 0;
+                    int ret = trackResPtr->transUV.Process(static_cast<double>(bcenter_x), static_cast<double>(bcenter_y), r);
+                    if (ret == 0) {
+                        //trkItem->m_NowX = r[0];
+                        //trkItem->m_NowY = r[1];
+                        jfx::Array result84;
+                        jfx::Array objPoint = { r[0], r[1] };
+                        trackResPtr->coordinatCvt.Dev2WGS84(objPoint, result84, false);
+                        lat = result84[0];
+                        lon = result84[1];
+                    }
+                    obj.score = info.prob;
+                    obj.h = 0;
+                    obj.w = 0;
+                    obj.l = 0;
+                    obj.Long = lon;
+                    obj.Lat = lat;
+                    // temp use
+                    std::map<int, std::string>::iterator itr0 = mCameraidx2ID.find(trackItem.cameraIdx);
+                    if (itr0 != mCameraidx2ID.end()) {
+                        obj.color_name = itr0->second;
+                    }
+
+                    std::map<int, OutObjType>::iterator itr = mObjClassId2Type.find(trackItem.m_Type);
+                    if (itr != mObjClassId2Type.end()) {
+                        obj.type = itr->second;
+                        std::map<OutObjType, std::string>::iterator itr2 = mObjType2Name.find(itr->second);
+                        if (itr2 != mObjType2Name.end()) {
+                            obj.name = itr2->second;
+                        }
+                    }
+
+                    fillObjSize(obj);
+
+                    objArray.array.emplace_back(obj);
+                }
+
+
+            }
+
+            mPubDataMutex.lock();
+            mPubDataList.emplace_back(objArray);
+            mPubDataMutex.unlock();
+
+
             // 跟踪
-            tracker.Run(
-                trackItem.detections, trackItem.detections_info,
-                [&add_detection, this](const uint32_t& id, const TrackImplPtr& trkItem) {
-                    add_detection.push_back(id);
-                },
-                [&lost_detection, this](const uint32_t& id, const TrackImplPtr& trkItem) {
-                    lost_detection.push_back(id);
-                });
-
-            // 目标打包
-            packRosObj(tracker, trackResPtr->transUV, trackResPtr->coordinatCvt, &trackItem);
+            //tracker.Run(
+            //    trackItem.detections, trackItem.detections_info,
+            //    [&add_detection, this](const uint32_t& id, const TrackImplPtr& trkItem) {
+            //        add_detection.push_back(id);
+            //    },
+            //    [&lost_detection, this](const uint32_t& id, const TrackImplPtr& trkItem) {
+            //        lost_detection.push_back(id);
+            //    });
+
+            //// 目标打包
+            //packRosObj(tracker, trackResPtr->transUV, trackResPtr->coordinatCvt, &trackItem);
         }
     }