modularization/src/detection/detection_lidar_PointPillars_MultiHead_1025/Tracker/track.cpp

#include "track.h"

//#include "dat/dat_tracker.hpp"

///
/// \brief CTrack
/// \param pt
/// \param region
/// \param deltaTime
/// \param accelNoiseMag
/// \param trackID
/// \param filterObjectSize
/// \param externalTrackerForLost
///
CTrack::CTrack(const CRegion& region,
        tracking::KalmanType kalmanType,
        track_t deltaTime,
        track_t accelNoiseMag,
        bool useAcceleration,
        size_t trackID,
        size_t regionID,
        tracking::FilterGoal filterGoal,
        tracking::LostTrackType externalTrackerForLost
        )
    :
      m_trackID(trackID),
      m_regionID(regionID),
      m_skippedFrames(0),
      m_lastRegion(region),
      m_predictionPoint(region.m_rect.center),
      m_predictionRect(region.m_rrect),
      m_predictionRect3D(region.m_rect),
      m_kalman(kalmanType, useAcceleration, deltaTime, accelNoiseMag),
      m_filterGoal(filterGoal),
      m_outOfTheFrame(false),
      m_externalTrackerForLost(externalTrackerForLost)
{
    if (filterGoal == tracking::FilterRect)
        m_kalman.Update(region.m_brect, true);
    else if(filterGoal == tracking::FilterRect3D)
        m_kalman.Update(region.m_rect, true);
    else
        m_kalman.Update(m_predictionPoint, true);

    m_trace.push_back(m_predictionPoint, m_predictionPoint);
}

///
/// \brief CTrack::CalcDistCenter
/// \param reg
/// \return
///
track_t CTrack::CalcDistCenter(const CRegion& reg) const
{
    cv::Point3f diff = m_predictionPoint - reg.m_rect.center;
    return sqrtf(sqr(diff.x) + sqr(diff.y));
}
///
/// \brief CTrack::CalcDistRect
/// \param reg
/// \return
///
track_t CTrack::CalcDistRect(const CRegion& reg) const
{
    std::array<track_t, 5> diff;
    diff[0] = reg.m_rrect.center.x - m_lastRegion.m_rrect.center.x;
    diff[1] = reg.m_rrect.center.y - m_lastRegion.m_rrect.center.y;
    diff[2] = static_cast<track_t>(m_lastRegion.m_rrect.size.width - reg.m_rrect.size.width);
    diff[3] = static_cast<track_t>(m_lastRegion.m_rrect.size.height - reg.m_rrect.size.height);
    diff[4] = static_cast<track_t>(m_lastRegion.m_rrect.angle - reg.m_rrect.angle);

    track_t dist = 0;
    for (size_t i = 0; i < diff.size(); ++i)
    {
        dist += sqr(diff[i]);
    }
    return sqrtf(dist);
}
///
/// \brief CTrack::CalcDistRect3D
/// \param reg
/// \return
///
track_t CTrack::CalcDistRect3D(const CRegion& reg) const
{
    std::array<track_t, 7> diff;
    diff[0] = reg.m_rect.center.x - m_lastRegion.m_rect.center.x;
    diff[1] = reg.m_rect.center.y - m_lastRegion.m_rect.center.y;
    diff[2] = 0;//reg.m_rect.center.z - m_lastRegion.m_rect.center.z;
    diff[3] = static_cast<track_t>(m_lastRegion.m_rect.size.width - reg.m_rect.size.width);
    diff[4] = static_cast<track_t>(m_lastRegion.m_rect.size.height - reg.m_rect.size.height);
    diff[5] = static_cast<track_t>(m_lastRegion.m_rect.size.length - reg.m_rect.size.length);
    diff[6] = 0;//static_cast<track_t>(m_lastRegion.m_rect.yaw - reg.m_rect.yaw);

    track_t dist = 0;
    for (size_t i = 0; i < diff.size(); ++i)
    {
        dist += sqr(diff[i]);
    }
    return sqrtf(dist);
}

