Merge branch 'unstable' into trackhattrackhat-1.2p1

30 files changed, 395 insertions, 164 deletions

diff --git a/cmake/mingw-w64.cmake b/cmake/mingw-w64.cmake
index 679f67de..7d097422 100644
--- a/cmake/mingw-w64.cmake
+++ b/cmake/mingw-w64.cmake
@@ -7,15 +7,19 @@ SET(CMAKE_SYSTEM_NAME Windows)
 SET(CMAKE_SYSTEM_VERSION 1)
 
 # specify the cross compiler
-set(c /c/mingw-w64/i686-5.2.0-posix-sjlj-rt_v4-rev0/mingw32/bin/i686-w64-mingw32-)
-
-SET(CMAKE_C_COMPILER    ${c}gcc)
-SET(CMAKE_CXX_COMPILER  ${c}g++)
-set(CMAKE_RC_COMPILER   ${c}windres)
-set(CMAKE_LINKER        ${c}ld)
-set(CMAKE_AR            ${c}gcc-ar      CACHE STRING "" FORCE)
-set(CMAKE_NM            ${c}gcc-nm      CACHE STRING "" FORCE)
-set(CMAKE_RANLIB        ${c}gcc-ranlib  CACHE STRING "" FORCE)
+set(p c:/mingw-w64/i686-5.3.0-posix-dwarf-rt_v4-rev0/mingw32/bin)
+set(c ${p}/i686-w64-mingw32-)
+set(CMAKE_MAKE_PROGRAM ${p}/mingw32-make.exe CACHE FILEPATH "" FORCE)
+
+set(e .exe)
+
+SET(CMAKE_C_COMPILER    ${c}gcc${e})
+SET(CMAKE_CXX_COMPILER  ${c}g++${e})
+set(CMAKE_RC_COMPILER   ${c}windres${e})
+set(CMAKE_LINKER        ${c}ld${e})
+set(CMAKE_AR            ${c}gcc-ar${e}      CACHE STRING "" FORCE)
+set(CMAKE_NM            ${c}gcc-nm${e}      CACHE STRING "" FORCE)
+set(CMAKE_RANLIB        ${c}gcc-ranlib${e}  CACHE STRING "" FORCE)
 
 SET(CMAKE_FIND_ROOT_PATH /usr/i686-w64-mingw32)
 
diff --git a/cmake/opentrack-qt.cmake b/cmake/opentrack-qt.cmake
index 12966827..65ec75fd 100644
--- a/cmake/opentrack-qt.cmake
+++ b/cmake/opentrack-qt.cmake
@@ -1,5 +1,5 @@
-find_package(Qt5 REQUIRED COMPONENTS Core Xml Network Widgets Gui QUIET)
+find_package(Qt5 REQUIRED COMPONENTS Core Network Widgets Gui QUIET)
 find_package(Qt5 COMPONENTS SerialPort QUIET)
-include_directories(SYSTEM ${Qt5Core_INCLUDE_DIRS} ${Qt5Xml_INCLUDE_DIRS} ${Qt5Gui_INCLUDE_DIRS} ${Qt5Widgets_INCLUDE_DIRS} ${Qt5Network_INCLUDE_DIRS})
-add_definitions(${Qt5Core_DEFINITIONS} ${Qt5Xml_DEFINITIONS} ${Qt5Gui_DEFINITIONS} ${Qt5Widgets_DEFINITIONS} ${Qt5Network_DEFINITIONS})
-set(MY_QT_LIBS ${Qt5Core_LIBRARIES} ${Qt5Gui_LIBRARIES} ${Qt5Widgets_LIBRARIES} ${Qt5Network_LIBRARIES} ${Qt5Xml_LIBRARIES})
+include_directories(SYSTEM ${Qt5Core_INCLUDE_DIRS} ${Qt5Gui_INCLUDE_DIRS} ${Qt5Widgets_INCLUDE_DIRS} ${Qt5Network_INCLUDE_DIRS})
+add_definitions(${Qt5Core_DEFINITIONS} ${Qt5Gui_DEFINITIONS} ${Qt5Widgets_DEFINITIONS} ${Qt5Network_DEFINITIONS})
+set(MY_QT_LIBS ${Qt5Core_LIBRARIES} ${Qt5Gui_LIBRARIES} ${Qt5Widgets_LIBRARIES} ${Qt5Network_LIBRARIES})
diff --git a/cmake/opentrack-tarball.cmake b/cmake/opentrack-tarball.cmake
index f26052f3..1f292af7 100644
--- a/cmake/opentrack-tarball.cmake
+++ b/cmake/opentrack-tarball.cmake
@@ -20,7 +20,7 @@ add_custom_target(tarball-real)
 add_custom_target(tarball-real2)
 add_custom_command(TARGET tarball-real COMMAND cmake -P ${CMAKE_SOURCE_DIR}/cmake/tarball.cmake)
 
