-
Notifications
You must be signed in to change notification settings - Fork 0
/
KalmanTracker.cpp
180 lines (137 loc) · 4.24 KB
/
KalmanTracker.cpp
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
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
#include "KalmanTracker.h"
int KalmanTracker::kf_count = 0;
// initialize Kalman filter
void KalmanTracker::init_kf(StateType stateMat)
{
int stateNum = 7;
int measureNum = 4;
kf = KalmanFilter(stateNum, measureNum, 0);
measurement = Mat::zeros(measureNum, 1, CV_32F);
kf.transitionMatrix = (Mat_<float>(stateNum, stateNum) <<
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);
setIdentity(kf.measurementMatrix);
setIdentity(kf.processNoiseCov, Scalar::all(1e-2));
setIdentity(kf.measurementNoiseCov, Scalar::all(1e-1));
setIdentity(kf.errorCovPost, Scalar::all(1));
// initialize state vector with bounding box in [cx,cy,s,r] style
kf.statePost.at<float>(0, 0) = stateMat.x + stateMat.width / 2;
kf.statePost.at<float>(1, 0) = stateMat.y + stateMat.height / 2;
kf.statePost.at<float>(2, 0) = stateMat.area();
kf.statePost.at<float>(3, 0) = stateMat.width / stateMat.height;
}
// Predict the estimated bounding box.
StateType KalmanTracker::predict()
{
// predict
Mat p = kf.predict();
m_age += 1;
if (m_time_since_update > 0)
m_hit_streak = 0;
m_time_since_update += 1;
StateType predictBox = get_rect_xysr(p.at<float>(0, 0), p.at<float>(1, 0), p.at<float>(2, 0), p.at<float>(3, 0));
m_history.push_back(predictBox);
return m_history.back();
}
// Update the state vector with observed bounding box.
void KalmanTracker::update(StateType stateMat)
{
m_time_since_update = 0;
m_history.clear();
m_hits += 1;
m_hit_streak += 1;
// measurement
measurement.at<float>(0, 0) = stateMat.x + stateMat.width / 2;
measurement.at<float>(1, 0) = stateMat.y + stateMat.height / 2;
measurement.at<float>(2, 0) = stateMat.area();
measurement.at<float>(3, 0) = stateMat.width / stateMat.height;
// update
kf.correct(measurement);
}
// Return the current state vector
StateType KalmanTracker::get_state()
{
Mat s = kf.statePost;
return get_rect_xysr(s.at<float>(0, 0), s.at<float>(1, 0), s.at<float>(2, 0), s.at<float>(3, 0));
}
// Convert bounding box from [cx,cy,s,r] to [x,y,w,h] style.
StateType KalmanTracker::get_rect_xysr(float cx, float cy, float s, float r)
{
float w = sqrt(s * r);
float h = s / w;
float x = (cx - w / 2);
float y = (cy - h / 2);
if (x < 0 && cx > 0)
x = 0;
if (y < 0 && cy > 0)
y = 0;
return StateType(x, y, w, h);
}
/*
// --------------------------------------------------------------------
// Kalman Filter Demonstrating, a 2-d ball demo
// --------------------------------------------------------------------
const int winHeight = 600;
const int winWidth = 800;
Point mousePosition = Point(winWidth >> 1, winHeight >> 1);
// mouse event callback
void mouseEvent(int event, int x, int y, int flags, void *param)
{
if (event == CV_EVENT_MOUSEMOVE) {
mousePosition = Point(x, y);
}
}
void TestKF();
void main()
{
TestKF();
}
void TestKF()
{
int stateNum = 4;
int measureNum = 2;
KalmanFilter kf = KalmanFilter(stateNum, measureNum, 0);
// initialization
Mat processNoise(stateNum, 1, CV_32F);
Mat measurement = Mat::zeros(measureNum, 1, CV_32F);
kf.transitionMatrix = *(Mat_<float>(stateNum, stateNum) <<
1, 0, 1, 0,
0, 1, 0, 1,
0, 0, 1, 0,
0, 0, 0, 1);
setIdentity(kf.measurementMatrix);
setIdentity(kf.processNoiseCov, Scalar::all(1e-2));
setIdentity(kf.measurementNoiseCov, Scalar::all(1e-1));
setIdentity(kf.errorCovPost, Scalar::all(1));
randn(kf.statePost, Scalar::all(0), Scalar::all(winHeight));
namedWindow("Kalman");
setMouseCallback("Kalman", mouseEvent);
Mat img(winHeight, winWidth, CV_8UC3);
while (1)
{
// predict
Mat prediction = kf.predict();
Point predictPt = Point(prediction.at<float>(0, 0), prediction.at<float>(1, 0));
// generate measurement
Point statePt = mousePosition;
measurement.at<float>(0, 0) = statePt.x;
measurement.at<float>(1, 0) = statePt.y;
// update
kf.correct(measurement);
// visualization
img.setTo(Scalar(255, 255, 255));
circle(img, predictPt, 8, CV_RGB(0, 255, 0), -1); // predicted point as green
circle(img, statePt, 8, CV_RGB(255, 0, 0), -1); // current position as red
imshow("Kalman", img);
char code = (char)waitKey(100);
if (code == 27 || code == 'q' || code == 'Q')
break;
}
destroyWindow("Kalman");
}
*/