//---------------------------------------------------------------------------
void TKalmanFilter::CreateUnscented(cv::Rect_<track_t> rect0, Point_t rectv0)
{
int MP = 4;
int DP = 8;
int CP = 0;
cv::Mat processNoiseCov = cv::Mat::zeros(DP, DP, Mat_t(1));
processNoiseCov.at<track_t>(0, 0) = 1e-3;
processNoiseCov.at<track_t>(1, 1) = 1e-3;
processNoiseCov.at<track_t>(2, 2) = 1e-3;
processNoiseCov.at<track_t>(3, 3) = 1e-3;
processNoiseCov.at<track_t>(4, 4) = 1e-3;
processNoiseCov.at<track_t>(5, 5) = 1e-3;
processNoiseCov.at<track_t>(6, 6) = 1e-3;
processNoiseCov.at<track_t>(7, 7) = 1e-3;
cv::Mat measurementNoiseCov = cv::Mat::zeros(MP, MP, Mat_t(1));
measurementNoiseCov.at<track_t>(0, 0) = 1e-3;
measurementNoiseCov.at<track_t>(1, 1) = 1e-3;
measurementNoiseCov.at<track_t>(2, 2) = 1e-3;
measurementNoiseCov.at<track_t>(3, 3) = 1e-3;
cv::Mat initState(DP, 1, Mat_t(1));
initState.at<track_t>(0, 0) = rect0.x;
initState.at<track_t>(1, 0) = rect0.y;
initState.at<track_t>(2, 0) = rectv0.x;
initState.at<track_t>(3, 0) = rectv0.y;
initState.at<track_t>(4, 0) = 0;
initState.at<track_t>(5, 0) = 0;
initState.at<track_t>(6, 0) = rect0.width;
initState.at<track_t>(7, 0) = rect0.height;
cv::Mat P = 1e-3 * cv::Mat::eye(DP, DP, Mat_t(1));
cv::Ptr<AcceleratedModel> model(new AcceleratedModel(m_deltaTime, true));
cv::tracking::UnscentedKalmanFilterParams params(DP, MP, CP, 0, 0, model);
params.dataType = Mat_t(1);
params.stateInit = initState.clone();
params.errorCovInit = P.clone();
params.measurementNoiseCov = measurementNoiseCov.clone();
params.processNoiseCov = processNoiseCov.clone();
params.alpha = 1;
params.beta = 2.0;
params.k = -2.0;
m_uncsentedKalman = cv::tracking::createUnscentedKalmanFilter(params);
m_initialized = true;
}
//---------------------------------------------------------------------------
void TKalmanFilter::CreateAugmentedUnscented(Point_t xy0, Point_t xyv0)
{
int MP = 2;
int DP = 6;
int CP = 0;
cv::Mat processNoiseCov = cv::Mat::zeros(DP, DP, Mat_t(1));
processNoiseCov.at<track_t>(0, 0) = 1e-14;
processNoiseCov.at<track_t>(1, 1) = 1e-14;
processNoiseCov.at<track_t>(2, 2) = 1e-6;
processNoiseCov.at<track_t>(3, 3) = 1e-6;
processNoiseCov.at<track_t>(4, 4) = 1e-6;
processNoiseCov.at<track_t>(5, 5) = 1e-6;
cv::Mat measurementNoiseCov = cv::Mat::zeros(MP, MP, Mat_t(1));
measurementNoiseCov.at<track_t>(0, 0) = 1e-6;
measurementNoiseCov.at<track_t>(1, 1) = 1e-6;
cv::Mat initState(DP, 1, Mat_t(1));
initState.at<track_t>(0, 0) = xy0.x;
initState.at<track_t>(1, 0) = xy0.y;
initState.at<track_t>(2, 0) = xyv0.x;
initState.at<track_t>(3, 0) = xyv0.y;
initState.at<track_t>(4, 0) = 0;
initState.at<track_t>(5, 0) = 0;
cv::Mat P = 1e-6 * cv::Mat::eye(DP, DP, Mat_t(1));
cv::Ptr<AcceleratedModel> model(new AcceleratedModel(m_deltaTime, false));
cv::tracking::AugmentedUnscentedKalmanFilterParams params(DP, MP, CP, 0, 0, model);
params.dataType = Mat_t(1);
params.stateInit = initState.clone();
params.errorCovInit = P.clone();
params.measurementNoiseCov = measurementNoiseCov.clone();
params.processNoiseCov = processNoiseCov.clone();
params.alpha = 1;
params.beta = 2.0;
params.k = -2.0;
m_uncsentedKalman = cv::tracking::createAugmentedUnscentedKalmanFilter(params);
m_initialized = true;
}
//---------------------------------------------------------------------------
Point_t TKalmanFilter::Update(Point_t p, bool DataCorrect)
{
cv::Mat measurement(2, 1, Mat_t(1));
if(!DataCorrect)
{
measurement.at<track_t>(0) = LastResult.x; //update using prediction
measurement.at<track_t>(1) = LastResult.y;
}
else
{
measurement.at<track_t>(0) = p.x; //update using measurements
measurement.at<track_t>(1) = p.y;
}
// Correction
cv::Mat estiMated = kalman->correct(measurement);
LastResult.x = estiMated.at<track_t>(0); //update using measurements
LastResult.y = estiMated.at<track_t>(1);
return LastResult;
}
//---------------------------------------------------------------------------
cv::Rect TKalmanFilter::Update(cv::Rect rect, bool dataCorrect)
{
if (!m_initialized)
{
if (m_initialRects.size() < MIN_INIT_VALS)
{
if (dataCorrect)
{
m_initialRects.push_back(rect);
}
}
if (m_initialRects.size() == MIN_INIT_VALS)
{
std::vector<Point_t> initialPoints;
Point_t averageSize(0, 0);
for (const auto& r : m_initialRects)
{
initialPoints.push_back(Point_t(r.x, r.y));
averageSize.x += r.width;
averageSize.y += r.height;
}
averageSize.x /= MIN_INIT_VALS;
averageSize.y /= MIN_INIT_VALS;
track_t kx = 0;
track_t bx = 0;
track_t ky = 0;
track_t by = 0;
get_lin_regress_params(initialPoints, 0, MIN_INIT_VALS, kx, bx, ky, by);
cv::Rect_<track_t> rect0(kx * (MIN_INIT_VALS - 1) + bx, ky * (MIN_INIT_VALS - 1) + by, averageSize.x, averageSize.y);
Point_t rectv0(kx, ky);
switch (m_type)
{
case TypeLinear:
CreateLinear(rect0, rectv0);
break;
case TypeUnscented:
#if USE_OCV_UKF
CreateUnscented(rect0, rectv0);
#else
CreateLinear(rect0, rectv0);
std::cerr << "UnscentedKalmanFilter was disabled in CMAKE! Set KalmanLinear in constructor." << std::endl;
#endif
break;
case TypeAugmentedUnscented:
CreateAugmentedUnscented(rect0, rectv0);
break;
}
}
}
if (m_initialized)
{
cv::Mat measurement(4, 1, Mat_t(1));
if (!dataCorrect)
{
measurement.at<track_t>(0) = m_lastRectResult.x; // update using prediction
measurement.at<track_t>(1) = m_lastRectResult.y;
measurement.at<track_t>(2) = m_lastRectResult.width;
measurement.at<track_t>(3) = m_lastRectResult.height;
}
else
{
measurement.at<track_t>(0) = static_cast<track_t>(rect.x); // update using measurements
measurement.at<track_t>(1) = static_cast<track_t>(rect.y);
measurement.at<track_t>(2) = static_cast<track_t>(rect.width);
measurement.at<track_t>(3) = static_cast<track_t>(rect.height);
}
// Correction
cv::Mat estimated;
switch (m_type)
{
case TypeLinear:
estimated = m_linearKalman->correct(measurement);
m_lastRectResult.x = estimated.at<track_t>(0); //update using measurements
m_lastRectResult.y = estimated.at<track_t>(1);
m_lastRectResult.width = estimated.at<track_t>(2);
m_lastRectResult.height = estimated.at<track_t>(3);
break;
case TypeUnscented:
case TypeAugmentedUnscented:
#if USE_OCV_UKF
estimated = m_uncsentedKalman->correct(measurement);
m_lastRectResult.x = estimated.at<track_t>(0); //update using measurements
m_lastRectResult.y = estimated.at<track_t>(1);
m_lastRectResult.width = estimated.at<track_t>(6);
m_lastRectResult.height = estimated.at<track_t>(7);
#else
estimated = m_linearKalman->correct(measurement);
m_lastRectResult.x = estimated.at<track_t>(0); //update using measurements
m_lastRectResult.y = estimated.at<track_t>(1);
m_lastRectResult.width = estimated.at<track_t>(2);
m_lastRectResult.height = estimated.at<track_t>(3);
std::cerr << "UnscentedKalmanFilter was disabled in CMAKE! Set KalmanLinear in constructor." << std::endl;
#endif
break;
}
}
else
{
if (dataCorrect)
{
m_lastRectResult.x = static_cast<track_t>(rect.x);
m_lastRectResult.y = static_cast<track_t>(rect.y);
m_lastRectResult.width = static_cast<track_t>(rect.width);
m_lastRectResult.height = static_cast<track_t>(rect.height);
}
}
return cv::Rect(static_cast<int>(m_lastRectResult.x), static_cast<int>(m_lastRectResult.y), static_cast<int>(m_lastRectResult.width), static_cast<int>(m_lastRectResult.height));
}
//---------------------------------------------------------------------------
Point_t TKalmanFilter::Update(Point_t pt, bool dataCorrect)
{
if (!m_initialized)
{
if (m_initialPoints.size() < MIN_INIT_VALS)
{
if (dataCorrect)
{
m_initialPoints.push_back(pt);
}
}
if (m_initialPoints.size() == MIN_INIT_VALS)
{
track_t kx = 0;
track_t bx = 0;
track_t ky = 0;
track_t by = 0;
get_lin_regress_params(m_initialPoints, 0, MIN_INIT_VALS, kx, bx, ky, by);
Point_t xy0(kx * (MIN_INIT_VALS - 1) + bx, ky * (MIN_INIT_VALS - 1) + by);
Point_t xyv0(kx, ky);
switch (m_type)
{
case TypeLinear:
CreateLinear(xy0, xyv0);
break;
case TypeUnscented:
#if USE_OCV_UKF
CreateUnscented(xy0, xyv0);
#else
CreateLinear(xy0, xyv0);
std::cerr << "UnscentedKalmanFilter was disabled in CMAKE! Set KalmanLinear in constructor." << std::endl;
#endif
break;
case TypeAugmentedUnscented:
CreateAugmentedUnscented(xy0, xyv0);
break;
}
}
}
if (m_initialized)
{
cv::Mat measurement(2, 1, Mat_t(1));
if (!dataCorrect)
{
measurement.at<track_t>(0) = m_lastPointResult.x; //update using prediction
measurement.at<track_t>(1) = m_lastPointResult.y;
}
else
{
measurement.at<track_t>(0) = pt.x; //update using measurements
measurement.at<track_t>(1) = pt.y;
}
// Correction
cv::Mat estimated;
switch (m_type)
{
case TypeLinear:
estimated = m_linearKalman->correct(measurement);
break;
case TypeUnscented:
case TypeAugmentedUnscented:
#if USE_OCV_UKF
estimated = m_uncsentedKalman->correct(measurement);
#else
estimated = m_linearKalman->correct(measurement);
std::cerr << "UnscentedKalmanFilter was disabled in CMAKE! Set KalmanLinear in constructor." << std::endl;
#endif
break;
}
m_lastPointResult.x = estimated.at<track_t>(0); //update using measurements
m_lastPointResult.y = estimated.at<track_t>(1);
}
else
{
if (dataCorrect)
{
m_lastPointResult = pt;
}
}
return m_lastPointResult;
}
