Added Kalman Filter of OpenCL version.

modules/ocl/include/opencv2/ocl/ocl.hpp

@@ -587,6 +587,7 @@ namespace cv
 
         CV_EXPORTS void cvtColor(const oclMat &src, oclMat &dst, int code , int dcn = 0);
 
+        CV_EXPORTS void setIdentity(oclMat& src, double val);
         //////////////////////////////// Filter Engine ////////////////////////////////
 
         /*!
@@ -1847,6 +1848,37 @@ namespace cv
 
             oclMat bgmodelUsedModes_; //keep track of number of modes per pixel
         };
+
+        /*!***************Kalman Filter*************!*/
+        class CV_EXPORTS KalmanFilter
+        {
+        public:
+            KalmanFilter();
+            //! the full constructor taking the dimensionality of the state, of the measurement and of the control vector
+            KalmanFilter(int dynamParams, int measureParams, int controlParams=0, int type=CV_32F);
+            //! re-initializes Kalman filter. The previous content is destroyed.
+            void init(int dynamParams, int measureParams, int controlParams=0, int type=CV_32F);
+
+            const oclMat& predict(const oclMat& control=oclMat());
+            const oclMat& correct(const oclMat& measurement);
+
+            oclMat statePre;           //!< predicted state (x'(k)): x(k)=A*x(k-1)+B*u(k)
+            oclMat statePost;          //!< corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))
+            oclMat transitionMatrix;   //!< state transition matrix (A)
+            oclMat controlMatrix;      //!< control matrix (B) (not used if there is no control)
+            oclMat measurementMatrix;  //!< measurement matrix (H)
+            oclMat processNoiseCov;    //!< process noise covariance matrix (Q)
+            oclMat measurementNoiseCov;//!< measurement noise covariance matrix (R)
+            oclMat errorCovPre;        //!< priori error estimate covariance matrix (P'(k)): P'(k)=A*P(k-1)*At + Q)*/
+            oclMat gain;               //!< Kalman gain matrix (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)
+            oclMat errorCovPost;       //!< posteriori error estimate covariance matrix (P(k)): P(k)=(I-K(k)*H)*P'(k)
+        private:
+            oclMat temp1;
+            oclMat temp2;
+            oclMat temp3;
+            oclMat temp4;
+            oclMat temp5;
+        };
     }
 }
 #if defined _MSC_VER && _MSC_VER >= 1200

modules/ocl/src/arithm.cpp

@@ -98,6 +98,7 @@ namespace cv
         extern const char *arithm_phase;
         extern const char *arithm_pow;
         extern const char *arithm_magnitudeSqr;
+        extern const char *arithm_setidentity;
         //extern const char * jhp_transpose_kernel;
         int64 kernelrealtotal = 0;
         int64 kernelalltotal = 0;
@@ -2342,3 +2343,62 @@ void cv::ocl::pow(const oclMat &x, double p, oclMat &y)
 
     arithmetic_pow_run(x, p, y, kernelName, &arithm_pow);
 }
+void cv::ocl::setIdentity(oclMat& src, double scalar)
+{
+    CV_Assert(src.empty() == false && src.rows == src.cols);
+    CV_Assert(src.type() == CV_32SC1 || src.type() == CV_32FC1);
+    int src_step = src.step/src.elemSize();
+    Context  *clCxt = Context::getContext();
+    size_t local_threads[] = {16, 16, 1};
+    size_t global_threads[] = {src.cols, src.rows, 1};
+
+    string kernelName = "setIdentityKernel";
+    if(src.type() == CV_32FC1)
+        kernelName += "_F1";
+    else if(src.type() == CV_32SC1)
+        kernelName += "_I1";
+    else
+    {
+        kernelName += "_D1";
+        if(!(clCxt->supportsFeature(Context::CL_DOUBLE)))
+        {
+            oclMat temp;
+            src.convertTo(temp, CV_32FC1);
+            temp.copyTo(src);
+        }
+
+    }
+
+
+    vector<pair<size_t , const void *> > args;
+    args.push_back( make_pair( sizeof(cl_mem), (void *)&src.data ));
+    args.push_back( make_pair( sizeof(cl_int), (void *)&src.rows));
+    args.push_back( make_pair( sizeof(cl_int), (void *)&src.cols));
+    args.push_back( make_pair( sizeof(cl_int), (void *)&src_step ));
+
+    int scalar_i = 0;
+    float scalar_f = 0.0f;
+    if(clCxt->supportsFeature(Context::CL_DOUBLE))
+    {
+        if(src.type() == CV_32SC1)
+        {
+            scalar_i = (int)scalar;
+            args.push_back(make_pair(sizeof(cl_int), (void*)&scalar_i));
+        }else
+            args.push_back(make_pair(sizeof(cl_double), (void*)&scalar));
+    }
+    else
+    {
+        if(src.type() == CV_32SC1)
+        {
+            scalar_i = (int)scalar;
+            args.push_back(make_pair(sizeof(cl_int), (void*)&scalar_i));
+        }else
+        {
+            scalar_f = (float)scalar;
+            args.push_back(make_pair(sizeof(cl_float), (void*)&scalar_f));
+        }
+    }
+
+    openCLExecuteKernel(clCxt, &arithm_setidentity, kernelName, global_threads, local_threads, args, -1, -1);
+}

