diff options
Diffstat (limited to 'FTNoIR_Tracker_PT/ftnoir_tracker_pt.cpp')
| -rw-r--r-- | FTNoIR_Tracker_PT/ftnoir_tracker_pt.cpp | 184 |
1 files changed, 184 insertions, 0 deletions
diff --git a/FTNoIR_Tracker_PT/ftnoir_tracker_pt.cpp b/FTNoIR_Tracker_PT/ftnoir_tracker_pt.cpp new file mode 100644 index 00000000..2e8430bc --- /dev/null +++ b/FTNoIR_Tracker_PT/ftnoir_tracker_pt.cpp @@ -0,0 +1,184 @@ +/* Copyright (c) 2012 Patrick Ruoff
+ *
+ * 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.
+ */
+
+#include "ftnoir_tracker_pt.h"
+#include <QHBoxLayout>
+#include <cmath>
+#include <QDebug>
+
+using namespace std;
+using namespace cv;
+using namespace boost;
+
+//-----------------------------------------------------------------------------
+Tracker::Tracker()
+ : frame_count(0), commands(0), video_widget(NULL)
+{
+ qDebug()<<"Tracker Const";
+ TrackerSettings settings;
+ settings.load_ini();
+ apply(settings);
+ qDebug()<<"Tracker Starting";
+ start();
+}
+
+Tracker::~Tracker()
+{
+ set_command(ABORT);
+ wait();
+ if (video_widget) delete video_widget;
+}
+
+void Tracker::set_command(Command command)
+{
+ QMutexLocker lock(&mutex);
+ commands |= command;
+}
+
+void Tracker::reset_command(Command command)
+{
+ QMutexLocker lock(&mutex);
+ commands &= ~command;
+}
+
+void Tracker::run()
+{
+ qDebug()<<"Tracker Thread started";
+ time.start();
+ forever
+ {
+ {
+ QMutexLocker lock(&mutex);
+
+ if (commands & ABORT) break;
+ if (commands & PAUSE) continue;
+ commands = 0;
+
+ float dt = time.elapsed() / 1000.0;
+ time.restart();
+
+ frame = camera.get_frame(dt);
+ if (!frame.empty())
+ {
+ const std::vector<cv::Vec2f>& points = point_extractor.extract_points(frame, dt, draw_frame);
+ point_tracker.track(points, camera.get_info().f, dt);
+ frame_count++;
+ }
+ }
+ msleep(sleep_time);
+ }
+ qDebug()<<"Tracker Thread stopping";
+}
+
+void Tracker::apply(const TrackerSettings& settings)
+{
+ apply_without_camindex(settings);
+ QMutexLocker lock(&mutex);
+ qDebug()<<"Tracker: setting cam index "<<settings.cam_index;
+ camera.set_index(settings.cam_index);
+ qDebug()<<"Tracker: done setting cam index";
+}
+
+void Tracker::apply_without_camindex(const TrackerSettings& settings)
+{
+ qDebug()<<"Tracker::apply_without_camindex";
+ QMutexLocker lock(&mutex);
+ camera.set_f(settings.cam_f);
+ point_extractor.threshold_val = settings.threshold;
+ point_extractor.min_size = settings.min_point_size;
+ point_extractor.max_size = settings.max_point_size;
+ point_tracker.point_model = boost::shared_ptr<PointModel>(new PointModel(settings.M01, settings.M02));
+ sleep_time = settings.sleep_time;
+ draw_frame = settings.video_widget;
+ t_MH = settings.t_MH;
+}
+
+//-----------------------------------------------------------------------------
+// ITracker interface
+void Tracker::Initialize(QFrame *videoframe)
+{
+ const int VIDEO_FRAME_WIDTH = 252;
+ const int VIDEO_FRAME_HEIGHT = 189;
+
+ qDebug("Tracker::Initialize()");
+ // setup video frame
+ videoframe->setAttribute(Qt::WA_NativeWindow);
+ videoframe->show();
+ video_widget = new VideoWidget(videoframe);
+ QHBoxLayout* layout = new QHBoxLayout();
+ layout->setContentsMargins(0, 0, 0, 0);
+ layout->addWidget(video_widget);
+ if (videoframe->layout()) delete videoframe->layout();
+ videoframe->setLayout(layout);
+ video_widget->resize(VIDEO_FRAME_WIDTH, VIDEO_FRAME_HEIGHT);
+}
+
+void Tracker::refreshVideo()
+{
+ if (video_widget)
+ {
+ Mat frame_copy;
+ shared_ptr< vector<Vec2f> > points;
+ //qDebug("Tracker::refreshVideo()");
+ {
+ QMutexLocker lock(&mutex);
+ if (!draw_frame || frame.empty()) return;
+
+ // copy the frame and points from the tracker thread
+ frame_copy = frame.clone();
+ points = shared_ptr< vector<Vec2f> >(new vector<Vec2f>(point_extractor.get_points()));
+ }
+
+ video_widget->update(frame_copy, points);
+ }
+}
+
+void Tracker::StartTracker(HWND parent_window)
+{
+ reset_command(PAUSE);
+}
+
+void Tracker::StopTracker(bool exit)
+{
+ set_command(PAUSE);
+}
+
+bool Tracker::GiveHeadPoseData(THeadPoseData *data)
+{
+ const float rad2deg = 180.0/3.14159265;
+ {
+ QMutexLocker lock(&mutex);
+ FrameTrafo pose = point_tracker.get_pose();
+
+ // get translation
+ Vec3f p = pose.R * t_MH + pose.t;
+ data->x = p[0] / 10.0; // convert to cm
+ data->y = p[1] / 10.0;
+ data->z = p[2] / 10.0;
+
+ // extract rotation angles from rotation matrix
+ const Matx33f& R = pose.R;
+ data->yaw = atan2( -R(2,0), sqrt(R(0,0)*R(0,0) + R(1,0)*R(1,0)) );
+ float cos_beta = cos(data->yaw);
+ if (cos_beta != 0)
+ {
+ data->pitch = rad2deg * atan2( R(2,1)/cos_beta, R(2,2)/cos_beta);
+ data->roll = rad2deg * atan2( R(1,0)/cos_beta, R(0,0)/cos_beta);
+ }
+ data->yaw *= rad2deg;
+ }
+ refreshVideo();
+ return true;
+}
+
+//-----------------------------------------------------------------------------
+#pragma comment(linker, "/export:GetTracker=_GetTracker@0")
+
+FTNOIR_TRACKER_BASE_EXPORT TRACKERHANDLE __stdcall GetTracker()
+{
+ return new Tracker;
+}
\ No newline at end of file |