-add_custom_command(TARGET tarball-real2 COMMAND /usr/bin/env sh
+add_custom_command(TARGET tarball-real2 COMMAND sh
     "${CMAKE_SOURCE_DIR}/make-tar.sh" "${CMAKE_INSTALL_PREFIX}"
     "${filename}" "${CMAKE_BINARY_DIR}")
 add_custom_target(tarball DEPENDS tarball-real)
diff --git a/gui/images/no-feed.png b/gui/images/no-feed.png
index 02aa227a..80c68f16 100644
--- a/gui/images/no-feed.png
+++ b/gui/images/no-feed.png
diff --git a/gui/settings.ui b/gui/settings.ui
index cb292a35..b00e0960 100644
--- a/gui/settings.ui
+++ b/gui/settings.ui
@@ -76,7 +76,7 @@
              </sizepolicy>
             </property>
             <property name="text">
-             <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;&lt;span style=&quot; font-weight:600;&quot;&gt;Center&lt;/span&gt; - use current pose as looking perfectly forward.&lt;br/&gt;&lt;span style=&quot; font-weight:600;&quot;&gt;Toggle&lt;/span&gt; - keep looking at same spot until toggle keypress.&lt;br/&gt;&lt;span style=&quot; font-weight:600;&quot;&gt;Zero&lt;/span&gt; - keep looking forward while the key is pressed.&lt;br/&gt;&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+             <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;&lt;span style=&quot; font-weight:600;&quot;&gt;Center&lt;/span&gt; - use current pose as looking perfectly forward.&lt;br/&gt;&lt;span style=&quot; font-weight:600;&quot;&gt;Toggle&lt;/span&gt; - keep looking at same spot until next toggle keypress.&lt;br/&gt;&lt;span style=&quot; font-weight:600;&quot;&gt;Zero&lt;/span&gt; - keep looking forward until next zero keypress.&lt;br/&gt;&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
             </property>
             <property name="wordWrap">
              <bool>true</bool>
diff --git a/make-tar.sh b/make-tar.sh
index abb83fdd..838300fc 100644
--- a/make-tar.sh
+++ b/make-tar.sh
@@ -6,8 +6,6 @@ bin="$3"
 
 branch="$(git rev-parse --abbrev-ref HEAD)"
 
-cmake --build "$bin" --target install -- -j4 || exit 1
-
 if : &&
     cd $(dirname -- "${prefix}") &&
     zip -9r "${filename}" $(basename "${prefix}")
diff --git a/settings/facetracknoir supported games.csv b/settings/facetracknoir supported games.csv
index 55b38a47..f460c516 100644
--- a/settings/facetracknoir supported games.csv
+++ b/settings/facetracknoir supported games.csv
@@ -24,6 +24,7 @@ No;Game Name;Game protocol;Supported since;Verified;By;INTERNATIONAL_ID;FTN_ID
 17;AirBook;FreeTrack20;V160;;;15201;0011D615A9C8430A201A00
 18;America's Army;FreeTrack20;V160;;;4101;00120BCF5753D98399E200
 19;America's Army 3;FreeTrack20;V170;;;14203;00133F3206BCEA252BB700
+579;American Truck Simulator;FreeTrack20;V160;;;13603;0243F6F5140400655ED300
 21;Apache: Air Assault;FreeTrack20;V160;V;paleta77;1875;001591D997A2D912AAA200
 519;Apex Motorsports;FreeTrack20;V160;;;3675;02078F3159271BBE676800
 22;Aprisoft Gartenplaner;FreeTrack20;V160;;;20014;0016F2F27A5762FA937A00
@@ -160,6 +161,7 @@ No;Game Name;Game protocol;Supported since;Verified;By;INTERNATIONAL_ID;FTN_ID
 552;FSX Steam Edition;FreeTrack20;V160;;;3852;02287F45F0FFBEF6102900
 147;FTAlpha;FreeTrack20;V160;;;20270;00936C69677F1D772AA900
 128;Farmer Simulator 2009;FreeTrack20;V160;;;1725;0080926ED8A1EB628AB300
+578;Farming Simulator 17;FreeTrack20;V170;;;1726;06BEF1CEFF524246DF1900
 129;Faros Driving Simulator;FreeTrack20;V160;;;8001;0081B1C6647A1EA839A800
 130;Faubert Lab Car Simulator;FreeTrack20;V160;;;20480;00822E542A7F2DB61F9700
 131;Ferrari Virtual Academy;FreeTrack20;V160;V;sosna1983 ;0;0083C0AF8C24B62D974000
@@ -300,6 +302,7 @@ No;Game Name;Game protocol;Supported since;Verified;By;INTERNATIONAL_ID;FTN_ID
 241;Mig Alley;FreeTrack20;V160;;;1501;00F17686E5A83289CDA700
 242;Mimik Vehicle Simulator;FreeTrack20;V160;;;20585;00F2AB1D58A8E35A531A00
 243;Miner Wars;FreeTrack20;V160;;;2550;00F3C5BEE48124A234A400
+580;Miscreated;FreeTrack20;V160;;;4750;0244EF8BBFE09A54D5B600
 245;ModelSim;FreeTrack20;V160;;;20540;00F544958F3A9A1FB11A00
 246;Morgan State University;FreeTrack20;V160;;;20010;00F65909437D3CA5339A00
 247;Motor Company;FreeTrack20;V160;;;11501;00F70288F37A629A7DD700
@@ -550,6 +553,7 @@ No;Game Name;Game protocol;Supported since;Verified;By;INTERNATIONAL_ID;FTN_ID
 459;X-Plane (32 and 64 bit);FreeTrack20;V160;V;EmBeES ;7701;01CB3FD0FC9D90FB943300
 460;X-Plane 6DOF Plugin;FreeTrack20;V160;;;10101;01CCA15F28AF7963596300
 461;X-Plane Pilot View (32 and 64 bit);FreeTrack20;V160;V;V4Friend;11301;01CD79EC0E28EFA953CA00
+581;X-Rebirth;FreeTrack20;V170;;;5503;157FD520BEAA889EC41800
 462;X-Tower for X-Plane;FreeTrack20;V160;;;3001;01CE05B390EA872AAF3200
 455;X1Alpha;FreeTrack20;V160;;;1825;01C76D2D1038CFD7135A00
 456;X2: The Threat;FreeTrack20;V160;;;5501;01C81039EF980EC8D30900
diff --git a/tracker-aruco/ar_video_widget.h b/tracker-aruco/ar_video_widget.h
index 6df2d626..ea14dfbf 100644
--- a/tracker-aruco/ar_video_widget.h
+++ b/tracker-aruco/ar_video_widget.h
@@ -13,7 +13,9 @@
 #include <QMutexLocker>
 #include <QPainter>
 #include <QPaintEvent>
-#include <opencv/cv.hpp>
+#include <opencv2/core.hpp>
+#include <opencv2/imgproc.hpp>
+#include <opencv2/calib3d.hpp>
 
 class ArucoVideoWidget : public QWidget
 {
diff --git a/tracker-pt/ftnoir_tracker_pt.h b/tracker-pt/ftnoir_tracker_pt.h
index dff0c30a..5f30c66f 100644
--- a/tracker-pt/ftnoir_tracker_pt.h
+++ b/tracker-pt/ftnoir_tracker_pt.h
@@ -41,7 +41,7 @@ public:
     void data(double* data) override;
 
     Affine pose() { return point_tracker.pose(); }
-    int  get_n_points() { return point_extractor.get_points().size(); }
+    int  get_n_points() { return point_extractor.get_n_points(); }
     bool get_cam_info(CamInfo* info) { QMutexLocker lock(&camera_mtx); return camera.get_info(*info); }
 public slots:
     void apply_settings();
diff --git a/tracker-pt/point_extractor.cpp b/tracker-pt/point_extractor.cpp
index 0208b11d..a1294c1e 100644
--- a/tracker-pt/point_extractor.cpp
+++ b/tracker-pt/point_extractor.cpp
@@ -15,6 +15,8 @@
 
 PointExtractor::PointExtractor()
 {
+    blobs.reserve(max_blobs);
+    points.reserve(max_blobs);
 }
 
 const std::vector<cv::Vec2f>& PointExtractor::extract_points(cv::Mat& frame)
@@ -34,30 +36,10 @@ const std::vector<cv::Vec2f>& PointExtractor::extract_points(cv::Mat& frame)
     const double region_size_min = s.min_point_size;
     const double region_size_max = s.max_point_size;
     
-    struct blob
-    {
-        double radius;
-        cv::Vec2d pos;
-        double confid;
-        bool taken;
-        double area;
-        blob(double radius, const cv::Vec2d& pos, double confid, double area) : radius(radius), pos(pos), confid(confid), taken(false), area(area)
-        {
-            //qDebug() << "radius" << radius << "pos" << pos[0] << pos[1] << "confid" << confid;
-        }
-        bool inside(const blob& other)
-        {
-            cv::Vec2d tmp = pos - other.pos;
-            return sqrt(tmp.dot(tmp)) < radius;
-        }
-    };
-    
-    // mask for everything that passes the threshold (or: the upper threshold of the hysteresis)
-    
-    std::vector<blob> blobs;
-    std::vector<std::vector<cv::Point>> contours;
-
     const int thres = s.threshold;
+
+    contours.clear();
+
     if (!s.auto_threshold)
     {
         cv::threshold(frame_gray, frame_bin, thres, 255, cv::THRESH_BINARY);
@@ -95,17 +77,11 @@ const std::vector<cv::Vec2f>& PointExtractor::extract_points(cv::Mat& frame)
         cv::findContours(frame_bin, contours, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE);
     }
 
-    int cnt = 0;
+    blobs.clear();
 
     for (auto& c : contours)
     {
-        if (cnt++ > 30)
-            break;
-
         const auto m = cv::moments(cv::Mat(c));
-        const double area = m.m00;
-        if (area == 0.)
-            continue;
         const cv::Vec2d pos(m.m10 / m.m00, m.m01 / m.m00);
 
         double radius;
@@ -147,9 +123,7 @@ const std::vector<cv::Vec2f>& PointExtractor::extract_points(cv::Mat& frame)
             cv::putText(frame, buf, cv::Point(pos[0]+30, pos[1]+20), cv::FONT_HERSHEY_DUPLEX, 1, cv::Scalar(0, 0, 255), 1);
         }
 
-        blobs.push_back(blob(radius, pos, confid, area));
-        
-        enum { max_blobs = 16 };
+        blobs.push_back(blob(radius, pos, confid));
         
         if (blobs.size() == max_blobs)
             break;
@@ -158,10 +132,7 @@ const std::vector<cv::Vec2f>& PointExtractor::extract_points(cv::Mat& frame)
     using b = const blob;
     std::sort(blobs.begin(), blobs.end(), [](b& b1, b& b2) {return b1.confid > b2.confid;});
     
-    points.reserve(blobs.size());
-    
     QMutexLocker l(&mtx);
-    
     points.clear();
     
     for (auto& b : blobs)
diff --git a/tracker-pt/point_extractor.h b/tracker-pt/point_extractor.h
index 030251ff..3e4661f9 100644
--- a/tracker-pt/point_extractor.h
+++ b/tracker-pt/point_extractor.h
@@ -12,9 +12,10 @@
 #include <opencv2/imgproc/imgproc.hpp>
 
 #include "ftnoir_tracker_pt_settings.h"
-
 #include <QMutex>
 
+#include <vector>
+
 class PointExtractor
 {
 public:
@@ -22,7 +23,7 @@ public:
     // 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);
-    const std::vector<cv::Vec2f>& get_points() { QMutexLocker l(&mtx); return points; }
+    int get_n_points() { QMutexLocker l(&mtx); return points.size(); }
     PointExtractor();
     
     settings_pt s;
@@ -33,6 +34,22 @@ private:
     cv::Mat frame_gray;
     cv::Mat frame_bin;
     cv::Mat hist;
+
+    enum { max_blobs = 16 };
+
+    struct blob
+    {
+        double radius;
+        cv::Vec2d pos;
+        double confid;
+        blob(double radius, const cv::Vec2d& pos, double confid) : radius(radius), pos(pos), confid(confid)
+        {
+            //qDebug() << "radius" << radius << "pos" << pos[0] << pos[1] << "confid" << confid;
+        }
+    };
+
+    std::vector<blob> blobs;
+    std::vector<std::vector<cv::Point>> contours;
 };
 
 #endif //POINTEXTRACTOR_H
diff --git a/tracker-pt/point_tracker.cpp b/tracker-pt/point_tracker.cpp
index aa6feb5b..25240635 100644
--- a/tracker-pt/point_tracker.cpp
+++ b/tracker-pt/point_tracker.cpp
@@ -194,7 +194,7 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order_, float
         float IJ0 = I0.dot(J0);
         float JJ0 = J0.dot(J0);
         float rho, theta;
-        if (JJ0 == II0) {
+        if (std::abs(JJ0 - II0) < 1e-6f) {
             rho = std::sqrt(std::abs(2*IJ0));
             theta = -PI/4;
             if (IJ0<0) theta *= -1;
@@ -202,7 +202,8 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order_, float
         else {
             rho = sqrt(sqrt( (JJ0-II0)*(JJ0-II0) + 4*IJ0*IJ0 ));
             theta = atan( -2*IJ0 / (JJ0-II0) );
-            if (JJ0 - II0 < 0) theta += PI;
+            // avoid branch misprediction
+            theta += (JJ0 - II0 < 0) * PI;
             theta /= 2;
         }
 
diff --git a/tracker-rs/CMakeLists.txt b/tracker-rs/CMakeLists.txt
index 901d68bb..9219aafd 100644
--- a/tracker-rs/CMakeLists.txt
+++ b/tracker-rs/CMakeLists.txt
@@ -1,5 +1,6 @@
 if(WIN32)
     opentrack_boilerplate(opentrack-tracker-rs)
-    install(FILES "${CMAKE_SOURCE_DIR}/tracker-rs/rs_impl/bin/opentrack-tracker-rs-impl.exe" DESTINATION . ${opentrack-perms})
-    install(FILES "${CMAKE_SOURCE_DIR}/tracker-rs/redist/intel_rs_sdk_runtime_websetup_7.0.23.8048.exe" DESTINATION ./contrib/ ${opentrack-perms})
+    target_link_libraries(opentrack-tracker-rs "${CMAKE_SOURCE_DIR}/tracker-rs/rs_impl/bin/opentrack-tracker-rs-impl.lib")
+    install(FILES "${CMAKE_SOURCE_DIR}/tracker-rs/rs_impl/bin/opentrack-tracker-rs-impl.dll" DESTINATION . ${opentrack-perms})
+    install(FILES "${CMAKE_SOURCE_DIR}/tracker-rs/redist/intel_rs_sdk_runtime_websetup_8.0.24.6528.exe" DESTINATION ./contrib/ ${opentrack-perms})
 endif()
diff --git a/tracker-rs/ftnoir_tracker_rs.cpp b/tracker-rs/ftnoir_tracker_rs.cpp
index e073fa3f..99c5e9d0 100644
--- a/tracker-rs/ftnoir_tracker_rs.cpp
+++ b/tracker-rs/ftnoir_tracker_rs.cpp
@@ -7,97 +7,118 @@
 
 #include "ftnoir_tracker_rs.h"
 #include "ftnoir_tracker_rs_controls.h"
-
+#include "imagewidget.h"
 #include "opentrack/plugin-api.hpp"
 #include <QMessageBox>
+#include <QProcess>
+#include <QStackedLayout>
+
+RSTracker::RSTracker() {
+    connect(&mTrackerWorkerThread, &RSTrackerWorkerThread::trackingHasFinished,
+            this, &RSTracker::handleTrackingEnded);
 
-RSTracker::RSTracker() : mPose{ 0,0,0, 0,0,0 } {
-    mThread.setObjectName("RSTrackerWorkerThread");
+    connect(&mPreviewUpdateTimer, &QTimer::timeout,
+            this, &RSTracker::updatePreview);
+}
 
-    mRealSenseImplProcess.moveToThread(&mThread);
-    mSocket.moveToThread(&mThread);
+void RSTracker::configurePreviewFrame()
+{
+    if(mImageWidget!=nullptr || mPreviewFrame==nullptr)
+        return;
 
-    connect(&mRealSenseImplProcess, SIGNAL(finished(int)),
-            this, SLOT(rsImplProcessFinished(int)), Qt::QueuedConnection);
+    mPreviewFrame->show();
 
-    qRegisterMetaType<QProcess::ProcessError>("QProcess::ProcessError");
-    connect(&mRealSenseImplProcess, SIGNAL(error(QProcess::ProcessError)),
-            this, SLOT(rsImplProcessError(QProcess::ProcessError)), Qt::QueuedConnection);
+    mImageWidget = new ImageWidget(mPreviewFrame);
+    mImageWidget->setSizePolicy(QSizePolicy::Ignored, QSizePolicy::Ignored);
 
-    connect(&mSocket, SIGNAL(readyRead()),
-            this, SLOT(readPendingUdpPoseData()), Qt::DirectConnection);
+    if(mPreviewFrame->layout() != nullptr){
+        delete mPreviewFrame->layout();
+    }
 
-    connect(&mThread, &QThread::started,
-            &mThread, [this]{
-        mSocket.bind(QHostAddress::LocalHost, 4242, QUdpSocket::DontShareAddress);
-        mRealSenseImplProcess.start("opentrack-tracker-rs-impl.exe", QProcess::NotOpen);
-    }, Qt::DirectConnection);
+    QLayout* layout = new QStackedLayout();
+    mPreviewFrame->setLayout(layout);
+    layout->addWidget(mImageWidget);
 
-    connect(&mThread, &QThread::finished,
-            &mThread, [this]{
-        mRealSenseImplProcess.kill();
-        mRealSenseImplProcess.waitForFinished();
-    }, Qt::DirectConnection);
+    mImageWidget->show();
 }
 
-void RSTracker::start_tracker(QFrame*)
+void RSTracker::start_tracker(QFrame* previewFrame)
 {
-    mThread.start();
+    mPreviewFrame = previewFrame;
+
+    configurePreviewFrame();
+
+    startPreview();
+
+    mTrackerWorkerThread.start(QThread::HighPriority);
+}
+
+void RSTracker::startPreview(){
+    mPreviewUpdateTimer.start(kPreviewUpdateInterval);
 }
 
-void RSTracker::readPendingUdpPoseData(){
-    double pose[6];
+void RSTracker::updatePreview(){
+    if(mImageWidget!=nullptr && mImageWidget->isEnabled())
+        mImageWidget->setImage(mTrackerWorkerThread.getPreview());
+}
 
-    while(mSocket.hasPendingDatagrams()) {
-        mSocket.readDatagram((char*)pose, sizeof(pose));
-        QMutexLocker foo(&mMutex);
-        memcpy(mPose, pose, sizeof(pose));
+void RSTracker::stopPreview(){
+    mPreviewUpdateTimer.stop();
+}
+
+void RSTracker::handleTrackingEnded(int exitCode){
+    stopPreview();
+
+    if(exitCode!=0)
+        showRealSenseErrorMessageBox(exitCode);
+}
+
+bool RSTracker::startSdkInstallationProcess()
+{
+    bool pStarted = QProcess::startDetached("contrib\\intel_rs_sdk_runtime_websetup_8.0.24.6528.exe --finstall=core,face3d --fnone=all");
+    if(!pStarted){
+        QMessageBox::warning(0, "Intel® RealSense™ Runtime Installation", "Installation process failed to start.", QMessageBox::Ok);
     }
+    return pStarted;
 }
 
-void RSTracker::rsImplProcessError(QProcess::ProcessError error){
-    if(error == QProcess::FailedToStart){
-        QMessageBox::warning(NULL, "RealSense Tracking Error", "Couldn't start the RealSense tracking module.\nMaybe opentrack-tracker-rs-impl.exe is missing.", QMessageBox::Ok);
+void RSTracker::showRealSenseErrorMessageBox(int exitCode)
+{
+    QMessageBox msgBox;
+    msgBox.setIcon(QMessageBox::Critical);
+    msgBox.setText("RealSense Tracking Error");
+    if(exitCode==-101){ //The implementation got an invalid handle from the RealSense SDK session/modules
+        msgBox.setInformativeText("Couldn't initialize RealSense tracking. Please install SDK Runtime R5.");
     }
-    else if(error == QProcess::Crashed){
-        QMessageBox::warning(NULL, "RealSense Tracking Error", "The RealSense tracking module has crashed.", QMessageBox::Ok);
+    else {
+        msgBox.setInformativeText("Status code: " + QString::number(exitCode) + ".\n\nNote that you need the latest camera drivers and the SDK runtime 2016 R1 to be installed.");
     }
-}
 
+    QPushButton* triggerSdkInstallation = msgBox.addButton("Install Runtime", QMessageBox::ActionRole);
+    msgBox.addButton(QMessageBox::Ok);
+    msgBox.exec();
 
-void RSTracker::rsImplProcessFinished(int exitCode){
-    if(exitCode!=0){
-        QMessageBox msgBox;
-        msgBox.setIcon(QMessageBox::Critical);
-        msgBox.setText("RealSense Tracking Error");
-        if(exitCode==-101){ //The implementation got an invalid handle from the RealSense SDK session/modules
-            msgBox.setInformativeText("Couldn't initialize RealSense tracking. Please install SDK Runtime R5.");
-        }
-        else {
-            msgBox.setInformativeText("Status code: " + QString::number(exitCode) + ".\n\nNote that you need the latest camera drivers and the SDK runtime R5 to be installed.");
-        }
-        QPushButton* triggerSdkInstallation = msgBox.addButton("Install Runtime", QMessageBox::ActionRole);
-        msgBox.addButton(QMessageBox::Ok);
-        msgBox.exec();
-
-        if(msgBox.clickedButton() == triggerSdkInstallation){
-            bool pStarted = QProcess::startDetached("contrib\\intel_rs_sdk_runtime_websetup_7.0.23.8048.exe --finstall=core,face3d --fnone=all");
-            if(!pStarted){
-                QMessageBox::warning(0, "Intel® RealSense™ Runtime Installation", "Installation process failed to start.", QMessageBox::Ok);
-            }
-        }
-    }
+    if(msgBox.clickedButton() == triggerSdkInstallation)
+        startSdkInstallationProcess();
 }
 
 void RSTracker::data(double *data)
 {
-    QMutexLocker foo(&mMutex);
-    memcpy(data, mPose, sizeof(mPose));
+    mTrackerWorkerThread.getPose(data);
 }
 
 RSTracker::~RSTracker() {
-    mThread.quit();
-    mThread.wait();
+    stopPreview();
+
+    if(mImageWidget!=nullptr)
+        delete mImageWidget;
+
+    if (mPreviewFrame!=nullptr && mPreviewFrame->layout()!=nullptr)
+        delete mPreviewFrame->layout();
+
+    mTrackerWorkerThread.requestInterruption();
+    mTrackerWorkerThread.quit();
+    mTrackerWorkerThread.wait();
 }
 
 QString RSTrackerMetaData::name() {
diff --git a/tracker-rs/ftnoir_tracker_rs.h b/tracker-rs/ftnoir_tracker_rs.h
index 7cda7370..845e0beb 100644
--- a/tracker-rs/ftnoir_tracker_rs.h
+++ b/tracker-rs/ftnoir_tracker_rs.h
@@ -7,34 +7,42 @@
  
 #pragma once
 
-#include "ui_ftnoir_tracker_rs_controls.h"
 #include "opentrack/plugin-api.hpp"
-#include <QProcess>
-#include <QMutex>
-#include <QThread>
-#include <QUdpSocket>
+#include "ui_ftnoir_tracker_rs_controls.h"
+#include  "ftnoir_tracker_rs_worker.h"
+#include <QTimer>
+
+class ImageWidget;
 
-class RSTracker : protected QObject, public ITracker
+class RSTracker : public QObject, public ITracker
 {
     Q_OBJECT
 
 public:
     RSTracker();
     ~RSTracker();
-	void start_tracker(QFrame *) override;
-	void data(double *data) override;
+    void start_tracker(QFrame *) override;
+    void data(double *data) override;
+
+public slots:
+    static bool startSdkInstallationProcess();
+
+protected:
+    void configurePreviewFrame();
 
 private:
-    QMutex mMutex;
-    QThread mThread;
-    QProcess mRealSenseImplProcess;
-    QUdpSocket mSocket;
-    double mPose[6];
+    RSTrackerWorkerThread mTrackerWorkerThread;
+    QTimer mPreviewUpdateTimer;
+    QWidget *mPreviewFrame;
+    ImageWidget *mImageWidget = nullptr;
+    const int kPreviewUpdateInterval = 30;
 
 private slots:
-    void rsImplProcessError(QProcess::ProcessError);
-    void rsImplProcessFinished(int);
-    void readPendingUdpPoseData();
+    void showRealSenseErrorMessageBox(int exitCode);
+    void startPreview();
+    void updatePreview();
+    void stopPreview();
+    void handleTrackingEnded(int exitCode);
 
 };
 
diff --git a/tracker-rs/ftnoir_tracker_rs_controls.cpp b/tracker-rs/ftnoir_tracker_rs_controls.cpp
index be18b3f8..baa01c91 100644
--- a/tracker-rs/ftnoir_tracker_rs_controls.cpp
+++ b/tracker-rs/ftnoir_tracker_rs_controls.cpp
@@ -7,9 +7,6 @@
 
 #include "ftnoir_tracker_rs_controls.h"
 
-#include <QProcess>
-#include <QMessageBox>
-
 RSTrackerControls::RSTrackerControls()
 {
     ui.setupUi(this);
@@ -20,13 +17,9 @@ RSTrackerControls::RSTrackerControls()
 
 void RSTrackerControls::doInstallRSRuntime()
 {
-    bool processStarted = QProcess::startDetached("contrib\\intel_rs_sdk_runtime_websetup_7.0.23.8048.exe --finstall=core,face3d --fnone=all");
-    if(processStarted){
+    bool pStarted = RSTracker::startSdkInstallationProcess();
+    if(pStarted == true)
         this->close();
-    }
-    else{
-        QMessageBox::warning(0, "Intel® RealSenseTM Runtime Installation", "Installation process failed to start.", QMessageBox::Ok);
-    }
 }
 
 void RSTrackerControls::doOK()
diff --git a/tracker-rs/ftnoir_tracker_rs_controls.h b/tracker-rs/ftnoir_tracker_rs_controls.h
index 010dac99..c7f1e7eb 100644
--- a/tracker-rs/ftnoir_tracker_rs_controls.h
+++ b/tracker-rs/ftnoir_tracker_rs_controls.h
@@ -16,7 +16,7 @@ public:
     void unregister_tracker() {}
 private:
     Ui::UIRSControls ui;
-    private slots:
+private slots:
     void doOK();
     void doCancel();
     void doInstallRSRuntime();
diff --git a/tracker-rs/ftnoir_tracker_rs_controls.ui b/tracker-rs/ftnoir_tracker_rs_controls.ui
index 023e14d3..30d06956 100644
--- a/tracker-rs/ftnoir_tracker_rs_controls.ui
+++ b/tracker-rs/ftnoir_tracker_rs_controls.ui
@@ -9,8 +9,8 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>378</width>
-    <height>193</height>
+    <width>484</width>
+    <height>244</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -36,15 +36,15 @@ The application will activate your camera for face tracking by the
 Intel® RealSense™ SDK. By design, the application has no direct access
 to any camera images.
 
-In order to use this tracker, you need a PC equipped with
-an Intel® RealSense™ R200 camera and the RealSense™ SDK R5 runtime.</string>
+In order to use this tracker, you need a PC equipped with an Intel® RealSense™ 
+F200 or SR300 camera and the RealSense™ SDK 2016 R1 runtime.</string>
      </property>
     </widget>
    </item>
    <item alignment="Qt::AlignHCenter|Qt::AlignVCenter">
     <widget class="QPushButton" name="triggerSDKInstallButton">
      <property name="text">
-      <string>Install SDK Runtime R5</string>
+      <string>Install Runtime</string>
      </property>
     </widget>
    </item>
diff --git a/tracker-rs/ftnoir_tracker_rs_worker.cpp b/tracker-rs/ftnoir_tracker_rs_worker.cpp
new file mode 100644
index 00000000..c0ea6749
--- /dev/null
+++ b/tracker-rs/ftnoir_tracker_rs_worker.cpp
@@ -0,0 +1,57 @@
+/*
+ * Copyright (c) 2015, Intel Corporation
+ *   Author: Xavier Hallade <xavier.hallade@intel.com>
+ * 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.
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include "ftnoir_tracker_rs_worker.h"
+#include "rs_impl/ftnoir_tracker_rs_impl.h"
+#include <QImage>
+
+RSTrackerWorkerThread::RSTrackerWorkerThread(): mPose{0,0,0,0,0,0}{
+    setObjectName("RSTrackerWorkerThread");
+    mPreviewRawData = (uchar*)malloc(1*kPreviewStreamWidth*kPreviewStreamHeight);
+    memset(mPreviewRawData, 125, kPreviewStreamWidth*kPreviewStreamHeight); //start with a gray image.
+}
+
+void RSTrackerWorkerThread::run(){
+    double pose[6];
+    int retValue;
+
+    retValue = rs_tracker_impl_start();
+    if(retValue==0)
+        emit trackingHasStarted();
+    else {
+        emit trackingHasFinished(retValue);
+        return;
+    }
+
+    while(!isInterruptionRequested()){
+        retValue = rs_tracker_impl_update_pose(pose);
+        if(retValue == 0){ // success
+            QMutexLocker lock(&mMutex);
+            memcpy(mPose, pose, sizeof(pose));
+        }
+        else if(retValue != -303){ // pose update failed and not because of a timeout (-303)
+            emit trackingHasFinished(retValue);
+            break;
+        }
+    }
+
+    rs_tracker_impl_end();
+}
+
+void RSTrackerWorkerThread::getPose(double *pose){
+    QMutexLocker lock(&mMutex);
+    memcpy(pose, mPose, sizeof(mPose));
+}
+
+const QImage RSTrackerWorkerThread::getPreview(){
+    rs_tracker_impl_get_preview(mPreviewRawData, kPreviewStreamWidth, kPreviewStreamHeight);
+    return QImage((const uchar*)mPreviewRawData, kPreviewStreamWidth, kPreviewStreamHeight, QImage::Format_Grayscale8).copy();//TODO: avoid deep copy?
+}
+
+RSTrackerWorkerThread::~RSTrackerWorkerThread() {
+    free(mPreviewRawData);
+}
diff --git a/tracker-rs/ftnoir_tracker_rs_worker.h b/tracker-rs/ftnoir_tracker_rs_worker.h
new file mode 100644
index 00000000..098d6c10
--- /dev/null
+++ b/tracker-rs/ftnoir_tracker_rs_worker.h
@@ -0,0 +1,38 @@
+/*
+ * Copyright (c) 2015, Intel Corporation
+ *   Author: Xavier Hallade <xavier.hallade@intel.com>
+ * 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.
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+ 
+#pragma once
+
+#include <QThread>
+#include <QMutex>
+
+class RSTrackerWorkerThread : public QThread
+{
+    Q_OBJECT
+
+public:
+    RSTrackerWorkerThread();
+    ~RSTrackerWorkerThread();
+
+    void getPose(double* pose);
+    const QImage getPreview();
+
+protected:
+    void run() override;
+
+private:
+    double mPose[6];
+    uchar* mPreviewRawData;
+    QMutex mMutex;
+    const int kPreviewStreamWidth = 640;
+    const int kPreviewStreamHeight = 480;
+
+signals:
+    void trackingHasStarted();
+    void trackingHasFinished(int);
+
+};
diff --git a/tracker-rs/imagewidget.cpp b/tracker-rs/imagewidget.cpp
new file mode 100644
index 00000000..63986234
--- /dev/null
+++ b/tracker-rs/imagewidget.cpp
@@ -0,0 +1,30 @@
+/*
+ * Copyright (c) 2015, Intel Corporation
+ *   Author: Xavier Hallade <xavier.hallade@intel.com>
+ * 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.
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include "imagewidget.h"
+#include <QPainter>
+
+ImageWidget::ImageWidget(QWidget *parent) :
+    QWidget(parent), mImage(640, 480, QImage::Format_Grayscale8)
+{
+    mImage.fill(Qt::gray);
+}
+
+void ImageWidget::setImage(const QImage image)
+{
+    {
+        QMutexLocker lock(&mMutex);
+        mImage = image;
+    }
+    repaint();
+}
+
+void ImageWidget::paintEvent(QPaintEvent*)
+{
+    QMutexLocker lock(&mMutex);
+    QPainter(this).drawImage(rect(), mImage, mImage.rect());
+}
diff --git a/tracker-rs/imagewidget.h b/tracker-rs/imagewidget.h
new file mode 100644
index 00000000..e8b6e61e
--- /dev/null
+++ b/tracker-rs/imagewidget.h
@@ -0,0 +1,27 @@
+/*
+ * Copyright (c) 2015, Intel Corporation
+ *   Author: Xavier Hallade <xavier.hallade@intel.com>
+ * 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.
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+ 
+#pragma once
+
+#include <QWidget>
+#include <QImage>
+#include <QMutex>
+
+class ImageWidget : public QWidget {
+  Q_OBJECT
+public:
+  ImageWidget(QWidget* parent = 0);
+  void setImage(const QImage image);
+
+private:
+  QImage mImage;
+  QMutex mMutex;
+
+protected:
+  void paintEvent(QPaintEvent* event);
+
+};
diff --git a/tracker-rs/redist/intel_rs_sdk_runtime_websetup_7.0.23.8048.exe b/tracker-rs/redist/intel_rs_sdk_runtime_websetup_7.0.23.8048.exe
deleted file mode 100644
index 2e2b365c..00000000
--- a/tracker-rs/redist/intel_rs_sdk_runtime_websetup_7.0.23.8048.exe
+++ /dev/null
diff --git a/tracker-rs/redist/intel_rs_sdk_runtime_websetup_8.0.24.6528.exe b/tracker-rs/redist/intel_rs_sdk_runtime_websetup_8.0.24.6528.exe
new file mode 100644
index 00000000..5b3704ed
--- /dev/null
+++ b/tracker-rs/redist/intel_rs_sdk_runtime_websetup_8.0.24.6528.exe
diff --git a/tracker-rs/rs_impl/bin/opentrack-tracker-rs-impl.dll b/tracker-rs/rs_impl/bin/opentrack-tracker-rs-impl.dll
new file mode 100644
index 00000000..d6e0a261
--- /dev/null
+++ b/tracker-rs/rs_impl/bin/opentrack-tracker-rs-impl.dll
diff --git a/tracker-rs/rs_impl/bin/opentrack-tracker-rs-impl.exe b/tracker-rs/rs_impl/bin/opentrack-tracker-rs-impl.exe
deleted file mode 100644
index e00ed69c..00000000
--- a/tracker-rs/rs_impl/bin/opentrack-tracker-rs-impl.exe
+++ /dev/null
diff --git a/tracker-rs/rs_impl/bin/opentrack-tracker-rs-impl.lib b/tracker-rs/rs_impl/bin/opentrack-tracker-rs-impl.lib
new file mode 100644
index 00000000..47743c50
--- /dev/null
+++ b/tracker-rs/rs_impl/bin/opentrack-tracker-rs-impl.lib
diff --git a/tracker-rs/rs_impl/build.bat b/tracker-rs/rs_impl/build.bat
index 3a44fed5..799630b2 100644
--- a/tracker-rs/rs_impl/build.bat
+++ b/tracker-rs/rs_impl/build.bat
@@ -3,4 +3,5 @@ IF DEFINED %VS120COMNTOOLS%] (
 	) ELSE (
 	cd "%VS140COMNTOOLS%\..\..\VC"
 	)
-vcvarsall x64 && cd %~dp0 && CL /nologo /Ox /DUNICODE /D_UNICODE /MT /I"%RSSDK_DIR%\opensource\include" ftnoir_tracker_rs_impl.cpp udp_sender.cpp "%RSSDK_DIR%\opensource\src\libpxc\libpxc.cpp" /link ADVAPI32.LIB Ws2_32.lib /SUBSYSTEM:CONSOLE /OUT:bin\opentrack-tracker-rs-impl.exe 
\ No newline at end of file
+
+vcvarsall x86 && cd %~dp0 && CL /nologo /Ox /DUNICODE /D_UNICODE /DEXPORT_RS_IMPL /MT /I"%RSSDK_DIR%\opensource\include" ftnoir_tracker_rs_impl.cpp "%RSSDK_DIR%\opensource\src\libpxc\libpxc.cpp" /link ADVAPI32.LIB /DLL /OUT:bin\opentrack-tracker-rs-impl.dll
\ No newline at end of file
diff --git a/tracker-rs/rs_impl/ftnoir_tracker_rs_impl.cpp b/tracker-rs/rs_impl/ftnoir_tracker_rs_impl.cpp
index c1ba5c75..75499c71 100644
--- a/tracker-rs/rs_impl/ftnoir_tracker_rs_impl.cpp
+++ b/tracker-rs/rs_impl/ftnoir_tracker_rs_impl.cpp
@@ -8,11 +8,18 @@
 #include "ftnoir_tracker_rs_impl.h"
 #include <pxcsensemanager.h>
 #include <pxcfaceconfiguration.h>
+#include <windows.h>
+
+const size_t kPreviewStreamWidth = 640;
+const size_t kPreviewStreamHeight = 480;
 
 PXCSenseManager* g_senseManager = NULL;
 PXCFaceData* g_faceData = NULL;
+void* g_previewImage = NULL;
+
+CRITICAL_SECTION g_criticalSection;
 
-pxcStatus rs_tracker_set_configuration(PXCFaceModule *faceModule){
+pxcStatus set_face_module_configuration(PXCFaceModule *faceModule){
 	pxcStatus retStatus;
 	PXCFaceConfiguration *faceConfig = NULL;
 
@@ -43,7 +50,37 @@ pxcStatus rs_tracker_set_configuration(PXCFaceModule *faceModule){
 	return PXC_STATUS_NO_ERROR;
 }
 
+pxcStatus retrieve_preview_from_frame(){
+	pxcStatus retStatus = PXC_STATUS_NO_ERROR;
+	PXCCapture::Sample* sample = g_senseManager->QuerySample();
+	PXCImage::ImageInfo info = sample->depth->QueryInfo();
+	
+	if(info.width>kPreviewStreamWidth || info.height>kPreviewStreamHeight)
+		return PXC_STATUS_PARAM_UNSUPPORTED;
+		
+	PXCImage::ImageData depthData;
+	retStatus = sample->depth->AcquireAccess(PXCImage::Access::ACCESS_READ, PXCImage::PIXEL_FORMAT_RGB32, &depthData);
+
+	if(retStatus == PXC_STATUS_NO_ERROR){
+		EnterCriticalSection(&g_criticalSection);
+		
+		for(int i=0; i<info.height; ++i) 
+			for(int j=0; j<info.width; ++j)
+				((unsigned char*)g_previewImage)[i*info.width+j]=((unsigned char*)depthData.planes[0])[(i+1)*4*info.width-4*(j+1)]; //mirror and convert from RGB32 to Y8.
+				
+		LeaveCriticalSection(&g_criticalSection);
+		
+		sample->depth->ReleaseAccess(&depthData);
+	}
+	
+	return retStatus;
+}
+
 int rs_tracker_impl_start(){
+	InitializeCriticalSection(&g_criticalSection);
+	g_previewImage = malloc(kPreviewStreamWidth*kPreviewStreamHeight);
+    memset(g_previewImage, 0, kPreviewStreamWidth*kPreviewStreamHeight);
+
 	pxcStatus retStatus;
 	PXCFaceModule *faceModule = NULL;
 
@@ -65,7 +102,7 @@ int rs_tracker_impl_start(){
 		return PXC_STATUS_HANDLE_INVALID;
 	}
 
-	retStatus = rs_tracker_set_configuration(faceModule);
+	retStatus = set_face_module_configuration(faceModule);
 	if (retStatus != PXC_STATUS_NO_ERROR){
 		rs_tracker_impl_end();
 		return PXC_STATUS_HANDLE_INVALID;
@@ -86,7 +123,18 @@ int rs_tracker_impl_start(){
 	return PXC_STATUS_NO_ERROR;
 }
 
-int rs_tracker_impl_update_pose(double *data){
+int rs_tracker_impl_get_preview(void* previewImageOut, int width, int height){
+	if(width!=kPreviewStreamWidth || height!=kPreviewStreamHeight || g_previewImage == NULL)
+		return -1;//TODO: improve errors communication.
+
+	EnterCriticalSection(&g_criticalSection);
+	memcpy(previewImageOut, g_previewImage, kPreviewStreamWidth*kPreviewStreamHeight);
+	LeaveCriticalSection(&g_criticalSection);
+
+	return PXC_STATUS_HANDLE_INVALID;
+}
+
+int rs_tracker_impl_update_pose(double *data){//TODO: add bool preview activated.
 	pxcStatus retStatus;
 	PXCFaceData::PoseEulerAngles angles;
 	PXCFaceData::HeadPosition headPosition;
@@ -95,9 +143,11 @@ int rs_tracker_impl_update_pose(double *data){
 	bool poseAnglesAvailable = false;
 	bool headPositionAvailable = false;
 
-	if (g_senseManager != NULL && g_faceData != NULL
+	if (g_senseManager != NULL && g_faceData != NULL && g_previewImage != NULL
                 && (retStatus = g_senseManager->AcquireFrame(true, 16)) == PXC_STATUS_NO_ERROR){
-
+				
+		retrieve_preview_from_frame();
+		
 		retStatus = g_faceData->Update();
 		if (retStatus != PXC_STATUS_NO_ERROR){
 			rs_tracker_impl_end();
@@ -105,8 +155,8 @@ int rs_tracker_impl_update_pose(double *data){
 		}
 
 		pxcI32 numberOfDetectedFaces = g_faceData->QueryNumberOfDetectedFaces();
-		for (pxcI32 i = 0; i < numberOfDetectedFaces; ++i) {
-			face = g_faceData->QueryFaceByIndex(0);
+		for(int i=0; i<numberOfDetectedFaces; ++i) {
+			face = g_faceData->QueryFaceByIndex(i);
 			if (face == NULL) continue;
 
 			pose = face->QueryPose();
@@ -128,13 +178,15 @@ int rs_tracker_impl_update_pose(double *data){
 			//yaw, pitch, roll: degrees
 			data[3] = -angles.yaw;
 			data[4] = angles.pitch;
-			data[5] = -angles.roll;
+			data[5] = angles.roll;
+			
+			break;
 		}
 
 		g_senseManager->ReleaseFrame();
 	}
 
-        return retStatus;
+    return retStatus;
 }
 
 int rs_tracker_impl_end(){
@@ -147,5 +199,10 @@ int rs_tracker_impl_end(){
 		g_senseManager->Release();
 		g_senseManager = NULL;
 	}
+	
+	DeleteCriticalSection(&g_criticalSection);
+	
+	free(g_previewImage);
+	
 	return PXC_STATUS_NO_ERROR;
 }
diff --git a/tracker-rs/rs_impl/ftnoir_tracker_rs_impl.h b/tracker-rs/rs_impl/ftnoir_tracker_rs_impl.h
index 0e4073d0..a4c03ce8 100644
--- a/tracker-rs/rs_impl/ftnoir_tracker_rs_impl.h
+++ b/tracker-rs/rs_impl/ftnoir_tracker_rs_impl.h
@@ -15,5 +15,6 @@
 extern "C" {                
     RSTRACKERIMPL_VISIBILITY int rs_tracker_impl_start();
     RSTRACKERIMPL_VISIBILITY int rs_tracker_impl_update_pose(double *pose);
+    RSTRACKERIMPL_VISIBILITY int rs_tracker_impl_get_preview(void* data, int width, int height);
     RSTRACKERIMPL_VISIBILITY int rs_tracker_impl_end();
 }