提交代码

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::VectorXd observation = Eigen::VectorXd::Zero(size);
+    for (auto iter : data)
+        observation << iter;
+    kf_->x_.head(size) << observation;
+    hit_streak_++;
+}
+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_++;
+
+    // observation - center_x, center_y, area, ratio
+    int size = data.size();
+    Eigen::VectorXd observation = Eigen::VectorXd::Zero(size);
+    for (auto iter : data)
+        observation << iter;
+    kf_->Update(observation);
+}
+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;
+}
+float BaseTrack::GetNIS() const
+{
+    if (kf_ == nullptr)
+        return 0;
+    return kf_->NIS_;
+}
+
+bool BaseTrack::IsLost()
+{
+    return coast_cycles_ > m_kMaxCoastCycles;
+}

BaseTracker/BaseTrack.h

+#pragma once
+
+#include <eigen3/Eigen/Dense>
+#include "kalman_filter.h"
+#include <vector>
+#include <memory>
+
+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 int GetStateData(std::vector<float>& data);
+    virtual float GetNIS() const;
+    virtual bool IsLost();
+
+    virtual float CalculateIou(std::vector<float>& data) = 0;
+
+    int coast_cycles_ = 0, hit_streak_ = 0;
+
+    int m_num_states = 0;
+    int m_num_obs = 0;
+
+    float m_prob = 0.0f;//计算iou的配置度
+
+    int m_iou_threshold = 0.3;//iou计算之后匹配的最小值
+    int m_kMaxCoastCycles = 2;//predict之后，计算几次认为丢失。
+
+
+    std::shared_ptr<mytracker::KalmanFilter> kf_ = nullptr;
+};

BaseTracker/BaseTracker.h

+#pragma once
+
+
+#include <iostream>
+//#include <eigen3/Eigen/Dense>
+//#include "kalman_filter.h"
+#include <map>
+//#include "BaseTrack.h"
+#include <memory>
+#include <vector>
+#include "iou.h"
+
+template<class T>
+class BaseTracker
+{
+public:
+	BaseTracker() {}
+
+	int Run(const std::vector<std::vector<float> >& detections,std::vector<uint64_t>& detectionsId,std::map<uint64_t,int>& addId,std::vector<uint64_t>& lostId);
+
+	std::map<uint64_t, std::shared_ptr<T> >& GetStates();
+
+
+	void AssociateDetectionsToTrackers(const std::vector<std::vector<float> >& detections, std::map<uint64_t, std::shared_ptr<T> >& tracks, std::map<uint64_t, int>& matched, std::vector<int>& unmatched_det);
+
+public:
+
+	std::map<uint64_t, std::shared_ptr<T> > m_tracker;
+
+	//CalculateIouCallback m_cbIou = nullptr;
+	//float m_iou_threshold = 0.3f;//匈牙利算法大于这个iou值为配置成功
+	//int m_kMaxCoastCycles = 2;//卡尔曼算法大于这个值就删除
+
+	uint64_t m_countId = 0;//生成物体id的累加值
+
+	//std::map<uint64_t, StateInfo> m_states;
+};
+
+
+template<class T>
+int BaseTracker<T>::Run(const std::vector<std::vector<float> >& detections, std::vector<uint64_t>& detectionsId, std::map<uint64_t, int>& addId, std::vector<uint64_t>& lostId)
+{
+    /*** Predict internal tracks from previous frame ***/
+    for (auto& track : m_tracker)
+    {
+        track.second->Predict();
+    }
+    if (detections.empty())
+    {
+        return 0;
+    }
+    detectionsId.resize(detections.size());
+    // Hash-map between track ID and associated detection bounding box
+    std::map<uint64_t, int> matched;
+    // vector of unassociated detections
+    std::vector<int> unmatched_det;
+
+    // return values - matched, unmatched_det
+    AssociateDetectionsToTrackers(detections, m_tracker, matched, unmatched_det);
+
+    /*** Update tracks with associated bbox ***/
+    for (const auto& match : matched)
+    {
+        const auto& id = match.first;
+        m_tracker[id]->Update(detections[match.second]);
+        detectionsId[match.second] = id;
+    }
+
+    /*** Create new tracks for unmatched detections ***/
+    for (const auto& det : unmatched_det)
+    {
+        std::shared_ptr<T> trackPtr = std::make_shared<T>();
+        trackPtr->Init(detections[det]);
+        // Create new track and generate new ID
+        uint64_t newId = ++m_countId;
+        m_tracker[newId] = trackPtr;
+        addId[newId] = det;
+    }
+
+    /*** Delete lose tracked tracks ***/
+    for (auto it = m_tracker.begin(); it != m_tracker.end();)
+    {
+        if (it->second->IsLost())
+        {
+            lostId.push_back(it->first);
+            it = m_tracker.erase(it);
+        }
+        else
+        {
+            it++;
+        }
+    }
+    return 0;
+}
+
+template<class T>
+std::map<uint64_t, std::shared_ptr<T> >& BaseTracker<T>::GetStates()
+{
+    return m_tracker;
+}
+
+template<class T>
+void BaseTracker<T>::AssociateDetectionsToTrackers(const std::vector<std::vector<float> >& detections, std::map<uint64_t, std::shared_ptr<T> >& tracks, std::map<uint64_t, int>& matched, std::vector<int>& unmatched_det)
+{
+    if (tracks.empty())
+    {
+        //不做匹配
+        for (int i = 0; i < detections.size(); i++)
+        {
+            unmatched_det.push_back(i);
+        }
+        return;
+    }
+
+    std::vector<std::vector<float>> iou_matrix;
+    // resize IOU matrix based on number of detection and tracks
+    iou_matrix.resize(detections.size(), std::vector<float>(tracks.size()));
+
+    std::vector<std::vector<float>> association;
+    // resize association matrix based on number of detection and tracks
+    association.resize(detections.size(), std::vector<float>(tracks.size()));
+
+
+    // row - detection, column - tracks
+    for (size_t i = 0; i < detections.size(); i++)
+    {
+        size_t j = 0;
+        for (const auto& trk : tracks)
+        {
+            iou_matrix[i][j] = trk.second->CalculateIou(detections[i]);
+            j++;
+        }
+    }
+
+    // Find association
+    HungarianMatching(iou_matrix, detections.size(), tracks.size(), association);
+
+    for (size_t i = 0; i < detections.size(); i++)
+    {
+        bool matched_flag = false;
+        size_t j = 0;
+        for (auto& trk : tracks)
+        {
+            if (0 == association[i][j])
+            {
+                // Filter out matched with low IOU
+                if (iou_matrix[i][j] >= trk.second->m_iou_threshold)
+                {
+                    matched[trk.first] = i;
+                    trk.second->m_prob = iou_matrix[i][j];
+                    matched_flag = true;
+                }
+                // It builds 1 to 1 association, so we can break from here
+                break;
+            }
+            j++;
+        }
+        // if detection cannot match with any tracks
+        if (!matched_flag) {
+            unmatched_det.push_back(i);
+        }
+    }
+}
+

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);
+
+

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;
+}
+
+
+float Track2D::CalculateIou(std::vector<float>& data)
+{
+    //auto trk = track->GetStateAsBbox();
+    //// get min/max points
+    //auto xx1 = std::max(det.tl().x, trk.tl().x);
+    //auto yy1 = std::max(det.tl().y, trk.tl().y);
+    //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;
+
+	return 0.0f;
+}

