decruft PT more, so it doesn't crash finally

14 files changed, 150 insertions, 339 deletions

diff --git a/ftnoir_tracker_pt/camera.cpp b/ftnoir_tracker_pt/camera.cpp
index 74b24093..861c83cc 100644
--- a/ftnoir_tracker_pt/camera.cpp
+++ b/ftnoir_tracker_pt/camera.cpp
@@ -188,11 +188,6 @@ bool CVCamera::_get_frame(Mat* frame)
 	return false;
 }
 
-void CVCamera::_set_index()
-{
-	if (active) restart();
-}
-
 void CVCamera::_set_fps()
 {
     if (cap) cap->set(CV_CAP_PROP_FPS, cam_desired.fps);
diff --git a/ftnoir_tracker_pt/camera.h b/ftnoir_tracker_pt/camera.h
index e2ba56c4..86cafd42 100644
--- a/ftnoir_tracker_pt/camera.h
+++ b/ftnoir_tracker_pt/camera.h
@@ -25,12 +25,11 @@ void get_camera_device_names(std::vector<std::string>& device_names);
 // ----------------------------------------------------------------------------
 struct CamInfo
 {
-	CamInfo() : res_x(0), res_y(0), fps(0), f(1) {}
+	CamInfo() : res_x(0), res_y(0), fps(0) {}
 
 	int res_x;
 	int res_y;
 	int fps;
-	float f;	// (focal length) / (sensor width)
 };
 
 // ----------------------------------------------------------------------------
@@ -39,7 +38,7 @@ class Camera
 {
 public:
         Camera() : dt_valid(0), dt_mean(0), desired_index(0), active_index(-1), active(false) {}
-        virtual ~Camera() {}
+        virtual ~Camera() = 0;
 
         // start/stop capturing
         virtual void start() = 0;
@@ -75,7 +74,7 @@ protected:
         CamInfo cam_info;
         CamInfo cam_desired;
 };
-
+inline Camera::~Camera() {}
 
 // ----------------------------------------------------------------------------
 // camera based on OpenCV's videoCapture
@@ -86,15 +85,14 @@ public:
 	CVCamera() : cap(NULL) {}
 	~CVCamera() { stop(); }
 
-	virtual void start();
-	virtual void stop();
+	void start() override;
+	void stop() override;
 
 protected:
-	virtual bool _get_frame(cv::Mat* frame);
-	virtual void _set_index();
-	virtual void _set_fps();
-	virtual void _set_res();
-    virtual void _set_device_index();
+	bool _get_frame(cv::Mat* frame) override;
+	void _set_fps() override;
+	void _set_res() override;
+    void _set_device_index() override;
 
     cv::VideoCapture* cap;
 };
