third_party/opencv
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #18869 from anna-khakimova:ak/kalman

Browse Source 
* 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.
This commit is contained in:
Anna Khakimova 2020-12-14 11:56:37 +03:00 committed by GitHub
parent 743f1810c7
commit 46e275dfe4
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 473 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

93
modules/gapi/include/opencv2/gapi/video.hpp
View File

@ -16,6 +16,32 @@
*/
namespace cv { namespace gapi {
/** @brief Structure for the Kalman filter's initialization parameters.*/
struct GAPI_EXPORTS KalmanParams
{
// initial state
//! corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))
Mat state;
//! posteriori error estimate covariance matrix (P(k)): P(k)=(I-K(k)*H)*P'(k)
Mat errorCov;
// dynamic system description
//! state transition matrix (A)
Mat transitionMatrix;
//! measurement matrix (H)
Mat measurementMatrix;
//! process noise covariance matrix (Q)
Mat processNoiseCov;
//! measurement noise covariance matrix (R)
Mat measurementNoiseCov;
//! control matrix (B) (Optional: not used if there's no control)
Mat controlMatrix;
};
namespace video
{
using GBuildPyrOutput = std::tuple<GArray<GMat>, GScalar>;
@ -129,6 +155,28 @@ G_TYPED_KERNEL(GBackgroundSubtractor, <GMat(GMat, BackgroundSubtractorParams)>,
}
};
void checkParams(const cv::gapi::KalmanParams& kfParams,
const cv::GMatDesc& measurement, const cv::GMatDesc& control = {});
G_TYPED_KERNEL(GKalmanFilter, <GMat(GMat, GOpaque<bool>, GMat, KalmanParams)>,
"org.opencv.video.KalmanFilter")
{
static GMatDesc outMeta(const GMatDesc& measurement, const GOpaqueDesc&,
const GMatDesc& control, const KalmanParams& kfParams)
{
checkParams(kfParams, measurement, control);
return measurement.withSize(Size(1, kfParams.transitionMatrix.rows));
}
};
G_TYPED_KERNEL(GKalmanFilterNoControl, <GMat(GMat, GOpaque<bool>, KalmanParams)>, "org.opencv.video.KalmanFilterNoControl")
{
static GMatDesc outMeta(const GMatDesc& measurement, const GOpaqueDesc&, const KalmanParams& kfParams)
{
checkParams(kfParams, measurement);
return measurement.withSize(Size(1, kfParams.transitionMatrix.rows));
}
};
} //namespace video
//! @addtogroup gapi_video
@ -250,6 +298,49 @@ The operation generates a foreground mask.
*/
GAPI_EXPORTS GMat BackgroundSubtractor(const GMat& src, const cv::gapi::video::BackgroundSubtractorParams& bsParams);
/** @brief Standard Kalman filter algorithm <http://en.wikipedia.org/wiki/Kalman_filter>.
@note Functional textual ID is "org.opencv.video.KalmanFilter"
@param measurement input matrix: 32-bit or 64-bit float 1-channel matrix containing measurements.
@param haveMeasurement dynamic input flag that indicates whether we get measurements
at a particular iteration .
@param control input matrix: 32-bit or 64-bit float 1-channel matrix contains control data
for changing dynamic system.
@param kfParams Set of initialization parameters for Kalman filter kernel.
@return Output matrix is predicted or corrected state. They can be 32-bit or 64-bit float
1-channel matrix @ref CV_32FC1 or @ref CV_64FC1.
@details If measurement matrix is given (haveMeasurements == true), corrected state will
be returned which corresponds to the pipeline
cv::KalmanFilter::predict(control) -> cv::KalmanFilter::correct(measurement).
Otherwise, predicted state will be returned which corresponds to the call of
cv::KalmanFilter::predict(control).
@sa cv::KalmanFilter
*/
GAPI_EXPORTS GMat KalmanFilter(const GMat& measurement, const GOpaque<bool>& haveMeasurement,
const GMat& control, const cv::gapi::KalmanParams& kfParams);
/** @overload
The case of Standard Kalman filter algorithm when there is no control in a dynamic system.
In this case the controlMatrix is empty and control vector is absent.
@note Function textual ID is "org.opencv.video.KalmanFilterNoControl"
@param measurement input matrix: 32-bit or 64-bit float 1-channel matrix containing measurements.
@param haveMeasurement dynamic input flag that indicates whether we get measurements
at a particular iteration.
@param kfParams Set of initialization parameters for Kalman filter kernel.
@return Output matrix is predicted or corrected state. They can be 32-bit or 64-bit float
1-channel matrix @ref CV_32FC1 or @ref CV_64FC1.
@sa cv::KalmanFilter
*/
GAPI_EXPORTS GMat KalmanFilter(const GMat& measurement, const GOpaque<bool>& haveMeasurement,
const cv::gapi::KalmanParams& kfParams);
//! @} gapi_video
} //namespace gapi
} //namespace cv
@ -264,6 +355,6 @@ template<> struct CompileArgTag<cv::gapi::video::BackgroundSubtractorParams>
}
};
} // namespace detail
} //namespace cv
} // namespace cv
#endif // OPENCV_GAPI_VIDEO_HPP