BaseTracker/Track2D.h

+#pragma once
+
+#include <vector>
+#include "BaseTrack.h"
+
+
+class Track2D :public BaseTrack
+{
+public:
+    // Constructor
+    Track2D();
+    ~Track2D() {}
+
+    virtual float CalculateIou(std::vector<float>& data);
+};

BaseTracker/Track3D.cpp

+
+#include "Track3D.h"
+#include <eigen3/Eigen/Dense>
+#include "Iou.h"
+
+Track3D::Track3D():BaseTrack(10, 7)
+{
+    std::shared_ptr<mytracker::KalmanFilter> kf = std::make_shared<mytracker::KalmanFilter>(10, 7);
+    kf->F_ <<
+        1, 0, 0, 0, 0, 0, 0, 1, 0, 0,
+        0, 1, 0, 0, 0, 0, 0, 0, 1, 0,
+        0, 0, 1, 0, 0, 0, 0, 0, 0, 1,
+        0, 0, 0, 1, 0, 0, 0, 0, 0, 0,
+        0, 0, 0, 0, 1, 0, 0, 0, 0, 0,
+        0, 0, 0, 0, 0, 1, 0, 0, 0, 0,
+        0, 0, 0, 0, 0, 0, 1, 0, 0, 0,
+        0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
+        0, 0, 0, 0, 0, 0, 0, 0, 1, 0,
+        0, 0, 0, 0, 0, 0, 0, 0, 0, 1;
+    kf->P_ <<
+        10, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+        0, 10, 0, 0, 0, 0, 0, 0, 0, 0,
+        0, 0, 10, 0, 0, 0, 0, 0, 0, 0,
+        0, 0, 0, 10, 0, 0, 0, 0, 0, 0,
+        0, 0, 0, 0, 10, 0, 0, 0, 0, 0,
+        0, 0, 0, 0, 0, 10, 0, 0, 0, 0,
+        0, 0, 0, 0, 0, 0, 10, 0, 0, 0,
+        0, 0, 0, 0, 0, 0, 0, 10000, 0, 0,
+        0, 0, 0, 0, 0, 0, 0, 0, 10000, 0,
+        0, 0, 0, 0, 0, 0, 0, 0, 0, 10000;
+    kf->H_ <<
+        1, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+        0, 1, 0, 0, 0, 0, 0, 0, 0, 0,
+        0, 0, 1, 0, 0, 0, 0, 0, 0, 0,
+        0, 0, 0, 1, 0, 0, 0, 0, 0, 0,
+        0, 0, 0, 0, 1, 0, 0, 0, 0, 0,
+        0, 0, 0, 0, 0, 1, 0, 0, 0, 0,
+        0, 0, 0, 0, 0, 0, 1, 0, 0, 0;
+    kf->Q_ <<
+        1, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+        0, 1, 0, 0, 0, 0, 0, 0, 0, 0,
+        0, 0, 1, 0, 0, 0, 0, 0, 0, 0,
+        0, 0, 0, 1, 0, 0, 0, 0, 0, 0,
+        0, 0, 0, 0, 1, 0, 0, 0, 0, 0,
+        0, 0, 0, 0, 0, 1, 0, 0, 0, 0,
+        0, 0, 0, 0, 0, 0, 1, 0, 0, 0,
+        0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0,
+        0, 0, 0, 0, 0, 0, 0, 0, 0.0001, 0,
+        0, 0, 0, 0, 0, 0, 0, 0, 0, 0.0001;
+    kf->R_ <<
+        1, 0, 0, 0, 0, 0, 0,
+        0, 1, 0, 0, 0, 0, 0,
+        0, 0, 1, 0, 0, 0, 0,
+        0, 0, 0, 10, 0, 0, 0,
+        0, 0, 0, 0, 10, 0, 0,
+        0, 0, 0, 0, 0, 10, 0,
+        0, 0, 0, 0, 0, 0, 10;
+
+    kf_ = kf;
+}
+
+float Track3D::CalculateIou(std::vector<float>& data)
+{
+
+    return 0.0f;
+}

BaseTracker/Track3D.h

+#pragma once
+
+#include <vector>
+#include "BaseTrack.h"
+#include "kalman_filter.h"
+
+
+class Track3D : public BaseTrack
+{
+public:
+    // Constructor
+    Track3D();
+    ~Track3D() {}
+
+    virtual float CalculateIou(std::vector<float>& data);
+
+};

BaseTracker/iou.cpp