modules/ocl/src/kalman.cpp

+/*M///////////////////////////////////////////////////////////////////////////////////////
+//
+//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+//  By downloading, copying, installing or using the software you agree to this license.
+//  If you do not agree to this license, do not download, install,
+//  copy or use the software.
+//
+//
+//                           License Agreement
+//                For Open Source Computer Vision Library
+//
+// Copyright (C) 2010-2012, Multicoreware, Inc., all rights reserved.
+// Copyright (C) 2010-2012, Advanced Micro Devices, Inc., all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// @Authors
+//     Jin Ma, jin@multicorewareinc.com
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+//   * Redistribution's of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistribution's in binary form must reproduce the above copyright notice,
+//     this list of conditions and the following disclaimer in the documentation
+//     and/or other oclMaterials provided with the distribution.
+//
+//   * The name of the copyright holders may not be used to endorse or promote products
+//     derived from this software without specific prior written permission.
+//
+// This software is provided by the copyright holders and contributors as is and
+// any express or implied warranties, including, but not limited to, the implied
+// warranties of merchantability and fitness for a particular purpose are disclaimed.
+// In no event shall the Intel Corporation or contributors be liable for any direct,
+// indirect, incidental, special, exemplary, or consequential damages
+// (including, but not limited to, procurement of substitute goods or services;
+// loss of use, data, or profits; or business interruption) however caused
+// and on any theory of liability, whether in contract, strict liability,
+// or tort (including negligence or otherwise) arising in any way out of
+// the use of this software, even if advised of the possibility of such damage.
+//
+//M*/
+#include "precomp.hpp"
+
+using namespace std;
+using namespace cv;
+using namespace cv::ocl;
+
+KalmanFilter::KalmanFilter() 
+{
+
+}
+
+KalmanFilter::KalmanFilter(int dynamParams, int measureParams, int controlParams, int type)
+{
+    init(dynamParams, measureParams, controlParams, type);
+}
+
+void KalmanFilter::init(int DP, int MP, int CP, int type)
+{
+    CV_Assert( DP > 0 && MP > 0 );
+    CV_Assert( type == CV_32F || type == CV_64F );
+    CP = cv::max(CP, 0);
+
+    statePre.create(DP, 1, type);
+    statePre.setTo(Scalar::all(0));
+    
+    statePost.create(DP, 1, type);
+    statePost.setTo(Scalar::all(0));
+
+    transitionMatrix.create(DP, DP, type);
+    setIdentity(transitionMatrix, 1);
+
+    processNoiseCov.create(DP, DP, type);
+    setIdentity(processNoiseCov, 1);
+
+    measurementNoiseCov.create(MP, MP, type);
+    setIdentity(measurementNoiseCov, 1);
+
+    measurementMatrix.create(MP, DP, type);
+    measurementMatrix.setTo(Scalar::all(0));
+
+    errorCovPre.create(DP, DP, type);
+    errorCovPre.setTo(Scalar::all(0));
+
+    errorCovPost.create(DP, DP, type);
+    errorCovPost.setTo(Scalar::all(0));
+
+    gain.create(DP, MP, type);
+    gain.setTo(Scalar::all(0));
+
+    if( CP > 0 )
+    {
+        controlMatrix.create(DP, CP, type);
+        controlMatrix.setTo(Scalar::all(0));
+    }
+    else
+        controlMatrix.release();
+
+    temp1.create(DP, DP, type);
+    temp2.create(MP, DP, type);
+    temp3.create(MP, MP, type);
+    temp4.create(MP, DP, type);
+    temp5.create(MP, 1, type);
+}
+
+CV_EXPORTS const oclMat& KalmanFilter::predict(const oclMat& control)
+{
+    gemm(transitionMatrix, statePost, 1, oclMat(), 0, statePre);
+    oclMat temp;
+
+    if(control.data) 
+        gemm(controlMatrix, control, 1, statePre, 1, statePre);
+    gemm(transitionMatrix, errorCovPost, 1, oclMat(), 0, temp1);
+    gemm(temp1, transitionMatrix, 1, processNoiseCov, 1, errorCovPre, GEMM_2_T);
+    statePre.copyTo(statePost);
+    return statePre;
+}
+
+CV_EXPORTS const oclMat& KalmanFilter::correct(const oclMat& measurement)
+{
+    CV_Assert(measurement.empty() == false);
+    gemm(measurementMatrix, errorCovPre, 1, oclMat(), 0, temp2);
+    gemm(temp2, measurementMatrix, 1, measurementNoiseCov, 1, temp3, GEMM_2_T);
+    Mat temp;
+    solve(Mat(temp3), Mat(temp2), temp, DECOMP_SVD);
+    temp4.upload(temp);
+    gain = temp4.t();
+    gemm(measurementMatrix, statePre, -1, measurement, 1, temp5);
+    gemm(gain, temp5, 1, statePre, 1, statePost);
+    gemm(gain, temp2, -1, errorCovPre, 1, errorCovPost);
+    return statePost;
+}
\ No newline at end of file

