Skip to content

Commit 46e275d

Browse files
author
Anna Khakimova
authored
Merge pull request opencv#18869 from anna-khakimova:ak/kalman
* GAPI: Kalman filter stateful kernel * Applied comments * Applied comments. Second iteration * Add overload without control vector * Remove structure constructor and dimension fields. * Add sample as test * Remove visualization from test-sample + correct doxygen comments * Applied comments.
1 parent 743f181 commit 46e275d

File tree

6 files changed

+473
-1
lines changed

6 files changed

+473
-1
lines changed

‎modules/gapi/include/opencv2/gapi/video.hpp

Lines changed: 92 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,32 @@
1616
*/
1717

1818
namespace cv { namespace gapi {
19+
20+
/** @brief Structure for the Kalman filter's initialization parameters.*/
21+
22+
struct GAPI_EXPORTS KalmanParams
23+
{
24+
// initial state
25+
26+
//! corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))
27+
Mat state;
28+
//! posteriori error estimate covariance matrix (P(k)): P(k)=(I-K(k)*H)*P'(k)
29+
Mat errorCov;
30+
31+
// dynamic system description
32+
33+
//! state transition matrix (A)
34+
Mat transitionMatrix;
35+
//! measurement matrix (H)
36+
Mat measurementMatrix;
37+
//! process noise covariance matrix (Q)
38+
Mat processNoiseCov;
39+
//! measurement noise covariance matrix (R)
40+
Mat measurementNoiseCov;
41+
//! control matrix (B) (Optional: not used if there's no control)
42+
Mat controlMatrix;
43+
};
44+
1945
namespace video
2046
{
2147
using GBuildPyrOutput = std::tuple<GArray<GMat>, GScalar>;
@@ -129,6 +155,28 @@ G_TYPED_KERNEL(GBackgroundSubtractor, <GMat(GMat, BackgroundSubtractorParams)>,
129155
}
130156
};
131157

158+
void checkParams(const cv::gapi::KalmanParams& kfParams,
159+
const cv::GMatDesc& measurement, const cv::GMatDesc& control = {});
160+
161+
G_TYPED_KERNEL(GKalmanFilter, <GMat(GMat, GOpaque<bool>, GMat, KalmanParams)>,
162+
"org.opencv.video.KalmanFilter")
163+
{
164+
static GMatDesc outMeta(const GMatDesc& measurement, const GOpaqueDesc&,
165+
const GMatDesc& control, const KalmanParams& kfParams)
166+
{
167+
checkParams(kfParams, measurement, control);
168+
return measurement.withSize(Size(1, kfParams.transitionMatrix.rows));
169+
}
170+
};
171+
172+
G_TYPED_KERNEL(GKalmanFilterNoControl, <GMat(GMat, GOpaque<bool>, KalmanParams)>, "org.opencv.video.KalmanFilterNoControl")
173+
{
174+
static GMatDesc outMeta(const GMatDesc& measurement, const GOpaqueDesc&, const KalmanParams& kfParams)
175+
{
176+
checkParams(kfParams, measurement);
177+
return measurement.withSize(Size(1, kfParams.transitionMatrix.rows));
178+
}
179+
};
132180
} //namespace video
133181

134182
//! @addtogroup gapi_video
@@ -250,6 +298,49 @@ The operation generates a foreground mask.
250298
*/
251299
GAPI_EXPORTS GMat BackgroundSubtractor(const GMat& src, const cv::gapi::video::BackgroundSubtractorParams& bsParams);
252300

301+
/** @brief Standard Kalman filter algorithm <http://en.wikipedia.org/wiki/Kalman_filter>.
302+
303+
@note Functional textual ID is "org.opencv.video.KalmanFilter"
304+
305+
@param measurement input matrix: 32-bit or 64-bit float 1-channel matrix containing measurements.
306+
@param haveMeasurement dynamic input flag that indicates whether we get measurements
307+
at a particular iteration .
308+
@param control input matrix: 32-bit or 64-bit float 1-channel matrix contains control data
309+
for changing dynamic system.
310+
@param kfParams Set of initialization parameters for Kalman filter kernel.
311+
312+
@return Output matrix is predicted or corrected state. They can be 32-bit or 64-bit float
313+
1-channel matrix @ref CV_32FC1 or @ref CV_64FC1.
314+
315+
@details If measurement matrix is given (haveMeasurements == true), corrected state will
316+
be returned which corresponds to the pipeline
317+
cv::KalmanFilter::predict(control) -> cv::KalmanFilter::correct(measurement).
318+
Otherwise, predicted state will be returned which corresponds to the call of
319+
cv::KalmanFilter::predict(control).
320+
@sa cv::KalmanFilter
321+
*/
322+
GAPI_EXPORTS GMat KalmanFilter(const GMat& measurement, const GOpaque<bool>& haveMeasurement,
323+
const GMat& control, const cv::gapi::KalmanParams& kfParams);
324+
325+
/** @overload
326+
The case of Standard Kalman filter algorithm when there is no control in a dynamic system.
327+
In this case the controlMatrix is empty and control vector is absent.
328+
329+
@note Function textual ID is "org.opencv.video.KalmanFilterNoControl"
330+
331+
@param measurement input matrix: 32-bit or 64-bit float 1-channel matrix containing measurements.
332+
@param haveMeasurement dynamic input flag that indicates whether we get measurements
333+
at a particular iteration.
334+
@param kfParams Set of initialization parameters for Kalman filter kernel.
335+
336+
@return Output matrix is predicted or corrected state. They can be 32-bit or 64-bit float
337+
1-channel matrix @ref CV_32FC1 or @ref CV_64FC1.
338+
339+
@sa cv::KalmanFilter
340+
*/
341+
GAPI_EXPORTS GMat KalmanFilter(const GMat& measurement, const GOpaque<bool>& haveMeasurement,
342+
const cv::gapi::KalmanParams& kfParams);
343+
253344
//! @} gapi_video
254345
} //namespace gapi
255346
} //namespace cv
@@ -264,6 +355,6 @@ template<> struct CompileArgTag<cv::gapi::video::BackgroundSubtractorParams>
264355
}
265356
};
266357
} // namespace detail
267-
} //namespace cv
358+
} // namespace cv
268359