+#include "Iou.h"
+
+
+void HungarianMatching(const std::vector<std::vector<float>>& iou_matrix,
+    size_t nrows, size_t ncols,
+    std::vector<std::vector<float>>& association)
+{
+    Matrix<float> matrix(nrows, ncols);
+    // Initialize matrix with IOU values
+    for (size_t i = 0; i < nrows; i++) {
+        for (size_t j = 0; j < ncols; j++) {
+            // Multiply by -1 to find max cost
+            if (iou_matrix[i][j] != 0) {
+                matrix(i, j) = -iou_matrix[i][j];
+            }
+            else {
+                // TODO: figure out why we have to assign value to get correct result
+                matrix(i, j) = 1.0f;
+            }
+        }
+    }
+
+    //    // Display begin matrix state.
+    //    for (size_t row = 0 ; row < nrows ; row++) {
+    //        for (size_t col = 0 ; col < ncols ; col++) {
+    //            std::cout.width(10);
+    //            std::cout << matrix(row,col) << ",";
+    //        }
+    //        std::cout << std::endl;
+    //    }
+    //    std::cout << std::endl;
+
+
+        // Apply Kuhn-Munkres algorithm to matrix.
+    Munkres<float> m;
+    m.solve(matrix);
+
+    //    // Display solved matrix.
+    //    for (size_t row = 0 ; row < nrows ; row++) {
+    //        for (size_t col = 0 ; col < ncols ; col++) {
+    //            std::cout.width(2);
+    //            std::cout << matrix(row,col) << ",";
+    //        }
+    //        std::cout << std::endl;
+    //    }
+    //    std::cout << std::endl;
+
+    for (size_t i = 0; i < nrows; i++) {
+        for (size_t j = 0; j < ncols; j++) {
+            association[i][j] = matrix(i, j);
+        }
+    }
+}
+
+
+struct CVPoint2f
+{
+    CVPoint2f(float _x, float _y):x(_x),y(_y)
+    {
+    }
+    CVPoint2f(const CVPoint2f& obj)
+    {
+        x = obj.x;
+        y = obj.y;
+    }
+    float x;
+    float y;
+};
+
+
+bool bInBox(const std::vector<CVPoint2f> &vpBoxA, const CVPoint2f &p)
+{
+    std::vector<CVPoint2f> corners = vpBoxA;
+    for(int i = 0; i<vpBoxA.size(); i++)   //01230123
+        corners.push_back(vpBoxA[i]);
+
+    std::vector< std::vector<double> > linesA;
+    for(int i = 0; i<vpBoxA.size(); i++)
+    {
+        CVPoint2f p1 = corners[i];
+        CVPoint2f p2 = corners[i+1];
+        CVPoint2f p3 = corners[i+2];
+        double a;
+        if(p1.x - p2.x == 0)
+            a = -(p1.y - p2.y)/0.0001;
+        else
+            a = -(p1.y - p2.y)/(p1.x - p2.x);
+
+        double b = 1;
+        double c = -a * p2.x - p2.y;
+        double d = a*p3.x + b*p3.y + c;
+
+        std::vector<double> line{a,b,c,d};
+        linesA.push_back(line);
+    }
+
+    for(int i=0; i<linesA.size(); i++)
+    {
+        std::vector<double > l = linesA[i];
+        double y = l[0] * p.x + l[1] * p.y +l[2];
+        if(y * l[3] < 0)
+            return false;
+    }
+    return true;
+}
+
+double InterSection_2D(const std::vector<CVPoint2f> &vpBoxA, const std::vector<CVPoint2f> &vpBoxB)
+{
+    double min_x, max_x, min_y, max_y;
+    min_x = vpBoxA[0].x;
+    max_x = vpBoxA[0].x;
+    min_y = vpBoxA[0].y;
+    max_y = vpBoxA[0].y;
+
+    for(int i=1; i<vpBoxA.size(); i++)
+    {
+        CVPoint2f p = vpBoxA[i];
+        if(p.x > max_x)
+            max_x = p.x;
+        if(p.x < min_x)
+            min_x = p.x;
+        if(p.y > max_y)
+            max_y = p.y;
+        if(p.y < min_y)
+            min_y = p.y;
+    }
+    for(int i=0; i<vpBoxB.size(); i++)
+    {
+        CVPoint2f p = vpBoxB[i];
+        if(p.x > max_x)
+            max_x = p.x;
+        if(p.x < min_x)
+            min_x = p.x;
+        if(p.y > max_y)
+            max_y = p.y;
+        if(p.y < min_y)
+            min_y = p.y;
+    }
+
+    //将两个BBox的定点坐标最小值设置为0, 以防止有负数的产生
+    std::vector<CVPoint2f> vpBoxAA = vpBoxA;
+    std::vector<CVPoint2f> vpBoxBB = vpBoxB;
+
+    //if(min_x < 0 && min_y < 0)
+    for(int i=0; i<vpBoxA.size(); i++)
+    {
+        vpBoxAA[i].x = vpBoxAA[i].x - min_x;
+        vpBoxAA[i].y = vpBoxAA[i].y - min_y;
+        vpBoxBB[i].x = vpBoxBB[i].x - min_x;
+        vpBoxBB[i].y = vpBoxBB[i].y - min_y;
+    }
+
+    int imax_x = (int)((max_x - min_x) * 10000);
+    int imax_y = (int)((max_y - min_y) * 10000);
+
+    double points_inA = 0, points_inB = 0, points_inAB = 0;
+    srand((int)time(0));
+    for(int i = 0; i<100000; i++)
+    {
+        int xx = rand()%(imax_x)+1;  //生成[1, imax_x]之间的整数
+        int yy = rand()%(imax_y)+1;  //生成[1, imax_y]之间的整数
+
+        CVPoint2f p((float)xx / 10000.0, (float)yy / 10000.0);
+        if( bInBox(vpBoxAA, p) )
+            ++points_inA;
+        if( bInBox(vpBoxBB, p) )
+            ++points_inB;
+        if( bInBox(vpBoxAA, p) && bInBox(vpBoxBB, p) )
+            ++points_inAB;
+    }
+
+    double iou = points_inAB / (points_inA + points_inB - points_inAB);
+    //cout<<"points_inA : "<<points_inA<<",  points_inB: "<<points_inB<<" ,points_inAB: "<<points_inAB<<endl;
+    return iou;
+}
+//double CuboidIoU(const Eigen::MatrixXd &truth_poses, const Eigen::MatrixXd &landmark_poses)
+double CuboidIoU(const std::vector<float> &truth_poses, const std::vector<float> &landmark_poses)
+{
+
+    std::vector<CVPoint2f>  vground_points;
+    std::vector<CVPoint2f>  vlandmark_points;
+    if(1)  //通过坐标旋转求取groundtruth立方体中 2D-Boundbox四个顶点的坐标
+    {
+        //double cen_x = truth_poses(0,0);
+        //double cen_y = truth_poses(0,1);
+        //double len = truth_poses(0,6);
+        //double wid = truth_poses(0,7);
+        //double yaw = truth_poses(0,5);
+        double cen_x = truth_poses[0];
+        double cen_y = truth_poses[1];
+        double len = truth_poses[6];
+        double wid = truth_poses[7];
+        double yaw = truth_poses[5];
+
+        double x, y, xx, yy;
+        x = cen_x - len;
+        y = cen_y - wid;
+        xx = (x - cen_x)*cos(yaw) - (y - cen_y)*sin(yaw) + cen_x;
+        yy = (x - cen_x)*sin(yaw) + (y - cen_y)*cos(yaw) + cen_y;
+        CVPoint2f p0(xx, yy);
+
+        x = cen_x - len;
+        y = cen_y + wid;
+        xx = (x - cen_x)*cos(yaw) - (y - cen_y)*sin(yaw) + cen_x;
+        yy = (x - cen_x)*sin(yaw) + (y - cen_y)*cos(yaw) + cen_y;
+        CVPoint2f p1(xx, yy);
+
+        x = cen_x + len;
+        y = cen_y + wid;
+        xx = (x - cen_x)*cos(yaw) - (y - cen_y)*sin(yaw) + cen_x;
+        yy = (x - cen_x)*sin(yaw) + (y - cen_y)*cos(yaw) + cen_y;
+        CVPoint2f p2(xx, yy);
+
+        x = cen_x + len;
+        y = cen_y - wid;
+        xx = (x - cen_x)*cos(yaw) - (y - cen_y)*sin(yaw) + cen_x;
+        yy = (x - cen_x)*sin(yaw) + (y - cen_y)*cos(yaw) + cen_y;
+        CVPoint2f p3(xx, yy);
+
+        vground_points = {p0, p1, p2, p3};
+    }
+
+    if(1)//通过坐标旋转求取landmark中 2D-Boundbox四个顶点的坐标
+    {
+        //double cen_x = landmark_poses(0,0);
+        //double cen_y = landmark_poses(0,1);
+        //double len = landmark_poses(0,6);
+        //double wid = landmark_poses(0,7);
+        //double yaw = landmark_poses(0,5);
+        double cen_x = landmark_poses[0];
+        double cen_y = landmark_poses[1];
+        double len = landmark_poses[6];
+        double wid = landmark_poses[7];
+        double yaw = landmark_poses[5];
+
+        double x, y, xx, yy;
+        x = cen_x - len;
+        y = cen_y - wid;
+        xx = (x - cen_x)*cos(yaw) - (y - cen_y)*sin(yaw) + cen_x;
+        yy = (x - cen_x)*sin(yaw) + (y - cen_y)*cos(yaw) + cen_y;
+        CVPoint2f p0(xx, yy);
+
+        x = cen_x - len;
+        y = cen_y + wid;
+        xx = (x - cen_x)*cos(yaw) - (y - cen_y)*sin(yaw) + cen_x;
+        yy = (x - cen_x)*sin(yaw) + (y - cen_y)*cos(yaw) + cen_y;
+        CVPoint2f p1(xx, yy);
+
+        x = cen_x + len;
+        y = cen_y + wid;
+        xx = (x - cen_x)*cos(yaw) - (y - cen_y)*sin(yaw) + cen_x;
+        yy = (x - cen_x)*sin(yaw) + (y - cen_y)*cos(yaw) + cen_y;
+        CVPoint2f p2(xx, yy);
+
+        x = cen_x + len;
+        y = cen_y - wid;
+        xx = (x - cen_x)*cos(yaw) - (y - cen_y)*sin(yaw) + cen_x;
+        yy = (x - cen_x)*sin(yaw) + (y - cen_y)*cos(yaw) + cen_y;
+        CVPoint2f p3(xx, yy);
+
+        vlandmark_points = {p0, p1, p2, p3};
+    }
+
+    double iou_2d = InterSection_2D(vlandmark_points, vground_points);
+    std::cout << " iou_2d = " << iou_2d << std::endl;
+    double iou_3d = 0;
+    if (iou_2d > 0) {
+        //double tru_minz = truth_poses(0, 2) - truth_poses(0, 8);
+        //double tru_maxz = truth_poses(0, 2) + truth_poses(0, 8);
+
+        //double land_minz = landmark_poses(0, 2) - landmark_poses(0, 8);
+        //double land_maxz = landmark_poses(0, 2) + landmark_poses(0, 8);
+
+        double tru_minz = truth_poses[2] - truth_poses[8];
+        double tru_maxz = truth_poses[2] + truth_poses[8];
+
+        double land_minz = landmark_poses[2] - landmark_poses[8];
+        double land_maxz = landmark_poses[2] + landmark_poses[8];
+        if (land_maxz <= tru_maxz && land_maxz >= tru_minz) {
+            double height_iou = (land_maxz - tru_minz) / (tru_maxz - land_minz);
+            iou_3d = iou_2d * height_iou;
+        } else if (tru_maxz < land_maxz && tru_maxz > land_minz) {
+            double height_iou = (tru_maxz - land_minz) / (land_maxz - tru_minz);
+            iou_3d = iou_2d * height_iou;
+        }
+    }
+
+    return iou_3d;
+}
+
+
+//void main(int argc, char **argv)
+//{
+//    Eigen::MatrixXd truth_poses(1,9);
+//    truth_poses<<-0.4875, -0.798913, -1.125, 0, 0, -1.27409, 0.240477, 0.235315, 0.375;
+//    Eigen::MatrixXd landmark_poses(1,9);
+//    landmark_poses<<-0.506616, -0.796303, -1.20499, 0, 0, -0.345443, 0.22,  0.22, 0.4 ;
+//    double iou_3d = CuboidIoU_once(truth_poses, landmark_poses);
+//    cout<<"the iou of two cuboid is: "<<iou_3d<<endl;
+//    return;
+//}
\ No newline at end of file

