generic-library/opencv
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Added Kalman Filter of OpenCL version.

Browse Source
This commit is contained in:
Jin Ma 2013-09-09 16:08:23 +08:00
parent ff1eb0d582
commit 2d5a1dacd0
5 changed files with 474 additions and 0 deletions
modules/ocl
include/opencv2/ocl
ocl.hpp
src
arithm.cppkalman.cpp
opencl
arithm_setidentity.cl
test
test_kalman.cpp

32
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

60
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);
}

135
modules/ocl/src/kalman.cpp Normal file

@ -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;
}

100
modules/ocl/src/opencl/arithm_setidentity.cl Normal file

@ -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;
}
}

147
modules/ocl/test/test_kalman.cpp Normal file

@ -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<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