summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
-rw-r--r--tracker-easy/kalman-filter-pose.cpp1
-rw-r--r--tracker-easy/settings.h4
-rw-r--r--tracker-easy/tracker-easy.cpp121
-rw-r--r--tracker-easy/tracker-easy.h22
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;