jfx_tracking.cpp

+#include <ros/ros.h>
+//#include "image_synchronizer.h"
+//#include <yaml-cpp/yaml.h>
+
+//#include "cv_config.h"
+#include "Track3D.h"
+#include "Track2D.h"
+#include "BaseTracker.h"
+#include "iou.h"
+
+//using namespace jfx_vision;
+
+//std::unique_ptr<VisionConfig>  jfx_vision::g_visionConfig;
+
+int main(int argc, char*argv[]) {
+    //ROS_INFO("START JFX_VISION_NODE");
+    ros::init(argc, argv, "jfx_vision");
+    ros::NodeHandle nh("~");
+
+
+    BaseTracker<Track3D> tracker3d;
+    BaseTracker<Track2D> tracker2d;
+
+    std::map<uint64_t, std::shared_ptr<Track3D> >& a = tracker3d.GetStates();
+    std::map<uint64_t, std::shared_ptr<Track2D> >& b = tracker2d.GetStates();
+    
+
+    //std::vector<float> truth_poses{ -0.4875, -0.798913, -1.125, 0, 0, -1.27409, 0.240477, 0.235315, 0.375 };
+    std::vector<float> truth_poses{ -0.1875, -0.798913, -1.0, 0, 0, -1.27409, 0.240477, 0.235315, 0.375 };
+    //std::vector<float> landmark_poses{ -0.506616, -0.796303, -1.20499, 0, 0, -0.345443, 0.22,  0.22, 0.4 };
+    std::vector<float> landmark_poses{ -0.4875, -0.798913, -1.125, 0, 0, -1.27409, 0.240477, 0.47063, 0.375 };
+    double iou_3d = CuboidIoU(truth_poses, landmark_poses);
+    std::cout<<"the iou of two cuboid is: "<< iou_3d << std::endl;
+
+    //std::string config_yaml = "/data/catkin_ws_xishan/src/rosrv2x/jfx_rvconfig/param/vision_config.yaml";
+    //nh.param<std::string>("vision_config_path", config_yaml, "");
+
+    //g_visionConfig.reset(new VisionConfig(config_yaml));
+
+    //int cam_size = g_visionConfig->globalConf.camera_size;
+    //ROS_INFO("cam_size:%d", cam_size);
+
+    // ROS_INFO("CAM[0] roi: h %d,w %d,x %d,y %d", g_visionConfig->cameraParamConfigs_map[0].roiRect.height, g_visionConfig->cameraParamConfigs_map[0].roiRect.width, 
+    // g_visionConfig->cameraParamConfigs_map[0].roiRect.x, g_visionConfig->cameraParamConfigs_map[0].roiRect.y);
+
+    //MultiCamImageSynchronizer<sensor_msgs::Image> sync(nh, cam_size); 
+
+    ros::spin();
+
+    return 0;
+}

