提交更新

src/BaseTracker/BaseTrack.cpp

+#include "BaseTrack.h"
+
+
+BaseTrack::BaseTrack(unsigned int num_states, unsigned int num_obs):m_num_states(num_states), m_num_obs(num_obs)
+{
+    //需要在派生类中赋值几个算法矩阵
+}
+
+void BaseTrack::Init(const std::vector<float>& data)
+{
+    if (kf_ == nullptr)
+        return;
+    int size = data.size();
+    if (size > m_num_obs)
+        return;
+    Eigen::VectorXf observation = Eigen::VectorXf::Zero(size);
+    for (int i = 0; i < data.size(); i++)
+        observation(i) = data[i];
+    kf_->x_.head(size) << observation;
+    hit_streak_++;
+    m_update_count++;
+}
+void BaseTrack::Predict()
+{
+    if (kf_ == nullptr)
+        return;
+    kf_->Predict();
+
+    // hit streak count will be reset
+    if (coast_cycles_ > 0) {
+        hit_streak_ = 0;
+    }
+    // accumulate coast cycle count
+    coast_cycles_++;
+}
+void BaseTrack::Update(const std::vector<float>& data)
+{
+    if (kf_ == nullptr)
+        return;
+    // get measurement update, reset coast cycle count
+    coast_cycles_ = 0;
+    // accumulate hit streak count
+    hit_streak_++;
+    m_update_count++;
+
+    // observation - center_x, center_y, area, ratio
+    int size = data.size();
+    Eigen::VectorXf observation = Eigen::VectorXf::Zero(size);
+    for (int i = 0; i < data.size(); i++)
+        observation(i) = data[i];
+    kf_->Update(observation);
+}
+void BaseTrack::UpdateDataCheck(const std::vector<float>& data, std::vector<float>& out)
+{
+
+}
+void BaseTrack::UpdateHit()
+{
+    if (kf_ == nullptr)
+        return;
+    // get measurement update, reset coast cycle count
+    coast_cycles_ = 0;
+    // accumulate hit streak count
+    hit_streak_++;
+    m_update_count++;
+}
+
+int BaseTrack::GetStateData(std::vector<float>& data)
+{
+    if (kf_ == nullptr)
+        return -1;
+    data.clear();
+    for (int i = 0; i < m_num_states; i++)
+        data.push_back(kf_->x_[i]);
+    return 0;
+}
+int BaseTrack::GetPredictData(std::vector<float>& data)
+{
+    if (kf_ == nullptr || m_num_obs > m_num_states)
+        return -1;
+    data.clear();
+    for (int i = 0; i < m_num_obs; i++)
+        data.push_back(kf_->x_predict_[i]);
+    return 0;
+}
+
+float BaseTrack::GetNIS() const
+{
+    if (kf_ == nullptr)
+        return 0;
+    return kf_->NIS_;
+}
+float BaseTrack::GetProb() const
+{
+    return m_prob;
+}
+
+bool BaseTrack::IsLost()
+{
+    return coast_cycles_ > m_kMaxCoastCycles;
+}
+bool BaseTrack::IsValid()
+{
+    return m_update_count >= m_updateValidCount;
+}
+int BaseTrack::GetIouData(std::vector<float>& data, int& obj_type)
+{
+    if (kf_ == nullptr || m_num_obs > m_num_states)
+        return -1;
+    data.clear();
+    for (int i = 0; i < m_num_obs; i++)
+        data.push_back(kf_->x_[i]);
+    return 0;
+}
+
+int BaseTrack::GetStatesNum()
+{
+    if (kf_ == nullptr)
+        return 0;
+    return kf_->num_states_;
+}
+int BaseTrack::GetObsNum()
+{
+    if (kf_ == nullptr)
+        return 0;
+    return kf_->num_obs_;
+}
+
+float* BaseTrack::GetStatesXPtr()
+{
+    if (kf_ == nullptr)
+        return nullptr;
+    return kf_->x_.data();
+}
+float* BaseTrack::GetPredictPtr()
+{
+    if (kf_ == nullptr)
+        return nullptr;
+    return kf_->P_.data();
+}
+float* BaseTrack::GetRPtr()
+{
+    if (kf_ == nullptr)
+        return nullptr;
+    return kf_->R_.data();
+}

