diff options
| -rw-r--r-- | tracker-easy/kalman-filter-pose.cpp | 1 | ||||
| -rw-r--r-- | tracker-easy/settings.h | 4 | ||||
| -rw-r--r-- | tracker-easy/tracker-easy.cpp | 121 | ||||
| -rw-r--r-- | tracker-easy/tracker-easy.h | 22 |
4 files changed, 89 insertions, 59 deletions
diff --git a/tracker-easy/kalman-filter-pose.cpp b/tracker-easy/kalman-filter-pose.cpp index 0f6bdbdb..42346bb5 100644 --- a/tracker-easy/kalman-filter-pose.cpp +++ b/tracker-easy/kalman-filter-pose.cpp @@ -81,6 +81,7 @@ namespace EasyTracker // [0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0] // [0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0] + // Those will scale the filtered values, 2 will give you half the raw input measurementMatrix.at<double>(0, 0) = 1; // x measurementMatrix.at<double>(1, 1) = 1; // y measurementMatrix.at<double>(2, 2) = 1; // z diff --git a/tracker-easy/settings.h b/tracker-easy/settings.h index ed5d0abd..dcfb6be0 100644 --- a/tracker-easy/settings.h +++ b/tracker-easy/settings.h @@ -48,8 +48,8 @@ namespace EasyTracker { clip_bz{ b, "clip-bz", 80 }; value<int> active_model_panel{ b, "active-model-panel", 0 }, - cap_x{ b, "cap-x", 40 }, - cap_y{ b, "cap-y", 60 }, + cap_x{ b, "cap-x", 35 }, + cap_y{ b, "cap-y", 55 }, cap_z{ b, "cap-z", 100 }; value<int> fov{ b, "camera-fov", 56 }; diff --git a/tracker-easy/tracker-easy.cpp b/tracker-easy/tracker-easy.cpp index e432b5da..e0ab5f7c 100644 --- a/tracker-easy/tracker-easy.cpp +++ b/tracker-easy/tracker-easy.cpp @@ -45,25 +45,18 @@ namespace EasyTracker connect(&iSettings.fov, value_::value_changed<int>(), this, &Tracker::set_fov, Qt::DirectConnection); set_fov(iSettings.fov); + // We could not get this working, nevermind + //connect(&iSettings.cam_fps, value_::value_changed<int>(), this, &Tracker::SetFps, Qt::DirectConnection); CreateModelFromSettings(); - - - //int nStates = 18; // the number of states - //int nMeasurements = 6; // the number of measured states - //int nInputs = 0; // the number of control actions - //double dt = 0.125; // time between measurements (1/FPS) - - iKf.Init(18,6,0,0033); } Tracker::~Tracker() { - // cv::destroyWindow("Preview"); - requestInterruption(); - wait(); + iThread.exit(); + iThread.wait(); QMutexLocker l(&camera_mtx); camera->stop(); @@ -315,51 +308,43 @@ namespace EasyTracker /// /// /// - void Tracker::run() + void Tracker::Tick() { maybe_reopen_camera(); + + iTimer.start(); - iFpsTimer.start(); - - while (!isInterruptionRequested()) + bool new_frame = false; { - iTimer.start(); - - bool new_frame = false; - { - QMutexLocker l(&camera_mtx); - - if (camera) - { - std::tie(iFrame, new_frame) = camera->get_frame(); - } - - } + QMutexLocker l(&camera_mtx); - if (new_frame) - { - ProcessFrame(); - } - else + if (camera) { - iSkippedFrameCount++; + std::tie(iFrame, new_frame) = camera->get_frame(); } + + } - // Pace ourselves, drastically reduce CPU usage - // TODO: Consider using QTimer instead of QThread - msleep(1000 / 55); + if (new_frame) + { + ProcessFrame(); + } + else + { + iSkippedFrameCount++; + } - // Compute FPS - double elapsed = iFpsTimer.elapsed_seconds(); - if (elapsed >= 1.0) - { - iFps = iFrameCount / elapsed; - iSkippedFps = iSkippedFrameCount / elapsed; - iFrameCount = 0; - iSkippedFrameCount = 0; - iFpsTimer.start(); - } + // Compute FPS + double elapsed = iFpsTimer.elapsed_seconds(); + if (elapsed >= 1.0) + { + iFps = iFrameCount / elapsed; + iSkippedFps = iSkippedFrameCount / elapsed; + iFrameCount = 0; + iSkippedFrameCount = 0; + iFpsTimer.start(); } + } bool Tracker::maybe_reopen_camera() @@ -378,6 +363,10 @@ namespace EasyTracker bool res = camera->start(iCameraInfo); // We got new our camera intrinsics, create corresponding matrices CreateCameraIntrinsicsMatrices(); + + // If ever the camera implementation provided an FPS now is the time to apply it + DoSetFps(iCameraInfo.fps); + return res; } @@ -387,6 +376,28 @@ namespace EasyTracker } + // Calling this from another thread than the one it belongs too after it's started somehow breaks our timer + void Tracker::SetFps(int aFps) + { + QMutexLocker l(&camera_mtx); + DoSetFps(aFps); + } + + void Tracker::DoSetFps(int aFps) + { + // Aplly FPS to timer + iTicker.setInterval(1000 / aFps + 1); + + // Reset Kalman filter + //int nStates = 18; // the number of states + //int nMeasurements = 6; // the number of measured states + //int nInputs = 0; // the number of control actions + //double dt = 0.125; // time between measurements (1/FPS) + double dt = 1000.0 / aFps; + iKf.Init(18, 6, 0, dt); + } + + module_status Tracker::start_tracker(QFrame* video_frame) { //video_frame->setAttribute(Qt::WA_NativeWindow); @@ -401,12 +412,26 @@ namespace EasyTracker // Create our camera camera = video::make_camera(iSettings.camera_name); - - start(QThread::HighPriority); + // Precise timer is needed otherwise the interval is not really respected + iTicker.setTimerType(Qt::PreciseTimer); + SetFps(iSettings.cam_fps); + iTicker.moveToThread(&iThread); + // Connect timer timeout signal to our tick slot + connect(&iTicker, SIGNAL(timeout()), SLOT(Tick()), Qt::DirectConnection); + // Start our timer once our thread is started + iTicker.connect(&iThread, SIGNAL(started()), SLOT(start())); + iFpsTimer.start(); // Kick off our FPS counter + iThread.setObjectName("EasyTrackerThread"); + iThread.setPriority(QThread::HighPriority); // Do we really want that? + iThread.start(); return {}; } + // + // That's called around 250 times per second. + // Therefore we better not do anything here other than provide current data. + // void Tracker::data(double *data) { if (ever_success.load(std::memory_order_relaxed)) diff --git a/tracker-easy/tracker-easy.h b/tracker-easy/tracker-easy.h index a84d725e..05fdb94a 100644 --- a/tracker-easy/tracker-easy.h +++ b/tracker-easy/tracker-easy.h @@ -39,11 +39,9 @@ namespace EasyTracker using namespace numeric_types; - struct Tracker : QThread, ITracker + struct Tracker : public QObject, ITracker { - // We had problem where Qt slots would not get disconnected upon object destruction. - // Issue seems to be gone now even without Q_OBJECT declaration, go figure. - //Q_OBJECT + Q_OBJECT public: friend class Dialog; @@ -54,20 +52,26 @@ namespace EasyTracker module_status start_tracker(QFrame* parent_window) override; void data(double* data) override; bool center() override; - + + private slots: + void Tick(); + + private: void CreateModelFromSettings(); void CreateCameraIntrinsicsMatrices(); void ProcessFrame(); - // From QThread - void run() override; + // bool maybe_reopen_camera(); void set_fov(int value); + void SetFps(int aFps); + void DoSetFps(int aFps); QMutex camera_mtx; - + QThread iThread; + QTimer iTicker; Settings iSettings; @@ -91,7 +95,7 @@ namespace EasyTracker // Statistics Timer iTimer; - Timer iFpsTimer; + Timer iFpsTimer; int iFrameCount = 0; int iSkippedFrameCount = 0; int iFps = 0; |