JeVoisBase  1.5
JeVois Smart Embedded Machine Vision Toolkit Base Modules
Share this page:
Kalman2D.C
Go to the documentation of this file.
1 // ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
2 //
3 // JeVois Smart Embedded Machine Vision Toolkit - Copyright (C) 2016 by Laurent Itti, the University of Southern
4 // California (USC), and iLab at USC. See http://iLab.usc.edu and http://jevois.org for information about this project.
5 //
6 // This file is part of the JeVois Smart Embedded Machine Vision Toolkit. This program is free software; you can
7 // redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software
8 // Foundation, version 2. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
9 // without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public
10 // License for more details. You should have received a copy of the GNU General Public License along with this program;
11 // if not, write to the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
12 //
13 // Contact information: Laurent Itti - 3641 Watt Way, HNB-07A - Los Angeles, CA 90089-2520 - USA.
14 // Tel: +1 213 740 3527 - itti@pollux.usc.edu - http://iLab.usc.edu - http://jevois.org
15 // ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
16 /*! \file */
17 
20 #include <cmath>
21 
22 // ####################################################################################################
23 Kalman2D::Kalman2D(std::string const & instance) :
24  jevois::Component(instance), itsKF(4, 2, 0), itsState(4, 1, CV_32F), itsProcessNoise(4, 1, CV_32F),
25  itsMeasurement(2, 1, CV_32F), itsLatest(2, 1, CV_32F), itsFresh(false)
26 {
27  cv::setIdentity(itsKF.measurementMatrix);
28 }
29 
30 // ####################################################################################################
32 { }
33 
34 // ####################################################################################################
36 {
37  itsKF.statePost.at<float>(0) = 0.0F; // middle of screen
38  itsKF.statePost.at<float>(1) = 0.0F; // middle of screen
39  itsKF.statePost.at<float>(2) = 0.0F; // not moving
40  itsKF.statePost.at<float>(3) = 0.0F; // not moving
41 
42  // Predict once to get rolling:
43  itsKF.predict();
44 }
45 
46 // ####################################################################################################
47 void Kalman2D::onParamChange(kalman2d::usevel const & JEVOIS_UNUSED_PARAM(param), bool const & newval)
48 {
49  if (newval) itsKF.transitionMatrix = (cv::Mat_<float>(4, 4) << 1,0,1,0, 0,1,0,1, 0,0,1,0, 0,0,0,1);
50  else itsKF.transitionMatrix = (cv::Mat_<float>(4, 4) << 1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1);
51 }
52 
53 // ####################################################################################################
54 void Kalman2D::onParamChange(kalman2d::procnoise const & JEVOIS_UNUSED_PARAM(param), float const & newval)
55 {
56  itsKF.processNoiseCov = (cv::Mat_<float>(4, 4) <<
57  newval * newval, 0, 0, 0,
58  0, newval * newval, 0, 0,
59  0, 0, 0.01F, 0,
60  0, 0, 0, 0.01F );
61 }
62 
63 // ####################################################################################################
64 void Kalman2D::onParamChange(kalman2d::measnoise const & JEVOIS_UNUSED_PARAM(param), float const & newval)
65 {
66  cv::setIdentity(itsKF.measurementNoiseCov, cv::Scalar::all(newval * newval));
67 }
68 
69 // ####################################################################################################
70 void Kalman2D::onParamChange(kalman2d::postnoise const & JEVOIS_UNUSED_PARAM(param), float const & newval)
71 {
72  cv::setIdentity(itsKF.errorCovPost, cv::Scalar::all(newval * newval));
73 }
74 
75 // ####################################################################################################
76 void Kalman2D::set(float x, float y)
77 {
78  itsMeasurement.at<float>(0) = x; itsMeasurement.at<float>(1) = y;
79  itsLatest = itsKF.correct(itsMeasurement);
80  itsFresh = true; // itsLatest is "fresh", no need to predict() at the next get()
81 }
82 
83 // ####################################################################################################
84 void Kalman2D::set(float x, float y, unsigned int imgwidth, unsigned int imgheight)
85 {
86  jevois::coords::imgToStd(x, y, imgwidth, imgheight, 0.0F);
87  this->set(x, y);
88 }
89 
90 // ####################################################################################################
91 void Kalman2D::get(float & x, float & y, float const eps)
92 {
93  // If we just had a fresh set(), use our latest result form correct(), otherwise predict the next position:
94  if (itsFresh)
95  {
96  x = itsLatest.at<float>(0);
97  y = itsLatest.at<float>(1);
98  itsFresh = false;
99 
100  // Now predict and update itsLatest:
101  itsLatest = itsKF.predict();
102  }
103  else
104  {
105  // First predict and update itsLatest:
106  itsLatest = itsKF.predict();
107 
108  x = itsLatest.at<float>(0);
109  y = itsLatest.at<float>(1);
110  }
111 
112  if (eps) { x = std::round(x / eps) * eps; y = std::round(y / eps) * eps; }
113 }
114 
115 // ####################################################################################################
116 void Kalman2D::get(float & x, float & y, unsigned int imgwidth, unsigned int imgheight, float const eps)
117 {
118  this->get(x, y, 0.0F);
119  jevois::coords::stdToImg(x, y, imgwidth, imgheight, eps);
120 }
121 
122 // ####################################################################################################
123 void Kalman2D::get(float & rawx, float & rawy, float & imgx, float & imgy,
124  unsigned int imgwidth, unsigned int imgheight, float const raweps, float const imgeps)
125 {
126  // First get the raw coords with no rounding:
127  this->get(rawx, rawy, 0.0F);
128  imgx = rawx;
129  imgy = rawy;
130 
131  // Now round the raw coords:
132  if (raweps) { rawx = std::round(rawx / raweps) * raweps; rawy = std::round(rawy / raweps) * raweps; }
133 
134  // Now compute the image coords with rounding:
135  jevois::coords::stdToImg(imgx, imgy, imgwidth, imgheight, imgeps);
136 }
Kalman2D(std::string const &instance)
Constructor.
Definition: Kalman2D.C:23
void imgToStd(float &x, float &y, RawImage const &camimg, float const eps=0.1F)
void get(float &x, float &y, float const eps=0.01F)
Function to call each time you want to get the Kalman-filtered coordinates.
Definition: Kalman2D.C:91
void stdToImg(float &x, float &y, RawImage const &camimg, float const eps=0.1F)
cv::Mat itsMeasurement
Definition: Kalman2D.H:122
cv::Mat itsLatest
Definition: Kalman2D.H:123
void set(float x, float y)
Function to call each time you have a new measurement (output from a vision algorithm) ...
Definition: Kalman2D.C:76
virtual ~Kalman2D()
Destructor.
Definition: Kalman2D.C:31
bool itsFresh
Definition: Kalman2D.H:124
void postInit() override
Definition: Kalman2D.C:35
cv::KalmanFilter itsKF
Definition: Kalman2D.H:119
void onParamChange(kalman2d::usevel const &param, bool const &newval)