src/BaseTracker/BaseTrack.h

+#pragma once
+
+#include <eigen3/Eigen/Dense>
+#include "kalman_filter.h"
+#include <vector>
+#include <memory>
+
+#define _PI_ 3.1415926
+
+
+class BaseTrack 
+{
+public:
+    // Constructor
+    BaseTrack(unsigned int num_states, unsigned int num_obs);
+
+    // Destructor
+    ~BaseTrack() = default;
+
+    //virtual std::shared_ptr<mytracker::KalmanFilter> InitKF(unsigned int num_states, unsigned int num_obs) { return nullptr; };
+    virtual void Init(const std::vector<float>& data);
+    virtual void Predict();
+    virtual void Update(const std::vector<float>& data);
+    virtual void UpdateDataCheck(const std::vector<float>& data, std::vector<float>& out);//对于输入数据进行修正
+    virtual void UpdateHit();
+    virtual int GetStateData(std::vector<float>& data);
+    virtual int GetPredictData(std::vector<float>& data);
+    virtual float GetNIS() const;
+    virtual float GetProb() const;
+    virtual bool IsLost();//数据是否丢失，如果不更新就会丢失
+    virtual bool IsValid();//数据是否有效
+
+    virtual int GetIouData(std::vector<float>& data,int& obj_type);
+    
+    virtual int GetIouDataOrder(std::vector<int>& order) = 0;
+    virtual int GetKFDataOrder(std::vector<int>& order) = 0;
+
+    virtual double CalculateIou(const std::vector<float>& data) = 0;
+
+    int GetStatesNum();
+    int GetObsNum();
+
+    float* GetStatesXPtr();
+    float* GetPredictPtr();
+    float* GetRPtr();
+
+    void SetIouThreshold(float threshold) { m_iou_threshold = threshold; }
+    void SetMaxCoastCycles(int cycles) { m_kMaxCoastCycles = cycles; }
+    void SetValidUpdateCount(int count) { m_updateValidCount = count; }
+    virtual void SetValues(std::vector<float>& data) {}
+
+    int coast_cycles_ = 0, hit_streak_ = 0;
+
+    int m_update_count = 0;//数据更新的次数
+
+    int m_num_states = 0;
+    int m_num_obs = 0;
+
+    float m_prob = 0.0f;//计算iou的配置度
+
+    float m_iou_threshold = 0.01;//iou计算之后匹配的最小值
+    int m_kMaxCoastCycles = 2;//predict之后，计算几次认为丢失。
+    int m_updateValidCount = 3;//更新多少次认为是一个有效数据。
+
+
+    std::shared_ptr<mytracker::KalmanFilter> kf_ = nullptr;
+};

src/BaseTracker/Iou.h

+#pragma once
+
+#include "munkres.h"
+#include "params.h"
+#include <vector>
+
+void HungarianMatching(const std::vector<std::vector<float>>& iou_matrix,
+    size_t nrows, size_t ncols,
+    std::vector<std::vector<float>>& association);
+
+double CuboidIoU(const std::vector<float>& truth_poses, const std::vector<float>& landmark_poses);
+
+

src/BaseTracker/Track2D.cpp

