rename case

1 files changed, 265 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..ef72f9a2
--- /dev/null
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
@@ -0,0 +1,265 @@
+/* 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>

+#include <QFile>

+#include <QCoreApplication>

+

+using namespace std;

+using namespace cv;

+using namespace boost;

+

+//#define PT_PERF_LOG	//log performance

+

+const float rad2deg = 180.0/3.14159265;

+const float deg2rad = 1.0/rad2deg;

+

+//-----------------------------------------------------------------------------

+Tracker::Tracker()

+    : mutex(QMutex::Recursive),

+      commands(0),

+	  video_widget(NULL), 

+	  video_frame(NULL),

+	  tracking_valid(false),

+      new_settings(nullptr)

+

+{

+	qDebug()<<"Tracker::Tracker";

+}

+

+Tracker::~Tracker()

+{

+	qDebug()<<"Tracker::~Tracker";

+	// terminate tracker thread

+	set_command(ABORT);

+	wait();

+    s.video_widget = false;

+    delete video_widget;

+    video_widget = NULL;

+    if (video_frame->layout()) delete video_frame->layout();

+}

+

+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";

+

+#ifdef PT_PERF_LOG

+	QFile log_file(QCoreApplication::applicationDirPath() + "/PointTrackerPerformance.txt");

+	if (!log_file.open(QIODevice::WriteOnly | QIODevice::Text)) return;

+	QTextStream log_stream(&log_file);

+#endif

+

+    time.start();

+    double dt;

+	bool new_frame;

+	forever

+	{   

+        if (commands & ABORT) break;

+        if (commands & PAUSE) continue;

+        commands = 0;

+	apply_inner();

+        dt = time.start() / 1000000000.;

+

+        new_frame = camera.get_frame(dt, &frame);

+

+        if (new_frame && !frame.empty())

+        {

+            QMutexLocker lock(&mutex);

+

+            frame = frame_rotation.rotate_frame(frame);

+            const std::vector<cv::Vec2f>& points = point_extractor.extract_points(frame, dt, true);

+            for (auto p : points)

+            {

+                auto p2 = cv::Point(p[0] * frame.cols + frame.cols/2, -p[1] * frame.cols + frame.rows/2);

+                cv::Scalar color(0, 255, 0);

+                cv::line(frame,

+                         cv::Point(p2.x - 20, p2.y),

+                         cv::Point(p2.x + 20, p2.y),

+                         color,

+                         4);

+                cv::line(frame,

+                         cv::Point(p2.x, p2.y - 20),

+                         cv::Point(p2.x, p2.y + 20),

+                         color,

+                         4);

+            }

+            tracking_valid = point_tracker.track(points, camera.get_info().f, dt);

+            video_widget->update_image(frame);

+        }

+#ifdef PT_PERF_LOG

+        log_stream<<"dt: "<<dt;

+        if (!frame.empty()) log_stream<<" fps: "<<camera.get_info().fps;

+        log_stream<<"\n";

+#endif

+	}

+

+	qDebug()<<"Tracker:: Thread stopping";

+}

+void Tracker::apply(settings& s)

+{

+    // caller guarantees object lifetime

+	new_settings = &s;

+}

+

+void Tracker::apply_inner()

+{

+    settings* tmp = new_settings.exchange(nullptr);

+    if (tmp == nullptr)

+        return;

+    auto& s = *tmp;

+    qDebug()<<"Tracker:: Applying settings";

+    camera.set_device_index(s.cam_index);

+    camera.set_res(s.cam_res_x, s.cam_res_y);

+    camera.set_fps(s.cam_fps);

+    camera.set_f(s.cam_f);

+    frame_rotation.rotation = static_cast<RotationType>(static_cast<int>(s.cam_roll));

+    point_extractor.threshold_val = s.threshold;

+    point_extractor.threshold_secondary_val = s.threshold_secondary;

+    point_extractor.min_size = s.min_point_size;

+    point_extractor.max_size = s.max_point_size;

+    {

+        cv::Vec3f M01(s.m01_x, s.m01_y, s.m01_z);

+        cv::Vec3f M02(s.m02_x, s.m02_y, s.m02_z);

+        point_tracker.point_model = boost::shared_ptr<PointModel>(new PointModel(M01, M02));

+    }

+    point_tracker.dynamic_pose_resolution = s.dyn_pose_res;

+    point_tracker.dt_reset = s.reset_time / 1000.0;

+    t_MH = cv::Vec3f(s.t_MH_x, s.t_MH_y, s.t_MH_z);

+    R_GC =  Matx33f( cos(deg2rad*s.cam_yaw), 0, sin(deg2rad*s.cam_yaw),

+		                                         0, 1,                             0,

+                    -sin(deg2rad*s.cam_yaw), 0, cos(deg2rad*s.cam_yaw));

+	R_GC = R_GC * Matx33f( 1,                                0,                               0,

+                           0,  cos(deg2rad*s.cam_pitch), sin(deg2rad*s.cam_pitch),

+                           0, -sin(deg2rad*s.cam_pitch), cos(deg2rad*s.cam_pitch));

+

+	FrameTrafo X_MH(Matx33f::eye(), t_MH);

+	X_GH_0 = R_GC * X_MH;

+

+	qDebug()<<"Tracker::apply ends";

+}

+

+void Tracker::reset()

+{

+	QMutexLocker lock(&mutex);

+	point_tracker.reset();

+}

+

+void Tracker::center()

+{

+	point_tracker.reset();	// we also do a reset here since there is no reset shortkey yet

+	QMutexLocker lock(&mutex);

+	FrameTrafo X_CM_0 = point_tracker.get_pose();

+	FrameTrafo X_MH(Matx33f::eye(), t_MH);

+	X_GH_0 = R_GC * X_CM_0 * X_MH;

+}

+

+bool Tracker::get_frame_and_points(cv::Mat& frame_copy, boost::shared_ptr< std::vector<Vec2f> >& points)

+{

+	QMutexLocker lock(&mutex);

+	if (frame.empty()) return false;

+		

+	// copy the frame and points from the tracker thread

+	frame_copy = frame.clone();

+	points = boost::shared_ptr< vector<Vec2f> >(new vector<Vec2f>(point_extractor.get_points()));

+	return true;

+}

+

+void Tracker::refreshVideo()

+{	

+	if (video_widget) video_widget->update_frame_and_points();

+}

+

+void Tracker::StartTracker(QFrame *parent_window)

+{

+    this->video_frame = parent_window;

+    video_frame->setAttribute(Qt::WA_NativeWindow);

+    video_frame->show();

+    video_widget = new PTVideoWidget(video_frame, this);

+    QHBoxLayout* video_layout = new QHBoxLayout(parent_window);

+    video_layout->setContentsMargins(0, 0, 0, 0);

+    video_layout->addWidget(video_widget);

+    video_frame->setLayout(video_layout);

+    video_widget->resize(video_frame->width(), video_frame->height());

+    camera.start();

+	apply(s);

+    start();

+    reset_command(PAUSE);

+}

+

+#ifndef OPENTRACK_API

+void Tracker::StopTracker(bool exit)

+{

+	set_command(PAUSE);

+}

+#endif

+

+#ifdef OPENTRACK_API

+#define THeadPoseData double

+#endif

+

+void Tracker::GetHeadPoseData(THeadPoseData *data)

+{

+	{

+		QMutexLocker lock(&mutex);

+

+        if (!tracking_valid) return;

+

+		FrameTrafo X_CM = point_tracker.get_pose();

+		FrameTrafo X_MH(Matx33f::eye(), t_MH);

+		FrameTrafo X_GH = R_GC * X_CM * X_MH;

+        Matx33f R = X_GH.R * X_GH_0.R.t();

+		Vec3f   t = X_GH.t - X_GH_0.t;		

+

+        // get translation(s)

+        if (s.bEnableX) data[TX] = t[0] / 10.0;	// convert to cm

+        if (s.bEnableY) data[TY] = t[1] / 10.0;

+        if (s.bEnableZ) data[TZ] = t[2] / 10.0;

+

+        // translate rotation matrix from opengl (G) to roll-pitch-yaw (E) frame

+		// -z -> x, y -> z, x -> -y

+		Matx33f R_EG( 0, 0,-1,

+		             -1, 0, 0,

+		              0, 1, 0);

+		R = R_EG * R * R_EG.t();

+

+		// extract rotation angles

+		float alpha, beta, gamma;

+		beta  = atan2( -R(2,0), sqrt(R(2,1)*R(2,1) + R(2,2)*R(2,2)) );

+		alpha = atan2( R(1,0), R(0,0));

+		gamma = atan2( R(2,1), R(2,2));		

+

+        if (s.bEnableYaw)   data[Yaw]  =   rad2deg * alpha;

+        if (s.bEnablePitch) data[Pitch] = - rad2deg * beta;	// FTNoIR expects a minus here

+        if (s.bEnableRoll)  data[Roll]  =   rad2deg * gamma;

+	}

+}

+

+//-----------------------------------------------------------------------------

+#ifdef OPENTRACK_API

+extern "C" FTNOIR_TRACKER_BASE_EXPORT ITracker* CALLING_CONVENTION GetConstructor()

+#else

+#pragma comment(linker, "/export:GetTracker=_GetTracker@0")

+FTNOIR_TRACKER_BASE_EXPORT ITrackerPtr __stdcall GetTracker()

+#endif

+{

+	return new Tracker;

+}