tracker/kalman_filter.cpp

+#include "kalman_filter.h"
+
+namespace mytracker {
+
+KalmanFilter::KalmanFilter(unsigned int num_states, unsigned int num_obs) :
+        num_states_(num_states), num_obs_(num_obs) {
+    /*** Predict ***/
+    // State vector
+    x_ = Eigen::VectorXd::Zero(num_states);
+    // Predicted(a prior) state vector
+    x_predict_ = Eigen::VectorXd::Zero(num_states);
+
+    // State transition matrix F_
+    F_ = Eigen::MatrixXd::Zero(num_states, num_states);
+
+    // Error covariance matrix P
+    P_ = Eigen::MatrixXd::Zero(num_states, num_states);
+    // Predicted(a prior) error covariance matrix
+    P_predict_ = Eigen::MatrixXd::Zero(num_states, num_states);
+
+    // Covariance matrix of process noise
+    Q_ = Eigen::MatrixXd::Zero(num_states, num_states);
+
+    /*** Update ***/
+    // Observation matrix
+    H_ = Eigen::MatrixXd::Zero(num_obs, num_states);
+
+    // Covariance matrix of observation noise
+    R_ = Eigen::MatrixXd::Zero(num_obs, num_obs);
+
+    log_likelihood_delta_ = 0.0;
+    NIS_ = 0.0;
+}
+
+
+void KalmanFilter::Coast() {
+    x_predict_ = F_ * x_;
+    P_predict_ = F_ * P_ * F_.transpose() + Q_;
+}
+
+
+void KalmanFilter::Predict() {
+    Coast();
+    x_ = x_predict_;
+    P_ = P_predict_;
+}
+
+
+Eigen::VectorXd KalmanFilter::PredictionToObservation(const Eigen::VectorXd &state) {
+    return (H_*state);
+}
+
+
+void KalmanFilter::Update(const Eigen::VectorXd& z) {
+    Eigen::VectorXd z_predict = PredictionToObservation(x_predict_);
+
+    // y - innovation, z - real observation, z_predict - predicted observation
+    Eigen::VectorXd y = z - z_predict;
+
+    Eigen::MatrixXd Ht = H_.transpose();
+
+    // S - innovation covariance
+    Eigen::MatrixXd S = H_ * P_predict_ * Ht + R_;
+
+    NIS_ = y.transpose() * S.inverse() * y;
+
+//    std::cout << std::endl;
+//    std::cout << "P_predict = " << std::endl;
+//    std::cout << P_predict_ << std::endl;
+//
+//
+//    std::cout << "Z = " << std::endl;
+//    std::cout << z << std::endl;
+//
+//    std::cout << "Z_pred = " << std::endl;
+//    std::cout << z_predict << std::endl;
+//
+//    std::cout << "y = " << std::endl;
+//    std::cout << y << std::endl;
+//
+//    std::cout << "S = " << std::endl;
+//    std::cout << S << std::endl;
+//
+//    std::cout << "NIS = " << NIS_ << std::endl;
+
+
+    // K - Kalman gain
+    Eigen::MatrixXd K = P_predict_ * Ht * S.inverse();
+
+    // Updated state estimation
+    x_ = x_predict_ + K * y;
+
+    Eigen::MatrixXd I = Eigen::MatrixXd::Identity(num_states_, num_states_);
+    // Joseph form
+    //P_ = (I - K * H_) * P_predict_ * (I - K * H_).transpose() + K * R_ * K.transpose();
+    // Optimal gain
+    P_ = (I - K * H_) * P_predict_;
+}
+
+
+float KalmanFilter::CalculateLogLikelihood(const Eigen::VectorXd& y, const Eigen::MatrixXd& S) {
+    float log_likelihood;
+
+    // Note: Computing log(M.determinant()) in Eigen C++ is risky for large matrices since it may overflow or underflow.
+    // compute the Cholesky decomposition of the innovation covariance matrix, because it is symmetric
+    // S = L * L^T = U^T * U
+    // then retrieve factor L in the decomposition
+    auto& L = S.llt().matrixL();
+
+    // find log determinant of innovation covariance matrix
+    float log_determinant = 0;
+    for (unsigned int i = 0; i < S.rows(); i++)
+        log_determinant += log(L(i, i));
+    log_determinant *= 2;
+
+    // log-likelihood expression for current iteration
+    log_likelihood = -0.5 * (y.transpose() * S.inverse() * y + num_obs_ * log(2 * M_PI) + log_determinant);
+
+    if (std::isnan(log_likelihood)) {
+    	log_likelihood = -1e50;
+	}
+
+	return log_likelihood;
+}
+
+}
\ No newline at end of file