+#include "Track2D.h"
+
+
+Track2D::Track2D():BaseTrack(8, 4)
+{
+    std::shared_ptr<mytracker::KalmanFilter> kf = std::make_shared<mytracker::KalmanFilter>(8, 4);
+    kf->F_ <<
+        1, 0, 0, 0, 1, 0, 0, 0,
+        0, 1, 0, 0, 0, 1, 0, 0,
+        0, 0, 1, 0, 0, 0, 1, 0,
+        0, 0, 0, 1, 0, 0, 0, 1,
+        0, 0, 0, 0, 1, 0, 0, 0,
+        0, 0, 0, 0, 0, 1, 0, 0,
+        0, 0, 0, 0, 0, 0, 1, 0,
+        0, 0, 0, 0, 0, 0, 0, 1;
+    kf->P_ <<
+        10, 0, 0, 0, 0, 0, 0, 0,
+        0, 10, 0, 0, 0, 0, 0, 0,
+        0, 0, 10, 0, 0, 0, 0, 0,
+        0, 0, 0, 10, 0, 0, 0, 0,
+        0, 0, 0, 0, 10000, 0, 0, 0,
+        0, 0, 0, 0, 0, 10000, 0, 0,
+        0, 0, 0, 0, 0, 0, 10000, 0,
+        0, 0, 0, 0, 0, 0, 0, 10000;
+    kf->H_ <<
+        1, 0, 0, 0, 0, 0, 0, 0,
+        0, 1, 0, 0, 0, 0, 0, 0,
+        0, 0, 1, 0, 0, 0, 0, 0,
+        0, 0, 0, 1, 0, 0, 0, 0;
+    kf->Q_ <<
+        1, 0, 0, 0, 0, 0, 0, 0,
+        0, 1, 0, 0, 0, 0, 0, 0,
+        0, 0, 1, 0, 0, 0, 0, 0,
+        0, 0, 0, 1, 0, 0, 0, 0,
+        0, 0, 0, 0, 0.01, 0, 0, 0,
+        0, 0, 0, 0, 0, 0.01, 0, 0,
+        0, 0, 0, 0, 0, 0, 0.0001, 0,
+        0, 0, 0, 0, 0, 0, 0, 0.0001;
+    kf->R_ <<
+        1, 0, 0, 0,
+        0, 1, 0, 0,
+        0, 0, 10, 0,
+        0, 0, 0, 10;
+
+    kf_ = kf;
+}
+
+
+double Track2D::CalculateIou(const std::vector<float>& data)
+{
+    std::vector<float> states;
+    GetStateData(states);
+
+    //auto xx1 = std::max(states[0], data[0]);
+    //auto yy1 = std::max(states[1], data[1]);
+    //auto xx2 = std::min(det.br().x, trk.br().x);
+    //auto yy2 = std::min(det.br().y, trk.br().y);
+    //auto w = std::max(0, xx2 - xx1);
+    //auto h = std::max(0, yy2 - yy1);
+
+    //// calculate area of intersection and union
+    //float det_area = det.area();
+    //float trk_area = trk.area();
+    //auto intersection_area = w * h;
+    //float union_area = det_area + trk_area - intersection_area;
+    //auto iou = intersection_area / union_area;
+
+    auto w = std::max(std::min((states[0] + states[2]), (data[0] + data[2])) - std::max(states[0], data[0]), 0.0f);
+    auto h = std::max(std::min((states[1] + states[3]), (data[1] + data[3])) - std::max(states[1], data[1]), 0.0f);
+
+    auto intersection_area = w * h;
+    float union_area = states[2] * states[3] + data[2] * data[3] - intersection_area;
+    auto iou = intersection_area / union_area;
+	return iou;
+}

src/BaseTracker/Track2D.h

+#pragma once
+
+#include <vector>
+#include "BaseTrack.h"
+#ifdef _QICHECHENG_
+#include "jfxrosperceiver/det_tracking.h"
+#define jfx_common_msgs jfxrosperceiver
+#else
+#include "jfx_common_msgs/det_tracking.h"
+#endif
+
+using trackOjbPtr = std::shared_ptr< jfx_common_msgs::det_tracking>;
+
+class Track2D :public BaseTrack
+{
+public:
+    // Constructor
+    Track2D();
+    ~Track2D() {}
+
+    trackOjbPtr m_obj = nullptr;
+
+    virtual int GetIouDataOrder(std::vector<int>& order) { return 0; };
+    virtual int GetKFDataOrder(std::vector<int>& order) { return 0; };
+
+    virtual double CalculateIou(const std::vector<float>& data);
+
+    static void MeasureIouData(const std::vector<float>& input, std::vector<float>& out, int& obj_type) {}
+};