269360
#endif // OPENCV_GAPI_VIDEO_HPP

‎modules/gapi/src/api/kernels_video.cpp

Lines changed: 58 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -57,5 +57,63 @@ GMat BackgroundSubtractor(const GMat& src, const BackgroundSubtractorParams& bsp
5757
return GBackgroundSubtractor::on(src, bsp);
5858
}
5959

60+
GMat KalmanFilter(const GMat& m, const cv::GOpaque<bool>& have_m, const GMat& c, const KalmanParams& kp)
61+
{
62+
return GKalmanFilter::on(m, have_m, c, kp);
63+
}
64+
65+
GMat KalmanFilter(const GMat& m, const cv::GOpaque<bool>& have_m, const KalmanParams& kp)
66+
{
67+
return GKalmanFilterNoControl::on(m, have_m, kp);
68+
}
69+
70+
namespace video {
71+
void checkParams(const cv::gapi::KalmanParams& kfParams,
72+
const cv::GMatDesc& measurement, const cv::GMatDesc& control)
73+
{
74+
int type = kfParams.transitionMatrix.type();
75+
GAPI_Assert(type == CV_32FC1 || type == CV_64FC1);
76+
int depth = CV_MAT_DEPTH(type);
77+
78+
bool controlCapable = !(control == GMatDesc{});
79+
80+
if (controlCapable)
81+
{
82+
GAPI_Assert(!kfParams.controlMatrix.empty());
83+
GAPI_Assert(control.depth == depth && control.chan == 1 &&
84+
control.size.height == kfParams.controlMatrix.cols &&
85+
control.size.width == 1);
86+
}
87+
else
88+
GAPI_Assert(kfParams.controlMatrix.empty());
89+
90+
GAPI_Assert(!kfParams.state.empty() && kfParams.state.type() == type);
91+
GAPI_Assert(!kfParams.errorCov.empty() && kfParams.errorCov.type() == type);
92+
GAPI_Assert(!kfParams.transitionMatrix.empty() && kfParams.transitionMatrix.type() == type);
93+
GAPI_Assert(!kfParams.processNoiseCov.empty() && kfParams.processNoiseCov.type() == type);
94+
GAPI_Assert(!kfParams.measurementNoiseCov.empty() && kfParams.measurementNoiseCov.type() == type);
95+
GAPI_Assert(!kfParams.measurementMatrix.empty() && kfParams.measurementMatrix.type() == type);
96+
GAPI_Assert(measurement.depth == depth && measurement.chan == 1);
97+
98+
int dDim = kfParams.transitionMatrix.cols;
99+
GAPI_Assert(kfParams.transitionMatrix.rows == dDim);
100+
101+
GAPI_Assert(kfParams.processNoiseCov.cols == dDim &&
102+
kfParams.processNoiseCov.rows == dDim);
103+
GAPI_Assert(kfParams.errorCov.cols == dDim && kfParams.errorCov.rows == dDim);
104+
GAPI_Assert(kfParams.state.rows == dDim && kfParams.state.cols == 1);
105+
GAPI_Assert(kfParams.measurementMatrix.cols == dDim);
106+
107+
int mDim = kfParams.measurementMatrix.rows;
108+
GAPI_Assert(kfParams.measurementNoiseCov.cols == mDim &&
109+
kfParams.measurementNoiseCov.rows == mDim);
110+
111+
if (controlCapable)
112+
GAPI_Assert(kfParams.controlMatrix.rows == dDim);
113+
114+
GAPI_Assert(measurement.size.height == mDim &&
115+
measurement.size.width == 1);
116+
}
117+
} // namespace video
60118
} //namespace gapi
61119
} //namespace cv

‎modules/gapi/src/backends/cpu/gcpuvideo.cpp

Lines changed: 69 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -107,13 +107,82 @@ GAPI_OCV_KERNEL_ST(GCPUBackgroundSubtractor,
107107
}
108108
};
109109