tracker/kalman_filter.h

+#pragma once
+
+#include <iostream>
+#include <eigen3/Eigen/Dense>
+
+namespace mytracker {
+
+// abstract class for Kalman filter
+// implementation could be KF/EKF/UKF...
+class KalmanFilter {
+public:
+    /**
+     * user need to define H matrix & R matrix
+     * @param num_states
+     * @param num_obs
+     */
+    // constructor
+    explicit KalmanFilter(unsigned int num_states, unsigned int num_obs);
+
+    // destructor
+    virtual ~KalmanFilter() = default;
+
+    /**
+     * Coast state and state covariance using the process model
+     * User can use this function without change the internal
+     * tracking state x_
+     */
+    virtual void Coast();
+
+    /**
+     * Predict without measurement update
+     */
+    void Predict();
+
+    /**
+     * This function maps the true state space into the observed space
+     * using the observation model
+     * User can implement their own method for more complicated models
+     */
+    virtual Eigen::VectorXd PredictionToObservation(const Eigen::VectorXd &state);
+
+    /**
+     * Updates the state by using Extended Kalman Filter equations
+     * @param z The measurement at k+1
+     */
+    virtual void Update(const Eigen::VectorXd &z);
+
+    /**
+     * Calculate marginal log-likelihood to evaluate different parameter choices
+     */
+    float CalculateLogLikelihood(const Eigen::VectorXd& y, const Eigen::MatrixXd& S);
+
+    // State vector
+    Eigen::VectorXd x_, x_predict_;
+
+    // Error covariance matrix
+    Eigen::MatrixXd P_, P_predict_;
+
+    // State transition matrix
+    Eigen::MatrixXd F_;
+
+    // Covariance matrix of process noise
+    Eigen::MatrixXd Q_;
+
+    // measurement matrix
+    Eigen::MatrixXd H_;
+
+    // covariance matrix of observation noise
+    Eigen::MatrixXd R_;
+
+    unsigned int num_states_, num_obs_;
+
+    float log_likelihood_delta_;
+
+    float NIS_;
+};
+}
\ No newline at end of file

tracker/matrix.h