src/BaseTracker/kf_gpu/bev_overlap_online.h

+#ifndef _BEV_OVERLAP_ONLINE_H_
+#define _BEV_OVERLAP_ONLINE_H_
+
+
+
+void bev_overlap(const int num_a, float* boxes_a, const int num_b, float* boxes_b, float* ans_overlap);
+
+
+#endif
\ No newline at end of file

src/BaseTracker/kf_gpu/common.h

+#ifndef COMMON_H
+#define COMMON_H
+
+// headers in STL
+#include <stdio.h>
+
+// headers in CUDA
+//#include <cuda_runtime_api.h>
+
+
+#define DIVUP(m, n) ((m) / (n) + ((m) % (n) > 0))
+
+#define GPU_CHECK(ans)                                                                                                 \
+  {                                                                                                                    \
+    GPUAssert((ans), __FILE__, __LINE__);                                                                              \
+  }
+inline void GPUAssert(cudaError_t code, const char* file, int line, bool abort = true)
+{
+  if (code != cudaSuccess)
+  {
+    fprintf(stderr, "GPUassert: %s %s %d\n", cudaGetErrorString(code), file, line);
+    if (abort)
+      exit(code);
+  }
+}
+
+//Memory Allocation Function
+//void cpuMalloc(float **data_ptr, size_t size)
+//{
+//	float *data;
+//	data = (float *) malloc( size);
+//	*data_ptr = data;
+//}
+
+
+//Ideintity Matrix Generation
+//void Identity(float *data, int n)
+//{
+//        for (int i = 0; i < (n*n); i=i+1)
+//                {
+//                if((i%(n+1))==0)
+//                        data[i] = 1;
+//                else
+//                        data[i] = 0;
+//                }
+//}
+
+
+
+#endif  // COMMON_H

src/BaseTracker/kf_gpu/kalman_batch_ops.cu