diff --git a/ftnoir_tracker_pt/frame_observer.cpp b/ftnoir_tracker_pt/frame_observer.cpp
deleted file mode 100644
index 76dee351..00000000
--- a/ftnoir_tracker_pt/frame_observer.cpp
+++ /dev/null
@@ -1,18 +0,0 @@
-/* Copyright (c) 2013 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 "frame_observer.h"
-
-//-----------------------------------------------------------------------------
-FrameProvider::~FrameProvider()
-{
-	QMutexLocker lock(&observer_mutex);
-    for (std::set<FrameObserver*>::iterator iter=frame_observers.begin(); iter!=frame_observers.end(); ++iter)
-	{
-		(*iter)->on_frame_provider_destroy();
-	}
-}
diff --git a/ftnoir_tracker_pt/frame_observer.h b/ftnoir_tracker_pt/frame_observer.h
deleted file mode 100644
index ca8ffb46..00000000
--- a/ftnoir_tracker_pt/frame_observer.h
+++ /dev/null
@@ -1,76 +0,0 @@
-/* Copyright (c) 2013 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.
- */
-
-#ifndef FRAME_OBSERVER_H
-#define FRAME_OBSERVER_H
-
-#include <QMutex>
-#include <opencv2/opencv.hpp>
-#ifndef OPENTRACK_API
-#   include <boost/shared_ptr.hpp>
-#else
-#   include <memory>
-#endif
-#include <set>
-
-//-----------------------------------------------------------------------------
-// Forward declarations
-class FrameObserver;
-
-//-----------------------------------------------------------------------------
-// Provides means to copy frame and point information if it has observers
-// Instantiate a FrameObserver to get the information
-class FrameProvider
-{
-	friend class FrameObserver;
-public:
-	~FrameProvider();
-
-protected:
-	virtual bool get_frame_and_points(cv::Mat& frame, std::shared_ptr< std::vector<cv::Vec2f> >& points) = 0;
-	
-	bool has_observers() const { QMutexLocker lock(&observer_mutex); return !frame_observers.empty(); }
-
-private:
-	mutable QMutex observer_mutex;
-	void add_observer(FrameObserver* obs)    { QMutexLocker lock(&observer_mutex); frame_observers.insert(obs); }
-	void remove_observer(FrameObserver* obs) { QMutexLocker lock(&observer_mutex); frame_observers.erase(obs); }
-	std::set<FrameObserver*> frame_observers;
-};
-
-//-----------------------------------------------------------------------------
-// Used to get frame and point information from MutexedFrameProvider
-// Destroy instance if not interested anymore since a living 
-// FrameObserver instance causes MutexedFrameProvider to provide the information, 
-// potentially reducing its performance
-class FrameObserver
-{
-public:
-	FrameObserver(FrameProvider* provider) : provider(provider) { 
-		provider->add_observer(this); 
-	}
-
-	~FrameObserver() { 
-		if (provider) provider->remove_observer(this); 
-	}
-
-	bool get_frame_and_points(cv::Mat& frame, std::shared_ptr< std::vector<cv::Vec2f> >& points) {
-		return provider ? provider->get_frame_and_points(frame, points) : false;
-	}
-
-	void on_frame_provider_destroy() { 
-		provider = NULL; 
-	}
-
-protected:
-	FrameProvider* provider;
-
-private:
-	FrameObserver(const FrameObserver&);
-};
-
-#endif //FRAME_OBSERVER_H
diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
index 23da97ca..1136ad4a 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
@@ -17,9 +17,6 @@ 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),
@@ -27,7 +24,6 @@ Tracker::Tracker()
 	  video_widget(NULL), 
 	  video_frame(NULL),
       new_settings(nullptr)
-
 {
 }
 
@@ -54,30 +50,28 @@ void Tracker::reset_command(Command command)
 
 void Tracker::run()
 {
-	qDebug()<<"Tracker:: Thread started";
+	qDebug()<< "pt: 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();
-	bool new_frame;
 	forever
 	{   
         if (commands & ABORT) break;
         commands = 0;
         apply_inner();
         const double dt = time.start() * 1e-9;
-        new_frame = camera.get_frame(dt, &frame);
+        const bool 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);
+            const std::vector<cv::Vec2f>& points = point_extractor.extract_points(frame);
             for (auto p : points)
             {
                 auto p2 = cv::Point(p[0] * frame.cols + frame.cols/2, -p[1] * frame.cols + frame.rows/2);
@@ -93,7 +87,8 @@ void Tracker::run()
                          color,
                          4);
             }
-            point_tracker.track(points, camera.get_info().f);
+            if (points.size() == PointModel::N_POINTS)
+                point_tracker.track(points, model);
             video_widget->update_image(frame);
         }
 #ifdef PT_PERF_LOG
@@ -119,6 +114,12 @@ void Tracker::apply_inner()
     reset();
     auto& s = *tmp;
     qDebug()<<"Tracker:: Applying settings";
+    
+    {
+        cv::Vec3f M01(s.m01_x, s.m01_y, s.m01_z);
+        cv::Vec3f M02(s.m02_x, s.m02_y, s.m02_z);
+        model = PointModel(M01, M02);
+    }
     camera.set_device_index(s.cam_index);
     camera.set_res(s.cam_res_x, s.cam_res_y);
     camera.set_fps(s.cam_fps);
@@ -127,11 +128,6 @@ void Tracker::apply_inner()
     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));
-    }
     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,
@@ -160,28 +156,12 @@ void Tracker::center()
 	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);
+    video_widget = new PTVideoWidget(video_frame);
     QHBoxLayout* video_layout = new QHBoxLayout(parent_window);
     video_layout->setContentsMargins(0, 0, 0, 0);
     video_layout->addWidget(video_widget);
diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.h b/ftnoir_tracker_pt/ftnoir_tracker_pt.h
index 6ddfbe5d..5bcfd37d 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt.h
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.h
@@ -12,7 +12,6 @@
 #   include "facetracknoir/plugin-api.hpp"
 #endif
 #include "ftnoir_tracker_pt_settings.h"
-#include "frame_observer.h"
 #include "camera.h"
 #include "point_extractor.h"
 #include "point_tracker.h"
@@ -34,30 +33,25 @@
 
 //-----------------------------------------------------------------------------
 // Constantly processes the tracking chain in a separate thread
-class Tracker : public ITracker, QThread, public FrameProvider
+class Tracker : public ITracker, protected QThread
 {
 public:
 	Tracker();
-    virtual ~Tracker();
-    virtual void StartTracker(QFrame* parent_window);
-    virtual void GetHeadPoseData(double* data);
-    virtual void refreshVideo();
+    ~Tracker() override;
+    void StartTracker(QFrame* parent_window) override;
+    void GetHeadPoseData(double* data) override;
 
     void apply(settings& s);
 	void apply_inner();
 	void center();
 	void reset();	// reset the trackers internal state variables
-	void run();
 
 	void get_pose(FrameTrafo* X_CM) { QMutexLocker lock(&mutex); *X_CM = point_tracker.get_pose(); }
 	int  get_n_points() { QMutexLocker lock(&mutex); return point_extractor.get_points().size(); }
 	void get_cam_info(CamInfo* info) { QMutexLocker lock(&mutex); *info = camera.get_info(); }
-
+protected:
+    void run() override;
 private:
-	// --- MutexedFrameProvider interface ---
-	virtual bool get_frame_and_points(cv::Mat& frame, std::shared_ptr< std::vector<cv::Vec2f> >& points);
-
-	// --- thread ---
 	QMutex mutex;
 	// thread commands
 	enum Command {
@@ -85,6 +79,11 @@ private:
     settings s;
     std::atomic<settings*> new_settings;
     Timer time;
+    
+    static constexpr double rad2deg = 180.0/3.14159265;
+    static constexpr double deg2rad = 3.14159265/180.0;
+    
+    PointModel model;
 };
 
 #undef VideoWidget
diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp
index 9529e268..6cd6135c 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp
@@ -22,13 +22,9 @@ using namespace std;
 //-----------------------------------------------------------------------------
 TrackerDialog::TrackerDialog()
     : tracker(NULL),
-	  video_widget_dialog(NULL),
-	  timer(this), 
-	  trans_calib_running(false)
+      timer(this),
+      trans_calib_running(false)
 {
-	qDebug()<<"TrackerDialog::TrackerDialog";
-	setAttribute(Qt::WA_DeleteOnClose, false);
-
 	ui.setupUi( this );
 
     vector<string> device_names;
@@ -147,6 +143,38 @@ void TrackerDialog::startstop_trans_calib(bool start)
 	}
 }
 
+void TrackerDialog::poll_tracker_info()
+{
+    if (tracker)
+    {	
+        QString to_print;
+        
+        // display caminfo
+        CamInfo info;
+        tracker->get_cam_info(&info);
+        to_print = QString::number(info.res_x)+"x"+QString::number(info.res_y)+" @ "+QString::number(info.fps)+" FPS";
+        ui.caminfo_label->setText(to_print);
+        
+        // display pointinfo
+        int n_points = tracker->get_n_points();
+        to_print = QString::number(n_points);
+        if (n_points == 3)
+            to_print += " OK!";
+        else
+            to_print += " BAD!";
+        ui.pointinfo_label->setText(to_print);
+        
+        // update calibration
+        if (trans_calib_running) trans_calib_step();
+    }
+    else
+    {
+        QString to_print = "Tracker offline";
+        ui.caminfo_label->setText(to_print);
+        ui.pointinfo_label->setText(to_print);
+    }
+}
+
 void TrackerDialog::trans_calib_step()
 {
 	if (tracker)
@@ -197,7 +225,7 @@ void TrackerDialog::do_apply_without_saving(QAbstractButton*)
 
 void TrackerDialog::doApply()
 {
-    save();
+    save();                                                                                                                                                                                                                                                                                                         
 }
 
 void TrackerDialog::doCancel()
@@ -206,71 +234,6 @@ void TrackerDialog::doCancel()
     close();
 }
 