58
modules/gapi/src/api/kernels_video.cpp
View File

@ -57,5 +57,63 @@ GMat BackgroundSubtractor(const GMat& src, const BackgroundSubtractorParams& bsp
return GBackgroundSubtractor::on(src, bsp);
}
GMat KalmanFilter(const GMat& m, const cv::GOpaque<bool>& have_m, const GMat& c, const KalmanParams& kp)
{
return GKalmanFilter::on(m, have_m, c, kp);
}
GMat KalmanFilter(const GMat& m, const cv::GOpaque<bool>& have_m, const KalmanParams& kp)
{
return GKalmanFilterNoControl::on(m, have_m, kp);
}
namespace video {
void checkParams(const cv::gapi::KalmanParams& kfParams,
const cv::GMatDesc& measurement, const cv::GMatDesc& control)
{
int type = kfParams.transitionMatrix.type();
GAPI_Assert(type == CV_32FC1 || type == CV_64FC1);
int depth = CV_MAT_DEPTH(type);
bool controlCapable = !(control == GMatDesc{});
if (controlCapable)
{
GAPI_Assert(!kfParams.controlMatrix.empty());
GAPI_Assert(control.depth == depth && control.chan == 1 &&
control.size.height == kfParams.controlMatrix.cols &&
control.size.width == 1);
}
else
GAPI_Assert(kfParams.controlMatrix.empty());
GAPI_Assert(!kfParams.state.empty() && kfParams.state.type() == type);
GAPI_Assert(!kfParams.errorCov.empty() && kfParams.errorCov.type() == type);
GAPI_Assert(!kfParams.transitionMatrix.empty() && kfParams.transitionMatrix.type() == type);
GAPI_Assert(!kfParams.processNoiseCov.empty() && kfParams.processNoiseCov.type() == type);
GAPI_Assert(!kfParams.measurementNoiseCov.empty() && kfParams.measurementNoiseCov.type() == type);
GAPI_Assert(!kfParams.measurementMatrix.empty() && kfParams.measurementMatrix.type() == type);
GAPI_Assert(measurement.depth == depth && measurement.chan == 1);
int dDim = kfParams.transitionMatrix.cols;
GAPI_Assert(kfParams.transitionMatrix.rows == dDim);
GAPI_Assert(kfParams.processNoiseCov.cols == dDim &&
kfParams.processNoiseCov.rows == dDim);
GAPI_Assert(kfParams.errorCov.cols == dDim && kfParams.errorCov.rows == dDim);
GAPI_Assert(kfParams.state.rows == dDim && kfParams.state.cols == 1);
GAPI_Assert(kfParams.measurementMatrix.cols == dDim);
int mDim = kfParams.measurementMatrix.rows;
GAPI_Assert(kfParams.measurementNoiseCov.cols == mDim &&
kfParams.measurementNoiseCov.rows == mDim);
if (controlCapable)
GAPI_Assert(kfParams.controlMatrix.rows == dDim);
GAPI_Assert(measurement.size.height == mDim &&
measurement.size.width == 1);
}
} // namespace video
} //namespace gapi
} //namespace cv

