15 files changed, 322 insertions, 99 deletions

diff --git a/tracker-rs/CMakeLists.txt b/tracker-rs/CMakeLists.txt
index 901d68bb..76ffd191 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})
+    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_7.0.23.8048.exe" DESTINATION ./contrib/ ${opentrack-perms})
 endif()
diff --git a/tracker-rs/ftnoir_tracker_rs.cpp b/tracker-rs/ftnoir_tracker_rs.cpp
index e073fa3f..7bb47256 100644
--- a/tracker-rs/ftnoir_tracker_rs.cpp
+++ b/tracker-rs/ftnoir_tracker_rs.cpp
@@ -7,97 +7,112 @@
 
 #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() : mPose{ 0,0,0, 0,0,0 } {
-    mThread.setObjectName("RSTrackerWorkerThread");
+RSTracker::RSTracker() {
+    connect(&mTrackerWorkerThread, &RSTrackerWorkerThread::trackingHasFinished,
+            this, &RSTracker::handleTrackingEnded);
 
-    mRealSenseImplProcess.moveToThread(&mThread);
-    mSocket.moveToThread(&mThread);
+    connect(&mPreviewUpdateTimer, &QTimer::timeout,
+            this, &RSTracker::updatePreview);
+}
 
-    connect(&mRealSenseImplProcess, SIGNAL(finished(int)),
-            this, SLOT(rsImplProcessFinished(int)), Qt::QueuedConnection);
+void RSTracker::configurePreviewFrame()
+{
+    if(mImageWidget!=nullptr || mPreviewFrame==nullptr)
+        return;
 
-    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);
 
-    connect(&mThread, &QThread::finished,
-            &mThread, [this]{
-        mRealSenseImplProcess.kill();
-        mRealSenseImplProcess.waitForFinished();
-    }, Qt::DirectConnection);
+    layout->addWidget(mImageWidget);
 }
 
-void RSTracker::start_tracker(QFrame*)
+void RSTracker::start_tracker(QFrame* previewFrame)
 {
-    mThread.start();
+    mPreviewFrame = previewFrame;
+
+    mTrackerWorkerThread.start(QThread::HighPriority);
+
+    configurePreviewFrame();
+
+    startPreview();
 }
 
-void RSTracker::readPendingUdpPoseData(){
-    double pose[6];
+void RSTracker::startPreview(){
+    mPreviewUpdateTimer.start(kPreviewUpdateInterval);
+}
 
-    while(mSocket.hasPendingDatagrams()) {
-        mSocket.readDatagram((char*)pose, sizeof(pose));
-        QMutexLocker foo(&mMutex);
-        memcpy(mPose, pose, sizeof(pose));
+void RSTracker::updatePreview(){
+    if(mImageWidget->isEnabled())
+        mImageWidget->setImage(mTrackerWorkerThread.getPreview());
+}
+
+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_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);
     }
+    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 R5 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 (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_worker.cpp b/tracker-rs/ftnoir_tracker_rs_worker.cpp
new file mode 100644
index 00000000..c387b731
--- /dev/null
+++ b/tracker-rs/ftnoir_tracker_rs_worker.cpp
@@ -0,0 +1,56 @@
+/*
+ * 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(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 && retValue!=-303){ // -303 is only a timeout.
+            emit trackingHasFinished(retValue);
+            break;
+        }
+
+        QMutexLocker lock(&mMutex);
+        memcpy(mPose, pose, sizeof(pose));
+    }
+
+    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..0136c621
--- /dev/null
+++ b/tracker-rs/imagewidget.cpp
@@ -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.
+ */
+
+#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)
+{
+    mImage = image;
+    repaint();
+}
+
+void ImageWidget::paintEvent(QPaintEvent*)
+{
+    QPainter painter(this);
+    painter.drawImage(rect(), mImage, mImage.rect());
+}
diff --git a/tracker-rs/imagewidget.h b/tracker-rs/imagewidget.h
new file mode 100644
index 00000000..8f800058
--- /dev/null
+++ b/tracker-rs/imagewidget.h
@@ -0,0 +1,26 @@
+/*
+ * 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>
+
+class ImageWidget : public QWidget {
+  Q_OBJECT
+public:
+  ImageWidget(QWidget* parent = 0);
+  void setImage(const QImage image);
+
+private:
+  QImage mImage;
+  uchar* mRawData;
+
+protected:
+  void paintEvent(QPaintEvent* event);
+
+};
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..9e75d4e3
--- /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..772cd340
--- /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();
 }