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

Anna Khakimova · web-flow · commit 46e275dfe461 · 2020-12-14T08:56:37.000Z
* 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.
diff --git 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<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 <https://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
diff --git 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<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
diff --git a/modules/gapi/src/backends/cpu/gcpuvideo.cpp b/modules/gapi/src/backends/cpu/gcpuvideo.cpp
@@ -107,13 +107,82 @@ 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
         < GCPUBuildOptFlowPyramid
         , 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
@@ -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
 
 
diff --git a/modules/gapi/test/common/gapi_video_tests_inl.hpp b/modules/gapi/test/common/gapi_video_tests_inl.hpp
diff --git a/modules/gapi/test/cpu/gapi_video_tests_cpu.cpp b/modules/gapi/test/cpu/gapi_video_tests_cpu.cpp
