Multitarget-tracker/src/Tracker/Kalman.h at master · CiscoDeepVision/Multitarget-tracker · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
#pragma once
#include "defines.h"
#include <memory>
#include <deque>

#include <opencv2/opencv.hpp>

#ifdef USE_OCV_UKF
#include <opencv2/tracking.hpp>
#include <opencv2/tracking/kalman_filters.hpp>
#endif

///
/// \brief The TKalmanFilter class
/// http://www.morethantechnical.com/2011/06/17/simple-kalman-filter-for-tracking-using-opencv-2-2-w-code/
///
class TKalmanFilter
{
public:
    TKalmanFilter(tracking::KalmanType type, bool useAcceleration, track_t deltaTime, track_t accelNoiseMag);
    ~TKalmanFilter() = default;

    Point_t GetPointPrediction();
    Point_t Update(Point_t pt, bool dataCorrect);

    cv::Rect GetRectPrediction();
    cv::Rect Update(cv::Rect rect, bool dataCorrect);

    cv::Vec<track_t, 2> GetVelocity() const;

    void GetPtStateAndResCov(cv::Mat& covar, cv::Mat& state) const;

private:
    cv::KalmanFilter m_linearKalman;
#ifdef USE_OCV_UKF
#if (((CV_VERSION_MAJOR == 4) && (CV_VERSION_MINOR < 5)) || ((CV_VERSION_MAJOR == 4) && (CV_VERSION_MINOR == 5) && (CV_VERSION_REVISION < 1)) || (CV_VERSION_MAJOR == 3))
    cv::Ptr<cv::tracking::UnscentedKalmanFilter> m_uncsentedKalman;
#else
    cv::Ptr<cv::detail::tracking::UnscentedKalmanFilter> m_uncsentedKalman;
#endif
#endif

    static constexpr size_t MIN_INIT_VALS = 4;
    std::vector<Point_t> m_initialPoints;
    std::vector<cv::Rect> m_initialRects;

    cv::Rect_<track_t> m_lastRectResult;
    cv::Rect_<track_t> m_lastRect;
    Point_t m_lastPointResult;
    track_t m_accelNoiseMag = 0.5f;
    track_t m_deltaTime = 0.2f;
    track_t m_deltaTimeMin = 0.2f;
    track_t m_deltaTimeMax = 2 * 0.2f;
    track_t m_lastDist = 0;
    track_t m_deltaStep = 0;
    static constexpr int m_deltaStepsCount = 20;
    tracking::KalmanType m_type = tracking::KalmanLinear;
    bool m_useAcceleration = false; // If set true then will be used motion model x(t) = x0 + v0 * t + a * t^2 / 2
    bool m_initialized = false;

	// Constant velocity model
    void CreateLinear(Point_t xy0, Point_t xyv0);
    void CreateLinear(cv::Rect_<track_t> rect0, Point_t rectv0);

	// Constant acceleration model
	// https://www.mathworks.com/help/driving/ug/linear-kalman-filters.html
	void CreateLinearAcceleration(Point_t xy0, Point_t xyv0);
	void CreateLinearAcceleration(cv::Rect_<track_t> rect0, Point_t rectv0);

#ifdef USE_OCV_UKF
    void CreateUnscented(Point_t xy0, Point_t xyv0);
    void CreateUnscented(cv::Rect_<track_t> rect0, Point_t rectv0);
    void CreateAugmentedUnscented(Point_t xy0, Point_t xyv0);
    void CreateAugmentedUnscented(cv::Rect_<track_t> rect0, Point_t rectv0);
#endif
};