110+
GAPI_OCV_KERNEL_ST(GCPUKalmanFilter, cv::gapi::video::GKalmanFilter, cv::KalmanFilter)
111+
{
112+
static void setup(const cv::GMatDesc&, const cv::GOpaqueDesc&,
113+
const cv::GMatDesc&, const cv::gapi::KalmanParams& kfParams,
114+
std::shared_ptr<cv::KalmanFilter> &state, const cv::GCompileArgs&)
115+
{
116+
state = std::make_shared<cv::KalmanFilter>(kfParams.transitionMatrix.rows, kfParams.measurementMatrix.rows,
117+
kfParams.controlMatrix.cols, kfParams.transitionMatrix.type());
118+
119+
// initial state
120+
state->statePost = kfParams.state;
121+
state->errorCovPost = kfParams.errorCov;
122+
123+
// dynamic system initialization
124+
state->controlMatrix = kfParams.controlMatrix;
125+
state->measurementMatrix = kfParams.measurementMatrix;
126+
state->transitionMatrix = kfParams.transitionMatrix;
127+
state->processNoiseCov = kfParams.processNoiseCov;
128+
state->measurementNoiseCov = kfParams.measurementNoiseCov;
129+
}
130+
131+
static void run(const cv::Mat& measurements, bool haveMeasurement,
132+
const cv::Mat& control, const cv::gapi::KalmanParams&,
133+
cv::Mat &out, cv::KalmanFilter& state)
134+
{
135+
cv::Mat pre = state.predict(control);
136+
137+
if (haveMeasurement)
138+
state.correct(measurements).copyTo(out);
139+
else
140+
pre.copyTo(out);
141+
}
142+
};
143+
144+
GAPI_OCV_KERNEL_ST(GCPUKalmanFilterNoControl, cv::gapi::video::GKalmanFilterNoControl, cv::KalmanFilter)
145+
{
146+
static void setup(const cv::GMatDesc&, const cv::GOpaqueDesc&,
147+
const cv::gapi::KalmanParams& kfParams,
148+
std::shared_ptr<cv::KalmanFilter> &state,
149+
const cv::GCompileArgs&)
150+
{
151+
state = std::make_shared<cv::KalmanFilter>(kfParams.transitionMatrix.rows, kfParams.measurementMatrix.rows,
152+
0, kfParams.transitionMatrix.type());
153+
// initial state
154+
state->statePost = kfParams.state;
155+
state->errorCovPost = kfParams.errorCov;
156+
157+
// dynamic system initialization
158+
state->measurementMatrix = kfParams.measurementMatrix;
159+
state->transitionMatrix = kfParams.transitionMatrix;
160+
state->processNoiseCov = kfParams.processNoiseCov;
161+
state->measurementNoiseCov = kfParams.measurementNoiseCov;
162+
}
163+
164+
static void run(const cv::Mat& measurements, bool haveMeasurement,
165+
const cv::gapi::KalmanParams&, cv::Mat &out,
166+
cv::KalmanFilter& state)
167+
{
168+
cv::Mat pre = state.predict();
169+
170+
if (haveMeasurement)
171+
state.correct(measurements).copyTo(out);
172+
else
173+
pre.copyTo(out);
174+
}
175+
};
176+
110177
cv::gapi::GKernelPackage cv::gapi::video::cpu::kernels()
111178
{
112179
static auto pkg = cv::gapi::kernels
113180
< GCPUBuildOptFlowPyramid
114181
, GCPUCalcOptFlowLK
115182
, GCPUCalcOptFlowLKForPyr
116183
, GCPUBackgroundSubtractor
184+
, GCPUKalmanFilter
185+
, GCPUKalmanFilterNoControl
117186
>();
118187
return pkg;
119188
}

‎modules/gapi/test/common/gapi_video_tests.hpp

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -31,6 +31,13 @@ GAPI_TEST_FIXTURE_SPEC_PARAMS(BuildPyr_CalcOptFlow_PipelineTest,
3131
GAPI_TEST_FIXTURE_SPEC_PARAMS(BackgroundSubtractorTest, FIXTURE_API(tuple<cv::gapi::video::BackgroundSubtractorType,double>,
3232
int, bool, double, std::string, std::size_t),
3333
6, typeAndThreshold, histLength, detectShadows, learningRate, filePath, testNumFrames)
34+
35+
GAPI_TEST_FIXTURE_SPEC_PARAMS(KalmanFilterTest, FIXTURE_API(int, int, int, int, int), 5, type, dDim, mDim, cDim, numIter)
36+
37+
GAPI_TEST_FIXTURE_SPEC_PARAMS(KalmanFilterNoControlTest, FIXTURE_API(int, int, int, int), 4, type, dDim, mDim, numIter)
38+
39+
GAPI_TEST_FIXTURE_SPEC_PARAMS(KalmanFilterCircleSampleTest, FIXTURE_API(int, int), 2, type, numIter)
40+
3441
} // opencv_test
3542

3643

0 commit comments

Comments
 (0)