fusion modify

src/fusion/lidar_radar_fusion_cnn/Tracker/Tracking.hpp

@@ -0,0 +1,179 @@
+#pragma once
+#include "fusionobjectarray.pb.h"
+#include "fusionobject.pb.h"
+#include "Ctracker.h"
+
+#include <iostream>
+#include <vector>
+#define FPS 8
+#define MIN_DETECTEDFRAMES 5
+///
+/// \brief 跟踪器参数设置
+///
+bool InitTracker(CTracker& tracker)
+{
+    TrackerSettings settings;
+    settings.SetDistance(tracking::DistRect3Ds); // 代价矩阵：两中心点之间的距离
+    settings.m_kalmanType = tracking::KalmanLinear; // 滤波器类型：卡尔曼线性滤波器
+    settings.m_filterGoal = tracking::FilterRect3D; // 滤波对象：Rect3D
+    settings.m_lostTrackType = tracking::TrackNone; // 丢失了的目标，不再追踪
+    settings.m_matchType = tracking::MatchHungrian; // 匹配算法：匈牙利
+    settings.m_dt = 1.f; // 卡尔曼滤波器的时间步长
+    settings.m_accelNoiseMag = 0.5f; // 卡尔曼的噪声放大器
+    settings.m_distThres = 30.f; // 匹配算法中的距离阈值
+    settings.m_minAreaRadiusPix = -1.f;//frame.rows / 20.f; // 目标的最小面积半径（像素）
+    settings.m_maximumAllowedSkippedFrames = 2; // 被跟踪目标允许未匹配到的最大次数，当超过这一数值，该目标的跟踪器将被移除
+    settings.m_maxTraceLength = 5; // 最大跟踪长度，即历史轨迹保留的最大长度
+
+    tracker.setSettings(settings);
+
+    return true;
+}
+
+
+///
+/// \brief 对融合后的目标进行跟踪，并以跟踪后的最优估计值更新融合目标的状态信息
+///
+iv::fusion::fusionobjectarray Tracking(iv::fusion::fusionobjectarray& fusionobjvec, CTracker& tracker)
+{
+#ifdef DEBUG_SHOW
+    std::cout<<"-------------------------------------------------"<<std::endl;
+#endif
+    iv::fusion::fusionobjectarray trackedobjvec;
+    trackedobjvec.clear_obj();
+    trackedobjvec.set_timestamp(fusionobjvec.timestamp());
+    regions_t regions;
+    cv::Point3f pointXYZ;
+    for(int i = 0;i<fusionobjvec.obj_size();i++)
+    {
+        pointXYZ.x = fusionobjvec.obj(i).centroid().x();
+        pointXYZ.y = fusionobjvec.obj(i).centroid().y();
+        pointXYZ.z = fusionobjvec.obj(i).centroid().z();
+        Rect3D rect;
+        rect.center = pointXYZ;
+        rect.size.width = fusionobjvec.obj(i).dimensions().x();
+        rect.size.height = fusionobjvec.obj(i).dimensions().y();
+        rect.size.length = fusionobjvec.obj(i).dimensions().z();
+        rect.yaw = fusionobjvec.obj(i).yaw();
+        CRegion region = CRegion(rect,fusionobjvec.obj(i).type(),fusionobjvec.obj(i).prob());
+        regions.push_back(region);
+#ifdef DEBUG_SHOW
+        std::cout<<"old id:"<<i<<std::endl;
+        std::cout<<"old type:"<<fusionobjvec.obj(i).type()<<std::endl;
+        std::cout<<"old x,y,z,w,h,l,yaw:"<<rect.center.x<<","<<rect.center.y<<","<<rect.center.z<<"  "<<rect.size.width<<","<<rect.size.height<<","<<rect.size.length<<"  "<<rect.yaw<<std::endl;
+#endif
+    }
+    tracker.Update(regions, cv::UMat(), 30);
+    auto tracks = tracker.GetTracks();
+#ifdef DEBUG_SHOW
+    std::cout<<"fusion size, tracker size:"<<regions.size()<<","<<tracks.size()<<std::endl;
+#endif
+    for (size_t i = 0; i < tracks.size(); i++)
+    {
+        const auto& track = tracks[i];
+        if(track.m_detectedFrames<MIN_DETECTEDFRAMES) continue;
+        int obj_id = track.m_regionID;
+        iv::fusion::fusionobject fusion_object;
+        if(obj_id != -1) // 当前融合目标成功匹配上跟踪器中的已有目标，则根据跟踪所得的最优估计值更新融合目标状态
+        {
+            fusion_object = fusionobjvec.obj(obj_id);
+            fusion_object.set_id(track.m_ID);
+
+            iv::fusion::PointXYZ centroid;
+            iv::fusion::PointXYZ *centerpoint;
+            centroid.set_x(track.m_rect.center.x);
+            centroid.set_y(track.m_rect.center.y);
+            centroid.set_z(track.m_rect.center.z);
+            centerpoint=fusion_object.mutable_centroid();
+            centerpoint->CopyFrom(centroid);
+/* not update */
+//            iv::fusion::Dimension dimension;
+//            iv::fusion::Dimension *obj_dimension;
+//            dimension.set_x(track.m_rect.size.width);
+//            dimension.set_y(track.m_rect.size.height);
+//            dimension.set_z(track.m_rect.size.length);
+//            obj_dimension=fusion_object.mutable_dimensions();
+//            obj_dimension->CopyFrom(dimension);
+
+//            fusion_object.set_yaw(track.m_rect.yaw);
+
+//            iv::fusion::VelXY vel_relative;
+//            iv::fusion::VelXY *velrelative;
+//            vel_relative.set_x(track.m_velocity[0]);
+//            vel_relative.set_y(track.m_velocity[1]);
+//            velrelative = fusion_object.mutable_vel_relative();
+//            velrelative->CopyFrom(vel_relative);
+
+            iv::fusion::PointXYZ point_historical;
+            for(int j=0;j<track.m_trace.size();j++)
+            {
+                point_historical.set_x(track.m_trace[j].x);
+                point_historical.set_y(track.m_trace[j].y);
+                point_historical.set_z(track.m_trace[j].z);
+                iv::fusion::PointXYZ *p = fusion_object.add_point_historical();
+                p->CopyFrom(point_historical);
+            }
+
+            iv::fusion::fusionobject *pe = trackedobjvec.add_obj();
+            pe->CopyFrom(fusion_object);
+        }else{ // 当前时刻没有融合目标与跟踪器中的已有目标匹配上，则将跟踪器中已有目标的预测结果增加到融合结果中
+            fusion_object.set_id(track.m_ID);
+            fusion_object.set_type(track.m_region.m_type);
+
+            iv::fusion::PointXYZ centroid;
+            iv::fusion::PointXYZ *centerpoint;
+            centroid.set_x(track.m_rect.center.x);
+            centroid.set_y(track.m_rect.center.y);
+            centroid.set_z(track.m_rect.center.z);
+            centerpoint=fusion_object.mutable_centroid();
+            centerpoint->CopyFrom(centroid);
+
+            iv::fusion::Dimension dimension;
+            iv::fusion::Dimension *obj_dimension;
+            dimension.set_x(track.m_region.m_rect.size.width);
+            dimension.set_y(track.m_region.m_rect.size.height);
+            dimension.set_z(track.m_region.m_rect.size.length);
+            obj_dimension=fusion_object.mutable_dimensions();
+            obj_dimension->CopyFrom(dimension);
+
+            fusion_object.set_yaw(track.m_region.m_rect.yaw);
+
+            iv::fusion::VelXY vel_relative;
+            iv::fusion::VelXY *velrelative;
+            vel_relative.set_x(track.m_velocity[0]*FPS);
+            vel_relative.set_y(track.m_velocity[1]*FPS);
+            velrelative = fusion_object.mutable_vel_relative();
+            velrelative->CopyFrom(vel_relative);
+
+            iv::fusion::PointXYZ point_historical;
+            for(int j=0;j<track.m_trace.size();j++)
+            {
+                point_historical.set_x(track.m_trace[j].x);
+                point_historical.set_y(track.m_trace[j].y);
+                point_historical.set_z(track.m_trace[j].z);
+                iv::fusion::PointXYZ *p = fusion_object.add_point_historical();
+                p->CopyFrom(point_historical);
+            }
+
+            iv::fusion::fusionobject *pe = trackedobjvec.add_obj();
+            pe->CopyFrom(fusion_object);
+        }
+#ifdef DEBUG_SHOW
+        std::cout<<"id:"<<fusion_object.id()<<"  "<<obj_id<<std::endl;
+        std::cout<<"type:"<<fusion_object.type()<<std::endl;
+        std::cout<<"update x,y,z,w,h,l,yaw,vx,vy:"<<fusion_object.centroid().x()<<","<<fusion_object.centroid().y()<<","<<fusion_object.centroid().z()<<"  "<<fusion_object.dimensions().x()<<","<<fusion_object.dimensions().y()<<","<<fusion_object.dimensions().z()<<"  "<<fusion_object.yaw()<<"  "<<fusion_object.vel_relative().x()<<","<<fusion_object.vel_relative().y()<<std::endl;
+#endif
+    }
+#ifdef DEBUG_SHOW
+    std::cout<<"trackedobjvec size:"<<trackedobjvec.obj_size()<<std::endl;
+#endif
+//    for (size_t i = 0; i < trackedobjvec.obj_size(); i++)
+//    {
+//        iv::fusion::fusionobject fusion_object;
+//        fusion_object = trackedobjvec.obj(i);
+//        std::cout<<"historical size:"<<fusion_object.point_historical_size()<<std::endl;
+//    }
+    regions.clear();
+    tracks.clear();
+    return trackedobjvec;
+}