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

Merge pull request #1413 from melody-rain:2.4_kalman_filter

Browse Source
This commit is contained in:
Roman Donchenko
2013-09-10 11:28:21 +04:00
committed by OpenCV Buildbot
parent 41b8479d03 a4ab3f4ea8
commit 260b283fb3
5 changed files with 473 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

60
modules/ocl/src/arithm.cpp
View File

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