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 679f67deb..7d097422a 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 129668271..65ec75fdc 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 f26052f33..1f292af77 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 02aa227a6..80c68f166 100644
--- a/gui/images/no-feed.png
+++ b/gui/images/no-feed.png
diff --git a/gui/settings.ui b/gui/settings.ui
index cb292a351..b00e0960e 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 abb83fddb..838300fcc 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 55b38a471..f460c5162 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 6df2d626d..ea14dfbf5 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 dff0c30af..5f30c66f0 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 0208b11d8..a1294c1e5 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 030251ff5..3e4661f90 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 aa6feb5b1..25240635f 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 901d68bb0..9219aafd0 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 e073fa3f4..99c5e9d01 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 7cda73704..845e0beba 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 be18b3f86..baa01c91f 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 010dac999..c7f1e7eb3 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 023e14d31..30d06956b 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 000000000..c0ea67492
--- /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 000000000..098d6c107
--- /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 000000000..639862341
--- /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 000000000..e8b6e61e7
--- /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 2e2b365cc..000000000
--- 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 000000000..5b3704ed1
--- /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 000000000..d6e0a2610
--- /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 e00ed69ce..000000000
--- 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 000000000..47743c509
--- /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 3a44fed5b..799630b2f 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 c1ba5c75e..75499c71c 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 0e4073d0a..a4c03ce89 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();
 }