modularization/src/fusion/lidar_radar_fusion_cnn/Tracker/Tracking.hpp

#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;
}