Easy Tracker: OpenCV point extractor.

13 files changed, 324 insertions, 181 deletions

diff --git a/tracker-easy/cv-point-extractor.cpp b/tracker-easy/cv-point-extractor.cpp
new file mode 100644
index 00000000..42f1a06b
--- /dev/null
+++ b/tracker-easy/cv-point-extractor.cpp
@@ -0,0 +1,106 @@
+/* Copyright (c) 2012 Patrick Ruoff
+ * Copyright (c) 2015-2017 Stanislaw Halik <sthalik@misaki.pl>
+ *
+ * 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 "cv-point-extractor.h"
+#include "frame.hpp"
+
+#include "cv/numeric.hpp"
+#include "compat/math.hpp"
+#include <opencv2/imgproc/types_c.h>
+
+#include <cmath>
+#include <algorithm>
+#include <cinttypes>
+#include <memory>
+
+#include <QDebug>
+
+using namespace numeric_types;
+
+
+
+CvPointExtractor::CvPointExtractor(const QString& module_name) : s(module_name)
+{
+    
+}
+
+
+void CvPointExtractor::extract_points(const cv::Mat& frame, cv::Mat& aPreview, std::vector<vec2>& points, std::vector<vec2>& imagePoints)
+{
+
+    // Contours detection
+    std::vector<std::vector<cv::Point> > contours;
+    cv::findContours(frame, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
+    
+    // Workout which countour are valid points
+    std::vector<std::vector<cv::Point> > balls;
+    std::vector<cv::Rect> ballsBox;
+    for (size_t i = 0; i < contours.size(); i++)
+    {
+        cv::drawContours(aPreview, contours, i, CV_RGB(255, 0, 0), 2);
+
+        cv::Rect bBox;
+        bBox = cv::boundingRect(contours[i]);
+
+        float ratio = (float)bBox.width / (float)bBox.height;
+        if (ratio > 1.0f)
+            ratio = 1.0f / ratio;
+
+       
+        // Searching for a bBox almost square
+        float minArea = s.min_point_size*s.min_point_size;
+        float maxArea = s.max_point_size*s.max_point_size;
+        if (bBox.width >= s.min_point_size
+            && bBox.height >= s.min_point_size
+            && bBox.width <= s.max_point_size
+            && bBox.height <= s.max_point_size
+            && bBox.area() >= minArea
+            && bBox.area() <= maxArea
+            /*&& ratio > 0.75 &&*/)
+        {
+            balls.push_back(contours[i]);
+            ballsBox.push_back(bBox);
+
+            vec2 center;
+            center[0] = bBox.x + bBox.width / 2;
+            center[1] = bBox.y + bBox.height / 2;
+            imagePoints.push_back(vec2(center));
+
+            cv::rectangle(aPreview, bBox, CV_RGB(0, 255, 0), 2);            
+        }
+    }
+
+    // Keep the three points which are highest, i.e. with lowest Y coordinates
+    // That's most usefull to discard noise from features below your cap/head.
+    // Typically noise comming from zippers and metal parts on your clothing.
+    // With a cap tracker it also successfully discards noise glasses.
+    // However it may not work as good with a clip user wearing glasses.
+    while (imagePoints.size() > 3) // Until we have no more than three points
+    {
+        int maxY = 0;
+        int index = -1;
+
+        // Search for the point with highest Y coordinate
+        for (size_t i = 0; i < imagePoints.size(); i++)
+        {
+            if (imagePoints[i][1] > maxY)
+            {
+                maxY = imagePoints[i][1];
+                index = i;
+            }
+        }
+
+        // Discard it
+        imagePoints.erase(imagePoints.begin() + index);
+    }
+
+
+}
+
+
+
diff --git a/tracker-easy/cv-point-extractor.h b/tracker-easy/cv-point-extractor.h
new file mode 100644
index 00000000..65293e57
--- /dev/null
+++ b/tracker-easy/cv-point-extractor.h
@@ -0,0 +1,34 @@
+/* Copyright (c) 2012 Patrick Ruoff
+ * Copyright (c) 2015-2016 Stanislaw Halik <sthalik@misaki.pl>
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include "tracker-easy-api.h"
+
+#include <vector>
+
+#include <opencv2/core.hpp>
+#include <opencv2/imgproc.hpp>
+
+
+using namespace numeric_types;
+
+
+class CvPointExtractor final : public pt_point_extractor
+{
+public:
+    // extracts points from frame and draws some processing info into frame, if draw_output is set
+    // dt: time since last call in seconds
+    void extract_points(const cv::Mat& frame, cv::Mat& preview_frame, std::vector<vec2>& points, std::vector<vec2>& imagePoints) override;
+    CvPointExtractor(const QString& module_name);
+
+    pt_settings s;
+};
+
+
+
diff --git a/tracker-easy/frame.cpp b/tracker-easy/frame.cpp
index 2464f85f..90d4c6f9 100644
--- a/tracker-easy/frame.cpp
+++ b/tracker-easy/frame.cpp
@@ -29,11 +29,11 @@ Preview& Preview::operator=(const cv::Mat& aFrame)
     const bool need_resize = iFrameRgb.cols != frame_out.cols || iFrameRgb.rows != frame_out.rows;
     if (need_resize)
     {
-        cv::resize(iFrameRgb, frame_copy, cv::Size(frame_out.cols, frame_out.rows), 0, 0, cv::INTER_NEAREST);
+        cv::resize(iFrameRgb, iFrameResized, cv::Size(frame_out.cols, frame_out.rows), 0, 0, cv::INTER_NEAREST);
     }        
     else
     {
-        iFrameRgb.copyTo(frame_copy);
+        iFrameRgb.copyTo(iFrameResized);
     }
         
     return *this;
@@ -42,9 +42,9 @@ Preview& Preview::operator=(const cv::Mat& aFrame)
 Preview::Preview(int w, int h)
 {
     ensure_size(frame_out, w, h, CV_8UC4);
-    ensure_size(frame_copy, w, h, CV_8UC3);
+    ensure_size(iFrameResized, w, h, CV_8UC3);
 
-    frame_copy.setTo(cv::Scalar(0, 0, 0));
+    iFrameResized.setTo(cv::Scalar(0, 0, 0));
 }
 
 QImage Preview::get_bitmap()
@@ -54,11 +54,11 @@ QImage Preview::get_bitmap()
     if (stride < 64 || stride < frame_out.cols * 4)
     {
         eval_once(qDebug() << "bad stride" << stride
-                           << "for bitmap size" << frame_copy.cols << frame_copy.rows);
+                           << "for bitmap size" << iFrameResized.cols << iFrameResized.rows);
         return QImage();
     }
 
-    cv::cvtColor(frame_copy, frame_out, cv::COLOR_BGR2BGRA);
+    cv::cvtColor(iFrameResized, frame_out, cv::COLOR_BGR2BGRA);
 
     return QImage((const unsigned char*) frame_out.data,
                   frame_out.cols, frame_out.rows,
@@ -68,18 +68,18 @@ QImage Preview::get_bitmap()
 
 void Preview::draw_head_center(Coordinates::f x, Coordinates::f y)
 {
-    auto [px_, py_] = Coordinates::to_pixel_pos(x, y, frame_copy.cols, frame_copy.rows);
+    auto [px_, py_] = Coordinates::to_pixel_pos(x, y, iFrameResized.cols, iFrameResized.rows);
 
     int px = iround(px_), py = iround(py_);
 
     constexpr int len = 9;
 
     static const cv::Scalar color(0, 255, 255);
-    cv::line(frame_copy,
+    cv::line(iFrameResized,
              cv::Point(px - len, py),
              cv::Point(px + len, py),
              color, 1);
-    cv::line(frame_copy,
+    cv::line(iFrameResized,
              cv::Point(px, py - len),
              cv::Point(px, py + len),
              color, 1);
diff --git a/tracker-easy/frame.hpp b/tracker-easy/frame.hpp
index 2a221de3..ba5e45e3 100644
--- a/tracker-easy/frame.hpp
+++ b/tracker-easy/frame.hpp
@@ -15,13 +15,14 @@ struct Preview
     QImage get_bitmap();
     void draw_head_center(Coordinates::f x, Coordinates::f y);
 
-    operator cv::Mat&() { return frame_copy; }
-    operator cv::Mat const&() const { return frame_copy; }
+    operator cv::Mat&() { return iFrameResized; }
+    operator cv::Mat const&() const { return iFrameResized; }
 
 private:
     static void ensure_size(cv::Mat& frame, int w, int h, int type);
 
-    cv::Mat frame_copy, frame_out;
+public:
+    cv::Mat iFrameResized, frame_out;
     cv::Mat iFrameRgb;
 };
 
diff --git a/tracker-easy/lang/nl_NL.ts b/tracker-easy/lang/nl_NL.ts
index 8f471ea0..86022183 100644
--- a/tracker-easy/lang/nl_NL.ts
+++ b/tracker-easy/lang/nl_NL.ts
@@ -2,6 +2,41 @@
 <!DOCTYPE TS>
 <TS version="2.1" language="nl_NL">
 <context>
+    <name>EasyTrackerDialog</name>
+    <message>
+        <source>Brightness %1/255</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>LED radius %1 pixels</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>%1 yaw samples. Yaw more to %2 samples for stable calibration.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>%1 pitch samples. Pitch more to %2 samples for stable calibration.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>%1 samples. Over %2, good!</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Stop calibration</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Start calibration</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Tracker offline</source>
+        <translation type="unfinished"></translation>
+    </message>
+</context>
+<context>
     <name>UICPTClientControls</name>
     <message>
         <source>PointTracker Settings</source>
@@ -234,41 +269,6 @@ Don&apos;t roll or change position.</source>
     </message>
 </context>
 <context>
-    <name>pt_impl::EasyTrackerDialog</name>
-    <message>
-        <source>Brightness %1/255</source>
-        <translation type="unfinished"></translation>
-    </message>
-    <message>
-        <source>LED radius %1 pixels</source>
-        <translation type="unfinished"></translation>
-    </message>
-    <message>
-        <source>%1 yaw samples. Yaw more to %2 samples for stable calibration.</source>
-        <translation type="unfinished"></translation>
-    </message>
-    <message>
-        <source>%1 pitch samples. Pitch more to %2 samples for stable calibration.</source>
-        <translation type="unfinished"></translation>
-    </message>
-    <message>
-        <source>%1 samples. Over %2, good!</source>
-        <translation type="unfinished"></translation>
-    </message>
-    <message>
-        <source>Stop calibration</source>
-        <translation type="unfinished"></translation>
-    </message>
-    <message>
-        <source>Start calibration</source>
-        <translation type="unfinished"></translation>
-    </message>
-    <message>
-        <source>Tracker offline</source>
-        <translation type="unfinished"></translation>
-    </message>
-</context>
-<context>
     <name>pt_module::metadata_pt</name>
     <message>
         <source>Easy Tracker 0.1</source>
diff --git a/tracker-easy/lang/ru_RU.ts b/tracker-easy/lang/ru_RU.ts
index eb58cdc7..8ed38fac 100644
--- a/tracker-easy/lang/ru_RU.ts
+++ b/tracker-easy/lang/ru_RU.ts
@@ -2,6 +2,41 @@
 <!DOCTYPE TS>
 <TS version="2.1" language="ru_RU">
 <context>
+    <name>EasyTrackerDialog</name>
+    <message>
+        <source>Brightness %1/255</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>LED radius %1 pixels</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>%1 yaw samples. Yaw more to %2 samples for stable calibration.</source>
+        <translation type="unfinished">По оси YAW выполнено: %1 замер(а/ов). Для стабильного результата необходимо не меньше %2</translation>
+    </message>
+    <message>
+        <source>%1 pitch samples. Pitch more to %2 samples for stable calibration.</source>
+        <translation type="unfinished">По оси Pitch выполнено: %1 замер(а/ов). Для стабильного результата необходимо не меньше %2</translation>
+    </message>
+    <message>
+        <source>%1 samples. Over %2, good!</source>
+        <translation type="unfinished">Получено %1 образца(-ов). Больше %2, отлично!!</translation>
+    </message>
+    <message>
+        <source>Stop calibration</source>
+        <translation type="unfinished">Остановить калибровку</translation>
+    </message>
+    <message>
+        <source>Start calibration</source>
+        <translation type="unfinished">Начать калибровку</translation>
+    </message>
+    <message>
+        <source>Tracker offline</source>
+        <translation type="unfinished">Отслеживание отключено</translation>
+    </message>
+</context>
+<context>
     <name>UICPTClientControls</name>
     <message>
         <source>PointTracker Settings</source>
@@ -239,41 +274,6 @@ ROLL или X/Y-смещения.</translation>
     </message>
 </context>
 <context>
-    <name>pt_impl::EasyTrackerDialog</name>
-    <message>
-        <source>Brightness %1/255</source>
-        <translation type="unfinished"></translation>
-    </message>
-    <message>
-        <source>LED radius %1 pixels</source>
-        <translation type="unfinished"></translation>
-    </message>
-    <message>
-        <source>%1 yaw samples. Yaw more to %2 samples for stable calibration.</source>
-        <translation type="unfinished">По оси YAW выполнено: %1 замер(а/ов). Для стабильного результата необходимо не меньше %2</translation>
-    </message>
-    <message>
-        <source>%1 pitch samples. Pitch more to %2 samples for stable calibration.</source>
-        <translation type="unfinished">По оси Pitch выполнено: %1 замер(а/ов). Для стабильного результата необходимо не меньше %2</translation>
-    </message>
-    <message>
-        <source>%1 samples. Over %2, good!</source>
-        <translation type="unfinished">Получено %1 образца(-ов). Больше %2, отлично!!</translation>
-    </message>
-    <message>
-        <source>Stop calibration</source>
-        <translation type="unfinished">Остановить калибровку</translation>
-    </message>
-    <message>
-        <source>Start calibration</source>
-        <translation type="unfinished">Начать калибровку</translation>
-    </message>
-    <message>
-        <source>Tracker offline</source>
-        <translation type="unfinished">Отслеживание отключено</translation>
-    </message>
-</context>
-<context>
     <name>pt_module::metadata_pt</name>
     <message>
         <source>Easy Tracker 0.1</source>
diff --git a/tracker-easy/lang/stub.ts b/tracker-easy/lang/stub.ts
index ced89412..98e2e0f8 100644
--- a/tracker-easy/lang/stub.ts
+++ b/tracker-easy/lang/stub.ts
@@ -2,6 +2,41 @@
 <!DOCTYPE TS>
 <TS version="2.1">
 <context>
+    <name>EasyTrackerDialog</name>
+    <message>
+        <source>Brightness %1/255</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>LED radius %1 pixels</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>%1 yaw samples. Yaw more to %2 samples for stable calibration.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>%1 pitch samples. Pitch more to %2 samples for stable calibration.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>%1 samples. Over %2, good!</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Stop calibration</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Start calibration</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Tracker offline</source>
+        <translation type="unfinished"></translation>
+    </message>
+</context>
+<context>
     <name>UICPTClientControls</name>
     <message>
         <source>PointTracker Settings</source>
@@ -234,41 +269,6 @@ Don&apos;t roll or change position.</source>
     </message>
 </context>
 <context>
-    <name>pt_impl::EasyTrackerDialog</name>
-    <message>
-        <source>Brightness %1/255</source>
-        <translation type="unfinished"></translation>
-    </message>
-    <message>
-        <source>LED radius %1 pixels</source>
-        <translation type="unfinished"></translation>
-    </message>
-    <message>
-        <source>%1 yaw samples. Yaw more to %2 samples for stable calibration.</source>
-        <translation type="unfinished"></translation>
-    </message>
-    <message>
-        <source>%1 pitch samples. Pitch more to %2 samples for stable calibration.</source>
-        <translation type="unfinished"></translation>
-    </message>
-    <message>
-        <source>%1 samples. Over %2, good!</source>
-        <translation type="unfinished"></translation>
-    </message>
-    <message>
-        <source>Stop calibration</source>
-        <translation type="unfinished"></translation>
-    </message>
-    <message>
-        <source>Start calibration</source>
-        <translation type="unfinished"></translation>
-    </message>
-    <message>
-        <source>Tracker offline</source>
-        <translation type="unfinished"></translation>
-    </message>
-</context>
-<context>
     <name>pt_module::metadata_pt</name>
     <message>
         <source>Easy Tracker 0.1</source>
diff --git a/tracker-easy/lang/zh_CN.ts b/tracker-easy/lang/zh_CN.ts
index ff272fdc..c2c3024a 100644
--- a/tracker-easy/lang/zh_CN.ts
+++ b/tracker-easy/lang/zh_CN.ts
@@ -2,6 +2,41 @@
 <!DOCTYPE TS>
 <TS version="2.1" language="zh_CN">
 <context>
+    <name>EasyTrackerDialog</name>
+    <message>
+        <source>Brightness %1/255</source>
+        <translation type="unfinished">亮度 %1/255</translation>
+    </message>
+    <message>
+        <source>LED radius %1 pixels</source>
+        <translation type="unfinished">光源半径 %1 像素</translation>
+    </message>
+    <message>
+        <source>%1 yaw samples. Yaw more to %2 samples for stable calibration.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>%1 pitch samples. Pitch more to %2 samples for stable calibration.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>%1 samples. Over %2, good!</source>
+        <translation type="unfinished">%1 样本。%2 正常</translation>
+    </message>
+    <message>
+        <source>Stop calibration</source>
+        <translation type="unfinished">停止校准</translation>
+    </message>
+    <message>
+        <source>Start calibration</source>
+        <translation type="unfinished">开始校准</translation>
+    </message>
+    <message>
+        <source>Tracker offline</source>
+        <translation type="unfinished">跟踪器脱机</translation>
+    </message>
+</context>
+<context>
     <name>UICPTClientControls</name>
     <message>
         <source>PointTracker Settings</source>
@@ -234,41 +269,6 @@ Don&apos;t roll or change position.</source>
     </message>
 </context>
 <context>
-    <name>pt_impl::EasyTrackerDialog</name>
-    <message>
-        <source>Brightness %1/255</source>
-        <translation type="unfinished">亮度 %1/255</translation>
-    </message>
-    <message>
-        <source>LED radius %1 pixels</source>
-        <translation type="unfinished">光源半径 %1 像素</translation>
-    </message>
-    <message>
-        <source>%1 yaw samples. Yaw more to %2 samples for stable calibration.</source>
-        <translation type="unfinished"></translation>
-    </message>
-    <message>
-        <source>%1 pitch samples. Pitch more to %2 samples for stable calibration.</source>
-        <translation type="unfinished"></translation>
-    </message>
-    <message>
-        <source>%1 samples. Over %2, good!</source>
-        <translation type="unfinished">%1 样本。%2 正常</translation>
-    </message>
-    <message>
-        <source>Stop calibration</source>
-        <translation type="unfinished">停止校准</translation>
-    </message>
-    <message>
-        <source>Start calibration</source>
-        <translation type="unfinished">开始校准</translation>
-    </message>
-    <message>
-        <source>Tracker offline</source>
-        <translation type="unfinished">跟踪器脱机</translation>
-    </message>
-</context>
-<context>
     <name>pt_module::metadata_pt</name>
     <message>
         <source>Easy Tracker 0.1</source>
diff --git a/tracker-easy/module.cpp b/tracker-easy/module.cpp
index 4f6df056..86abffbc 100644
--- a/tracker-easy/module.cpp
+++ b/tracker-easy/module.cpp
@@ -5,11 +5,12 @@
 #include "module.hpp"
 #include "frame.hpp"
 #include "point_extractor.h"
+#include "cv-point-extractor.h"
 
 
 #include <memory>
 
-static const QString module_name = "tracker-pt";
+static const QString module_name = "tracker-easy";
 
 #ifdef __clang__
 #   pragma clang diagnostic ignored "-Wweak-vtables"
@@ -21,7 +22,7 @@ struct pt_module_traits final : pt_runtime_traits
 {
     pointer<pt_point_extractor> make_point_extractor() const override
     {
-        return pointer<pt_point_extractor>(new PointExtractor(module_name));
+        return pointer<pt_point_extractor>(new CvPointExtractor(module_name));
     }
 
     QString get_module_name() const override
@@ -31,14 +32,14 @@ struct pt_module_traits final : pt_runtime_traits
 
 };
 
-struct tracker_pt : Tracker_PT
+struct tracker_pt : EasyTracker
 {
     tracker_pt() : EasyTracker(pointer<pt_runtime_traits>(new pt_module_traits))
     {
     }
 };
 
-struct dialog_pt : TrackerDialog_PT
+struct dialog_pt : EasyTrackerDialog
 {
     dialog_pt();
 };
diff --git a/tracker-easy/tracker-easy-dialog.cpp b/tracker-easy/tracker-easy-dialog.cpp
index b4a1dd30..6f7642ab 100644
--- a/tracker-easy/tracker-easy-dialog.cpp
+++ b/tracker-easy/tracker-easy-dialog.cpp
@@ -20,8 +20,6 @@ using namespace options;
 
 static void init_resources() { Q_INIT_RESOURCE(tracker_easy); }
 
-namespace pt_impl {
-
 EasyTrackerDialog::EasyTrackerDialog(const QString& module_name) :
     s(module_name),
     tracker(nullptr),
@@ -276,5 +274,3 @@ void EasyTrackerDialog::unregister_tracker()
     poll_tracker_info();
     timer.stop();
 }
-
-} // ns pt_impl
diff --git a/tracker-easy/tracker-easy-dialog.h b/tracker-easy/tracker-easy-dialog.h
index 24f1ff84..1f21f92b 100644
--- a/tracker-easy/tracker-easy-dialog.h
+++ b/tracker-easy/tracker-easy-dialog.h
@@ -17,7 +17,6 @@
 #include <QTimer>
 #include <QMutex>
 
-namespace pt_impl {
 
 class EasyTrackerDialog : public ITrackerDialog
 {
@@ -50,6 +49,3 @@ protected:
     Ui::UICPTClientControls ui;
 };
 
-} // ns pt_impl
-
-using TrackerDialog_PT = pt_impl::EasyTrackerDialog;
diff --git a/tracker-easy/tracker-easy.cpp b/tracker-easy/tracker-easy.cpp
index 1783ef51..425979ef 100644
--- a/tracker-easy/tracker-easy.cpp
+++ b/tracker-easy/tracker-easy.cpp
@@ -19,13 +19,13 @@
 #include <QFile>
 #include <QCoreApplication>
 
-#include <opencv2\calib3d.hpp>
+#include <opencv2/calib3d.hpp>
+#include <opencv2/highgui/highgui.hpp>
 
 #include <iostream>
 
 using namespace options;
 
-namespace pt_impl {
 
 EasyTracker::EasyTracker(pointer<pt_runtime_traits> const& traits) :
     traits { traits },
@@ -45,11 +45,14 @@ EasyTracker::EasyTracker(pointer<pt_runtime_traits> const& traits) :
 
 EasyTracker::~EasyTracker()
 {
+    //
+    cv::destroyWindow("Preview");
+
     requestInterruption();
     wait();
 
     QMutexLocker l(&camera_mtx);
-    camera->stop();
+    camera->stop();    
 }
 
 
@@ -120,19 +123,30 @@ void EasyTracker::run()
             //TODO: We should not assume channel size of 1 byte
             iMatFrame = cv::Mat(iFrame.height, iFrame.width, CV_MAKETYPE(CV_8U,iFrame.channels), iFrame.data, iFrame.stride);
 
-            const bool preview_visible = check_is_visible();
 
+            const bool preview_visible = check_is_visible();
             if (preview_visible)
             {
-                iPreview = iMatFrame;                
+                iPreview = iMatFrame;
             }
-                
 
             iImagePoints.clear();
-            point_extractor->extract_points(iMatFrame, iPreview, points, iImagePoints);
+            point_extractor->extract_points(iMatFrame, iPreview.iFrameRgb, points, iImagePoints);
             point_count.store(points.size(), std::memory_order_relaxed);
 
-            const bool success = points.size() >= KPointCount;
+            
+            if (preview_visible)
+            {
+                //iPreview = iMatFrame;
+                cv::imshow("Preview", iPreview.iFrameRgb);
+                cv::waitKey(1);
+            }
+            else
+            {
+                cv::destroyWindow("Preview");
+            }
+
+            const bool success = points.size() >= KPointCount || iImagePoints.size() >= KPointCount;
 
             int topPointIndex = -1;
 
@@ -290,7 +304,10 @@ void EasyTracker::run()
                 if (topPointIndex != -1)
                 {
                     // Render a cross to indicate which point is the head
-                    iPreview.draw_head_center(points[topPointIndex][0], points[topPointIndex][1]);
+                    if (points.size() >= 3)
+                    {
+                        iPreview.draw_head_center(points[topPointIndex][0], points[topPointIndex][1]);
+                    }                    
                 }
                 
                 widget->update_image(iPreview.get_bitmap());
@@ -367,6 +384,3 @@ int EasyTracker::get_n_points()
     return (int)point_count.load(std::memory_order_relaxed);
 }
 
-
-
-} // ns pt_impl
diff --git a/tracker-easy/tracker-easy.h b/tracker-easy/tracker-easy.h
index 03e603bd..6dea5712 100644
--- a/tracker-easy/tracker-easy.h
+++ b/tracker-easy/tracker-easy.h
@@ -25,7 +25,6 @@
 #include <QMutex>
 #include <QLayout>
 
-namespace pt_impl {
 
 class EasyTrackerDialog;
 
@@ -90,7 +89,3 @@ private:
     // Best angles
     cv::Vec3d iBestAngles;
 };
-
-} // ns pt_impl
-
-using Tracker_PT = pt_impl::EasyTracker;