modules/ocl/src/opencl/arithm_setidentity.cl

+/*M///////////////////////////////////////////////////////////////////////////////////////
+//
+//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+//  By downloading, copying, installing or using the software you agree to this license.
+//  If you do not agree to this license, do not download, install,
+//  copy or use the software.
+//
+//
+//                           License Agreement
+//                For Open Source Computer Vision Library
+//
+// Copyright (C) 2010-2012, Multicoreware, Inc., all rights reserved.
+// Copyright (C) 2010-2012, Advanced Micro Devices, Inc., all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// @Authors
+//    Jin Ma jin@multicorewareinc.com
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+//   * Redistribution's of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistribution's in binary form must reproduce the above copyright notice,
+//     this list of conditions and the following disclaimer in the documentation
+//     and/or other oclMaterials provided with the distribution.
+//
+//   * The name of the copyright holders may not be used to endorse or promote products
+//     derived from this software without specific prior written permission.
+//
+// This software is provided by the copyright holders and contributors as is and
+// any express or implied warranties, including, but not limited to, the implied
+// warranties of merchantability and fitness for a particular purpose are disclaimed.
+// In no event shall the Intel Corporation or contributors be liable for any direct,
+// indirect, incidental, special, exemplary, or consequential damages
+// (including, but not limited to, procurement of substitute goods or services;
+// loss of use, data, or profits; or business interruption) however caused
+// and on any theory of liability, whether in contract, strict liability,
+// or tort (including negligence or otherwise) arising in any way out of
+// the use of this software, even if advised of the possibility of such damage.
+//
+//M*/
+#if defined (DOUBLE_SUPPORT)
+#ifdef cl_khr_fp64
+#pragma OPENCL EXTENSION cl_khr_fp64:enable
+#elif defined (cl_amd_fp64)
+#pragma OPENCL EXTENSION cl_amd_fp64:enable
+#endif
+#endif
+
+
+#if defined (DOUBLE_SUPPORT)
+#define DATA_TYPE double
+#else
+#define DATA_TYPE float
+#endif
+
+__kernel void setIdentityKernel_F1(__global float* src, int src_row, int src_col, int src_step, DATA_TYPE scalar)
+{
+    int x = get_global_id(0);
+    int y = get_global_id(1);
+
+    if(x < src_col && y < src_row)
+    {
+        if(x == y)
+            src[y * src_step + x] = scalar;
+        else
+            src[y * src_step + x] = 0 * scalar;
+    }
+}
+
+__kernel void setIdentityKernel_D1(__global DATA_TYPE* src, int src_row, int src_col, int src_step, DATA_TYPE scalar)
+{
+    int x = get_global_id(0);
+    int y = get_global_id(1);
+
+    if(x < src_col && y < src_row)
+    {
+        if(x == y)
+            src[y * src_step + x] = scalar;
+        else
+            src[y * src_step + x] = 0 * scalar;
+    }
+}
+
+__kernel void setIdentityKernel_I1(__global int* src, int src_row, int src_col, int src_step, int scalar)
+{
+    int x = get_global_id(0);
+    int y = get_global_id(1);
+
+    if(x < src_col && y < src_row)
+    {
+        if(x == y)
+            src[y * src_step + x] = scalar;
+        else
+            src[y * src_step + x] = 0 * scalar;
+    }
+}

modules/ocl/test/test_kalman.cpp

