dos2unix only

1 files changed, 264 insertions, 264 deletions

diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
index 219c8990..3fa6910d 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
@@ -1,264 +1,264 @@
-/* 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;

-

-//#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 = std::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, std::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 = std::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)

-        data[TX] = t[0] / 10.0;	// convert to cm

-        data[TY] = t[1] / 10.0;

-        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));		

-

-        data[Yaw]   =   rad2deg * alpha;

-        data[Pitch] = - rad2deg * beta;	// FTNoIR expects a minus here

-        data[Roll]  =   rad2deg * gamma;

-	}

-}

-

-//-----------------------------------------------------------------------------

-#ifdef OPENTRACK_API

-extern "C" OPENTRACK_EXPORT ITracker* CALLING_CONVENTION GetConstructor()

-#else

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

-OPENTRACK_EXPORT ITrackerPtr __stdcall GetTracker()

-#endif

-{

-	return new Tracker;

-}

+/* 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;
+
+//#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 = std::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, std::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 = std::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)
+        data[TX] = t[0] / 10.0;	// convert to cm
+        data[TY] = t[1] / 10.0;
+        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));		
+
+        data[Yaw]   =   rad2deg * alpha;
+        data[Pitch] = - rad2deg * beta;	// FTNoIR expects a minus here
+        data[Roll]  =   rad2deg * gamma;
+	}
+}
+
+//-----------------------------------------------------------------------------
+#ifdef OPENTRACK_API
+extern "C" OPENTRACK_EXPORT ITracker* CALLING_CONVENTION GetConstructor()
+#else
+#pragma comment(linker, "/export:GetTracker=_GetTracker@0")
+OPENTRACK_EXPORT ITrackerPtr __stdcall GetTracker()
+#endif
+{
+	return new Tracker;
+}