+#include "common.h"
+#include <cuda_runtime.h>
+
+
+__global__ void MatSub(float* C, const float* A, const float* B, int bs, int no)
+{
+    int i = blockDim.x * blockIdx.x + threadIdx.x;
+    if (i < bs * no)
+        C[i] = A[i] - B[i];
+}
+
+
+
+// __global__ void KalmanUpdateS2(float* d_S, float* d_P, const int bs, const int ns, const int no){
+
+// 	int tid = blockDim.x * blockIdx.x + threadIdx.x;
+// 	if (tid >= bs)	return;
+// 	int p_stride = 11; 					
+// 	int s_stride = 8; 					
+
+// 	for (int i = 0; i < no; i++){
+// 		d_S[tid * no * no + i * s_stride] = d_P[tid * ns * ns + i * p_stride] + 1;
+// 	}
+// }
+
+
+
+__global__ void KalmanUpdateS2(float* d_S, float* d_P, float* d_R, const int bs, const int ns, const int no){
+
+int tid = blockDim.x * blockIdx.x + threadIdx.x;
+if (tid >= bs)	return;
+int p_stride = 11; 					
+int s_stride = 8; 					
+
+for (int i = 0; i < no; i++){
+	d_S[tid * no * no + i * s_stride] = d_P[tid * ns * ns + i * p_stride] + d_R[i * s_stride];
+}
+}
+
+
+__global__ void KalmanUpdateS3(float* d_K, float* d_P, float* d_S, const int bs, const int ns, const int no){
+
+	int tid = blockDim.x * blockIdx.x + threadIdx.x;
+	if (tid >= bs) return;
+	int p_stride = 11; 
+	int k_stride = 8;      				
+	int s_stride = 8; 
+
+	for (int i = 0; i < ns; i++){
+		if (i < no){
+			d_K[tid * ns * no + i * k_stride] = d_P[tid * ns * ns + i * p_stride] * (1 / d_S[tid * no * no + i * s_stride]);
+		}
+		else{
+			d_K[tid * ns * no + no * no + (i - no) * k_stride] = d_P[tid * ns * ns + ns * no + (i - no) * p_stride] * (1 / d_S[tid * no * no + (i - no) * s_stride]);
+		}
+	}	
+}
+
+
+__global__ void KalmanUpdateS4(float* d_X, float* d_K, float* d_Y, const int bs, const int ns, const int no){
+
+	int tid = blockDim.x * blockIdx.x + threadIdx.x;
+	if (tid >= bs) return;  				
+	int k_stride = 8; 
+
+	for (int i = 0; i < ns; i++){
+		if (i < no){
+			d_X[tid * ns + i] += d_K[tid * ns * no + i * k_stride] * d_Y[tid * no + i];
+		}
+		else{
+			
+			d_X[tid * ns + i] += d_K[tid * ns * no + no * no + (i - no) * k_stride] * d_Y[tid * no + i - no];
+		}
+	}	
+}
+
+
+__global__ void KalmanUpdateS5(float* d_P, float* d_K, const int bs, const int ns, const int no){
+
+	int tid = blockDim.x * blockIdx.x + threadIdx.x;
+	if (tid >= bs) return;
+	int p_stride = 11;      				
+	int k_stride = 8; 
+
+	for (int i = ns - 1; i > -1; i--){
+		if (i < no){
+			float IKH_tl = 1 - d_K[tid * ns * no + i * k_stride];
+
+			d_P[tid * ns * ns + i * p_stride] *= IKH_tl;						
+			if (i < 3) d_P[tid * ns * ns + no + i * p_stride] *= IKH_tl; 				
+		}
+		else{		
+			float IKH_bl = 0 - d_K[tid * ns * no + no * no + (i - no) * k_stride];
+			float IKH_br = 1;
+			float P_bl = d_P[tid * ns * ns + ns * no + (i - no) * p_stride];
+			float P_br = d_P[tid * ns * ns + i * p_stride];	
+			float P_tl = d_P[tid * ns * ns + (i - no) * p_stride];
+			float P_tr = d_P[tid * ns * ns + no + (i - no) * p_stride];
+
+			d_P[tid * ns * ns + ns * no + (i - no) * p_stride] = IKH_bl * P_tl + IKH_br * P_bl;	
+			d_P[tid * ns * ns + i * p_stride] = IKH_bl * P_tr + IKH_br * P_br;							
+		}
+	}	
+}
+
+
+
+
+

src/BaseTracker/kf_gpu/kalman_update_batch_online.h

+#ifndef _KALMAN_UPDATE_BATCH_ONLINE_H_
+#define _KALMAN_UPDATE_BATCH_ONLINE_H_
+
+
+
+void kalman_update_batch(float* Z,// measurement size = bs * no
+	float* X,     // in-place update states  size = bs * ns
+	float* P,     // in-place update predict size = bs * ns * ns
+	float* R,		//R covariance matrix of observation noise no * no
+	float* HX,	   // H*X  size = bs * no
+	const int bs,
+	const int ns,  //ns = 10
+	const int no   // no = 7
+);
+
+
+#endif  

src/Component.cpp

