mirror of
https://github.com/opencv/opencv.git
synced 2026-07-30 15:53:03 +04:00
Merge pull request #18869 from anna-khakimova:ak/kalman
* GAPI: Kalman filter stateful kernel * Applied comments * Applied comments. Second iteration * Add overload without control vector * Remove structure constructor and dimension fields. * Add sample as test * Remove visualization from test-sample + correct doxygen comments * Applied comments.
This commit is contained in:
Anna Khakimova
GitHub
@@ -31,6 +31,13 @@ GAPI_TEST_FIXTURE_SPEC_PARAMS(BuildPyr_CalcOptFlow_PipelineTest,
|
||||
GAPI_TEST_FIXTURE_SPEC_PARAMS(BackgroundSubtractorTest, FIXTURE_API(tuple<cv::gapi::video::BackgroundSubtractorType,double>,
|
||||
int, bool, double, std::string, std::size_t),
|
||||
6, typeAndThreshold, histLength, detectShadows, learningRate, filePath, testNumFrames)
|
||||
|
||||
GAPI_TEST_FIXTURE_SPEC_PARAMS(KalmanFilterTest, FIXTURE_API(int, int, int, int, int), 5, type, dDim, mDim, cDim, numIter)
|
||||
|
||||
GAPI_TEST_FIXTURE_SPEC_PARAMS(KalmanFilterNoControlTest, FIXTURE_API(int, int, int, int), 4, type, dDim, mDim, numIter)
|
||||
|
||||
GAPI_TEST_FIXTURE_SPEC_PARAMS(KalmanFilterCircleSampleTest, FIXTURE_API(int, int), 2, type, numIter)
|
||||
|
||||
} // opencv_test
|
||||
|
||||
|
||||
|
||||
@@ -131,6 +131,229 @@ TEST_P(BackgroundSubtractorTest, AccuracyTest)
|
||||
// Allowing 1% difference of all pixels between G-API and reference OpenCV results
|
||||
testBackgroundSubtractorStreaming(gapiBackSub, pOCVBackSub, 1, 1, learningRate, testNumFrames);
|
||||
}
|
||||
|
||||
inline void initKalmanParams(cv::gapi::KalmanParams& kp, int type, int dDim, int mDim, int cDim)
|
||||
{
|
||||
kp.state = Mat::zeros(dDim, 1, type);
|
||||
cv::randu(kp.state, Scalar::all(0), Scalar::all(0.1));
|
||||
kp.errorCov = Mat::eye(dDim, dDim, type);
|
||||
|
||||
kp.transitionMatrix = Mat::ones(dDim, dDim, type) * 2;
|
||||
kp.processNoiseCov = Mat::eye(dDim, dDim, type)*(1e-5);
|
||||
kp.measurementMatrix = Mat::eye(mDim, dDim, type) * 2;
|
||||
kp.measurementNoiseCov = Mat::eye(mDim, mDim, type)*(1e-5);
|
||||
|
||||
if (cDim > 0)
|
||||
kp.controlMatrix = Mat::eye(dDim, cDim, type)* (1e-3);
|
||||
}
|
||||
|
||||
inline void initKalmanFilter(const cv::gapi::KalmanParams& kp,
|
||||
cv::KalmanFilter& ocvKalman, bool control)
|
||||
{
|
||||
kp.state.copyTo(ocvKalman.statePost);
|
||||
kp.errorCov.copyTo(ocvKalman.errorCovPost);
|
||||
|
||||
kp.transitionMatrix.copyTo(ocvKalman.transitionMatrix);
|
||||
kp.measurementMatrix.copyTo(ocvKalman.measurementMatrix);
|
||||
kp.measurementNoiseCov.copyTo(ocvKalman.measurementNoiseCov);
|
||||
kp.processNoiseCov.copyTo(ocvKalman.processNoiseCov);
|
||||
|
||||
if (control)
|
||||
kp.controlMatrix.copyTo(ocvKalman.controlMatrix);
|
||||
}
|
||||
|
||||
TEST_P(KalmanFilterTest, AccuracyTest)
|
||||
{
|
||||
cv::gapi::KalmanParams kp;
|
||||
initKalmanParams(kp, type, dDim, mDim, cDim);
|
||||
|
||||
// OpenCV reference KalmanFilter initialization
|
||||
cv::KalmanFilter ocvKalman(dDim, mDim, cDim, type);
|
||||
initKalmanFilter(kp, ocvKalman, true);
|
||||
|
||||
//measurement vector
|
||||
cv::Mat measure_vec(mDim, 1, type);
|
||||
|
||||
//control vector
|
||||
cv::Mat ctrl_vec = Mat::zeros(cDim > 0 ? cDim : 2, 1, type);
|
||||
|
||||
// G-API Kalman's output state
|
||||
cv::Mat gapiKState(dDim, 1, type);
|
||||
// OCV Kalman's output state
|
||||
cv::Mat ocvKState(dDim, 1, type);
|
||||
|
||||
// G-API graph initialization
|
||||
cv::GMat m, ctrl;
|
||||
cv::GOpaque<bool> have_m;
|
||||
cv::GMat out = cv::gapi::KalmanFilter(m, have_m, ctrl, kp);
|
||||
cv::GComputation comp(cv::GIn(m, have_m, ctrl), cv::GOut(out));
|
||||
|
||||
cv::RNG& rng = cv::theRNG();
|
||||
bool haveMeasure;
|
||||
|
||||
for (int i = 0; i < numIter; i++)
|
||||
{
|
||||
haveMeasure = (rng(2u) == 1) ? true : false; // returns 0 or 1 - whether we have measurement at this iteration or not
|
||||
|
||||
if (haveMeasure)
|
||||
cv::randu(measure_vec, Scalar::all(-1), Scalar::all(1));
|
||||
if (cDim > 0)
|
||||
cv::randu(ctrl_vec, Scalar::all(-1), Scalar::all(1));
|
||||
|
||||
// G-API KalmanFilter call
|
||||
comp.apply(cv::gin(measure_vec, haveMeasure, ctrl_vec), cv::gout(gapiKState));
|
||||
// OpenCV KalmanFilter call
|
||||
ocvKState = cDim > 0 ? ocvKalman.predict(ctrl_vec) : ocvKalman.predict();
|
||||
if (haveMeasure)
|
||||
ocvKState = ocvKalman.correct(measure_vec);
|
||||
}
|
||||
|
||||
// Comparison //////////////////////////////////////////////////////////////
|
||||
{
|
||||
double diff = 0;
|
||||
vector<int> idx;
|
||||
EXPECT_TRUE(cmpEps(gapiKState, ocvKState, &diff, 1.0, &idx, false) >= 0);
|
||||
}
|
||||
}
|
||||
|
||||
TEST_P(KalmanFilterNoControlTest, AccuracyTest)
|
||||
{
|
||||
cv::gapi::KalmanParams kp;
|
||||
initKalmanParams(kp, type, dDim, mDim, 0);
|
||||
|
||||
// OpenCV reference KalmanFilter initialization
|
||||
cv::KalmanFilter ocvKalman(dDim, mDim, 0, type);
|
||||
initKalmanFilter(kp, ocvKalman, false);
|
||||
|
||||
//measurement vector
|
||||
cv::Mat measure_vec(mDim, 1, type);
|
||||
|
||||
// G-API Kalman's output state
|
||||
cv::Mat gapiKState(dDim, 1, type);
|
||||
// OCV Kalman's output state
|
||||
cv::Mat ocvKState(dDim, 1, type);
|
||||
|
||||
// G-API graph initialization
|
||||
cv::GMat m;
|
||||
cv::GOpaque<bool> have_m;
|
||||
cv::GMat out = cv::gapi::KalmanFilter(m, have_m, kp);
|
||||
cv::GComputation comp(cv::GIn(m, have_m), cv::GOut(out));
|
||||
|
||||
cv::RNG& rng = cv::theRNG();
|
||||
bool haveMeasure;
|
||||
|
||||
for (int i = 0; i < numIter; i++)
|
||||
{
|
||||
haveMeasure = (rng(2u) == 1) ? true : false; // returns 0 or 1 - whether we have measurement at this iteration or not
|
||||
|
||||
if (haveMeasure)
|
||||
cv::randu(measure_vec, Scalar::all(-1), Scalar::all(1));
|
||||
|
||||
// G-API
|
||||
comp.apply(cv::gin(measure_vec, haveMeasure), cv::gout(gapiKState));
|
||||
|
||||
// OpenCV
|
||||
ocvKState = ocvKalman.predict();
|
||||
if (haveMeasure)
|
||||
ocvKState = ocvKalman.correct(measure_vec);
|
||||
}
|
||||
|
||||
// Comparison //////////////////////////////////////////////////////////////
|
||||
{
|
||||
double diff = 0;
|
||||
vector<int> idx;
|
||||
EXPECT_TRUE(cmpEps(gapiKState, ocvKState, &diff, 1.0, &idx, false) >= 0);
|
||||
}
|
||||
}
|
||||
|
||||
TEST_P(KalmanFilterCircleSampleTest, AccuracyTest)
|
||||
{
|
||||
// auxiliary variables
|
||||
cv::Mat processNoise(2, 1, type);
|
||||
// For comparison
|
||||
double diff = 0;
|
||||
vector<int> idx;
|
||||
|
||||
// Input mesurement
|
||||
cv::Mat measurement = Mat::zeros(1, 1, type);
|
||||
// Angle and it's delta(phi, delta_phi)
|
||||
cv::Mat state(2, 1, type);
|
||||
|
||||
// G-API graph initialization
|
||||
cv::gapi::KalmanParams kp;
|
||||
|
||||
kp.state = Mat::zeros(2, 1, type);
|
||||
cv::randn(kp.state, Scalar::all(0), Scalar::all(0.1));
|
||||
|
||||
kp.errorCov = Mat::eye(2, 2, type);
|
||||
|
||||
if (type == CV_32F)
|
||||
kp.transitionMatrix = (Mat_<float>(2, 2) << 1, 1, 0, 1);
|
||||
else
|
||||
kp.transitionMatrix = (Mat_<double>(2, 2) << 1, 1, 0, 1);
|
||||
|
||||
kp.processNoiseCov = Mat::eye(2, 2, type) * (1e-5);
|
||||
kp.measurementMatrix = Mat::eye(1, 2, type);
|
||||
kp.measurementNoiseCov = Mat::eye(1, 1, type) * (1e-1);
|
||||
|
||||
cv::GMat m;
|
||||
cv::GOpaque<bool> have_measure;
|
||||
cv::GMat out = cv::gapi::KalmanFilter(m, have_measure, kp);
|
||||
cv::GComputation comp(cv::GIn(m, have_measure), cv::GOut(out));
|
||||
|
||||
// OCV Kalman initialization
|
||||
cv::KalmanFilter KF(2, 1, 0);
|
||||
initKalmanFilter(kp, KF, false);
|
||||
|
||||
cv::randn(state, Scalar::all(0), Scalar::all(0.1));
|
||||
|
||||
// GAPI Corrected state
|
||||
cv::Mat gapiState(2, 1, type);
|
||||
// OCV Corrected state
|
||||
cv::Mat ocvCorrState(2, 1, type);
|
||||
// OCV Predicted state
|
||||
cv::Mat ocvPreState(2, 1, type);
|
||||
|
||||
bool haveMeasure;
|
||||
|
||||
for (int i = 0; i < numIter; ++i)
|
||||
{
|
||||
// Get OCV Prediction
|
||||
ocvPreState = KF.predict();
|
||||
|
||||
GAPI_DbgAssert(cv::norm(kp.measurementNoiseCov, KF.measurementNoiseCov, cv::NORM_INF) == 0);
|
||||
// generation measurement
|
||||
cv::randn(measurement, Scalar::all(0), Scalar::all((type == CV_32FC1) ?
|
||||
kp.measurementNoiseCov.at<float>(0) : kp.measurementNoiseCov.at<double>(0)));
|
||||
|
||||
GAPI_DbgAssert(cv::norm(kp.measurementMatrix, KF.measurementMatrix, cv::NORM_INF) == 0);
|
||||
measurement += kp.measurementMatrix*state;
|
||||
|
||||
if (cv::theRNG().uniform(0, 4) != 0)
|
||||
{
|
||||
haveMeasure = true;
|
||||
ocvCorrState = KF.correct(measurement);
|
||||
comp.apply(cv::gin(measurement, haveMeasure), cv::gout(gapiState));
|
||||
EXPECT_TRUE(cmpEps(gapiState, ocvCorrState, &diff, 1.0, &idx, false) >= 0);
|
||||
}
|
||||
else
|
||||
{
|
||||
// Get GAPI Prediction
|
||||
haveMeasure = false;
|
||||
comp.apply(cv::gin(measurement, haveMeasure), cv::gout(gapiState));
|
||||
EXPECT_TRUE(cmpEps(gapiState, ocvPreState, &diff, 1.0, &idx, false) >= 0);
|
||||
}
|
||||
|
||||
GAPI_DbgAssert(cv::norm(kp.processNoiseCov, KF.processNoiseCov, cv::NORM_INF) == 0);
|
||||
cv::randn(processNoise, Scalar(0), Scalar::all(sqrt(type == CV_32FC1 ?
|
||||
kp.processNoiseCov.at<float>(0, 0):
|
||||
kp.processNoiseCov.at<double>(0, 0))));
|
||||
|
||||
GAPI_DbgAssert(cv::norm(kp.transitionMatrix, KF.transitionMatrix, cv::NORM_INF) == 0);
|
||||
state = kp.transitionMatrix*state + processNoise;
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
} // opencv_test
|
||||
|
||||
|
||||
@@ -111,4 +111,28 @@ INSTANTIATE_TEST_CASE_MACRO_P(WITH_VIDEO(BackgroundSubtractorTestCPU),
|
||||
Values(-1, 0, 0.5, 1),
|
||||
Values("cv/video/768x576.avi"),
|
||||
Values(3)));
|
||||
|
||||
INSTANTIATE_TEST_CASE_MACRO_P(KalmanFilterTestCPU,
|
||||
KalmanFilterTest,
|
||||
Combine(Values(VIDEO_CPU),
|
||||
Values(CV_32FC1, CV_64FC1),
|
||||
Values(2,5),
|
||||
Values(2,5),
|
||||
Values(2),
|
||||
Values(5)));
|
||||
|
||||
INSTANTIATE_TEST_CASE_MACRO_P(KalmanFilterTestCPU,
|
||||
KalmanFilterNoControlTest,
|
||||
Combine(Values(VIDEO_CPU),
|
||||
Values(CV_32FC1, CV_64FC1),
|
||||
Values(3),
|
||||
Values(4),
|
||||
Values(3)));
|
||||
|
||||
INSTANTIATE_TEST_CASE_MACRO_P(KalmanFilterTestCPU,
|
||||
KalmanFilterCircleSampleTest,
|
||||
Combine(Values(VIDEO_CPU),
|
||||
Values(CV_32FC1, CV_64FC1),
|
||||
Values(5)));
|
||||
|
||||
} // opencv_test
|
||||
|
||||
Reference in New Issue
Block a user