//---------------------------------------------------------------------------
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));
}
int main(int argc, char *argv[])
{
LPLUT AToB0, BToA0;
LPGAMMATABLE PreLinear[3];
LPGAMMATABLE Lin;
CARGO Cargo;
cmsHPROFILE hProfile;
cmsCIEXYZ wp;
fprintf(stderr, "Creating lcmscmy.icm...");
wp.X = 55.6549;
wp.Y = 59.0485;
wp.Z = 72.5494;
cmsXYZ2xyY(&Cus, &wp);
InitCargo(&Cargo);
hProfile = cmsCreateLabProfile(&Cus);
// Create linearization
Lin = CreateLinear();
PreLinear[0] = Lin;
PreLinear[1] = Lin;
PreLinear[2] = Lin;
AToB0 = cmsAllocLUT();
BToA0 = cmsAllocLUT();
cmsAlloc3DGrid(AToB0, 33, 3, 3);
cmsAlloc3DGrid(BToA0, 33, 3, 3);
cmsSample3DGrid(AToB0, Reverse, &Cargo, 0);
cmsSample3DGrid(BToA0, Forward, &Cargo, 0);
cmsAllocLinearTable(AToB0, PreLinear, 1);
cmsAllocLinearTable(BToA0, PreLinear, 2);
cmsAddTag(hProfile, icSigAToB0Tag, AToB0);
cmsAddTag(hProfile, icSigBToA0Tag, BToA0);
cmsSetColorSpace(hProfile, icSigCmyData);
cmsAddTag(hProfile, icSigProfileDescriptionTag, "Little cms CMY mixing");
cmsAddTag(hProfile, icSigCopyrightTag, "Copyright (c) Marti Maria, 2002. All rights reserved.");
cmsAddTag(hProfile, icSigDeviceMfgDescTag, "Little cms");
cmsAddTag(hProfile, icSigDeviceModelDescTag, "CMY mixing");
_cmsSaveProfile(hProfile, "lcmscmy.icm");
cmsFreeGamma(Lin);
cmsFreeLUT(AToB0);
cmsFreeLUT(BToA0);
cmsCloseProfile(hProfile);
FreeCargo(&Cargo);
fprintf(stderr, "Done.\n");
return 0;
}
//---------------------------------------------------------------------------
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;
}
