Merge branch 'master' of http://git.corp.roadlinks.cn:2020/oscar/jfx_kalman_filter_src

ros2/Track3DEx.cpp

 
 #include "Track3DEx.h"
-
+#include <eigen3/Eigen/Dense>
+#include "Iou.h"
+#include "Component.h"
+#include <opencv2/opencv.hpp>
+#include <algorithm>
 
 void Track3DEx::Init(const std::vector<float>& data)
 {
@@ -20,13 +24,13 @@ void Track3DEx::UpdateDataCheck(const std::vector<float>& data, std::vector<floa
         return;
     if (data.size() != 11)
     {
-        SDK_LOG(SDK_INFO, "UpdateDataCheck data size is not 11");
+        // SDK_LOG(SDK_INFO, "UpdateDataCheck data size is not 11");
         return;
     }
     std::vector<float> tmp(data.begin() + 1, data.begin() + 8);
     double rot_y = tmp[3];
     int dataSource = data[8];
-    if (dataSource == 1 && m_points.size() >= 5 && (abs(m_points[4].x - m_points[0].x) > DIST_THRED || abs(m_points[4].y - m_points[0].y) > DIST_THRED))
+    if (dataSource == 1 && m_points.size() >= 5 && (abs(m_points[4].x - m_points[0].x) > 0.5 || abs(m_points[4].y - m_points[0].y) > 0.5))
     {
         double center_rot_y = correct_angle(m_points);
         m_center_rot_y = center_rot_y;
@@ -43,7 +47,7 @@ void Track3DEx::UpdateDataCheck(const std::vector<float>& data, std::vector<floa
     }
     else if (dataSource > 1 && m_points.size() >= 5 )
     {
-        if ((abs(m_points[4].x - m_points[0].x) > DIST_THRED || abs(m_points[4].y - m_points[0].y) > DIST_THRED))//5个点首尾有距离差才计算方向
+        if ((abs(m_points[4].x - m_points[0].x) > 0.5 || abs(m_points[4].y - m_points[0].y) > 0.5))//5个点首尾有距离差才计算方向
         {
             double center_rot_y = correct_angle(m_points);
             m_center_rot_y = center_rot_y;
@@ -82,7 +86,7 @@ double Track3DEx::CalculateIou(const std::vector<float>& data)
 {
     if (data.size() != 11)
     {
-        SDK_LOG(SDK_INFO, "CalculateIou data size != 9");
+        // SDK_LOG(SDK_INFO, "CalculateIou data size != 9");
         return 0.0f;
     }
     if (m_obj == nullptr)
@@ -94,7 +98,7 @@ double Track3DEx::CalculateIou(const std::vector<float>& data)
     int obj_cameraId = m_obj->cameraId;
     int obj_trackingId = m_obj->trackingId;
 
-    if(input_dataSource == obj_dataSource && obj_dataSource > 1 && obj_cameraId > 0 && obj_trackingId > 0)
+    if(obj_cameraId > 0 && obj_trackingId > 0)
     {
         if(obj_cameraId == input_cameraId && input_trackingId == obj_trackingId)
         {

ros2/TrackingRos2.cpp

@@ -154,7 +154,7 @@ void TrackingRos2::TrackingPorcess(objTrackListPtr objsPtr) {
   m_tracking.m_tracker.Run(inputH, 7, 10, detectionsId, updateId,addId, lostId);
   uint64_t rTime = GetCurTime() - begin;
   // SDK_LOG(SDK_INFO, "m_tracker.Run time = %llu", rTime);
-  std::map<uint64_t, std::shared_ptr<Track3D> >& trackers =
+  std::map<uint64_t, std::shared_ptr<Track3DEx> >& trackers =
       m_tracking.m_tracker.GetStates();
   static int count = 0;
   static int calcCount = 0;
@@ -198,10 +198,13 @@ void TrackingRos2::TrackingPorcess(objTrackListPtr objsPtr) {
       obj.kinematics.twist_with_covariance =
           input_obj.object.kinematics.twist_with_covariance;
       obj.shape = input_obj.object.shape;
-
+      int camera_id = 0;
+      int tracking_id = 0;
       if(addId.find(iter.first) == addId.end())
       {
         obj.object_id = iter.second->m_obj->obj->object_id;
+        camera_id = iter.second->m_obj->cameraId;
+        tracking_id = iter.second->m_obj->trackingId;
       }
       else
       {
@@ -238,9 +241,9 @@ void TrackingRos2::TrackingPorcess(objTrackListPtr objsPtr) {
       TrackStructObj strObj = {};
       strObj.frame = timestamp;
       strObj.type = input_obj.object.classification[0].label;
-      strObj.dataSource = input_obj.dataSource;
-      strObj.cameraId = input_obj.camera_id;
-      strObj.trackingId = input_obj.tracking_id;
+      strObj.dataSource = input_obj.data_source;
+      strObj.cameraId = input_obj.camera_id == 0 ? camera_id: input_obj.camera_id;
+      strObj.trackingId = input_obj.tracking_id == 0 ? tracking_id: input_obj.tracking_id;
       strObj.obj = std::make_shared<autoware_auto_perception_msgs::msg::TrackedObject>(obj);
       iter.second->m_obj = std::make_shared<TrackStructObj>(strObj);  //不仅状态变量还有类型时间戳的m_obj数据更新（Tracker3D.h的智能指针）
     } else          //未匹配但跟踪链未被删除