OpenShot Library | libopenshot 0.3.3
Loading...
Searching...
No Matches
KalmanTracker.cpp
Go to the documentation of this file.
1// © OpenShot Studios, LLC
2//
3// SPDX-License-Identifier: LGPL-3.0-or-later
4
6// KalmanTracker.cpp: KalmanTracker Class Implementation Declaration
7
8#include "KalmanTracker.h"
9#include <ctime>
10
11using namespace std;
12using namespace cv;
13
14// initialize Kalman filter
15void KalmanTracker::init_kf(
16 StateType stateMat)
17{
18 int stateNum = 7;
19 int measureNum = 4;
20 kf = KalmanFilter(stateNum, measureNum, 0);
21
22 measurement = Mat::zeros(measureNum, 1, CV_32F);
23
24 kf.transitionMatrix = (Mat_<float>(7, 7) << 1, 0, 0, 0, 1, 0, 0,
25
26 0, 1, 0, 0, 0, 1, 0,
27 0, 0, 1, 0, 0, 0, 1,
28 0, 0, 0, 1, 0, 0, 0,
29 0, 0, 0, 0, 1, 0, 0,
30 0, 0, 0, 0, 0, 1, 0,
31 0, 0, 0, 0, 0, 0, 1);
32
33 setIdentity(kf.measurementMatrix);
34 setIdentity(kf.processNoiseCov, Scalar::all(1e-1));
35 setIdentity(kf.measurementNoiseCov, Scalar::all(1e-4));
36 setIdentity(kf.errorCovPost, Scalar::all(1e-2));
37
38 // initialize state vector with bounding box in [cx,cy,s,r] style
39 kf.statePost.at<float>(0, 0) = stateMat.x + stateMat.width / 2;
40 kf.statePost.at<float>(1, 0) = stateMat.y + stateMat.height / 2;
41 kf.statePost.at<float>(2, 0) = stateMat.area();
42 kf.statePost.at<float>(3, 0) = stateMat.width / stateMat.height;
43}
44
45// Predict the estimated bounding box.
47{
48 // predict
49 Mat p = kf.predict();
50 m_age += 1;
51
52 if (m_time_since_update > 0)
53 m_hit_streak = 0;
55
56 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));
57
58 m_history.push_back(predictBox);
59 return m_history.back();
60}
61
63{
64 // predict
65 Mat p = kf.predict();
66
67 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));
68
69 return predictBox;
70}
71
72// Update the state vector with observed bounding box.
74 StateType stateMat)
75{
77 m_history.clear();
78 m_hits += 1;
79 m_hit_streak += 1;
80
81 // measurement
82 measurement.at<float>(0, 0) = stateMat.x + stateMat.width / 2;
83 measurement.at<float>(1, 0) = stateMat.y + stateMat.height / 2;
84 measurement.at<float>(2, 0) = stateMat.area();
85 measurement.at<float>(3, 0) = stateMat.width / stateMat.height;
86
87 // update
88 kf.correct(measurement);
89 // time_t now = time(0);
90 // convert now to string form
91
92 // detect_times.push_back(dt);
93}
94
95// Return the current state vector
97{
98 Mat s = kf.statePost;
99 return get_rect_xysr(s.at<float>(0, 0), s.at<float>(1, 0), s.at<float>(2, 0), s.at<float>(3, 0));
100}
101
102// Convert bounding box from [cx,cy,s,r] to [x,y,w,h] style.
104 float cx,
105 float cy,
106 float s,
107 float r)
108{
109 float w = sqrt(s * r);
110 float h = s / w;
111 float x = (cx - w / 2);
112 float y = (cy - h / 2);
113
114 if (x < 0 && cx > 0)
115 x = 0;
116 if (y < 0 && cy > 0)
117 y = 0;
118
119 return StateType(x, y, w, h);
120}
#define StateType
StateType predict()
StateType get_state()
void update(StateType stateMat)
StateType predict2()
StateType get_rect_xysr(float cx, float cy, float s, float r)