summaryrefslogtreecommitdiffhomepage
path: root/tracker-easy/tracker-easy.h
blob: 4510fc7d707da9fba61339937f25f8ce756d8375 (plain)
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
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
/* Copyright (c) 2019 Stephane Lenclud
 *
 * Permission to use, copy, modify, and/or distribute this software for any
 * purpose with or without fee is hereby granted, provided that the above
 * copyright notice and this permission notice appear in all copies.
 */

#pragma once

#include "api/plugin-api.hpp"
#include "cv/numeric.hpp"
#include "video/video-widget.hpp"
#include "video/camera.hpp"
#include "compat/timer.hpp"


#include "preview.h"
#include "settings.h"
#include "point-extractor.h"
#include "kalman-filter-pose.h"

#include <atomic>
#include <memory>
#include <vector>

#include <opencv2/core.hpp>
#include <opencv2/video/tracking.hpp>

#include <QThread>
#include <QMutex>
#include <QLayout>

namespace EasyTracker
{

    namespace VertexPosition
    {
        enum Type
        {
            Top = 0,
            Right,
            Left,
            TopRight,
            TopLeft,
            Center
        };
    }

    static const QString KModuleName = "tracker-easy";

    class Dialog;

    using namespace numeric_types;

    struct Tracker : public QObject, ITracker
    {
        Q_OBJECT
    public:        
        friend class Dialog;

        explicit Tracker();
        ~Tracker() override;

        // From ITracker
        module_status start_tracker(QFrame* parent_window) override;
        void data(double* data) override;
        bool center() override;

    private slots:
        void Tick();


    private:
        void UpdateModel();
        void CreateCameraIntrinsicsMatrices();
        void ProcessFrame();        
        void MatchVertices(int& aTopIndex, int& aRightIndex, int& aLeftIndex, int& aCenterIndex, int& aTopRight, int& aTopLeft);
        void MatchThreeOrFourVertices(int& aTopIndex, int& aRightIndex, int& aLeftIndex, int& aCenterIndex);
        void MatchFiveVertices(int& aTopIndex, int& aRightIndex, int& aLeftIndex, int& aTopRight, int& aTopLeft);
        void MatchClipVertices(int& aTopIndex, int& aMiddleIndex, int& aBottomIndex);
        
        
        //

        bool CheckCamera();
        void set_fov(int value);
        void SetFps(int aFps);
        void DoSetFps(int aFps);
        void UpdateSettings();

        QMutex camera_mtx;
        QThread iThread;
        QTimer iTicker;

        Settings iSettings;

        std::unique_ptr<QLayout> layout;
        std::vector<cv::Point> iPoints;

        int preview_width = 320, preview_height = 240;

        PointExtractor iPointExtractor;

        std::unique_ptr<video::impl::camera> camera;
        video::impl::camera::info iCameraInfo;
        std::unique_ptr<video_widget> widget;

        video::frame iFrame;
        cv::Mat iMatFrame;
        Preview iPreview;

        std::atomic<bool> ever_success = false;
        mutable QMutex iProcessLock, iDataLock;

        //// Copy the settings need by our thread to avoid dead locks
        // Deadzone
        int iDeadzoneEdge=0;
        int iDeadzoneHalfEdge=0;
        // Solver
        int iSolver = cv::SOLVEPNP_P3P;
        bool iDebug = false;
        ////

        // Statistics
        Timer iTimer;
        Timer iFpsTimer;        
        int iFrameCount = 0;
        int iSkippedFrameCount = 0;
        int iFps = 0;
        int iSkippedFps = 0;
        uint iBadSolutionCount = 0;
        uint iGoodSolutionCount = 0;

        //
        KalmanFilterPose iKf;

        // Vertices defining the model we are tracking
        std::vector<cv::Point3f> iModel;
        // Bitmap points corresponding to model vertices
        std::vector<cv::Point2f> iTrackedPoints;

        std::vector<cv::Rect> iTrackedRects;

        // Intrinsics camera matrix
        cv::Matx33d iCameraMatrix { cv::Matx33d::zeros() };
        // Intrinsics distortion coefficients as a matrix
        cv::Matx<double, 8, 1> iDistCoeffsMatrix;
        // Translation solutions
        std::vector<cv::Mat> iTranslations;
        // Rotation solutions
        std::vector<cv::Mat> iRotations;
        // Angle solutions, pitch, yaw, roll, in this order
        std::vector<cv::Vec3d> iAngles;
        // The index of our best solution in the above arrays
        int iBestSolutionIndex = -1;
        // Best translation
        cv::Vec3d iBestTranslation;
        // Best angles
        cv::Vec3d iBestAngles;
        // Time at which we found our last best solution
        Timer iBestTime;
        // Center translation
        cv::Vec3d iCenterTranslation = {0,0,0};
        // Center angles
        cv::Vec3d iCenterAngles = { 0,0,0 };
    };

}