69
modules/gapi/src/backends/cpu/gcpuvideo.cpp
View File

@ -107,6 +107,73 @@ GAPI_OCV_KERNEL_ST(GCPUBackgroundSubtractor,
}
};
GAPI_OCV_KERNEL_ST(GCPUKalmanFilter, cv::gapi::video::GKalmanFilter, cv::KalmanFilter)
{
static void setup(const cv::GMatDesc&, const cv::GOpaqueDesc&,
const cv::GMatDesc&, const cv::gapi::KalmanParams& kfParams,
std::shared_ptr<cv::KalmanFilter> &state, const cv::GCompileArgs&)
{
state = std::make_shared<cv::KalmanFilter>(kfParams.transitionMatrix.rows, kfParams.measurementMatrix.rows,
kfParams.controlMatrix.cols, kfParams.transitionMatrix.type());
// initial state
state->statePost = kfParams.state;
state->errorCovPost = kfParams.errorCov;
// dynamic system initialization
state->controlMatrix = kfParams.controlMatrix;
state->measurementMatrix = kfParams.measurementMatrix;
state->transitionMatrix = kfParams.transitionMatrix;
state->processNoiseCov = kfParams.processNoiseCov;
state->measurementNoiseCov = kfParams.measurementNoiseCov;
}
static void run(const cv::Mat& measurements, bool haveMeasurement,
const cv::Mat& control, const cv::gapi::KalmanParams&,
cv::Mat &out, cv::KalmanFilter& state)
{
cv::Mat pre = state.predict(control);
if (haveMeasurement)
state.correct(measurements).copyTo(out);
else
pre.copyTo(out);
}
};
GAPI_OCV_KERNEL_ST(GCPUKalmanFilterNoControl, cv::gapi::video::GKalmanFilterNoControl, cv::KalmanFilter)
{
static void setup(const cv::GMatDesc&, const cv::GOpaqueDesc&,
const cv::gapi::KalmanParams& kfParams,
std::shared_ptr<cv::KalmanFilter> &state,
const cv::GCompileArgs&)
{
state = std::make_shared<cv::KalmanFilter>(kfParams.transitionMatrix.rows, kfParams.measurementMatrix.rows,
0, kfParams.transitionMatrix.type());
// initial state
state->statePost = kfParams.state;
state->errorCovPost = kfParams.errorCov;
// dynamic system initialization
state->measurementMatrix = kfParams.measurementMatrix;
state->transitionMatrix = kfParams.transitionMatrix;
state->processNoiseCov = kfParams.processNoiseCov;
state->measurementNoiseCov = kfParams.measurementNoiseCov;
}
static void run(const cv::Mat& measurements, bool haveMeasurement,
const cv::gapi::KalmanParams&, cv::Mat &out,
cv::KalmanFilter& state)
{
cv::Mat pre = state.predict();
if (haveMeasurement)
state.correct(measurements).copyTo(out);
else
pre.copyTo(out);
}
};
cv::gapi::GKernelPackage cv::gapi::video::cpu::kernels()
{
static auto pkg = cv::gapi::kernels
@ -114,6 +181,8 @@ cv::gapi::GKernelPackage cv::gapi::video::cpu::kernels()
, GCPUCalcOptFlowLK
, GCPUCalcOptFlowLKForPyr
, GCPUBackgroundSubtractor
, GCPUKalmanFilter
, GCPUKalmanFilterNoControl
>();
return pkg;
}

