SortTracker/SortTrack.cpp at main · nambn007/SortTracker · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
#include "SortTrack.h"

namespace tracker
{
SortTrack::SortTrack(Detection det)
{
    int n = NUM_STATE; // Number of states
    int m = NUM_MESUAREMENTS; // Number of mesuarements

    this->kf = KalmanFilter(n, m);

    Eigen::MatrixXd F(n, n); // System dynamics matrix
    Eigen::MatrixXd H(m, n); // Output matrix

    Eigen::MatrixXd Q(n, n); // Process noise convariance
    Eigen::MatrixXd P(n, n); // Esitmate noise convariance
    Eigen::MatrixXd R(m, m); // Measurement noise convariance

    F << 1, 0, 0, 0, 1, 0, 0,
         0, 1, 0, 0, 0, 1, 0,
         0, 0, 1, 0, 0, 0, 1,
         0, 0, 0, 1, 0, 0, 0,
         0, 0, 0, 0, 1, 0, 0,
         0, 0, 0, 0, 0, 1, 0,
         0, 0, 0, 0, 0, 0, 1;

    H << 1, 0, 0, 0, 0, 0, 0,
         0, 1, 0, 0, 0, 0, 0,
         0, 0, 1, 0, 0, 0, 0,
         0, 0, 0, 1, 0, 0, 0;

    R.block(2, 2, 2, 2) *= 10.0;
    P.block(4, 4, 3, 3) *= 1000;
    P *= 10.0f;
    Q(Q.rows() - 1, Q.cols() - 1) *= 0.01;
    Q.block(4, 4, 3, 3) *= 0.01;

    this->kf.x.head(4) = convertDet2VectorXd(det);

    this->time_since_update = 0;
    this->id = SortTrack::count++;
    this->det_history.clear();
    this->hits = 0;
    this->hit_streak = 0;
    this->age = 0;
}

void SortTrack::update(Detection det)
{
    this->time_since_update = 0;
    this->det_history.clear();
    this->hits += 1;
    this->hit_streak += 1;
    this->kf.update(convertDet2VectorXd(det));
}

Detection SortTrack::predict()
{
    if ((this->kf.x[6] + this->kf.x[2]) <= 0) {
        this->kf.x[6] *= 0.0f;
    }
    this->kf.predict();
    this->age += 1;
    if (this->time_since_update > 0) {
        this->hit_streak = 0;
    }
    this->time_since_update += 1;
    this->det_history.push_back(convertVectorXdtoDetection(this->kf.x));
    return this->det_history.back();
}

Detection SortTrack::getStateDetection()
{
    return convertVectorXdtoDetection(this->kf.x);
}
}