///
/// \brief CTrack::Update
/// \*param region
/// \param dataCorrect
/// \param max_trace_length
/// \param prevFrame
/// \param currFrame
/// \param trajLen
///
void CTrack::Update(
        const CRegion& region,
        bool dataCorrect,
        size_t max_trace_length,
        cv::UMat prevFrame,
        cv::UMat currFrame,
        int trajLen
        )
{
    if (m_filterGoal == tracking::FilterRect) // Kalman filter for object coordinates and size
        RectUpdate(region, dataCorrect, prevFrame, currFrame);
    if (m_filterGoal == tracking::FilterRect3D) // Kalman filter for object coordinates and size
        Rect3DUpdate(region, dataCorrect, prevFrame, currFrame);
    else // Kalman filter only for object center
        PointUpdate(region.m_rect.center, region.m_rrect.size, dataCorrect, currFrame.size());

    if (dataCorrect)
    {
        //std::cout << m_lastRegion.m_brect << " - " << region.m_brect << std::endl;

        m_lastRegion = region;
        m_trace.push_back(m_predictionPoint, region.m_rect.center);

        CheckStatic(trajLen, currFrame, region);
    }
    else
    {
        m_trace.push_back(m_predictionPoint);
    }

    if (m_trace.size() > max_trace_length)
        m_trace.pop_front(m_trace.size() - max_trace_length);
}

///
/// \brief CTrack::IsStatic
/// \return
///
bool CTrack::IsStatic() const
{
    return m_isStatic;
}

///
/// \brief CTrack::IsStaticTimeout
/// \param framesTime
/// \return
///
bool CTrack::IsStaticTimeout(int framesTime) const
{
    return (m_staticFrames > framesTime);
}

///
/// \brief CTrack::IsOutOfTheFrame
/// \return
///
bool CTrack::IsOutOfTheFrame() const
{
    return m_outOfTheFrame;
}

///
//cv::RotatedRect CTrack::CalcPredictionEllipse(cv::Size_<track_t> minRadius) const
//{
//    // Move ellipse to velocity
//    auto velocity = m_kalman.GetVelocity();
//    Point_t d(3.f * velocity[0], 3.f * velocity[1]);

//    cv::RotatedRect rrect(m_predictionPoint, cv::Size2f(std::max(minRadius.width, fabs(d.x)), std::max(minRadius.height, fabs(d.y))), 0);

//    if (fabs(d.x) + fabs(d.y) > 4) // pix
//    {
//        if (fabs(d.x) > 0.0001f)
//        {
//            track_t l = std::min(rrect.size.width, rrect.size.height) / 3;

//            track_t p2_l = sqrtf(sqr(d.x) + sqr(d.y));
//            rrect.center.x = l * d.x / p2_l + m_predictionPoint.x;
//            rrect.center.y = l * d.y / p2_l + m_predictionPoint.y;

//            rrect.angle = atanf(d.y / d.x);
//        }
//        else
//        {
//            rrect.center.y += d.y / 3;
//            rrect.angle = static_cast<float>(CV_PI / 2.);
//        }
//    }
//    return rrect;
//}

///
/// \brief CTrack::IsInsideArea
///        If result <= 1 then center of the object is inside ellipse with prediction and velocity
/// \param pt
/// \return
///
track_t CTrack::IsInsideArea(const Point_t& pt, const cv::RotatedRect& rrect) const
{
    Point_t pt_(pt.x - rrect.center.x, pt.y - rrect.center.y);
    track_t r = sqrtf(sqr(pt_.x) + sqr(pt_.y));
    track_t t = (r > 1) ? acosf(pt_.x / r) : 0;
    track_t t_ = t - rrect.angle;
    Point_t pt_rotated(r * cosf(t_), r * sinf(t_));

    return sqr(pt_rotated.x) / sqr(rrect.size.width) + sqr(pt_rotated.y) / sqr(rrect.size.height);
}