7
modules/gapi/test/common/gapi_video_tests.hpp
View File

@ -31,6 +31,13 @@ GAPI_TEST_FIXTURE_SPEC_PARAMS(BuildPyr_CalcOptFlow_PipelineTest,
GAPI_TEST_FIXTURE_SPEC_PARAMS(BackgroundSubtractorTest, FIXTURE_API(tuple<cv::gapi::video::BackgroundSubtractorType,double>,
int, bool, double, std::string, std::size_t),
6, typeAndThreshold, histLength, detectShadows, learningRate, filePath, testNumFrames)
GAPI_TEST_FIXTURE_SPEC_PARAMS(KalmanFilterTest, FIXTURE_API(int, int, int, int, int), 5, type, dDim, mDim, cDim, numIter)
GAPI_TEST_FIXTURE_SPEC_PARAMS(KalmanFilterNoControlTest, FIXTURE_API(int, int, int, int), 4, type, dDim, mDim, numIter)
GAPI_TEST_FIXTURE_SPEC_PARAMS(KalmanFilterCircleSampleTest, FIXTURE_API(int, int), 2, type, numIter)
} // opencv_test

223
modules/gapi/test/common/gapi_video_tests_inl.hpp
View File

@ -131,6 +131,229 @@ TEST_P(BackgroundSubtractorTest, AccuracyTest)
// Allowing 1% difference of all pixels between G-API and reference OpenCV results
testBackgroundSubtractorStreaming(gapiBackSub, pOCVBackSub, 1, 1, learningRate, testNumFrames);
}
inline void initKalmanParams(cv::gapi::KalmanParams& kp, int type, int dDim, int mDim, int cDim)
{
kp.state = Mat::zeros(dDim, 1, type);
cv::randu(kp.state, Scalar::all(0), Scalar::all(0.1));
kp.errorCov = Mat::eye(dDim, dDim, type);
kp.transitionMatrix = Mat::ones(dDim, dDim, type) * 2;
kp.processNoiseCov = Mat::eye(dDim, dDim, type)*(1e-5);
kp.measurementMatrix = Mat::eye(mDim, dDim, type) * 2;
kp.measurementNoiseCov = Mat::eye(mDim, mDim, type)*(1e-5);
if (cDim > 0)
kp.controlMatrix = Mat::eye(dDim, cDim, type)* (1e-3);
}
inline void initKalmanFilter(const cv::gapi::KalmanParams& kp,
cv::KalmanFilter& ocvKalman, bool control)
{
kp.state.copyTo(ocvKalman.statePost);
kp.errorCov.copyTo(ocvKalman.errorCovPost);
kp.transitionMatrix.copyTo(ocvKalman.transitionMatrix);
kp.measurementMatrix.copyTo(ocvKalman.measurementMatrix);
kp.measurementNoiseCov.copyTo(ocvKalman.measurementNoiseCov);
kp.processNoiseCov.copyTo(ocvKalman.processNoiseCov);
if (control)
kp.controlMatrix.copyTo(ocvKalman.controlMatrix);
}
TEST_P(KalmanFilterTest, AccuracyTest)
{
cv::gapi::KalmanParams kp;
initKalmanParams(kp, type, dDim, mDim, cDim);
// OpenCV reference KalmanFilter initialization
cv::KalmanFilter ocvKalman(dDim, mDim, cDim, type);
initKalmanFilter(kp, ocvKalman, true);
//measurement vector
cv::Mat measure_vec(mDim, 1, type);
//control vector
cv::Mat ctrl_vec = Mat::zeros(cDim > 0 ? cDim : 2, 1, type);
// G-API Kalman's output state
cv::Mat gapiKState(dDim, 1, type);
// OCV Kalman's output state
cv::Mat ocvKState(dDim, 1, type);
// G-API graph initialization
cv::GMat m, ctrl;
cv::GOpaque<bool> have_m;
cv::GMat out = cv::gapi::KalmanFilter(m, have_m, ctrl, kp);
cv::GComputation comp(cv::GIn(m, have_m, ctrl), cv::GOut(out));
cv::RNG& rng = cv::theRNG();
bool haveMeasure;
for (int i = 0; i < numIter; i++)
{
haveMeasure = (rng(2u) == 1) ? true : false; // returns 0 or 1 - whether we have measurement at this iteration or not
if (haveMeasure)
cv::randu(measure_vec, Scalar::all(-1), Scalar::all(1));
if (cDim > 0)
cv::randu(ctrl_vec, Scalar::all(-1), Scalar::all(1));
// G-API KalmanFilter call
comp.apply(cv::gin(measure_vec, haveMeasure, ctrl_vec), cv::gout(gapiKState));
// OpenCV KalmanFilter call
ocvKState = cDim > 0 ? ocvKalman.predict(ctrl_vec) : ocvKalman.predict();
if (haveMeasure)
ocvKState = ocvKalman.correct(measure_vec);
}
// Comparison //////////////////////////////////////////////////////////////
{
double diff = 0;
vector<int> idx;
EXPECT_TRUE(cmpEps(gapiKState, ocvKState, &diff, 1.0, &idx, false) >= 0);
}
}
TEST_P(KalmanFilterNoControlTest, AccuracyTest)
{
cv::gapi::KalmanParams kp;
initKalmanParams(kp, type, dDim, mDim, 0);
// OpenCV reference KalmanFilter initialization
cv::KalmanFilter ocvKalman(dDim, mDim, 0, type);
initKalmanFilter(kp, ocvKalman, false);
//measurement vector
cv::Mat measure_vec(mDim, 1, type);
// G-API Kalman's output state
cv::Mat gapiKState(dDim, 1, type);
// OCV Kalman's output state
cv::Mat ocvKState(dDim, 1, type);
// G-API graph initialization
cv::GMat m;
cv::GOpaque<bool> have_m;
cv::GMat out = cv::gapi::KalmanFilter(m, have_m, kp);
cv::GComputation comp(cv::GIn(m, have_m), cv::GOut(out));
cv::RNG& rng = cv::theRNG();
bool haveMeasure;
for (int i = 0; i < numIter; i++)
{
haveMeasure = (rng(2u) == 1) ? true : false; // returns 0 or 1 - whether we have measurement at this iteration or not
if (haveMeasure)
cv::randu(measure_vec, Scalar::all(-1), Scalar::all(1));
// G-API
comp.apply(cv::gin(measure_vec, haveMeasure), cv::gout(gapiKState));
// OpenCV
ocvKState = ocvKalman.predict();
if (haveMeasure)
ocvKState = ocvKalman.correct(measure_vec);
}
// Comparison //////////////////////////////////////////////////////////////
{
double diff = 0;
vector<int> idx;
EXPECT_TRUE(cmpEps(gapiKState, ocvKState, &diff, 1.0, &idx, false) >= 0);
}
}
TEST_P(KalmanFilterCircleSampleTest, AccuracyTest)
{
// auxiliary variables
cv::Mat processNoise(2, 1, type);
// For comparison
double diff = 0;
vector<int> idx;
// Input mesurement
cv::Mat measurement = Mat::zeros(1, 1, type);
// Angle and it's delta(phi, delta_phi)
cv::Mat state(2, 1, type);
// G-API graph initialization
cv::gapi::KalmanParams kp;
kp.state = Mat::zeros(2, 1, type);
cv::randn(kp.state, Scalar::all(0), Scalar::all(0.1));
kp.errorCov = Mat::eye(2, 2, type);
if (type == CV_32F)
kp.transitionMatrix = (Mat_<float>(2, 2) << 1, 1, 0, 1);
else
kp.transitionMatrix = (Mat_<double>(2, 2) << 1, 1, 0, 1);
kp.processNoiseCov = Mat::eye(2, 2, type) * (1e-5);
kp.measurementMatrix = Mat::eye(1, 2, type);
kp.measurementNoiseCov = Mat::eye(1, 1, type) * (1e-1);
cv::GMat m;
cv::GOpaque<bool> have_measure;
cv::GMat out = cv::gapi::KalmanFilter(m, have_measure, kp);
cv::GComputation comp(cv::GIn(m, have_measure), cv::GOut(out));
// OCV Kalman initialization
cv::KalmanFilter KF(2, 1, 0);
initKalmanFilter(kp, KF, false);
cv::randn(state, Scalar::all(0), Scalar::all(0.1));
// GAPI Corrected state
cv::Mat gapiState(2, 1, type);
// OCV Corrected state
cv::Mat ocvCorrState(2, 1, type);
// OCV Predicted state
cv::Mat ocvPreState(2, 1, type);
bool haveMeasure;
for (int i = 0; i < numIter; ++i)
{
// Get OCV Prediction
ocvPreState = KF.predict();
GAPI_DbgAssert(cv::norm(kp.measurementNoiseCov, KF.measurementNoiseCov, cv::NORM_INF) == 0);
// generation measurement
cv::randn(measurement, Scalar::all(0), Scalar::all((type == CV_32FC1) ?
kp.measurementNoiseCov.at<float>(0) : kp.measurementNoiseCov.at<double>(0)));
GAPI_DbgAssert(cv::norm(kp.measurementMatrix, KF.measurementMatrix, cv::NORM_INF) == 0);
measurement += kp.measurementMatrix*state;
if (cv::theRNG().uniform(0, 4) != 0)
{
haveMeasure = true;
ocvCorrState = KF.correct(measurement);
comp.apply(cv::gin(measurement, haveMeasure), cv::gout(gapiState));
EXPECT_TRUE(cmpEps(gapiState, ocvCorrState, &diff, 1.0, &idx, false) >= 0);
}
else
{
// Get GAPI Prediction
haveMeasure = false;
comp.apply(cv::gin(measurement, haveMeasure), cv::gout(gapiState));
EXPECT_TRUE(cmpEps(gapiState, ocvPreState, &diff, 1.0, &idx, false) >= 0);
}
GAPI_DbgAssert(cv::norm(kp.processNoiseCov, KF.processNoiseCov, cv::NORM_INF) == 0);
cv::randn(processNoise, Scalar(0), Scalar::all(sqrt(type == CV_32FC1 ?
kp.processNoiseCov.at<float>(0, 0):
kp.processNoiseCov.at<double>(0, 0))));
GAPI_DbgAssert(cv::norm(kp.transitionMatrix, KF.transitionMatrix, cv::NORM_INF) == 0);
state = kp.transitionMatrix*state + processNoise;
}
}
#endif
} // opencv_test

24
modules/gapi/test/cpu/gapi_video_tests_cpu.cpp
View File

@ -111,4 +111,28 @@ INSTANTIATE_TEST_CASE_MACRO_P(WITH_VIDEO(BackgroundSubtractorTestCPU),
Values(-1, 0, 0.5, 1),
Values("cv/video/768x576.avi"),
Values(3)));
INSTANTIATE_TEST_CASE_MACRO_P(KalmanFilterTestCPU,
KalmanFilterTest,
Combine(Values(VIDEO_CPU),
Values(CV_32FC1, CV_64FC1),
Values(2,5),
Values(2,5),
Values(2),
Values(5)));
INSTANTIATE_TEST_CASE_MACRO_P(KalmanFilterTestCPU,
KalmanFilterNoControlTest,
Combine(Values(VIDEO_CPU),
Values(CV_32FC1, CV_64FC1),
Values(3),
Values(4),
Values(3)));
INSTANTIATE_TEST_CASE_MACRO_P(KalmanFilterTestCPU,
KalmanFilterCircleSampleTest,
Combine(Values(VIDEO_CPU),
Values(CV_32FC1, CV_64FC1),
Values(5)));
} // opencv_test