mirrors/opencv_contrib
1
0
Fork 0
Code Pull Requests Packages Releases Activity

vendor: OpenCV 5.0.0 snapshot at 755e50675d97db9b7d449d8bd6b09888646f6c6e

Browse Source
This commit is contained in:
Gitea Mirror Bot
2026-08-22 00:11:13 +08:00
commit 12022378a3
3872 changed files with 2513409 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
modules/tracking/test/test_aukf.cpp
+438
View File
@@ -0,0 +1,438 @@
/*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) 2015, OpenCV Foundation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// 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 materials 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"
#include "opencv2/tracking/kalman_filters.hpp"
namespace opencv_test { namespace {
using namespace cv::detail;
// In this two tests Augmented Unscented Kalman Filter are applied to the dynamic system from example "The reentry problem" from
// "A New Extension of the Kalman Filter to Nonlinear Systems" by Simon J. Julier and Jeffrey K. Uhlmann.
class BallisticModel: public UkfSystemModel
{
static const double step_h;
Mat diff_eq(const Mat& x)
{
double x1 = x.at<double>(0, 0);
double x2 = x.at<double>(1, 0);
double x3 = x.at<double>(2, 0);
double x4 = x.at<double>(3, 0);
double x5 = x.at<double>(4, 0);
const double h0 = 9.3;
const double beta0 = 0.59783;
const double Gm = 3.9860044 * 1e5;
const double r_e = 6374;
const double r = sqrt( x1*x1 + x2*x2 );
const double v = sqrt( x3*x3 + x4*x4 );
const double d = - beta0 * exp( ( r_e - r )/h0 ) * exp( x5 ) * v;
const double g = - Gm / (r*r*r);
Mat fx = x.clone();
fx.at<double>(0, 0) = x3;
fx.at<double>(1, 0) = x4;
fx.at<double>(2, 0) = d * x3 + g * x1;
fx.at<double>(3, 0) = d * x4 + g * x2;
fx.at<double>(4, 0) = 0.0;
return fx;
}
public:
void stateConversionFunction(const Mat& x_k, const Mat& u_k, const Mat& v_k, Mat& x_kplus1)
{
Mat v = sqrt(step_h) * v_k.clone();
v.at<double>(0, 0) = 0.0;
v.at<double>(1, 0) = 0.0;
Mat k1 = diff_eq( x_k ) + v;
Mat tmp = x_k + step_h*0.5*k1;
Mat k2 = diff_eq( tmp ) + v;
tmp = x_k + step_h*0.5*k2;
Mat k3 = diff_eq( tmp ) + v;
tmp = x_k + step_h*k3;
Mat k4 = diff_eq( tmp ) + v;
x_kplus1 = x_k + (1.0/6.0)*step_h*( k1 + 2.0*k2 + 2.0*k3 + k4 ) + u_k;
}
void measurementFunction(const Mat& x_k, const Mat& n_k, Mat& z_k)
{
double x1 = x_k.at<double>(0, 0);
double x2 = x_k.at<double>(1, 0);
double x1_r = 6374.0;
double x2_r = 0.0;
double R = sqrt( pow( x1 - x1_r, 2 ) + pow( x2 - x2_r, 2 ) );
double Phi = atan( (x2 - x2_r)/(x1 - x1_r) );
R += n_k.at<double>(0, 0);
Phi += n_k.at<double>(1, 0);
z_k.at<double>(0, 0) = R;
z_k.at<double>(1, 0) = Phi;
}
};
const double BallisticModel::step_h = 0.05;
TEST(AUKF, br_landing_point)
{
const double abs_error = 0.1;
const int nIterations = 4000; // number of iterations before landing
const double landing_coordinate = 2.5; // the expected landing coordinate
const double alpha = 1;
const double beta = 2.0;
const double kappa = -2.0;
int MP = 2;
int DP = 5;
int CP = 0;
int type = CV_64F;
Mat processNoiseCov = Mat::zeros( DP, DP, type );
processNoiseCov.at<double>(0, 0) = 1e-14;
processNoiseCov.at<double>(1, 1) = 1e-14;
processNoiseCov.at<double>(2, 2) = 2.4065 * 1e-5;
processNoiseCov.at<double>(3, 3) = 2.4065 * 1e-5;
processNoiseCov.at<double>(4, 4) = 1e-6;
Mat processNoiseCovSqrt = Mat::zeros( DP, DP, type );
sqrt( processNoiseCov, processNoiseCovSqrt );
Mat measurementNoiseCov = Mat::zeros( MP, MP, type );
measurementNoiseCov.at<double>(0, 0) = 1e-3*1e-3;
measurementNoiseCov.at<double>(1, 1) = 0.13*0.13;
Mat measurementNoiseCovSqrt = Mat::zeros( MP, MP, type );
sqrt( measurementNoiseCov, measurementNoiseCovSqrt );
RNG rng( 117 );
Mat state( DP, 1, type );
state.at<double>(0, 0) = 6500.4;
state.at<double>(1, 0) = 349.14;
state.at<double>(2, 0) = -1.8093;
state.at<double>(3, 0) = -6.7967;
state.at<double>(4, 0) = 0.6932;
Mat initState = state.clone();
initState.at<double>(4, 0) = 0.0;
Mat P = 1e-6 * Mat::eye( DP, DP, type );
P.at<double>(4, 4) = 1.0;
Mat measurement( MP, 1, type );
Mat q( DP, 1, type );
Mat r( MP, 1, type );
Ptr<BallisticModel> model( new BallisticModel() );
AugmentedUnscentedKalmanFilterParams params( DP, MP, CP, 0, 0, model );
params.stateInit = initState.clone();
params.errorCovInit = P.clone();
params.measurementNoiseCov = measurementNoiseCov.clone();
params.processNoiseCov = processNoiseCov.clone();
params.alpha = alpha;
params.beta = beta;
params.k = kappa;
Ptr<UnscentedKalmanFilter> augmentedUncsentedKalmanFilter = createAugmentedUnscentedKalmanFilter(params);
Mat correctStateUKF( DP, 1, type );
Mat u = Mat::zeros( DP, 1, type );
for (int i = 0; i<nIterations; i++)
{
rng.fill( q, RNG::NORMAL, Scalar::all(0), Scalar::all(1) );
q = processNoiseCovSqrt*q;
rng.fill( r, RNG::NORMAL, Scalar::all(0), Scalar::all(1) );
r = measurementNoiseCovSqrt*r;
model->stateConversionFunction(state, u, q, state);
model->measurementFunction(state, r, measurement);
augmentedUncsentedKalmanFilter->predict();
correctStateUKF = augmentedUncsentedKalmanFilter->correct( measurement );
}
double landing_y = correctStateUKF.at<double>(1, 0);
ASSERT_NEAR(landing_coordinate, landing_y, abs_error);
}
TEST(DISABLED_AUKF, DISABLED_br_mean_squared_error)
{
const double velocity_treshold = 0.004;
const double state_treshold = 0.04;
const int nIterations = 4000; // number of iterations before landing
const double alpha = 1;
const double beta = 2.0;
const double kappa = -2.0;
int MP = 2;
int DP = 5;
int CP = 0;
int type = CV_64F;
Mat processNoiseCov = Mat::zeros( DP, DP, type );
processNoiseCov.at<double>(0, 0) = 1e-14;
processNoiseCov.at<double>(1, 1) = 1e-14;
processNoiseCov.at<double>(2, 2) = 2.4065 * 1e-5;
processNoiseCov.at<double>(3, 3) = 2.4065 * 1e-5;
processNoiseCov.at<double>(4, 4) = 1e-6;
Mat processNoiseCovSqrt = Mat::zeros( DP, DP, type );
sqrt( processNoiseCov, processNoiseCovSqrt );
Mat measurementNoiseCov = Mat::zeros( MP, MP, type );
measurementNoiseCov.at<double>(0, 0) = 1e-3*1e-3;
measurementNoiseCov.at<double>(1, 1) = 0.13*0.13;
Mat measurementNoiseCovSqrt = Mat::zeros( MP, MP, type );
sqrt( measurementNoiseCov, measurementNoiseCovSqrt );
RNG rng( 464 );
Mat state( DP, 1, type );
state.at<double>(0, 0) = 6500.4;
state.at<double>(1, 0) = 349.14;
state.at<double>(2, 0) = -1.8093;
state.at<double>(3, 0) = -6.7967;
state.at<double>(4, 0) = 0.6932;
Mat initState = state.clone();
Mat initStateKF = state.clone();
initStateKF.at<double>(4, 0) = 0.0;
Mat P = 1e-6 * Mat::eye( DP, DP, type );
P.at<double>(4, 4) = 1.0;
Mat measurement( MP, 1, type );
Mat q( DP, 1, type);
Mat r( MP, 1, type);
Ptr<BallisticModel> model( new BallisticModel() );
AugmentedUnscentedKalmanFilterParams params( DP, MP, CP, 0, 0, model );
params.stateInit = initStateKF.clone();
params.errorCovInit = P.clone();
params.measurementNoiseCov = measurementNoiseCov.clone();
params.processNoiseCov = processNoiseCov.clone();
params.alpha = alpha;
params.beta = beta;
params.k = kappa;
Mat predictStateUKF( DP, 1, type );
Mat correctStateUKF( DP, 1, type );
Mat errors = Mat::zeros( nIterations, 4, type );
Mat u = Mat::zeros( DP, 1, type );
for (int j = 0; j<100; j++)
{
cv::Ptr<UnscentedKalmanFilter> augmentedUncsentedKalmanFilter = createAugmentedUnscentedKalmanFilter(params);
state = initState.clone();
for (int i = 0; i<nIterations; i++)
{
rng.fill( q, RNG::NORMAL, Scalar::all(0), Scalar::all(1) );
q = processNoiseCovSqrt*q;
rng.fill( r, RNG::NORMAL, Scalar::all(0), Scalar::all(1) );
r = measurementNoiseCovSqrt*r;
model->stateConversionFunction(state, u, q, state);
model->measurementFunction(state, r, measurement);
predictStateUKF = augmentedUncsentedKalmanFilter->predict();
correctStateUKF = augmentedUncsentedKalmanFilter->correct( measurement );
Mat errorUKF = state - correctStateUKF;
for (int l = 0; l<4; l++)
errors.at<double>(i, l) += pow( errorUKF.at<double>(l, 0), 2.0 );
}
}
errors = errors/100.0;
sqrt( errors, errors );
double max_x1 = cvtest::norm(errors.col(0), NORM_INF);
double max_x2 = cvtest::norm(errors.col(1), NORM_INF);
double max_x3 = cvtest::norm(errors.col(2), NORM_INF);
double max_x4 = cvtest::norm(errors.col(3), NORM_INF);
ASSERT_GE( state_treshold, max_x1 );
ASSERT_GE( state_treshold, max_x2 );
ASSERT_GE( velocity_treshold, max_x3 );
ASSERT_GE( velocity_treshold, max_x4 );
}
// In this test Augmented Unscented Kalman Filter are applied to the univariate nonstationary growth model (UNGM).
// This model was used in example from "Unscented Kalman filtering for additive noise case: Augmented vs. non-augmented"
// by Yuanxin Wu and Dewen Hu.
class UnivariateNonstationaryGrowthModel: public UkfSystemModel
{
public:
void stateConversionFunction(const Mat& x_k, const Mat& u_k, const Mat& v_k, Mat& x_kplus1)
{
double x = x_k.at<double>(0, 0);
double n = u_k.at<double>(0, 0);
double q = v_k.at<double>(0, 0);
double u = u_k.at<double>(0, 0);
double x1 = 0.5*x + 25*( x/(x*x + 1) ) + 8*cos( 1.2*(n-1) ) + q + u;
x_kplus1.at<double>(0, 0) = x1;
}
void measurementFunction(const Mat& x_k, const Mat& n_k, Mat& z_k)
{
double x = x_k.at<double>(0, 0);
double r = n_k.at<double>(0, 0);
double y = x*x/20.0 + r;
z_k.at<double>(0, 0) = y;
}
};
TEST(AUKF, DISABLED_ungm_mean_squared_error)
{
const double alpha = 1.5;
const double beta = 2.0;
const double kappa = 0.0;
const double mse_treshold = 0.05;
const int nIterations = 500; // number of observed iterations
int MP = 1;
int DP = 1;
int CP = 0;
int type = CV_64F;
Ptr<UnivariateNonstationaryGrowthModel> model( new UnivariateNonstationaryGrowthModel() );
AugmentedUnscentedKalmanFilterParams params( DP, MP, CP, 0, 0, model );
Mat processNoiseCov = Mat::zeros( DP, DP, type );
processNoiseCov.at<double>(0, 0) = 1.0;
Mat processNoiseCovSqrt = Mat::zeros( DP, DP, type );
sqrt( processNoiseCov, processNoiseCovSqrt );
Mat measurementNoiseCov = Mat::zeros( MP, MP, type );
measurementNoiseCov.at<double>(0, 0) = 1.0;
Mat measurementNoiseCovSqrt = Mat::zeros( MP, MP, type );
sqrt( measurementNoiseCov, measurementNoiseCovSqrt );
Mat P = Mat::eye( DP, DP, type );
Mat state( DP, 1, type );
state.at<double>(0, 0) = 0.1;
Mat initState = state.clone();
initState.at<double>(0, 0) = 0.0;
params.errorCovInit = P;
params.measurementNoiseCov = measurementNoiseCov;
params.processNoiseCov = processNoiseCov;
params.stateInit = initState.clone();
params.alpha = alpha;
params.beta = beta;
params.k = kappa;
Mat correctStateAUKF( DP, 1, type );
Mat measurement( MP, 1, type );
Mat exactMeasurement( MP, 1, type );
Mat q( DP, 1, type );
Mat r( MP, 1, type );
Mat u( DP, 1, type );
Mat zero = Mat::zeros( MP, 1, type );
RNG rng( 216 );
double average_error = 0.0;
for (int j = 0; j<1000; j++)
{
cv::Ptr<UnscentedKalmanFilter> augmentedUncsentedKalmanFilter = createAugmentedUnscentedKalmanFilter( params );
state = params.stateInit.clone();
double mse = 0.0;
for (int i = 0; i<nIterations; i++)
{
rng.fill( q, RNG::NORMAL, Scalar::all(0), Scalar::all(1) );
rng.fill( r, RNG::NORMAL, Scalar::all(0), Scalar::all(1) );
q = processNoiseCovSqrt*q;
r = measurementNoiseCovSqrt*r;
u.at<double>(0, 0) = (double)i;
model->stateConversionFunction(state, u, q, state);
model->measurementFunction(state, zero, exactMeasurement);
model->measurementFunction(state, r, measurement);
augmentedUncsentedKalmanFilter->predict( u );
correctStateAUKF = augmentedUncsentedKalmanFilter->correct( measurement );
mse += pow( state.at<double>(0, 0) - correctStateAUKF.at<double>(0, 0), 2.0 );
}
mse /= nIterations;
average_error += mse;
}
average_error /= 1000.0;
ASSERT_GE( mse_treshold, average_error );
}
}} // namespace
modules/tracking/test/test_main.cpp
+21
View File
@@ -0,0 +1,21 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html.
#include "test_precomp.hpp"
static
void initTrackingTests()
{
const char* extraTestDataPath =
#ifdef WINRT
NULL;
#else
getenv("OPENCV_DNN_TEST_DATA_PATH");
#endif
if (extraTestDataPath)
cvtest::addDataSearchPath(extraTestDataPath);
cvtest::addDataSearchSubDirectory(""); // override "cv" prefix below to access without "../dnn" hacks
}
CV_TEST_MAIN("cv", initTrackingTests())
modules/tracking/test/test_precomp.hpp
+10
View File
@@ -0,0 +1,10 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html.
#ifndef __OPENCV_TEST_PRECOMP_HPP__
#define __OPENCV_TEST_PRECOMP_HPP__
#include "opencv2/ts.hpp"
#include "opencv2/tracking.hpp"
#endif
modules/tracking/test/test_trackerParametersIO.cpp
+132
View File
@@ -0,0 +1,132 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html.
#include "test_precomp.hpp"
#include <opencv2/tracking/tracking_legacy.hpp>
//using namespace cv::tracking::legacy;
namespace opencv_test { namespace {
TEST(MEDIAN_FLOW_Parameters, IO)
{
legacy::TrackerMedianFlow::Params parameters;
parameters.maxLevel = 10;
parameters.maxMedianLengthOfDisplacementDifference = 11;
parameters.pointsInGrid = 12;
parameters.winSize = Size(6, 5);
parameters.winSizeNCC = Size(41, 40);
parameters.termCriteria.maxCount = 100;
parameters.termCriteria.epsilon = 0.1;
FileStorage fsWriter("parameters.xml", FileStorage::WRITE + FileStorage::MEMORY);
parameters.write(fsWriter);
String serializedParameters = fsWriter.releaseAndGetString();
FileStorage fsReader(serializedParameters, FileStorage::READ + FileStorage::MEMORY);
legacy::TrackerMedianFlow::Params readParameters;
readParameters.read(fsReader.root());
ASSERT_EQ(parameters.maxLevel, readParameters.maxLevel);
ASSERT_EQ(parameters.maxMedianLengthOfDisplacementDifference,
readParameters.maxMedianLengthOfDisplacementDifference);
ASSERT_EQ(parameters.pointsInGrid, readParameters.pointsInGrid);
ASSERT_EQ(parameters.winSize, readParameters.winSize);
ASSERT_EQ(parameters.winSizeNCC, readParameters.winSizeNCC);
ASSERT_EQ(parameters.termCriteria.epsilon, readParameters.termCriteria.epsilon);
ASSERT_EQ(parameters.termCriteria.maxCount, readParameters.termCriteria.maxCount);
}
TEST(MEDIAN_FLOW_Parameters, Default_Value_If_Absent)
{
legacy::TrackerMedianFlow::Params defaultParameters;
FileStorage fsReader(String("%YAML 1.0"), FileStorage::READ + FileStorage::MEMORY);
legacy::TrackerMedianFlow::Params readParameters;
readParameters.read(fsReader.root());
ASSERT_EQ(defaultParameters.maxLevel, readParameters.maxLevel);
ASSERT_EQ(defaultParameters.maxMedianLengthOfDisplacementDifference,
readParameters.maxMedianLengthOfDisplacementDifference);
ASSERT_EQ(defaultParameters.pointsInGrid, readParameters.pointsInGrid);
ASSERT_EQ(defaultParameters.winSize, readParameters.winSize);
ASSERT_EQ(defaultParameters.winSizeNCC, readParameters.winSizeNCC);
ASSERT_EQ(defaultParameters.termCriteria.epsilon, readParameters.termCriteria.epsilon);
ASSERT_EQ(defaultParameters.termCriteria.maxCount, readParameters.termCriteria.maxCount);
}
TEST(KCF_Parameters, IO)
{
legacy::TrackerKCF::Params parameters;
parameters.sigma = 0.3f;
parameters.lambda = 0.02f;
parameters.interp_factor = 0.08f;
parameters.output_sigma_factor = 1.0f/ 32.0f;
parameters.resize=false;
parameters.max_patch_size=90*90;
parameters.split_coeff=false;
parameters.wrap_kernel=true;
parameters.desc_npca = TrackerKCF::CN;
parameters.desc_pca = TrackerKCF::GRAY;
parameters.compress_feature=false;
parameters.compressed_size=3;
parameters.pca_learning_rate=0.2f;
FileStorage fsWriter("parameters.xml", FileStorage::WRITE + FileStorage::MEMORY);
parameters.write(fsWriter);
String serializedParameters = fsWriter.releaseAndGetString();
FileStorage fsReader(serializedParameters, FileStorage::READ + FileStorage::MEMORY);
legacy::TrackerKCF::Params readParameters;
readParameters.read(fsReader.root());
ASSERT_DOUBLE_EQ(parameters.sigma, readParameters.sigma);
ASSERT_DOUBLE_EQ(parameters.lambda, readParameters.lambda);
ASSERT_DOUBLE_EQ(parameters.interp_factor, readParameters.interp_factor);
ASSERT_DOUBLE_EQ(parameters.output_sigma_factor, readParameters.output_sigma_factor);
ASSERT_EQ(parameters.resize, readParameters.resize);
ASSERT_EQ(parameters.max_patch_size, readParameters.max_patch_size);
ASSERT_EQ(parameters.split_coeff, readParameters.split_coeff);
ASSERT_EQ(parameters.wrap_kernel, readParameters.wrap_kernel);
ASSERT_EQ(parameters.desc_npca, readParameters.desc_npca);
ASSERT_EQ(parameters.desc_pca, readParameters.desc_pca);
ASSERT_EQ(parameters.compress_feature, readParameters.compress_feature);
ASSERT_EQ(parameters.compressed_size, readParameters.compressed_size);
ASSERT_DOUBLE_EQ(parameters.pca_learning_rate, readParameters.pca_learning_rate);
}
TEST(KCF_Parameters, Default_Value_If_Absent)
{
legacy::TrackerKCF::Params defaultParameters;
FileStorage fsReader(String("%YAML 1.0"), FileStorage::READ + FileStorage::MEMORY);
legacy::TrackerKCF::Params readParameters;
readParameters.read(fsReader.root());
ASSERT_DOUBLE_EQ(defaultParameters.sigma, readParameters.sigma);
ASSERT_DOUBLE_EQ(defaultParameters.lambda, readParameters.lambda);
ASSERT_DOUBLE_EQ(defaultParameters.interp_factor, readParameters.interp_factor);
ASSERT_DOUBLE_EQ(defaultParameters.output_sigma_factor, readParameters.output_sigma_factor);
ASSERT_EQ(defaultParameters.resize, readParameters.resize);
ASSERT_EQ(defaultParameters.max_patch_size, readParameters.max_patch_size);
ASSERT_EQ(defaultParameters.split_coeff, readParameters.split_coeff);
ASSERT_EQ(defaultParameters.wrap_kernel, readParameters.wrap_kernel);
ASSERT_EQ(defaultParameters.desc_npca, readParameters.desc_npca);
ASSERT_EQ(defaultParameters.desc_pca, readParameters.desc_pca);
ASSERT_EQ(defaultParameters.compress_feature, readParameters.compress_feature);
ASSERT_EQ(defaultParameters.compressed_size, readParameters.compressed_size);
ASSERT_DOUBLE_EQ(defaultParameters.pca_learning_rate, readParameters.pca_learning_rate);
}
}} // namespace
modules/tracking/test/test_trackers.cpp
+236
View File
@@ -0,0 +1,236 @@
/*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) 2013, OpenCV Foundation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// 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 materials 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"
#define TEST_LEGACY
#include <opencv2/tracking/tracking_legacy.hpp>
//#define DEBUG_TEST
#ifdef DEBUG_TEST
#include <opencv2/highgui.hpp>
#endif
namespace opencv_test { namespace {
//using namespace cv::tracking;
#define TESTSET_NAMES testing::Values("david","dudek","faceocc2")
const string TRACKING_DIR = "tracking";
const string FOLDER_IMG = "data";
const string FOLDER_OMIT_INIT = "initOmit";
// Check used "cmake" version in case of errors
// Check compiler command line options for <opencv>/modules include
#include "video/test/test_trackers.impl.hpp"
/****************************************************************************************\
* Tests registrations *
\****************************************************************************************/
//[TESTDATA]
PARAM_TEST_CASE(DistanceAndOverlap, string)
{
string dataset;
virtual void SetUp()
{
dataset = GET_PARAM(0);
}
};
TEST_P(DistanceAndOverlap, MedianFlow)
{
TrackerTest<legacy::Tracker> test(legacy::TrackerMedianFlow::create(), dataset, 35, .5f, NoTransform, 1, 1);
test.run();
}
TEST_P(DistanceAndOverlap, Boosting)
{
TrackerTest<legacy::Tracker> test(legacy::TrackerBoosting::create(), dataset, 70, .7f, NoTransform);
test.run();
}
TEST_P(DistanceAndOverlap, KCF)
{
TrackerTest<Tracker, Rect> test(TrackerKCF::create(), dataset, 20, .35f, NoTransform, 5);
test.run();
}
#ifdef TEST_LEGACY
TEST_P(DistanceAndOverlap, KCF_legacy)
{
TrackerTest<legacy::Tracker> test(legacy::TrackerKCF::create(), dataset, 20, .35f, NoTransform, 5);
test.run();
}
#endif
TEST_P(DistanceAndOverlap, TLD)
{
TrackerTest<legacy::Tracker> test(legacy::TrackerTLD::create(), dataset, 40, .45f, NoTransform);
test.run();
}
TEST_P(DistanceAndOverlap, MOSSE)
{
TrackerTest<legacy::Tracker> test(legacy::TrackerMOSSE::create(), dataset, 22, .7f, NoTransform);
test.run();
}
TEST_P(DistanceAndOverlap, CSRT)
{
TrackerTest<Tracker, Rect> test(TrackerCSRT::create(), dataset, 22, .7f, NoTransform);
test.run();
}
#ifdef TEST_LEGACY
TEST_P(DistanceAndOverlap, CSRT_legacy)
{
TrackerTest<legacy::Tracker> test(legacy::TrackerCSRT::create(), dataset, 22, .7f, NoTransform);
test.run();
}
#endif
/***************************************************************************************/
//Tests with shifted initial window
TEST_P(DistanceAndOverlap, Shifted_Data_MedianFlow)
{
TrackerTest<legacy::Tracker> test(legacy::TrackerMedianFlow::create(), dataset, 80, .2f, CenterShiftLeft, 1, 1);
test.run();
}
TEST_P(DistanceAndOverlap, Shifted_Data_Boosting)
{
TrackerTest<legacy::Tracker> test(legacy::TrackerBoosting::create(), dataset, 80, .65f, CenterShiftLeft);
test.run();
}
TEST_P(DistanceAndOverlap, Shifted_Data_KCF)
{
TrackerTest<Tracker, Rect> test(TrackerKCF::create(), dataset, 20, .4f, CenterShiftLeft, 5);
test.run();
}
#ifdef TEST_LEGACY
TEST_P(DistanceAndOverlap, Shifted_Data_KCF_legacy)
{
TrackerTest<legacy::Tracker> test(legacy::TrackerKCF::create(), dataset, 20, .4f, CenterShiftLeft, 5);
test.run();
}
#endif
TEST_P(DistanceAndOverlap, Shifted_Data_TLD)
{
TrackerTest<legacy::Tracker> test(legacy::TrackerTLD::create(), dataset, 30, .35f, CenterShiftLeft);
test.run();
}
TEST_P(DistanceAndOverlap, Shifted_Data_MOSSE)
{
TrackerTest<legacy::Tracker> test(legacy::TrackerMOSSE::create(), dataset, 13, .69f, CenterShiftLeft);
test.run();
}
TEST_P(DistanceAndOverlap, Shifted_Data_CSRT)
{
TrackerTest<Tracker, Rect> test(TrackerCSRT::create(), dataset, 13, .69f, CenterShiftLeft);
test.run();
}
#ifdef TEST_LEGACY
TEST_P(DistanceAndOverlap, Shifted_Data_CSRT_legacy)
{
TrackerTest<legacy::Tracker> test(legacy::TrackerCSRT::create(), dataset, 13, .69f, CenterShiftLeft);
test.run();
}
#endif
/***************************************************************************************/
//Tests with scaled initial window
TEST_P(DistanceAndOverlap, Scaled_Data_MedianFlow)
{
TrackerTest<legacy::Tracker> test(legacy::TrackerMedianFlow::create(), dataset, 25, .5f, Scale_1_1, 1, 1);
test.run();
}
TEST_P(DistanceAndOverlap, Scaled_Data_Boosting)
{
TrackerTest<legacy::Tracker> test(legacy::TrackerBoosting::create(), dataset, 80, .7f, Scale_1_1);
test.run();
}
TEST_P(DistanceAndOverlap, Scaled_Data_KCF)
{
TrackerTest<Tracker, Rect> test(TrackerKCF::create(), dataset, 20, .4f, Scale_1_1, 5);
test.run();
}
#ifdef TEST_LEGACY
TEST_P(DistanceAndOverlap, Scaled_Data_KCF_legacy)
{
TrackerTest<legacy::Tracker> test(legacy::TrackerKCF::create(), dataset, 20, .4f, Scale_1_1, 5);
test.run();
}
#endif
TEST_P(DistanceAndOverlap, Scaled_Data_TLD)
{
TrackerTest<legacy::Tracker> test(legacy::TrackerTLD::create(), dataset, 30, .45f, Scale_1_1);
test.run();
}
TEST_P(DistanceAndOverlap, Scaled_Data_MOSSE)
{
TrackerTest<legacy::Tracker> test(legacy::TrackerMOSSE::create(), dataset, 22, 0.69f, Scale_1_1, 1);
test.run();
}
TEST_P(DistanceAndOverlap, Scaled_Data_CSRT)
{
TrackerTest<Tracker, Rect> test(TrackerCSRT::create(), dataset, 22, 0.69f, Scale_1_1, 1);
test.run();
}
#ifdef TEST_LEGACY
TEST_P(DistanceAndOverlap, Scaled_Data_CSRT_legacy)
{
TrackerTest<Tracker, Rect> test(TrackerCSRT::create(), dataset, 22, 0.69f, Scale_1_1, 1);
test.run();
}
#endif
INSTANTIATE_TEST_CASE_P(Tracking, DistanceAndOverlap, TESTSET_NAMES);
}} // namespace
modules/tracking/test/test_twist.cpp
+111
View File
@@ -0,0 +1,111 @@
#include "test_precomp.hpp"
#include "opencv2/core.hpp"
#include "opencv2/tracking/twist.hpp"
namespace opencv_test
{
namespace
{
using namespace cv::detail::tracking;
float const eps = 1e-4f;
class TwistTest : public ::testing::Test
{
protected:
cv::Mat J, K;
void SetUp() override
{
cv::Matx33f K = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
this->K = cv::Mat(K);
}
};
TEST_F(TwistTest, TestInteractionMatrix)
{
// import machinevisiontoolbox as mv
// cam = mv.CentralCamera()
// print(cam.K)
// print(cam.visjac_p([1, 1], 2.0))
// [[1. 0. 0.]
// [0. 1. 0.]
// [0. 0. 1.]]
// [[-0.5 0. 0.5 1. -2. 1. ]
// [ 0. -0.5 0.5 2. -1. -1. ]]
cv::Mat uv = cv::Mat(2, 1, CV_32F, {1.0f, 1.0f});
cv::Mat depth = cv::Mat(1, 1, CV_32F, {2.0f});
computeInteractionMatrix(uv, depth, K, J);
ASSERT_EQ(J.cols, 6);
ASSERT_EQ(J.rows, 2);
float expected[2][6] = {{-0.5f, 0.0f, 0.5f, 1.0f, -2.0f, 1.0f},
{0.0f, -0.5f, 0.5f, 2.0f, -1.0f, -1.0f}};
for (int i = 0; i < 2; i++)
for (int j = 0; j < 6; j++)
ASSERT_NEAR(J.at<float>(i, j), expected[i][j], eps);
}
TEST_F(TwistTest, TestComputeWithZeroPixelVelocities)
{
cv::Mat uv = cv::Mat(2, 2, CV_32F, {1.0f, 0.0f, 3.0f, 0.0f});
cv::Mat depths = cv::Mat(1, 2, CV_32F, {1.1f, 1.0f});
cv::Mat duv = cv::Mat(4, 1, CV_32F, {0.0f, 0.0f, 0.0f, 0.0f});
cv::Vec6d result = computeTwist(uv, duv, depths, K);
for (int i = 0; i < 6; i++)
ASSERT_NEAR(result[i], 0.0, eps);
}
TEST_F(TwistTest, TestComputeWithNonZeroPixelVelocities)
{
// import machinevisiontoolbox as mv
// cam = mv.CentralCamera()
// pixels = np.array([[1, 2, 3],
// [1, 2, 3]], dtype=float)
// depths = np.array([1.0, 2.0, 3.0])
// Jac = cam.visjac_p(pixels, depths)
// duv = np.array([1, 2, 1, 3, 1, 4])
// twist = np.linalg.lstsq(Jac, duv, rcond=None)[0]
// print(twist)
// print(Jac)
// [ 0.5 0.5 1.875 0.041667 -0.041667 -0.5 ]
// [[ -1. 0. 1. 1. -2. 1. ]
// [ 0. -1. 1. 2. -1. -1. ]
// [ -0.5 0. 1. 4. -5. 2. ]
// [ 0. -0.5 1. 5. -4. -2. ]
// [ -0.333333 0. 1. 9. -10. 3. ]
// [ 0. -0.333333 1. 10. -9. -3. ]]
float uv_data[] = {1.0f, 2.0f, 3.0f, 1.0f, 2.0f, 3.0f};
cv::Mat uv = cv::Mat(2, 3, CV_32F, uv_data);
float depth_data[] = {1.0f, 2.0f, 3.0f};
cv::Mat depth = cv::Mat(1, 3, CV_32F, depth_data);
float duv_data[] = {1.0f, 2.0f, 1.0f, 3.0f, 1.0f, 4.0f};
cv::Mat duv = cv::Mat(6, 1, CV_32F, duv_data);
computeInteractionMatrix(uv, depth, K, J);
ASSERT_EQ(J.cols, 6);
ASSERT_EQ(J.rows, 6);
float expected_jac[6][6] = {{-1.0f, 0.0f, 1.0f, 1.0f, -2.0f, 1.0f},
{0.0f, -1.0f, 1.0f, 2.0f, -1.0f, -1.0f},
{-0.5f, 0.0f, 1.0f, 4.0f, -5.0f, 2.0f},
{0.0f, -0.5f, 1.0f, 5.0f, -4.0f, -2.0f},
{-0.333333f, 0.0f, 1.0f, 9.0f, -10.0f, 3.0f},
{0.0f, -0.333333f, 1.0f, 10.0f, -9.0f, -3.0f}};
for (int i = 0; i < 6; i++)
for (int j = 0; j < 6; j++)
ASSERT_NEAR(J.at<float>(i, j), expected_jac[i][j], eps);
cv::Vec6d result = computeTwist(uv, duv, depth, K);
float expected_twist[6] = {0.5f, 0.5f, 1.875f, 0.041667f, -0.041667f, -0.5f};
for (int i = 0; i < 6; i++)
ASSERT_NEAR(result[i], expected_twist[i], eps);
}
} // namespace
} // namespace opencv_test
modules/tracking/test/test_ukf.cpp
+436
View File
@@ -0,0 +1,436 @@
/*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) 2015, OpenCV Foundation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// 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 materials 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"
#include "opencv2/tracking/kalman_filters.hpp"
namespace opencv_test { namespace {
using namespace cv::detail;
// In this two tests Unscented Kalman Filter are applied to the dynamic system from example "The reentry problem" from
// "A New Extension of the Kalman Filter to Nonlinear Systems" by Simon J. Julier and Jeffrey K. Uhlmann.
class BallisticModel: public UkfSystemModel
{
static const double step;
Mat diff_eq(const Mat& x)
{
double x1 = x.at<double>(0, 0);
double x2 = x.at<double>(1, 0);
double x3 = x.at<double>(2, 0);
double x4 = x.at<double>(3, 0);
double x5 = x.at<double>(4, 0);
const double h0 = 9.3;
const double beta0 = 0.59783;
const double Gm = 3.9860044 * 1e5;
const double r_e = 6374;
const double r = sqrt( x1*x1 + x2*x2 );
const double v = sqrt( x3*x3 + x4*x4 );
const double d = - beta0 * exp( ( r_e - r )/h0 ) * exp( x5 ) * v;
const double g = - Gm / (r*r*r);
Mat fx = x.clone();
fx.at<double>(0, 0) = x3;
fx.at<double>(1, 0) = x4;
fx.at<double>(2, 0) = d * x3 + g * x1;
fx.at<double>(3, 0) = d * x4 + g * x2;
fx.at<double>(4, 0) = 0.0;
return fx;
}
public:
void stateConversionFunction(const Mat& x_k, const Mat& u_k, const Mat& v_k, Mat& x_kplus1)
{
Mat v = sqrt(step) * v_k.clone();
v.at<double>(0, 0) = 0.0;
v.at<double>(1, 0) = 0.0;
Mat k1 = diff_eq( x_k ) + v;
Mat tmp = x_k + step*0.5*k1;
Mat k2 = diff_eq( tmp ) + v;
tmp = x_k + step*0.5*k2;
Mat k3 = diff_eq( tmp ) + v;
tmp = x_k + step*k3;
Mat k4 = diff_eq( tmp ) + v;
x_kplus1 = x_k + (1.0/6.0)*step*( k1 + 2.0*k2 + 2.0*k3 + k4 ) + u_k;
}
void measurementFunction(const Mat& x_k, const Mat& n_k, Mat& z_k)
{
double x1 = x_k.at<double>(0, 0);
double x2 = x_k.at<double>(1, 0);
double x1_r = 6374.0;
double x2_r = 0.0;
double R = sqrt( pow( x1 - x1_r, 2 ) + pow( x2 - x2_r, 2 ) );
double Phi = atan( (x2 - x2_r)/(x1 - x1_r) );
R += n_k.at<double>(0, 0);
Phi += n_k.at<double>(1, 0);
z_k.at<double>(0, 0) = R;
z_k.at<double>(1, 0) = Phi;
}
};
const double BallisticModel::step = 0.05;
TEST(UKF, br_landing_point)
{
const double abs_error = 0.1;
const int nIterations = 4000; // number of iterations before landing
const double landing_coordinate = 2.5; // the expected landing coordinate
const double alpha = 1;
const double beta = 2.0;
const double kappa = -2.0;
int MP = 2;
int DP = 5;
int CP = 0;
int type = CV_64F;
Mat processNoiseCov = Mat::zeros( DP, DP, type );
processNoiseCov.at<double>(0, 0) = 1e-14;
processNoiseCov.at<double>(1, 1) = 1e-14;
processNoiseCov.at<double>(2, 2) = 2.4065 * 1e-5;
processNoiseCov.at<double>(3, 3) = 2.4065 * 1e-5;
processNoiseCov.at<double>(4, 4) = 1e-6;
Mat processNoiseCovSqrt = Mat::zeros( DP, DP, type );
sqrt( processNoiseCov, processNoiseCovSqrt );
Mat measurementNoiseCov = Mat::zeros( MP, MP, type );
measurementNoiseCov.at<double>(0, 0) = 1e-3*1e-3;
measurementNoiseCov.at<double>(1, 1) = 0.13*0.13;
Mat measurementNoiseCovSqrt = Mat::zeros( MP, MP, type );
sqrt( measurementNoiseCov, measurementNoiseCovSqrt );
RNG rng( 117 );
Mat state( DP, 1, type );
state.at<double>(0, 0) = 6500.4;
state.at<double>(1, 0) = 349.14;
state.at<double>(2, 0) = -1.8093;
state.at<double>(3, 0) = -6.7967;
state.at<double>(4, 0) = 0.6932;
Mat initState = state.clone();
initState.at<double>(4, 0) = 0.0;
Mat P = 1e-6 * Mat::eye( DP, DP, type );
P.at<double>(4, 4) = 1.0;
Mat measurement( MP, 1, type );
Mat q( DP, 1, type );
Mat r( MP, 1, type );
Ptr<BallisticModel> model( new BallisticModel() );
UnscentedKalmanFilterParams params( DP, MP, CP, 0, 0, model );
params.stateInit = initState.clone();
params.errorCovInit = P.clone();
params.measurementNoiseCov = measurementNoiseCov.clone();
params.processNoiseCov = processNoiseCov.clone();
params.alpha = alpha;
params.beta = beta;
params.k = kappa;
Ptr<UnscentedKalmanFilter> uncsentedKalmanFilter = createUnscentedKalmanFilter(params);
Mat correctStateUKF( DP, 1, type );
Mat u = Mat::zeros( DP, 1, type );
for (int i = 0; i<nIterations; i++)
{
rng.fill( q, RNG::NORMAL, Scalar::all(0), Scalar::all(1) );
q = processNoiseCovSqrt*q;
rng.fill( r, RNG::NORMAL, Scalar::all(0), Scalar::all(1) );
r = measurementNoiseCovSqrt*r;
model->stateConversionFunction(state, u, q, state);
model->measurementFunction(state, r, measurement);
uncsentedKalmanFilter->predict();
correctStateUKF = uncsentedKalmanFilter->correct( measurement );
}
double landing_y = correctStateUKF.at<double>(1, 0);
ASSERT_NEAR(landing_coordinate, landing_y, abs_error);
}
TEST(UKF, DISABLED_br_mean_squared_error)
{
const double velocity_treshold = 0.09;
const double state_treshold = 0.9;
const int nIterations = 4000; // number of iterations before landing
const double alpha = 1;
const double beta = 2.0;
const double kappa = -2.0;
int MP = 2;
int DP = 5;
int CP = 0;
int type = CV_64F;
Mat processNoiseCov = Mat::zeros( DP, DP, type );
processNoiseCov.at<double>(0, 0) = 1e-14;
processNoiseCov.at<double>(1, 1) = 1e-14;
processNoiseCov.at<double>(2, 2) = 2.4065 * 1e-5;
processNoiseCov.at<double>(3, 3) = 2.4065 * 1e-5;
processNoiseCov.at<double>(4, 4) = 1e-6;
Mat processNoiseCovSqrt = Mat::zeros( DP, DP, type );
sqrt( processNoiseCov, processNoiseCovSqrt );
Mat measurementNoiseCov = Mat::zeros( MP, MP, type );
measurementNoiseCov.at<double>(0, 0) = 1e-3*1e-3;
measurementNoiseCov.at<double>(1, 1) = 0.13*0.13;
Mat measurementNoiseCovSqrt = Mat::zeros( MP, MP, type );
sqrt( measurementNoiseCov, measurementNoiseCovSqrt );
RNG rng( 464 );
Mat state( DP, 1, type );
state.at<double>(0, 0) = 6500.4;
state.at<double>(1, 0) = 349.14;
state.at<double>(2, 0) = -1.8093;
state.at<double>(3, 0) = -6.7967;
state.at<double>(4, 0) = 0.6932;
Mat initState = state.clone();
Mat initStateKF = state.clone();
initStateKF.at<double>(4, 0) = 0.0;
Mat P = 1e-6 * Mat::eye( DP, DP, type );
P.at<double>(4, 4) = 1.0;
Mat measurement( MP, 1, type );
Mat q( DP, 1, type);
Mat r( MP, 1, type);
Ptr<BallisticModel> model( new BallisticModel() );
UnscentedKalmanFilterParams params( DP, MP, CP, 0, 0, model );
params.stateInit = initStateKF.clone();
params.errorCovInit = P.clone();
params.measurementNoiseCov = measurementNoiseCov.clone();
params.processNoiseCov = processNoiseCov.clone();
params.alpha = alpha;
params.beta = beta;
params.k = kappa;
Mat predictStateUKF( DP, 1, type );
Mat correctStateUKF( DP, 1, type );
Mat errors = Mat::zeros( nIterations, 4, type );
Mat u = Mat::zeros( DP, 1, type );
for (int j = 0; j<100; j++)
{
Ptr<UnscentedKalmanFilter> uncsentedKalmanFilter = createUnscentedKalmanFilter(params);
state = initState.clone();
for (int i = 0; i<nIterations; i++)
{
rng.fill( q, RNG::NORMAL, Scalar::all(0), Scalar::all(1) );
q = processNoiseCovSqrt*q;
rng.fill( r, RNG::NORMAL, Scalar::all(0), Scalar::all(1) );
r = measurementNoiseCovSqrt*r;
model->stateConversionFunction(state, u, q, state);
model->measurementFunction(state, r, measurement);
predictStateUKF = uncsentedKalmanFilter->predict();
correctStateUKF = uncsentedKalmanFilter->correct( measurement );
Mat errorUKF = state - correctStateUKF;
for (int l = 0; l<4; l++)
errors.at<double>(i, l) += pow( errorUKF.at<double>(l, 0), 2.0 );
}
}
errors = errors/100.0;
sqrt( errors, errors );
double max_x1 = cvtest::norm(errors.col(0), NORM_INF);
double max_x2 = cvtest::norm(errors.col(1), NORM_INF);
double max_x3 = cvtest::norm(errors.col(2), NORM_INF);
double max_x4 = cvtest::norm(errors.col(3), NORM_INF);
ASSERT_GE( state_treshold, max_x1 );
ASSERT_GE( state_treshold, max_x2 );
ASSERT_GE( velocity_treshold, max_x3 );
ASSERT_GE( velocity_treshold, max_x4 );
}
//In this test Unscented Kalman Filter are applied to the univariate nonstationary growth model (UNGM).
//This model was used in example from "Unscented Kalman filtering for additive noise case: Augmented vs. non-augmented"
//by Yuanxin Wu and Dewen Hu.
class UnivariateNonstationaryGrowthModel: public UkfSystemModel
{
public:
void stateConversionFunction(const Mat& x_k, const Mat& u_k, const Mat& v_k, Mat& x_kplus1)
{
double x = x_k.at<double>(0, 0);
double n = u_k.at<double>(0, 0);
double q = v_k.at<double>(0, 0);
double u = u_k.at<double>(0, 0);
double x1 = 0.5*x + 25*( x/(x*x + 1) ) + 8*cos( 1.2*(n-1) ) + q + u;
x_kplus1.at<double>(0, 0) = x1;
}
void measurementFunction(const Mat& x_k, const Mat& n_k, Mat& z_k)
{
double x = x_k.at<double>(0, 0);
double r = n_k.at<double>(0, 0);
double y = x*x/20.0 + r;
z_k.at<double>(0, 0) = y;
}
};
TEST(UKF, DISABLED_ungm_mean_squared_error)
{
const double alpha = 1.5;
const double beta = 2.0;
const double kappa = 0.0;
const double mse_treshold = 0.5;
const int nIterations = 500; // number of observed iterations
int MP = 1;
int DP = 1;
int CP = 0;
int type = CV_64F;
Ptr<UnivariateNonstationaryGrowthModel> model( new UnivariateNonstationaryGrowthModel() );
UnscentedKalmanFilterParams params( DP, MP, CP, 0, 0, model );
Mat processNoiseCov = Mat::zeros( DP, DP, type );
processNoiseCov.at<double>(0, 0) = 1.0;
Mat processNoiseCovSqrt = Mat::zeros( DP, DP, type );
sqrt( processNoiseCov, processNoiseCovSqrt );
Mat measurementNoiseCov = Mat::zeros( MP, MP, type );
measurementNoiseCov.at<double>(0, 0) = 1.0;
Mat measurementNoiseCovSqrt = Mat::zeros( MP, MP, type );
sqrt( measurementNoiseCov, measurementNoiseCovSqrt );
Mat P = Mat::eye( DP, DP, type );
Mat state( DP, 1, type );
state.at<double>(0, 0) = 0.1;
Mat initState = state.clone();
initState.at<double>(0, 0) = 0.0;
params.errorCovInit = P;
params.measurementNoiseCov = measurementNoiseCov;
params.processNoiseCov = processNoiseCov;
params.stateInit = initState.clone();
params.alpha = alpha;
params.beta = beta;
params.k = kappa;
Mat correctStateAUKF( DP, 1, type );
Mat measurement( MP, 1, type );
Mat exactMeasurement( MP, 1, type );
Mat q( DP, 1, type );
Mat r( MP, 1, type );
Mat u( DP, 1, type );
Mat zero = Mat::zeros( MP, 1, type );
RNG rng( 216 );
double average_error = 0.0;
for (int j = 0; j<1000; j++)
{
cv::Ptr<UnscentedKalmanFilter> uncsentedKalmanFilter = createUnscentedKalmanFilter( params );
state.at<double>(0, 0) = 0.1;
double mse = 0.0;
for (int i = 0; i<nIterations; i++)
{
rng.fill( q, RNG::NORMAL, Scalar::all(0), Scalar::all(1) );
rng.fill( r, RNG::NORMAL, Scalar::all(0), Scalar::all(1) );
q = processNoiseCovSqrt*q;
r = measurementNoiseCovSqrt*r;
u.at<double>(0, 0) = (double)i;
model->stateConversionFunction(state, u, q, state);
model->measurementFunction(state, zero, exactMeasurement);
model->measurementFunction(state, r, measurement);
uncsentedKalmanFilter->predict( u );
correctStateAUKF = uncsentedKalmanFilter->correct( measurement );
mse += pow( state.at<double>(0, 0) - correctStateAUKF.at<double>(0, 0), 2.0 );
}
mse /= nIterations;
average_error += mse;
}
average_error /= 1000.0;
ASSERT_GE( mse_treshold, average_error );
}
}} // namespace