+/*
+ *   Copyright (c) 2007 John Weaver
+ *
+ *   This program is free software; you can redistribute it and/or modify
+ *   it under the terms of the GNU General Public License as published by
+ *   the Free Software Foundation; either version 2 of the License, or
+ *   (at your option) any later version.
+ *
+ *   This program is distributed in the hope that it will be useful,
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *   GNU General Public License for more details.
+ *
+ *   You should have received a copy of the GNU General Public License
+ *   along with this program; if not, write to the Free Software
+ *   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
+ */
+
+#ifndef _MATRIX_H_
+#define _MATRIX_H_
+
+#include <initializer_list>
+#include <cstdlib>
+#include <ostream>
+
+template <class T>
+class Matrix {
+public:
+  Matrix();
+  Matrix(const size_t rows, const size_t columns);
+  Matrix(const std::initializer_list<std::initializer_list<T>> init);
+  Matrix(const Matrix<T> &other);
+  Matrix<T> & operator= (const Matrix<T> &other);
+  ~Matrix();
+  // all operations modify the matrix in-place.
+  void resize(const size_t rows, const size_t columns, const T default_value = 0);
+  void clear();
+  T& operator () (const size_t x, const size_t y);
+  const T& operator () (const size_t x, const size_t y) const;
+  const T min() const;
+  const T max() const;
+  inline size_t minsize() { return ((m_rows < m_columns) ? m_rows : m_columns); }
+  inline size_t columns() const { return m_columns;}
+  inline size_t rows() const { return m_rows;}
+
+  friend std::ostream& operator<<(std::ostream& os, const Matrix &matrix)
+  {
+      os << "Matrix:" << std::endl;
+      for (size_t row = 0 ; row < matrix.rows() ; row++ )
+      {
+          for (size_t col = 0 ; col < matrix.columns() ; col++ )
+          {
+              os.width(8);
+              os << matrix(row, col) << ",";
+          }
+          os << std::endl;
+      }
+      return os;
+  }
+
+private:
+  T **m_matrix;
+  size_t m_rows;
+  size_t m_columns;
+};
+
+#include <cassert>
+#include <cstdlib>
+#include <algorithm>
+
+/*export*/ template <class T>
+Matrix<T>::Matrix() {
+    m_rows = 0;
+    m_columns = 0;
+    m_matrix = nullptr;
+}
+
+
+/*export*/ template <class T>
+Matrix<T>::Matrix(const std::initializer_list<std::initializer_list<T>> init) {
+    m_matrix = nullptr;
+    m_rows = init.size();
+    if ( m_rows == 0 ) {
+        m_columns = 0;
+    } else {
+        m_columns = init.begin()->size();
+        if ( m_columns > 0 ) {
+            resize(m_rows, m_columns);
+        }
+    }
+
+    size_t i = 0, j;
+    for ( auto row = init.begin() ; row != init.end() ; ++row, ++i ) {
+        assert ( row->size() == m_columns && "All rows must have the same number of columns." );
+        j = 0;
+        for ( auto value = row->begin() ; value != row->end() ; ++value, ++j ) {
+            m_matrix[i][j] = *value;
+        }
+    }
+}
+
+/*export*/ template <class T>
+Matrix<T>::Matrix(const Matrix<T> &other) {
+    if ( other.m_matrix != nullptr ) {
+        // copy arrays
+        m_matrix = nullptr;
+        resize(other.m_rows, other.m_columns);
+        for ( size_t i = 0 ; i < m_rows ; i++ ) {
+            for ( size_t j = 0 ; j < m_columns ; j++ ) {
+                m_matrix[i][j] = other.m_matrix[i][j];
+            }
+        }
+    } else {
+        m_matrix = nullptr;
+        m_rows = 0;
+        m_columns = 0;
+    }
+}
+
+/*export*/ template <class T>
+Matrix<T>::Matrix(const size_t rows, const size_t columns) {
+    m_matrix = nullptr;
+    resize(rows, columns);
+}
+
+/*export*/ template <class T>
+Matrix<T> &
+Matrix<T>::operator= (const Matrix<T> &other) {
+    if ( other.m_matrix != nullptr ) {
+        // copy arrays
+        resize(other.m_rows, other.m_columns);
+        for ( size_t i = 0 ; i < m_rows ; i++ ) {
+            for ( size_t j = 0 ; j < m_columns ; j++ ) {
+                m_matrix[i][j] = other.m_matrix[i][j];
+            }
+        }
+    } else {
+        // free arrays
+        for ( size_t i = 0 ; i < m_columns ; i++ ) {
+            delete [] m_matrix[i];
+        }
+
+        delete [] m_matrix;
+
+        m_matrix = nullptr;
+        m_rows = 0;
+        m_columns = 0;
+    }
+
+    return *this;
+}
+
+/*export*/ template <class T>
+Matrix<T>::~Matrix() {
+    if ( m_matrix != nullptr ) {
+        // free arrays
+        for ( size_t i = 0 ; i < m_rows ; i++ ) {
+            delete [] m_matrix[i];
+        }
+
+        delete [] m_matrix;
+    }
+    m_matrix = nullptr;
+}
+
+/*export*/ template <class T>
+void
+Matrix<T>::resize(const size_t rows, const size_t columns, const T default_value) {
+    assert ( rows > 0 && columns > 0 && "Columns and rows must exist." );
+
+    if ( m_matrix == nullptr ) {
+        // alloc arrays
+        m_matrix = new T*[rows]; // rows
+        for ( size_t i = 0 ; i < rows ; i++ ) {
+            m_matrix[i] = new T[columns]; // columns
+        }
+
+        m_rows = rows;
+        m_columns = columns;
+        clear();
+    } else {
+        // save array pointer
+        T **new_matrix;
+        // alloc new arrays
+        new_matrix = new T*[rows]; // rows
+        for ( size_t i = 0 ; i < rows ; i++ ) {
+            new_matrix[i] = new T[columns]; // columns
+            for ( size_t j = 0 ; j < columns ; j++ ) {
+                new_matrix[i][j] = default_value;
+            }
+        }
+
+        // copy data from saved pointer to new arrays
+        size_t minrows = std::min(rows, m_rows);
+        size_t mincols = std::min(columns, m_columns);
+        for ( size_t x = 0 ; x < minrows ; x++ ) {
+            for ( size_t y = 0 ; y < mincols ; y++ ) {
+                new_matrix[x][y] = m_matrix[x][y];
+            }
+        }
+
+        // delete old arrays
+        if ( m_matrix != nullptr ) {
+            for ( size_t i = 0 ; i < m_rows ; i++ ) {
+                delete [] m_matrix[i];
+            }
+
+            delete [] m_matrix;
+        }
+
+        m_matrix = new_matrix;
+    }
+
+    m_rows = rows;
+    m_columns = columns;
+}
+
+/*export*/ template <class T>
+void
+Matrix<T>::clear() {
+    assert( m_matrix != nullptr );
+
+    for ( size_t i = 0 ; i < m_rows ; i++ ) {
+        for ( size_t j = 0 ; j < m_columns ; j++ ) {
+            m_matrix[i][j] = 0;
+        }
+    }
+}
+
+/*export*/ template <class T>
+inline T&
+Matrix<T>::operator ()(const size_t x, const size_t y) {
+    assert ( x < m_rows );
+    assert ( y < m_columns );
+    assert ( m_matrix != nullptr );
+    return m_matrix[x][y];
+}
+
+
+/*export*/ template <class T>
+inline const T&
+Matrix<T>::operator ()(const size_t x, const size_t y) const {
+    assert ( x < m_rows );
+    assert ( y < m_columns );
+    assert ( m_matrix != nullptr );
+    return m_matrix[x][y];
+}
+
+
+/*export*/ template <class T>
+const T
+Matrix<T>::min() const {
+    assert( m_matrix != nullptr );
+    assert ( m_rows > 0 );
+    assert ( m_columns > 0 );
+    T min = m_matrix[0][0];
+
+    for ( size_t i = 0 ; i < m_rows ; i++ ) {
+        for ( size_t j = 0 ; j < m_columns ; j++ ) {
+            min = std::min<T>(min, m_matrix[i][j]);
+        }
+    }
+
+    return min;
+}
+
+
+/*export*/ template <class T>
+const T
+Matrix<T>::max() const {
+    assert( m_matrix != nullptr );
+    assert ( m_rows > 0 );
+    assert ( m_columns > 0 );
+    T max = m_matrix[0][0];
+
+    for ( size_t i = 0 ; i < m_rows ; i++ ) {
+        for ( size_t j = 0 ; j < m_columns ; j++ ) {
+            max = std::max<T>(max, m_matrix[i][j]);
+        }
+    }
+
+    return max;
+}
+
+#endif /* !defined(_MATRIX_H_) */
+