///
/// \brief CTrack::WidthDist
/// \param reg
/// \return
///
track_t CTrack::WidthDist(const CRegion& reg) const
{
    if (m_lastRegion.m_rect.size.width < reg.m_rect.size.width)
        return m_lastRegion.m_rect.size.width / reg.m_rect.size.width;
    else
        return reg.m_rect.size.width / m_lastRegion.m_rect.size.width;
}

///
/// \brief CTrack::HeightDist
/// \param reg
/// \return
///
track_t CTrack::HeightDist(const CRegion& reg) const
{
    if (m_lastRegion.m_rect.size.height < reg.m_rect.size.height)
        return m_lastRegion.m_rect.size.height / reg.m_rect.size.height;
    else
        return reg.m_rect.size.height / m_lastRegion.m_rect.size.height;
}

///
/// \brief CTrack::CheckStatic
/// \param trajLen
/// \return
///
bool CTrack::CheckStatic(int trajLen, cv::UMat currFrame, const CRegion& region)
{
    if (!trajLen || static_cast<int>(m_trace.size()) < trajLen)
    {
        m_isStatic = false;
        m_staticFrames = 0;
        m_staticFrame = cv::UMat();
    }
    else
    {
        track_t kx = 0;
        track_t bx = 0;
        track_t ky = 0;
        track_t by = 0;
        get_lin_regress_params(m_trace, m_trace.size() - trajLen, m_trace.size(), kx, bx, ky, by);
        track_t speed = sqrt(sqr(kx * trajLen) + sqr(ky * trajLen));
        const track_t speedThresh = 10;
        if (speed < speedThresh)
        {
            if (!m_isStatic)
            {
                m_staticFrame = currFrame.clone();
                m_staticRect = region.m_rect;
            }

            ++m_staticFrames;
            m_isStatic = true;
        }
        else
        {
            m_isStatic = false;
            m_staticFrames = 0;
            m_staticFrame = cv::UMat();
        }
    }

    return m_isStatic;
}

///
/// \brief GetLastRect
/// \return
///
Rect3D CTrack::GetLastRect() const
{
    if (m_filterGoal == tracking::FilterRect)
        return Rect3D(cv::Point3f(m_predictionRect.center.x,m_predictionRect.center.y,0),Size3D(m_predictionRect.boundingRect().width,m_predictionRect.boundingRect().height,0),0);
    else if(m_filterGoal == tracking::FilterRect3D)
        return m_predictionRect3D;
    else
        return Rect3D(cv::Point3f(m_predictionPoint.x, m_predictionPoint.y,0),Size3D(0,0,0),0);

}
///
/// \brief CTrack::LastRegion
/// \return
///
const CRegion& CTrack::LastRegion() const
{
    return m_lastRegion;
}

///
/// \brief CTrack::ConstructObject
/// \return
///
TrackingObject CTrack::ConstructObject() const
{
    return TrackingObject(GetLastRect(), m_lastRegion, m_trackID, m_regionID, m_trace, IsStatic(), IsOutOfTheFrame(), m_kalman.GetVelocity(), m_detectedFrames);
}

///
/// \brief CTrack::SkippedFrames
/// \return
///
size_t CTrack::SkippedFrames() const
{
    return m_skippedFrames;
}

