diff --git a/modules/ocl/include/opencv2/ocl/ocl.hpp b/modules/ocl/include/opencv2/ocl/ocl.hpp index 5cd69b316..7ceb2183e 100644 --- a/modules/ocl/include/opencv2/ocl/ocl.hpp +++ b/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 diff --git a/modules/ocl/src/arithm.cpp b/modules/ocl/src/arithm.cpp index 11e9c50f4..819c01390 100644 --- a/modules/ocl/src/arithm.cpp +++ b/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 > 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); +} diff --git a/modules/ocl/src/kalman.cpp b/modules/ocl/src/kalman.cpp new file mode 100644 index 000000000..2ee7f9656 --- /dev/null +++ b/modules/ocl/src/kalman.cpp @@ -0,0 +1,135 @@ +/*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 diff --git a/modules/ocl/src/opencl/arithm_setidentity.cl b/modules/ocl/src/opencl/arithm_setidentity.cl new file mode 100644 index 000000000..0604ae81d --- /dev/null +++ b/modules/ocl/src/opencl/arithm_setidentity.cl @@ -0,0 +1,100 @@ +/*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; + } +} diff --git a/modules/ocl/test/test_kalman.cpp b/modules/ocl/test/test_kalman.cpp new file mode 100644 index 000000000..db3f19e87 --- /dev/null +++ b/modules/ocl/test/test_kalman.cpp @@ -0,0 +1,147 @@ +/////////////////////////////////////////////////////////////////////////////////////// +// +// 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