OpenShot Library | libopenshot  0.3.3
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 
11 using namespace std;
12 using namespace cv;
13 
14 // initialize Kalman filter
15 void 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;
54  m_time_since_update += 1;
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 {
76  m_time_since_update = 0;
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 }
StateType
#define StateType
Definition: KalmanTracker.h:15
KalmanTracker::get_rect_xysr
StateType get_rect_xysr(float cx, float cy, float s, float r)
Definition: KalmanTracker.cpp:103
KalmanTracker::predict2
StateType predict2()
Definition: KalmanTracker.cpp:62
KalmanTracker::update
void update(StateType stateMat)
Definition: KalmanTracker.cpp:73
KalmanTracker::get_state
StateType get_state()
Definition: KalmanTracker.cpp:96
KalmanTracker.h
KalmanTracker::predict
StateType predict()
Definition: KalmanTracker.cpp:46