///
/// \brief CTrack::SkippedFrames
/// \return
///
size_t& CTrack::SkippedFrames()
{
    return m_skippedFrames;
}
///
/// \brief CTrack::DetectedFrames
/// \return
///
size_t& CTrack::DetectedFrames()
{
    return m_detectedFrames;
}
///
/// \brief RectUpdate
/// \param region
/// \param dataCorrect
/// \param prevFrame
/// \param currFrame
///
void CTrack::RectUpdate(
        const CRegion& region,
        bool dataCorrect,
        cv::UMat prevFrame,
        cv::UMat currFrame
        )
{
    m_kalman.GetRectPrediction();

    bool recalcPrediction = true;

    auto Clamp = [](int& v, int& size, int hi) -> int
    {
        int res = 0;

        if (size < 1)
            size = 0;

        if (v < 0)
        {
            res = v;
            v = 0;
            return res;
        }
        else if (v + size > hi - 1)
        {
            res = v;
            v = hi - 1 - size;
            if (v < 0)
            {
                size += v;
                v = 0;
            }
            res -= v;
            return res;
        }
        return res;
    };

    auto UpdateRRect = [&](cv::Rect prevRect, cv::Rect newRect)
    {
        m_predictionRect.center.x += newRect.x - prevRect.x;
        m_predictionRect.center.y += newRect.y - prevRect.y;
        m_predictionRect.size.width *= newRect.width / static_cast<float>(prevRect.width);
        m_predictionRect.size.height *= newRect.height / static_cast<float>(prevRect.height);
    };

    switch (m_externalTrackerForLost)
    {
    case tracking::TrackNone:
        break;
    }

    if (recalcPrediction)
        UpdateRRect(m_predictionRect.boundingRect(), m_kalman.Update(region.m_brect, dataCorrect));

    cv::Rect brect = m_predictionRect.boundingRect();
    int dx = Clamp(brect.x, brect.width, currFrame.cols);
    int dy = Clamp(brect.y, brect.height, currFrame.rows);

    m_outOfTheFrame = (dx != 0) || (dy != 0) || (brect.width < 2) || (brect.height < 2);

    m_predictionPoint = cv::Point3f(m_predictionRect.center.x,m_predictionRect.center.y,0);

    //std::cout << "brect = " << brect << ", dx = " << dx << ", dy = " << dy << ", outOfTheFrame = " << m_outOfTheFrame << ", predictionPoint = " << m_predictionPoint << std::endl;
}
///
/// \brief Rect3DUpdate
/// \param region
/// \param dataCorrect
/// \param prevFrame
/// \param currFrame
///
void CTrack::Rect3DUpdate(
        const CRegion& region,
        bool dataCorrect,
        cv::UMat prevFrame,
        cv::UMat currFrame
        )
{
    m_kalman.GetRect3DPrediction();

    switch (m_externalTrackerForLost)
    {
    case tracking::TrackNone:
        break;
    }

    m_predictionRect3D = m_kalman.Update(region.m_rect, dataCorrect);

    m_predictionPoint = m_predictionRect3D.center;

    //std::cout << "brect = " << brect << ", dx = " << dx << ", dy = " << dy << ", outOfTheFrame = " << m_outOfTheFrame << ", predictionPoint = " << m_predictionPoint << std::endl;
}

///
/// \brief PointUpdate
/// \param pt
/// \param dataCorrect
///
void CTrack::PointUpdate(
        const cv::Point3f& pt,
        const cv::Size& newObjSize,
        bool dataCorrect,
        const cv::Size& frameSize
        )
{
    m_kalman.GetPointPrediction();

    m_predictionPoint = m_kalman.Update(pt, dataCorrect);

    if (dataCorrect)
    {
        const int a1 = 1;
        const int a2 = 9;
        m_predictionRect.size.width = (a1 * newObjSize.width + a2 * m_predictionRect.size.width) / (a1 + a2);
        m_predictionRect.size.height = (a1 * newObjSize.height + a2 * m_predictionRect.size.height) / (a1 + a2);
    }

    auto Clamp = [](track_t& v, int hi) -> bool
    {
        if (v < 0)
        {
            v = 0;
            return true;
        }
        else if (hi && v > hi - 1)
        {
            v = static_cast<track_t>(hi - 1);
            return true;
        }
        return false;
    };
    auto p = m_predictionPoint;
    m_outOfTheFrame = Clamp(p.x, frameSize.width) || Clamp(p.y, frameSize.height) || (m_predictionRect.size.width < 2) || (m_predictionRect.size.height < 2);

    //std::cout << "predictionRect = " << m_predictionRect.boundingRect() << ", outOfTheFrame = " << m_outOfTheFrame << ", predictionPoint = " << m_predictionPoint << std::endl;
}