diff --git a/modules/tracking/include/opencv2/tracking/kalman_filters.hpp b/modules/tracking/include/opencv2/tracking/kalman_filters.hpp index e733b22ba..7a89c87dd 100644 --- a/modules/tracking/include/opencv2/tracking/kalman_filters.hpp +++ b/modules/tracking/include/opencv2/tracking/kalman_filters.hpp @@ -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. diff --git a/modules/tracking/src/augmented_unscented_kalman.cpp b/modules/tracking/src/augmented_unscented_kalman.cpp index 953e422fc..0ba8b03ab 100644 --- a/modules/tracking/src/augmented_unscented_kalman.cpp +++ b/modules/tracking/src/augmented_unscented_kalman.cpp @@ -47,50 +47,6 @@ namespace cv namespace tracking { -/* Cholesky decomposition - The function performs 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 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 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 - 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 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( covMatrix.ptr(), covMatrix.step, covMatrix.rows, covMatrixL.ptr() ); +// covMatrixL = cholesky( covMatrix ) + if ( dataType == CV_64F ) + choleskyDecomposition( + covMatrix.ptr(), covMatrix.step, covMatrix.rows, + covMatrixL.ptr(), covMatrixL.step ); + else if ( dataType == CV_32F ) + choleskyDecomposition( + covMatrix.ptr(), covMatrix.step, covMatrix.rows, + covMatrixL.ptr(), 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(mean, covMatrix, coef); - if ( dataType == CV_32F ) return getSigmaPoints(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 ) ); diff --git a/modules/tracking/src/precomp.hpp b/modules/tracking/src/precomp.hpp index 0d67de88d..660cdfcef 100644 --- a/modules/tracking/src/precomp.hpp +++ b/modules/tracking/src/precomp.hpp @@ -45,10 +45,63 @@ #include "opencv2/tracking.hpp" #include "opencv2/core/utility.hpp" #include "opencv2/core/ocl.hpp" +#include +#include "opencv2/core/hal/hal.hpp" namespace cv { extern const double ColorNames[][10]; -} + + namespace tracking { + + /* Cholesky decomposition + The function performs 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 bool + inline callHalCholesky( _Tp* L, size_t lstep, int lsize ); + + template<> bool + inline callHalCholesky( float* L, size_t lstep, int lsize ) + { + return hal::Cholesky32f(L, lstep, lsize, NULL, 0, 0); + } + + template<> bool + inline callHalCholesky( double* L, size_t lstep, int lsize) + { + return hal::Cholesky64f(L, lstep, lsize, NULL, 0, 0); + } + + template 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 diff --git a/modules/tracking/src/unscented_kalman.cpp b/modules/tracking/src/unscented_kalman.cpp index 8cfa9d38b..c29d7dcf6 100644 --- a/modules/tracking/src/unscented_kalman.cpp +++ b/modules/tracking/src/unscented_kalman.cpp @@ -47,49 +47,6 @@ namespace cv namespace tracking { -/* Cholesky decomposition - The function performs 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 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 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 - 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 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( covMatrix.ptr(), covMatrix.step, covMatrix.rows, covMatrixL.ptr() ); + if ( dataType == CV_64F ) + choleskyDecomposition( + covMatrix.ptr(), covMatrix.step, covMatrix.rows, + covMatrixL.ptr(), covMatrixL.step ); + else if ( dataType == CV_32F ) + choleskyDecomposition( + covMatrix.ptr(), covMatrix.step, covMatrix.rows, + covMatrixL.ptr(), 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(mean, covMatrix, coef); - if ( dataType == CV_32F ) return getSigmaPoints(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 ) );