From 46e275dfe4616b07cd2492c17698d1a0be0aa6df Mon Sep 17 00:00:00 2001 From: Anna Khakimova Date: Mon, 14 Dec 2020 11:56:37 +0300 Subject: [PATCH] Merge pull request #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. --- modules/gapi/include/opencv2/gapi/video.hpp | 93 +++++++- modules/gapi/src/api/kernels_video.cpp | 58 +++++ modules/gapi/src/backends/cpu/gcpuvideo.cpp | 69 ++++++ modules/gapi/test/common/gapi_video_tests.hpp | 7 + .../gapi/test/common/gapi_video_tests_inl.hpp | 223 ++++++++++++++++++ .../gapi/test/cpu/gapi_video_tests_cpu.cpp | 24 ++ 6 files changed, 473 insertions(+), 1 deletion(-) diff --git a/modules/gapi/include/opencv2/gapi/video.hpp b/modules/gapi/include/opencv2/gapi/video.hpp index 3c1341d2b1..10965b0aa6 100644 --- a/modules/gapi/include/opencv2/gapi/video.hpp +++ b/modules/gapi/include/opencv2/gapi/video.hpp @@ -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, GScalar>; @@ -129,6 +155,28 @@ G_TYPED_KERNEL(GBackgroundSubtractor, , } }; +void checkParams(const cv::gapi::KalmanParams& kfParams, + const cv::GMatDesc& measurement, const cv::GMatDesc& control = {}); + +G_TYPED_KERNEL(GKalmanFilter, , 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, , 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 . + +@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& 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& haveMeasurement, + const cv::gapi::KalmanParams& kfParams); + //! @} gapi_video } //namespace gapi } //namespace cv @@ -264,6 +355,6 @@ template<> struct CompileArgTag } }; } // namespace detail -} //namespace cv +} // namespace cv #endif // OPENCV_GAPI_VIDEO_HPP diff --git a/modules/gapi/src/api/kernels_video.cpp b/modules/gapi/src/api/kernels_video.cpp index b7c825f624..5eeaef2234 100644 --- a/modules/gapi/src/api/kernels_video.cpp +++ b/modules/gapi/src/api/kernels_video.cpp @@ -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& have_m, const GMat& c, const KalmanParams& kp) +{ + return GKalmanFilter::on(m, have_m, c, kp); +} + +GMat KalmanFilter(const GMat& m, const cv::GOpaque& 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 diff --git a/modules/gapi/src/backends/cpu/gcpuvideo.cpp b/modules/gapi/src/backends/cpu/gcpuvideo.cpp index bc526d7bde..075b5f9ad5 100644 --- a/modules/gapi/src/backends/cpu/gcpuvideo.cpp +++ b/modules/gapi/src/backends/cpu/gcpuvideo.cpp @@ -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 &state, const cv::GCompileArgs&) + { + state = std::make_shared(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 &state, + const cv::GCompileArgs&) + { + state = std::make_shared(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; } diff --git a/modules/gapi/test/common/gapi_video_tests.hpp b/modules/gapi/test/common/gapi_video_tests.hpp index ab12528259..78f0b69657 100644 --- a/modules/gapi/test/common/gapi_video_tests.hpp +++ b/modules/gapi/test/common/gapi_video_tests.hpp @@ -31,6 +31,13 @@ GAPI_TEST_FIXTURE_SPEC_PARAMS(BuildPyr_CalcOptFlow_PipelineTest, GAPI_TEST_FIXTURE_SPEC_PARAMS(BackgroundSubtractorTest, FIXTURE_API(tuple, 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 diff --git a/modules/gapi/test/common/gapi_video_tests_inl.hpp b/modules/gapi/test/common/gapi_video_tests_inl.hpp index 099fb6ad33..a376057715 100644 --- a/modules/gapi/test/common/gapi_video_tests_inl.hpp +++ b/modules/gapi/test/common/gapi_video_tests_inl.hpp @@ -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 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 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 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 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 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_(2, 2) << 1, 1, 0, 1); + else + kp.transitionMatrix = (Mat_(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 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(0) : kp.measurementNoiseCov.at(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(0, 0): + kp.processNoiseCov.at(0, 0)))); + + GAPI_DbgAssert(cv::norm(kp.transitionMatrix, KF.transitionMatrix, cv::NORM_INF) == 0); + state = kp.transitionMatrix*state + processNoise; + } +} + #endif } // opencv_test diff --git a/modules/gapi/test/cpu/gapi_video_tests_cpu.cpp b/modules/gapi/test/cpu/gapi_video_tests_cpu.cpp index c84b904072..6d8a4fe7f9 100644 --- a/modules/gapi/test/cpu/gapi_video_tests_cpu.cpp +++ b/modules/gapi/test/cpu/gapi_video_tests_cpu.cpp @@ -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