//--------------------------------------------------------------------------- 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; }