+///////////////////////////////////////////////////////////////////////////////////////
+//
+//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+//  By downloading, copying, installing or using the software you agree to this license.
+//  If you do not agree to this license, do not download, install,
+//  copy or use the software.
+//
+//
+//                           License Agreement
+//                For Open Source Computer Vision Library
+// Copyright (C) 2010-2012, Multicoreware, Inc., all rights reserved.
+// Copyright (C) 2010-2012, Advanced Micro Devices, Inc., all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// @Authors
+//    Jin Ma, jin@multicorewareinc.com
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+//   * Redistribution's of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistribution's in binary form must reproduce the above copyright notice,
+//     this list of conditions and the following disclaimer in the documentation
+//     and/or other oclMaterials provided with the distribution.
+//
+//   * The name of the copyright holders may not be used to endorse or promote products
+//     derived from this software without specific prior written permission.
+//
+// This software is provided by the copyright holders and contributors "as is" and
+// any express or implied warranties, including, but not limited to, the implied
+// warranties of merchantability and fitness for a particular purpose are disclaimed.
+// In no event shall the Intel Corporation or contributors be liable for any direct,
+// indirect, incidental, special, exemplary, or consequential damages
+// (including, but not limited to, procurement of substitute goods or services;
+// loss of use, data, or profits; or business interruption) however caused
+// and on any theory of liability, whether in contract, strict liability,
+// or tort (including negligence or otherwise) arising in any way out of
+// the use of this software, even if advised of the possibility of such damage.
+//
+//M*/
+
+#include "test_precomp.hpp"
+#ifdef HAVE_OPENCL
+using namespace cv;
+using namespace cv::ocl;
+using namespace cvtest;
+using namespace testing;
+using namespace std;
+
+//////////////////////////////////////////////////////////////////////////
+PARAM_TEST_CASE(Kalman, int, int)
+{
+    int size_;
+    int iteration;
+    virtual void SetUp()
+    {
+        size_ = GET_PARAM(0);
+        iteration = GET_PARAM(1);
+    }
+};
+
+TEST_P(Kalman, Accuracy)
+{
+    const int Dim = size_;
+    const int Steps = iteration;
+    const double max_init = 1;
+    const double max_noise = 0.1;
+
+    cv::RNG &rng = TS::ptr()->get_rng();
+
+    Mat sample_mat(Dim, 1, CV_32F), temp_mat;
+    oclMat Sample(Dim, 1, CV_32F);
+    oclMat Temp(Dim, 1, CV_32F);
+    Mat Temp_cpu(Dim, 1, CV_32F);
+
+    Size size(Sample.cols, Sample.rows);
+
+    sample_mat =  randomMat(rng, size, Sample.type(), -max_init, max_init, false);
+    Sample.upload(sample_mat);
+
+    //ocl start
+    cv::ocl::KalmanFilter kalman_filter_ocl;
+    kalman_filter_ocl.init(Dim, Dim);
+
+    cv::ocl::setIdentity(kalman_filter_ocl.errorCovPre, 1);
+    cv::ocl::setIdentity(kalman_filter_ocl.measurementMatrix, 1);
+    cv::ocl::setIdentity(kalman_filter_ocl.errorCovPost, 1);
+
+    kalman_filter_ocl.measurementNoiseCov.setTo(Scalar::all(0));
+    kalman_filter_ocl.statePre.setTo(Scalar::all(0));
+    kalman_filter_ocl.statePost.setTo(Scalar::all(0));
+
+    kalman_filter_ocl.correct(Sample);
+    //ocl end
+
+    //cpu start 
+    cv::KalmanFilter kalman_filter_cpu;
+
+    kalman_filter_cpu.init(Dim, Dim);
+
+    cv::setIdentity(kalman_filter_cpu.errorCovPre, 1);
+    cv::setIdentity(kalman_filter_cpu.measurementMatrix, 1);
+    cv::setIdentity(kalman_filter_cpu.errorCovPost, 1);
+
+    kalman_filter_cpu.measurementNoiseCov.setTo(Scalar::all(0));
+    kalman_filter_cpu.statePre.setTo(Scalar::all(0));
+    kalman_filter_cpu.statePost.setTo(Scalar::all(0));
+
+    kalman_filter_cpu.correct(sample_mat);
+    //cpu end
+    //test begin
+    for(int i = 0; i<Steps; i++)
+    {
+        kalman_filter_ocl.predict();
+        kalman_filter_cpu.predict();
+
+        cv::gemm(kalman_filter_cpu.transitionMatrix, sample_mat, 1, cv::Mat(), 0, Temp_cpu);
+
+        Size size1(Temp.cols, Temp.rows);
+        Mat temp = randomMat(rng, size1, Temp.type(), 0, 0xffff, false);
+
+
+        cv::multiply(2, temp, temp);
+
+        cv::subtract(temp, 1, temp);
+
+        cv::multiply(max_noise, temp, temp);
+
+        cv::add(temp, Temp_cpu, Temp_cpu);
+
+        Temp.upload(Temp_cpu);
+        Temp.copyTo(Sample);
+        Temp_cpu.copyTo(sample_mat);
+
+        kalman_filter_ocl.correct(Temp);
+        kalman_filter_cpu.correct(Temp_cpu);
+    }
+    //test end
+    EXPECT_MAT_NEAR(kalman_filter_cpu.statePost, kalman_filter_ocl.statePost, 0);
+}
+INSTANTIATE_TEST_CASE_P(OCL_Video, Kalman, Combine(Values(3, 7), Values(30)));
+
+#endif // HAVE_OPENCL
+