tracker/munkres.cpp

+/*
+ *   Copyright (c) 2007 John Weaver
+ *   Copyright (c) 2015 Miroslav Krajicek
+ *
+ *   This program is free software; you can redistribute it and/or modify
+ *   it under the terms of the GNU General Public License as published by
+ *   the Free Software Foundation; either version 2 of the License, or
+ *   (at your option) any later version.
+ *
+ *   This program is distributed in the hope that it will be useful,
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *   GNU General Public License for more details.
+ *
+ *   You should have received a copy of the GNU General Public License
+ *   along with this program; if not, write to the Free Software
+ *   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
+ */
+
+#include "munkres.h"
+
+template class Munkres<double>;
+template class Munkres<float>;
+template class Munkres<int>;
+

tracker/params.h

+#ifndef EDGE_TRACKER_PARAMS_H
+#define EDGE_TRACKER_PARAMS_H
+
+constexpr int kMaxCoastCycles = 2;
+
+constexpr int kMinHits = 4;
+
+// Set threshold to 0 to accept all detections
+constexpr float kMinConfidence = 0.5;
+
+#endif

tracker/track.cpp

+#include "track.h"
+
+
+Track::Track() : kf_(8, 4) {
+
+    /*** Define constant velocity model ***/
+    // state - center_x, center_y, width, height, v_cx, v_cy, v_width, v_height
+    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;
+
+    // Give high uncertainty to the unobservable initial velocities
+    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;
+}
+
+
+// Get predicted locations from existing trackers
+// dt is time elapsed between the current and previous measurements
+void Track::Predict() {
+    kf_.Predict();
+
+    // hit streak count will be reset
+    if (coast_cycles_ > 0) {
+        hit_streak_ = 0;
+    }
+    // accumulate coast cycle count
+    coast_cycles_++;
+}
+
+
+// Update matched trackers with assigned detections
+void Track::Update(const cv::Rect& bbox) {
+
+    // get measurement update, reset coast cycle count
+    coast_cycles_ = 0;
+    // accumulate hit streak count
+    hit_streak_++;
+
+    // observation - center_x, center_y, area, ratio
+    Eigen::VectorXd observation = ConvertBboxToObservation(bbox);
+    kf_.Update(observation);
+
+
+}
+
+
+// Create and initialize new trackers for unmatched detections, with initial bounding box
+void Track::Init(const cv::Rect &bbox) {
+    kf_.x_.head(4) << ConvertBboxToObservation(bbox);
+    hit_streak_++;
+}
+
+
+/**
+ * Returns the current bounding box estimate
+ * @return
+ */
+cv::Rect Track::GetStateAsBbox() const {
+    return ConvertStateToBbox(kf_.x_);
+}
+
+
+float Track::GetNIS() const {
+    return kf_.NIS_;
+}
+
+
+/**
+ * Takes a bounding box in the form [x, y, width, height] and returns z in the form
+ * [x, y, s, r] where x,y is the centre of the box and s is the scale/area and r is
+ * the aspect ratio
+ *
+ * @param bbox
+ * @return
+ */
+Eigen::VectorXd Track::ConvertBboxToObservation(const cv::Rect& bbox) const{
+    Eigen::VectorXd observation = Eigen::VectorXd::Zero(4);
+    auto width = static_cast<float>(bbox.width);
+    auto height = static_cast<float>(bbox.height);
+    float center_x = bbox.x + width / 2;
+    float center_y = bbox.y + height / 2;
+    observation << center_x, center_y, width, height;
+    return observation;
+}
+
+
+/**
+ * Takes a bounding box in the centre form [x,y,s,r] and returns it in the form
+ * [x1,y1,x2,y2] where x1,y1 is the top left and x2,y2 is the bottom right
+ *
+ * @param state
+ * @return
+ */
+cv::Rect Track::ConvertStateToBbox(const Eigen::VectorXd &state) const {
+    // state - center_x, center_y, width, height, v_cx, v_cy, v_width, v_height
+    auto width = static_cast<int>(state[2]);
+    auto height = static_cast<int>(state[3]);
+    auto tl_x = static_cast<int>(state[0] - width / 2.0);
+    auto tl_y = static_cast<int>(state[1] - height / 2.0);
+    cv::Rect rect(cv::Point(tl_x, tl_y), cv::Size(width, height));
+    return rect;
+}
\ No newline at end of file

tracker/track.h

+#pragma once
+
+#include <opencv2/core.hpp>
+
+#undef Success
+#include "kalman_filter.h"
+
+class Track {
+public:
+    // Constructor
+    Track();
+
+    // Destructor
+    ~Track() = default;
+
+    void Init(const cv::Rect& bbox);
+    void Predict();
+    void Update(const cv::Rect& bbox);
+    cv::Rect GetStateAsBbox() const;
+    float GetNIS() const;
+
+    int coast_cycles_ = 0, hit_streak_ = 0;
+
+private:
+    Eigen::VectorXd ConvertBboxToObservation(const cv::Rect& bbox) const;
+    cv::Rect ConvertStateToBbox(const Eigen::VectorXd &state) const;
+
+    mytracker::KalmanFilter kf_;
+};

tracker/utils.h

+#pragma once
+
+constexpr int kNumColors = 32;
+
+constexpr int kMaxCoastCycles = 1;
+
+constexpr int kMinHits = 3;
+
+// Set threshold to 0 to accept all detections
+constexpr float kMinConfidence = 0.6;
\ No newline at end of file