Chiebot-Mirror
/
opencv_contrib
8
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Remove duplicated code in UnscentedKalmanFilter

Browse Source
pull/867/head
Vladislav Sovrasov 8 years ago
parent b1cd048afc
commit 36f64dd17f
4 changed files with 87 additions and 130 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 4
      modules/tracking/include/opencv2/tracking/kalman_filters.hpp
  2. 80
      modules/tracking/src/augmented_unscented_kalman.cpp
  3. 55
      modules/tracking/src/precomp.hpp
  4. 78
      modules/tracking/src/unscented_kalman.cpp

4
modules/tracking/include/opencv2/tracking/kalman_filters.hpp
Unescape View File

@ -62,13 +62,13 @@ public:
* @param control - the current control vector,
* @return the predicted estimate of the state.
*/
virtual Mat predict( const Mat& control = Mat() ) = 0;
virtual Mat predict( InputArray control = noArray() ) = 0;
/** The function performs correction step of the algorithm
* @param measurement - the current measurement vector,
* @return the corrected estimate of the state.
*/
virtual Mat correct( const Mat& measurement ) = 0;
virtual Mat correct( InputArray measurement ) = 0;
/**
* @return the process noise cross-covariance matrix.

80
modules/tracking/src/augmented_unscented_kalman.cpp
Unescape View File

@ -47,50 +47,6 @@ namespace cv
namespace tracking
{
/* Cholesky decomposition
The function performs Cholesky decomposition <https://en.wikipedia.org/wiki/Cholesky_decomposition>.
A - the Hermitian, positive-definite matrix,
astep - size of row in A,
asize - number of cols and rows in A,
L - the lower triangular matrix, A = L*Lt.
*/
template<typename _Tp> bool
inline choleskyDecomposition( const _Tp* A, size_t astep, const int asize, _Tp* L )
{
int i, j, k;
double s;
astep /= sizeof(A[0]);
for( i = 0; i < asize; i++ )
{
for( j = 0; j < i; j++ )
{
s = A[i*astep + j];
for( k = 0; k < j; k++ )
s -= L[i*astep + k]*L[j*astep + k];
L[i*astep + j] = (_Tp)(s/L[j*astep + j]);
}
s = A[i*astep + i];
for( k = 0; k < i; k++ )
{
double t = L[i*astep + k];
s -= t*t;
}
if( s < std::numeric_limits<_Tp>::epsilon() )
return false;
L[i*astep + i] = (_Tp)(std::sqrt(s));
}
for( i = 0; i < asize; i++ )
for( j = i+1; j < asize; j++ )
{
L[i*astep + j] = 0.0;
}
return true;
}
void AugmentedUnscentedKalmanFilterParams::
init( int dp, int mp, int cp, double processNoiseCovDiag, double measurementNoiseCovDiag,
Ptr<UkfSystemModel> dynamicalSystem, int type )
@ -179,10 +135,6 @@ class AugmentedUnscentedKalmanFilterImpl: public UnscentedKalmanFilter
Mat r; // zero vector of process noise for getting transitionSPFuncVals,
Mat q; // zero vector of measurement noise for getting measurementSPFuncVals
template <typename T>
Mat getSigmaPoints(const Mat& mean, const Mat& covMatrix, double coef);
Mat getSigmaPoints(const Mat& mean, const Mat& covMatrix, double coef);
public:
@ -190,8 +142,8 @@ public:
AugmentedUnscentedKalmanFilterImpl(const AugmentedUnscentedKalmanFilterParams& params);
~AugmentedUnscentedKalmanFilterImpl();
Mat predict(const Mat& control);
Mat correct(const Mat& measurement);
Mat predict(InputArray control);
Mat correct(InputArray measurement);
Mat getProcessNoiseCov() const;
Mat getMeasurementNoiseCov() const;
@ -303,7 +255,6 @@ AugmentedUnscentedKalmanFilterImpl::~AugmentedUnscentedKalmanFilterImpl()
}
template <typename T>
Mat AugmentedUnscentedKalmanFilterImpl::getSigmaPoints(const Mat &mean, const Mat &covMatrix, double coef)
{
// x_0 = mean
@ -313,11 +264,18 @@ Mat AugmentedUnscentedKalmanFilterImpl::getSigmaPoints(const Mat &mean, const Ma
int n = mean.rows;
Mat points = repeat(mean, 1, 2*n+1);
// covMatrixL = cholesky( covMatrix )
Mat covMatrixL = covMatrix.clone();
covMatrixL.setTo(0);
choleskyDecomposition<T>( covMatrix.ptr<T>(), covMatrix.step, covMatrix.rows, covMatrixL.ptr<T>() );
// covMatrixL = cholesky( covMatrix )
if ( dataType == CV_64F )
choleskyDecomposition<double>(
covMatrix.ptr<double>(), covMatrix.step, covMatrix.rows,
covMatrixL.ptr<double>(), covMatrixL.step );
else if ( dataType == CV_32F )
choleskyDecomposition<float>(
covMatrix.ptr<float>(), covMatrix.step, covMatrix.rows,
covMatrixL.ptr<float>(), covMatrixL.step );
covMatrixL = coef * covMatrixL;
Mat p_plus = points( Rect( 1, 0, n, n ) );
@ -329,16 +287,9 @@ Mat AugmentedUnscentedKalmanFilterImpl::getSigmaPoints(const Mat &mean, const Ma
return points;
}
Mat AugmentedUnscentedKalmanFilterImpl::getSigmaPoints(const Mat& mean, const Mat& covMatrix, double coef)
{
if ( dataType == CV_64F ) return getSigmaPoints<double>(mean, covMatrix, coef);
if ( dataType == CV_32F ) return getSigmaPoints<float>(mean, covMatrix, coef);
return Mat();
}
Mat AugmentedUnscentedKalmanFilterImpl::predict(const Mat& control)
Mat AugmentedUnscentedKalmanFilterImpl::predict(InputArray _control)
{
Mat control = _control.getMat();
// get sigma points from xa* and Pa
sigmaPoints = getSigmaPoints( stateAug, errorCovAug, sqrt( tmpLambda ) );
@ -368,8 +319,9 @@ Mat AugmentedUnscentedKalmanFilterImpl::predict(const Mat& control)
return state.clone();
}
Mat AugmentedUnscentedKalmanFilterImpl::correct(const Mat& measurement)
Mat AugmentedUnscentedKalmanFilterImpl::correct(InputArray _measurement)
{
Mat measurement = _measurement.getMat();
// get sigma points from xa* and Pa
sigmaPoints = getSigmaPoints( stateAug, errorCovAug, sqrt( tmpLambda ) );

55
modules/tracking/src/precomp.hpp
Unescape View File

@ -45,10 +45,63 @@
#include "opencv2/tracking.hpp"
#include "opencv2/core/utility.hpp"
#include "opencv2/core/ocl.hpp"
#include <typeinfo>
#include "opencv2/core/hal/hal.hpp"
namespace cv
{
extern const double ColorNames[][10];
}
namespace tracking {
/* Cholesky decomposition
The function performs Cholesky decomposition <https://en.wikipedia.org/wiki/Cholesky_decomposition>.
A - the Hermitian, positive-definite matrix,
astep - size of row in A,
asize - number of cols and rows in A,
L - the lower triangular matrix, A = L*Lt.
*/
template<typename _Tp> bool
inline callHalCholesky( _Tp* L, size_t lstep, int lsize );
template<> bool
inline callHalCholesky<float>( float* L, size_t lstep, int lsize )
{
return hal::Cholesky32f(L, lstep, lsize, NULL, 0, 0);
}
template<> bool
inline callHalCholesky<double>( double* L, size_t lstep, int lsize)
{
return hal::Cholesky64f(L, lstep, lsize, NULL, 0, 0);
}
template<typename _Tp> bool
inline choleskyDecomposition( const _Tp* A, size_t astep, int asize, _Tp* L, size_t lstep )
{
bool success = false;
astep /= sizeof(_Tp);
lstep /= sizeof(_Tp);
for(int i = 0; i < asize; i++)
for(int j = 0; j <= i; j++)
L[i*lstep + j] = A[i*astep + j];
success = callHalCholesky(L, lstep*sizeof(_Tp), asize);
if(success)
{
for(int i = 0; i < asize; i++ )
for(int j = i + 1; j < asize; j++ )
L[i*lstep + j] = 0.0;
}
return success;
}
} // tracking
} // cv
#endif

78
modules/tracking/src/unscented_kalman.cpp
Unescape View File

@ -47,49 +47,6 @@ namespace cv
namespace tracking
{
/* Cholesky decomposition
The function performs Cholesky decomposition <https://en.wikipedia.org/wiki/Cholesky_decomposition>.
A - the Hermitian, positive-definite matrix,
astep - size of row in A,
asize - number of cols and rows in A,
L - the lower triangular matrix, A = L*Lt.
*/
template<typename _Tp> bool
inline choleskyDecomposition( const _Tp* A, size_t astep, const int asize, _Tp* L )
{
int i, j, k;
double s;
astep /= sizeof(A[0]);
for( i = 0; i < asize; i++ )
{
for( j = 0; j < i; j++ )
{
s = A[i*astep + j];
for( k = 0; k < j; k++ )
s -= L[i*astep + k]*L[j*astep + k];
L[i*astep + j] = (_Tp)(s/L[j*astep + j]);
}
s = A[i*astep + i];
for( k = 0; k < i; k++ )
{
double t = L[i*astep + k];
s -= t*t;
}
if( s < std::numeric_limits<_Tp>::epsilon() )
return false;
L[i*astep + i] = (_Tp)(std::sqrt(s));
}
for( i = 0; i < asize; i++ )
for( j = i+1; j < asize; j++ )
{
L[i*astep + j] = 0.0;
}
return true;
}
void UnscentedKalmanFilterParams::
init( int dp, int mp, int cp, double processNoiseCovDiag, double measurementNoiseCovDiag,
Ptr<UkfSystemModel> dynamicalSystem, int type )
@ -166,11 +123,6 @@ class UnscentedKalmanFilterImpl: public UnscentedKalmanFilter
Mat r; // zero vector of process noise for getting transitionSPFuncVals,
Mat q; // zero vector of measurement noise for getting measurementSPFuncVals
//get sigma points
template <typename T>
Mat getSigmaPoints( const Mat& mean, const Mat& covMatrix, double coef );
Mat getSigmaPoints( const Mat& mean, const Mat& covMatrix, double coef );
public:
@ -180,11 +132,11 @@ public:
// perform prediction step
// control - the optional control vector, CP x 1
Mat predict( const Mat& control = Mat() );
Mat predict( InputArray control = noArray() );
// perform correction step
// measurement - current measurement vector, MP x 1
Mat correct( const Mat& measurement );
Mat correct( InputArray measurement );
// Get system parameters
Mat getProcessNoiseCov() const;
@ -282,7 +234,6 @@ UnscentedKalmanFilterImpl::~UnscentedKalmanFilterImpl()
q.release();
}
template <typename T>
Mat UnscentedKalmanFilterImpl::getSigmaPoints(const Mat &mean, const Mat &covMatrix, double coef)
{
// x_0 = mean
@ -293,10 +244,17 @@ Mat UnscentedKalmanFilterImpl::getSigmaPoints(const Mat &mean, const Mat &covMat
Mat points = repeat(mean, 1, 2*n+1);
Mat covMatrixL = covMatrix.clone();
covMatrixL.setTo(0);
// covMatrixL = cholesky( covMatrix )
choleskyDecomposition<T>( covMatrix.ptr<T>(), covMatrix.step, covMatrix.rows, covMatrixL.ptr<T>() );
if ( dataType == CV_64F )
choleskyDecomposition<double>(
covMatrix.ptr<double>(), covMatrix.step, covMatrix.rows,
covMatrixL.ptr<double>(), covMatrixL.step );
else if ( dataType == CV_32F )
choleskyDecomposition<float>(
covMatrix.ptr<float>(), covMatrix.step, covMatrix.rows,
covMatrixL.ptr<float>(), covMatrixL.step );
covMatrixL = coef * covMatrixL;
Mat p_plus = points( Rect( 1, 0, n, n ) );
@ -308,16 +266,9 @@ Mat UnscentedKalmanFilterImpl::getSigmaPoints(const Mat &mean, const Mat &covMat
return points;
}
Mat UnscentedKalmanFilterImpl::getSigmaPoints(const Mat &mean, const Mat &covMatrix, double coef)
{
if ( dataType == CV_64F ) return getSigmaPoints<double>(mean, covMatrix, coef);
if ( dataType == CV_32F ) return getSigmaPoints<float>(mean, covMatrix, coef);
return Mat();
}
Mat UnscentedKalmanFilterImpl::predict(const Mat& control)
Mat UnscentedKalmanFilterImpl::predict(InputArray _control)
{
Mat control = _control.getMat();
// get sigma points from x* and P
sigmaPoints = getSigmaPoints( state, errorCov, sqrt( tmpLambda ) );
@ -345,8 +296,9 @@ Mat UnscentedKalmanFilterImpl::predict(const Mat& control)
return state.clone();
}
Mat UnscentedKalmanFilterImpl::correct(const Mat& measurement)
Mat UnscentedKalmanFilterImpl::correct(InputArray _measurement)
{
Mat measurement = _measurement.getMat();
// get sigma points from x* and P
sigmaPoints = getSigmaPoints( state, errorCov, sqrt( tmpLambda ) );

Write Preview
Loading…
Cancel
Save