-void TrackerDialog::widget_destroyed(QObject* obj)
-{
-	if (obj == video_widget_dialog) {
-		// widget was / will be already deleted by Qt
-		destroy_video_widget(false);
-	}
-}
-
-void TrackerDialog::create_video_widget()
-{
-	// this should not happen but better be sure
-	if (video_widget_dialog) destroy_video_widget(); 
-	if (!tracker) return;
-
-	video_widget_dialog = new VideoWidgetDialog(this, tracker);
-	video_widget_dialog->setAttribute( Qt::WA_DeleteOnClose );
-	connect( video_widget_dialog, SIGNAL(destroyed(QObject*)),  this, SLOT(widget_destroyed(QObject*)) );
-	video_widget_dialog->show();
-}
-
-void TrackerDialog::destroy_video_widget(bool do_delete /*= true*/)
-{
-	if (video_widget_dialog) {
-		if (do_delete) delete video_widget_dialog;
-		video_widget_dialog = NULL;
-	}
-}
-
-void TrackerDialog::poll_tracker_info()
-{
-	if (tracker)
-	{	
-		QString to_print;
-
-		// display caminfo
-		CamInfo info;
-		tracker->get_cam_info(&info);
-		to_print = QString::number(info.res_x)+"x"+QString::number(info.res_y)+" @ "+QString::number(info.fps)+" FPS";
-		ui.caminfo_label->setText(to_print);
-
-		// display pointinfo
-		int n_points = tracker->get_n_points();
-		to_print = QString::number(n_points);
-		if (n_points == 3)
-			to_print += " OK!";
-		else
-			to_print += " BAD!";
-		ui.pointinfo_label->setText(to_print);
-
-		// update calibration
-		if (trans_calib_running) trans_calib_step();
-
-		// update videowidget
-		if (video_widget_dialog) {
-			video_widget_dialog->get_video_widget()->update_frame_and_points();
-		}
-	}
-	else
-	{
-		QString to_print = "Tracker offline";
-		ui.caminfo_label->setText(to_print);
-		ui.pointinfo_label->setText(to_print);
-	}
-}
-
 void TrackerDialog::registerTracker(ITracker *t)
 {
 	qDebug()<<"TrackerDialog:: Tracker registered";
@@ -285,7 +248,6 @@ void TrackerDialog::unRegisterTracker()
 {
 	qDebug()<<"TrackerDialog:: Tracker un-registered";
 	tracker = NULL;
-	destroy_video_widget();
 	ui.tcalib_button->setEnabled(false); 
 	//ui.center_button->setEnabled(false);
 }
diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.h b/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.h
index dbb93f30..bff12dd0 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.h
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.h
@@ -23,13 +23,13 @@
 
 //-----------------------------------------------------------------------------
 // The dialog that shows up when the user presses "Settings"
-class TrackerDialog : public QWidget, Ui::UICPTClientControls, public ITrackerDialog
+class TrackerDialog : public QWidget, public ITrackerDialog
 {
 	Q_OBJECT
 public:
 	TrackerDialog();
-	void registerTracker(ITracker *tracker);
-	void unRegisterTracker();
+	void registerTracker(ITracker *tracker) override;
+	void unRegisterTracker() override;
     void save();
 	void trans_calib_step();
 
@@ -40,14 +40,9 @@ public slots:
     void do_apply_without_saving(QAbstractButton *);
 
 	void startstop_trans_calib(bool start);
-	void widget_destroyed(QObject* obj);
-	void create_video_widget();
 	void poll_tracker_info();
     void set_model(int idx);
-
-protected:
-	void destroy_video_widget(bool do_delete = true);
-
+private:
 	void set_model_clip();
 	void set_model_cap();
 	void set_model_custom();
@@ -56,7 +51,6 @@ protected:
 
     settings s;
 	Tracker* tracker;
-    VideoWidgetDialog* video_widget_dialog;
 	QTimer timer;
 
 	TranslationCalibrator trans_calib;
diff --git a/ftnoir_tracker_pt/point_extractor.cpp b/ftnoir_tracker_pt/point_extractor.cpp
index b0e29270..819bf5e8 100644
--- a/ftnoir_tracker_pt/point_extractor.cpp
+++ b/ftnoir_tracker_pt/point_extractor.cpp
@@ -20,7 +20,7 @@ PointExtractor::PointExtractor(){
 	//freopen("CON", "w", stderr);
 }
 // ----------------------------------------------------------------------------
-const vector<Vec2f>& PointExtractor::extract_points(Mat frame, float /*dt*/, bool draw_output)
+const vector<Vec2f>& PointExtractor::extract_points(Mat& frame)
 {
 	const int W = frame.cols;
 	const int H = frame.rows; 
@@ -60,7 +60,7 @@ const vector<Vec2f>& PointExtractor::extract_points(Mat frame, float /*dt*/, boo
 		threshold(frame_gray, frame_bin, t, 255, THRESH_BINARY);
 		threshold(frame_gray, frame_bin_low,std::max(float(1), t - (t*hyst)), 255, THRESH_BINARY);
 
-		if(draw_output) frame_bin.copyTo(frame_bin_copy);
+		frame_bin.copyTo(frame_bin_copy);
         if(frame_last.empty()){
 			frame_bin.copyTo(frame_last);
 		}else{
@@ -141,23 +141,21 @@ const vector<Vec2f>& PointExtractor::extract_points(Mat frame, float /*dt*/, boo
 	}
 	
 	// draw output image
-	if (draw_output) {
-		vector<Mat> channels;
-		if(secondary==0){
-			frame_bin.setTo(170, frame_bin);
-			channels.push_back(frame_gray + frame_bin);
-			channels.push_back(frame_gray - frame_bin);
-			channels.push_back(frame_gray - frame_bin);
-		}else{
-			frame_bin_copy.setTo(120, frame_bin_copy);
-			frame_bin_low.setTo(90, frame_bin_low);
-			channels.push_back(frame_gray + frame_bin_copy);
-			channels.push_back(frame_gray + frame_last_and_low);
-			channels.push_back(frame_gray + frame_bin_low);
-			//channels.push_back(frame_gray + frame_bin);
-		}
-		merge(channels, frame);
-	}
+    vector<Mat> channels;
+    if(secondary==0){
+        frame_bin.setTo(170, frame_bin);
+        channels.push_back(frame_gray + frame_bin);
+        channels.push_back(frame_gray - frame_bin);
+        channels.push_back(frame_gray - frame_bin);
+    }else{
+        frame_bin_copy.setTo(120, frame_bin_copy);
+        frame_bin_low.setTo(90, frame_bin_low);
+        channels.push_back(frame_gray + frame_bin_copy);
+        channels.push_back(frame_gray + frame_last_and_low);
+        channels.push_back(frame_gray + frame_bin_low);
+        //channels.push_back(frame_gray + frame_bin);
+    }
+    merge(channels, frame);
 
 	return points;
 }
diff --git a/ftnoir_tracker_pt/point_extractor.h b/ftnoir_tracker_pt/point_extractor.h
index 8a76747b..21d548af 100644
--- a/ftnoir_tracker_pt/point_extractor.h
+++ b/ftnoir_tracker_pt/point_extractor.h
@@ -19,7 +19,7 @@ public:
 	// extracts points from frame and draws some processing info into frame, if draw_output is set
 	// dt: time since last call in seconds
 	// WARNING: returned reference is valid as long as object
-	const std::vector<cv::Vec2f>& extract_points(cv::Mat frame, float dt, bool draw_output);
+	const std::vector<cv::Vec2f>& extract_points(cv::Mat &frame);
 	const std::vector<cv::Vec2f>& get_points() { return points; }
 	PointExtractor();
 
diff --git a/ftnoir_tracker_pt/point_tracker.cpp b/ftnoir_tracker_pt/point_tracker.cpp
index 5f57baf5..8a633c5d 100644
--- a/ftnoir_tracker_pt/point_tracker.cpp
+++ b/ftnoir_tracker_pt/point_tracker.cpp
@@ -33,10 +33,14 @@ static void set_row(Matx33f& m, int i, const Vec3f& v)
 	m(i,2) = v[2];
 }
 
-// ----------------------------------------------------------------------------
+PointModel::PointModel() :
+    M01 { 0, 0, 0 },
+    M02 { 0, 0, 0 }
+{
+}
+
 PointModel::PointModel(Vec3f M01, Vec3f M02)
-	: M01(M01), 
-	  M02(M02)
+	: M01(M01), M02(M02)
 {
 	// calculate u
 	u = M01.cross(M02);
@@ -107,27 +111,31 @@ void PointTracker::reset()
 	X_CM = FrameTrafo();
 }
 
-void PointTracker::track(const vector<Vec2f>& points, float f)
+void PointTracker::track(const vector<Vec2f>& projected_points, const PointModel& model)
 {
-    find_correspondences(points, f);
-	(void) POSIT(f);
-	//qDebug()<<"Number of POSIT iterations: "<<n_iter;
+    const PointOrder& order = find_correspondences(projected_points, model);
+	int iters = POSIT(model, order);
+	qDebug()<<"POSIT iterations:"<<iters;
 }
 
-void PointTracker::find_correspondences(const std::vector<cv::Vec2f>& points, float f)
+PointTracker::PointOrder PointTracker::find_correspondences(const std::vector<cv::Vec2f>& projected_points, const PointModel& model)
 {
     // ... otherwise we look at the distance to the projection of the expected model points 
     // project model points under current pose
     Vec2f p_exp[3];
-    p_exp[0] = project(Vec3f(0,0,0), f);
-    p_exp[1] = project(point_model->M01, f);
-    p_exp[2] = project(point_model->M02, f);
+    p_exp[0] = project(Vec3f(0,0,0));
+    p_exp[1] = project(model.get_M01());
+    p_exp[2] = project(model.get_M02());
     
     // set correspondences by minimum distance to projected model point
     bool point_taken[PointModel::N_POINTS];
     for (int i=0; i<PointModel::N_POINTS; ++i)
         point_taken[i] = false;
     
+    PointOrder p;
+    for (int i=0; i<PointModel::N_POINTS; ++i)
+        p.points[i] = Vec2f(0, 0);
+    
     for (int i=0; i<PointModel::N_POINTS; ++i)
     {
         float min_sdist = 1e4;
@@ -135,7 +143,7 @@ void PointTracker::find_correspondences(const std::vector<cv::Vec2f>& points, fl
         // find closest point to projected model point i
         for (int j=0; j<PointModel::N_POINTS; ++j)
         {
-            Vec2f d = p_exp[i]-points[j];
+            Vec2f d = p_exp[i]-projected_points[j];
             float sdist = d.dot(d);
             if (sdist < min_sdist)
             {
@@ -144,15 +152,16 @@ void PointTracker::find_correspondences(const std::vector<cv::Vec2f>& points, fl
             }
         }
         // if one point is closest to more than one model point, abort
-        if (point_taken[min_idx]) return;
+        if (point_taken[min_idx]) return p;
         point_taken[min_idx] = true;
-        p[i] = points[min_idx];
+        p.points[i] = projected_points[min_idx];
     }
+    return p;
 }
 
 
 
-int PointTracker::POSIT(float f)
+int PointTracker::POSIT(const PointModel& model, const PointOrder& order_)
 {
 	// POSIT algorithm for coplanar points as presented in
 	// [Denis Oberkampf, Daniel F. DeMenthon, Larry S. Davis: "Iterative Pose Estimation Using Coplanar Feature Points"]
@@ -182,24 +191,26 @@ int PointTracker::POSIT(float f)
 
 	const int MAX_ITER = 100;
 	const float EPS_THRESHOLD = 1e-4;
+    
+    const cv::Vec2f* order = order_.points;
 
 	int i=1;
 	for (; i<MAX_ITER; ++i)
 	{
-		epsilon_1 = k.dot(point_model->M01)/Z0;
-		epsilon_2 = k.dot(point_model->M02)/Z0;
+		epsilon_1 = k.dot(model.M01)/Z0;
+		epsilon_2 = k.dot(model.M02)/Z0;
 
 		// vector of scalar products <I0, M0i> and <J0, M0i>
-		Vec2f I0_M0i(p[1][0]*(1.0 + epsilon_1) - p[0][0], 
-			         p[2][0]*(1.0 + epsilon_2) - p[0][0]);
-		Vec2f J0_M0i(p[1][1]*(1.0 + epsilon_1) - p[0][1],
-			         p[2][1]*(1.0 + epsilon_2) - p[0][1]);
+		Vec2f I0_M0i(order[1][0]*(1.0 + epsilon_1) - order[0][0], 
+			         order[2][0]*(1.0 + epsilon_2) - order[0][0]);
+		Vec2f J0_M0i(order[1][1]*(1.0 + epsilon_1) - order[0][1],
+			         order[2][1]*(1.0 + epsilon_2) - order[0][1]);
 
 		// construct projection of I, J onto M0i plane: I0 and J0
-		I0_coeff = point_model->P * I0_M0i;
-		J0_coeff = point_model->P * J0_M0i;
-		I0 = I0_coeff[0]*point_model->M01 + I0_coeff[1]*point_model->M02;
-		J0 = J0_coeff[0]*point_model->M01 + J0_coeff[1]*point_model->M02;
+		I0_coeff = model.P * I0_M0i;
+		J0_coeff = model.P * J0_M0i;
+		I0 = I0_coeff[0]*model.M01 + I0_coeff[1]*model.M02;
+		J0 = J0_coeff[0]*model.M01 + J0_coeff[1]*model.M02;
 
 		// calculate u component of I, J		
 		float II0 = I0.dot(I0);
@@ -219,11 +230,11 @@ int PointTracker::POSIT(float f)
 		}
 
 		// construct the two solutions
-		I_1 = I0 + rho*cos(theta)*point_model->u;	
-		I_2 = I0 - rho*cos(theta)*point_model->u;
+		I_1 = I0 + rho*cos(theta)*model.u;	
+		I_2 = I0 - rho*cos(theta)*model.u;
 
-		J_1 = J0 + rho*sin(theta)*point_model->u;
-		J_2 = J0 - rho*sin(theta)*point_model->u;
+		J_1 = J0 + rho*sin(theta)*model.u;
+		J_2 = J0 - rho*sin(theta)*model.u;
 
 		float norm_const = 1.0/norm(I_1); // all have the same norm
 		
@@ -240,7 +251,7 @@ int PointTracker::POSIT(float f)
 		set_row(R_2, 2, I_2.cross(J_2));
 
 		// the single translation solution
-		Z0 = norm_const * f;
+		Z0 = norm_const * focal_length;
 
 		// pick the rotation solution closer to the expected one
 		// in simple metric d(A,B) = || I - A * B^T ||
@@ -263,8 +274,8 @@ int PointTracker::POSIT(float f)
 
 	// apply results
 	X_CM.R = *R_current;
-	X_CM.t[0] = p[0][0] * Z0/f;
-	X_CM.t[1] = p[0][1] * Z0/f;
+	X_CM.t[0] = order[0][0] * Z0/focal_length;
+	X_CM.t[1] = order[0][1] * Z0/focal_length;
 	X_CM.t[2] = Z0;
 
 	return i;
diff --git a/ftnoir_tracker_pt/point_tracker.h b/ftnoir_tracker_pt/point_tracker.h
index a1f6f041..0339f392 100644
--- a/ftnoir_tracker_pt/point_tracker.h
+++ b/ftnoir_tracker_pt/point_tracker.h
@@ -14,7 +14,7 @@
 #else
 #   include <memory>
 #endif
-#include <list>
+#include <vector>
 
 // ----------------------------------------------------------------------------
 // Affine frame trafo
@@ -60,9 +60,10 @@ public:
 	static constexpr int N_POINTS = 3;
 
 	PointModel(cv::Vec3f M01, cv::Vec3f M02);
+    PointModel();
 
-	const cv::Vec3f& get_M01() const { return M01; }
-	const cv::Vec3f& get_M02() const { return M02; }
+	inline const cv::Vec3f& get_M01() const { return M01; }
+	inline const cv::Vec3f& get_M02() const { return M02; }
 
 private:
 	cv::Vec3f M01;	// M01 in model frame
@@ -86,27 +87,27 @@ class PointTracker
 {
 public:
 	PointTracker();
-
 	// track the pose using the set of normalized point coordinates (x pos in range -0.5:0.5)
 	// f : (focal length)/(sensor width)
 	// dt : time since last call
-	void track(const std::vector<cv::Vec2f>& points, float f);
-	std::shared_ptr<PointModel> point_model;
-
+	void track(const std::vector<cv::Vec2f>& projected_points, const PointModel& model);
 	FrameTrafo get_pose() const { return X_CM; }
 	void reset();
 
 private:
-	inline cv::Vec2f project(const cv::Vec3f& v_M, float f)
+    // the points in model order
+    typedef struct { cv::Vec2f points[PointModel::N_POINTS]; } PointOrder;
+    static constexpr float focal_length = 1.0f;    
+    
+	inline cv::Vec2f project(const cv::Vec3f& v_M)
 	{
 		cv::Vec3f v_C = X_CM * v_M;
-		return cv::Vec2f(f*v_C[0]/v_C[2], f*v_C[1]/v_C[2]);
+		return cv::Vec2f(focal_length*v_C[0]/v_C[2], focal_length*v_C[1]/v_C[2]);
 	}
 
-    void find_correspondences(const std::vector<cv::Vec2f>& points, float f);
-    int POSIT(float f);  // The POSIT algorithm, returns the number of iterations
+    PointOrder find_correspondences(const std::vector<cv::Vec2f>& projected_points, const PointModel &model);
+    int POSIT(const PointModel& point_model, const PointOrder& order);  // The POSIT algorithm, returns the number of iterations
     
-	cv::Vec2f p[PointModel::N_POINTS];	// the points in model order
 	FrameTrafo X_CM; // trafo from model to camera
 };
 
diff --git a/ftnoir_tracker_pt/pt_video_widget.cpp b/ftnoir_tracker_pt/pt_video_widget.cpp
index cb3dc48e..aefb8199 100644
--- a/ftnoir_tracker_pt/pt_video_widget.cpp
+++ b/ftnoir_tracker_pt/pt_video_widget.cpp
@@ -22,24 +22,6 @@ void PTVideoWidget::update_image(const cv::Mat& frame)
     freshp = true;
 }
 
-// ----------------------------------------------------------------------------
-VideoWidgetDialog::VideoWidgetDialog(QWidget *parent, FrameProvider* provider)
-    : QDialog(parent),
-      video_widget(NULL)
-{
-    const int VIDEO_FRAME_WIDTH  = 640;
-    const int VIDEO_FRAME_HEIGHT = 480;
-
-    video_widget = new PTVideoWidget(this, provider);
-
-    QHBoxLayout* layout = new QHBoxLayout();
-    layout->setContentsMargins(0, 0, 0, 0);
-    layout->addWidget(video_widget);
-    if (this->layout()) delete this->layout();
-    setLayout(layout);
-    resize(VIDEO_FRAME_WIDTH, VIDEO_FRAME_HEIGHT);
-}
-
 void PTVideoWidget::update_and_repaint()
 {
     QMutexLocker foo(&mtx);
diff --git a/ftnoir_tracker_pt/pt_video_widget.h b/ftnoir_tracker_pt/pt_video_widget.h
index 1be5f5f2..de2c7efb 100644
--- a/ftnoir_tracker_pt/pt_video_widget.h
+++ b/ftnoir_tracker_pt/pt_video_widget.h
@@ -7,7 +7,6 @@
 
 #pragma once
 
-#include "frame_observer.h"
 #include <QObject>
 #include <QTime>
 #include <QDialog>
@@ -24,15 +23,16 @@
 #include <QPainter>
 #include <QPaintEvent>
 #include <QTimer>
+#include <QMutex>
+#include <QMutexLocker>
 
-class PTVideoWidget : public QWidget, public FrameObserver
+class PTVideoWidget : public QWidget
 {
     Q_OBJECT
 
 public:
-    PTVideoWidget(QWidget *parent, FrameProvider* provider) :
+    PTVideoWidget(QWidget *parent) :
         QWidget(parent),
-        /* to avoid linker errors */ FrameObserver(provider),
         freshp(false)
     {
         connect(&timer, SIGNAL(timeout()), this, SLOT(update_and_repaint()));
@@ -52,20 +52,5 @@ private:
     QImage texture;
     QTimer timer;
     cv::Mat _frame;
-    bool freshp;
-};
-
-// ----------------------------------------------------------------------------
-// A VideoWidget embedded in a dialog frame
-class VideoWidgetDialog : public QDialog
-{
-    Q_OBJECT
-public:
-	VideoWidgetDialog(QWidget *parent, FrameProvider* provider);
-	virtual ~VideoWidgetDialog() {}
-
-    PTVideoWidget* get_video_widget() { return video_widget; }
-
-private:
-    PTVideoWidget* video_widget;
+    volatile bool freshp;
 };