+
+#include "Component.h"
+#include <sys/time.h>
+#include <unistd.h>
+
+uint64_t GetCurTime()
+{
+    struct timeval time;
+    gettimeofday(&time, NULL);
+    uint64_t seconds = time.tv_sec;
+    uint64_t ttt = seconds * 1000 * 1000 + time.tv_usec;
+    return ttt;
+}
+
+std::string GetMatrixStr(const float* data_ptr, int col, int row)
+{
+    std::string str;
+    for (int i = 0; i < row; i++)
+    {
+        for (int j = 0; j < col; j++)
+        {
+            char log[128] = {};
+            sprintf(log, "%f,", data_ptr[i*col+j]);
+            str += log;
+        }
+    }
+    return str;
+}
+
+std::string GetMatrixStr(const std::vector<std::vector<float>>& data_ptr, int col, int row)
+{
+    std::string str;
+    for (int i = 0; i < col; i++)
+    {
+        for (int j = 0; j < row; j++)
+        {
+            char log[128] = {};
+            sprintf(log, "%f,", data_ptr[i][j]);
+            str += log;
+        }
+    }
+    return str;
+}
+std::string GetMatrixStr(const std::vector<std::vector<double>>& data_ptr, int col, int row)
+{
+    std::string str;
+    for (int i = 0; i < col; i++)
+    {
+        for (int j = 0; j < row; j++)
+        {
+            char log[128] = {};
+            sprintf(log, "%f,", data_ptr[i][j]);
+            str += log;
+        }
+    }
+    return str;
+}
+
+std::string GetMatrixStr(float** data_ptr, int col, int row)
+{
+    std::string str;
+    for (int i = 0; i < row; i++)
+    {
+        for (int j = 0; j < col; j++)
+        {
+            char log[128] = {};
+            sprintf(log, "%f,", data_ptr[i][j]);
+            str += log;
+        }
+    }
+    return str;
+}
+
+
+std::string GetTimeStr(uint64_t timestamp)
+{
+    time_t tt = timestamp / 1000;
+    int misc = timestamp % 1000;
+    struct tm* ptr;
+    ptr = localtime(&tt);
+    // printf("time: %d \n", tt);
+    char str[80];
+    strftime(str, sizeof(str), "%Y-%m-%d %H:%M:%S", ptr);
+    //2018-09-19 16:01:37.517
+    char tStr[128] = {};
+    sprintf(tStr, ".%d", misc);
+    std::string timeStr = std::string(str) + std::string(tStr);
+    return timeStr;
+}
+
+double calcIntersectionArea(cv::RotatedRect rect1, cv::RotatedRect rect2)
+{
+    std::vector<cv::Point2f> vertices;
+    int intersectionType = cv::rotatedRectangleIntersection(rect1, rect2, vertices);
+    if (vertices.size() == 0)
+        return 0.0;
+    else
+    {
+        std::vector<cv::Point2f> order_pts;
+        // 找到交集（交集的区域），对轮廓的各个点进行排序
+
+        cv::convexHull(cv::Mat(vertices), order_pts, true);
+        double area = cv::contourArea(order_pts);
+        //float inner = (float)(area / (areaRect1 + areaRect2 - area + 0.0001));
+        return area;
+    }
+}
+
+float calcIntersectionRate(cv::RotatedRect rect1, cv::RotatedRect rect2)
+{
+    double area = calcIntersectionArea(rect1, rect2);
+
+    float iou_2d = area / (rect1.size.width * rect1.size.height + rect2.size.width * rect2.size.height);
+    return iou_2d;
+}

src/Component.h

+#pragma once
+
+#include <string>
+#include <vector>
+#include <opencv2/opencv.hpp>
+
+uint64_t GetCurTime();
+
+std::string GetTimeStr(uint64_t timestamp);
+
+
+std::string GetMatrixStr(const float* data_ptr, int col, int row);
+std::string GetMatrixStr(const std::vector<std::vector<float>>& data_ptr, int col, int row);
+std::string GetMatrixStr(const std::vector<std::vector<double>>& data_ptr, int col, int row);
+std::string GetMatrixStr(float** data_ptr, int col, int row);
+
+double calcIntersectionArea(cv::RotatedRect rect1, cv::RotatedRect rect2);
+
+float calcIntersectionRate(cv::RotatedRect rect1, cv::RotatedRect rect2);