提交更新

Tracking/TrackingObj.h

@@ -16,7 +16,7 @@ using objTrackListPtr = std::shared_ptr<int>;
 #endif
 
 #include "BaseTracker.h"
-#include "Track3D.h"
+#include "Track3DEx.h"
 #include "SafeQueue.hpp"
 //#include "TrackingMsg.h"
 
@@ -36,7 +36,7 @@ namespace juefx_tracking
         void ThreadTrackingProcess();
 
         SafeQueue<objTrackListPtr> m_objsQueue;//队列
-        BaseTracker<Track3D> m_tracker;
+        BaseTracker<Track3DEx> m_tracker;
         bool m_isTrackingRun = false;//记录是否在运行
         std::thread m_trackingThread;//运行事件线程
 

ros2/RosMsg.h

@@ -14,6 +14,9 @@ struct TrackStructObj
 {
     uint64_t frame;
     int type;
+    int dataSource;
+    int cameraId;
+    int trackingId;
     std::shared_ptr<autoware_auto_perception_msgs::msg::TrackedObject>  obj;
 };
 

ros2/Track3DEx.cpp

+
+#include "Track3DEx.h"
+
+
+void Track3DEx::Init(const std::vector<float>& data)
+{
+    if (data.size() != 11)
+        return;
+    std::vector<float> tmp(data.begin() + 1, data.begin() + 8);
+    //修正角度在-PI到PI
+    while (tmp[3] > _PI_)
+        tmp[3] -= 2 * _PI_;
+    while (tmp[3] < -_PI_)
+        tmp[3] += 2 * _PI_;
+    BaseTrack::Init(tmp);
+}
+void Track3DEx::UpdateDataCheck(const std::vector<float>& data, std::vector<float>& out)
+{
+    if (kf_ == nullptr)
+        return;
+    if (data.size() != 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))
+    {
+        double center_rot_y = correct_angle(m_points);
+        m_center_rot_y = center_rot_y;
+        //SDK_LOG(SDK_INFO, "center_rot_y = %f,rot_y = %f", center_rot_y,rot_y);
+        double rotate = abs(center_rot_y - rot_y);
+        while (rotate > _PI_)
+            rotate -= 2 * _PI_;
+        while (rotate < -_PI_)
+            rotate += 2 * _PI_;
+        if (rotate > _PI_ / 2.0f && rotate < _PI_ * 3 / 2.0f)//夹角在90到270度，认为角度计算错误，差180度
+        {
+            rot_y += _PI_;
+        }
+    }
+    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个点首尾有距离差才计算方向
+        {
+            double center_rot_y = correct_angle(m_points);
+            m_center_rot_y = center_rot_y;
+            rot_y = center_rot_y;
+        }
+        else//没有距离差就使用最后一次的方向
+            rot_y = m_center_rot_y;
+    }
+    double x_angle = kf_->x_[3];
+    double deta = rot_y - x_angle;
+    double deta2 = rot_y + _PI_ - x_angle;
+    while (deta >= _PI_)
+    {
+        deta -= _PI_ * 2;
+    }
+    while (deta < -_PI_)
+    {
+        deta += _PI_ * 2;
+    }
+    while (deta2 >= _PI_)
+    {
+        deta2 -= _PI_ * 2;
+    }
+    while (deta2 < -_PI_)
+    {
+        deta2 += _PI_ * 2;
+    }
+    float detaT = abs(deta) > abs(deta2) ? deta2:deta;
+    rot_y = x_angle + detaT;
+    out = tmp;
+    out[3] = rot_y;
+
+}
+
+double Track3DEx::CalculateIou(const std::vector<float>& data)
+{
+    if (data.size() != 11)
+    {
+        SDK_LOG(SDK_INFO, "CalculateIou data size != 9");
+        return 0.0f;
+    }
+    if (m_obj == nullptr)
+        return 0.0f;
+    int input_dataSource = data[8];
+    int input_cameraId = data[9];
+    int input_trackingId = data[10];
+    int obj_dataSource = m_obj->dataSource;
+    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 == input_cameraId && input_trackingId == obj_trackingId)
+        {
+            return 1.0f;
+        }
+    }
+    int input_type = data[0];
+    int obj_type = m_obj->type;
+    if (input_type != obj_type)
+        return 0.0f;
+    std::vector<float> states;
+    GetStateData(states);
+
+    float v = sqrt(states[7] * states[7] + states[8] * states[8]);
+
+    cv::RotatedRect rect1 = cv::RotatedRect(cv::Point2f(states[0], states[1]), cv::Size2f(states[4] + v, states[5]*1.5f), states[3]*180 / 3.1415926);
+    cv::RotatedRect rect2 = cv::RotatedRect(cv::Point2f(data[1], data[2]), cv::Size2f(data[5], data[6]), data[4]*180/3.1415926);
+
+    uint64_t begin = GetCurTime();
+    double iou_3d = 0.0f;
+
+    double area = calcIntersectionArea(rect1,rect2);
+    if (area > 0)
+    {
+        if (1)//interHeight > 0)
+        {
+            iou_3d = area / (states[4] * states[5] + data[5] * data[6]);
+        }
+    }
+    return iou_3d;
+}
\ No newline at end of file

ros2/Track3DEx.h

+#pragma once
+
+#include "Track3D.h"
+
+
+
+class Track3DEx : public Track3D
+{
+public:
+    Track3DEx(){}
+    ~Track3DEx(){}
+
+    virtual void Init(const std::vector<float>& data);
+
+    virtual void UpdateDataCheck(const std::vector<float>& data, std::vector<float>& out);//对于输入数据进行修正
+
+    virtual double CalculateIou(const std::vector<float>& data);
+
+};
\ No newline at end of file

ros2/TrackingRos2.cpp

@@ -125,6 +125,8 @@ void TrackingRos2::TrackingPorcess(objTrackListPtr objsPtr) {
     data.push_back(dim.y);
     data.push_back(dim.z);
     data.push_back(dataSource);
+    data.push_back(objsPtr->feature_objects[i].camera_id);
+    data.push_back(objsPtr->feature_objects[i].tracking_id);
     inputH.emplace_back(data);
     // if (prob < m_high_low_score)
     // {
@@ -233,6 +235,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.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          //未匹配但跟踪链未被删除