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