tracker-rs: updated to SDK 2016 R2. Added more debug information and safeguards.

10 files changed, 128 insertions, 88 deletions

diff --git a/installer/opentrack-installer.iss b/installer/opentrack-installer.iss
index e1a8cf31..23c83181 100644
--- a/installer/opentrack-installer.iss
+++ b/installer/opentrack-installer.iss
@@ -89,7 +89,7 @@ var
 begin
 if RSCameraDriverDetectedAndEulaAccepted then
   begin
-    NonUiBlockingExec(ExpandConstant('{app}\contrib\intel_rs_sdk_runtime_websetup_8.0.24.6528.exe'), 
+    NonUiBlockingExec(ExpandConstant('{app}\contrib\intel_rs_sdk_runtime_websetup_10.0.26.0396.exe'), 
     '--silent --no-progress --acceptlicense=yes --front --finstall=core,face3d --fnone=all');
   end
 end;
diff --git a/tracker-rs/CMakeLists.txt b/tracker-rs/CMakeLists.txt
index 8fda5633..e81dc5ae 100644
--- a/tracker-rs/CMakeLists.txt
+++ b/tracker-rs/CMakeLists.txt
@@ -2,6 +2,6 @@ if(WIN32)
     opentrack_boilerplate(opentrack-tracker-rs)
     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})
+    install(FILES "${CMAKE_SOURCE_DIR}/tracker-rs/redist/intel_rs_sdk_runtime_websetup_10.0.26.0396.exe" DESTINATION ./contrib/ ${opentrack-perms})
     install(FILES "${CMAKE_SOURCE_DIR}/tracker-rs/redist/RS_EULA.rtf" DESTINATION ./contrib/ ${opentrack-perms})
 endif()
diff --git a/tracker-rs/ftnoir_tracker_rs.cpp b/tracker-rs/ftnoir_tracker_rs.cpp
index 99c5e9d0..d543c49c 100644
--- a/tracker-rs/ftnoir_tracker_rs.cpp
+++ b/tracker-rs/ftnoir_tracker_rs.cpp
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, Intel Corporation
+ * Copyright (c) 2015-2016, 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.
@@ -12,6 +12,7 @@
 #include <QMessageBox>
 #include <QProcess>
 #include <QStackedLayout>
+#include <QDebug>
 
 RSTracker::RSTracker() {
     connect(&mTrackerWorkerThread, &RSTrackerWorkerThread::trackingHasFinished,
@@ -44,6 +45,8 @@ void RSTracker::configurePreviewFrame()
 
 void RSTracker::start_tracker(QFrame* previewFrame)
 {
+	qDebug() << "tracker_rs: starting tracker";
+
     mPreviewFrame = previewFrame;
 
     configurePreviewFrame();
@@ -54,16 +57,20 @@ void RSTracker::start_tracker(QFrame* previewFrame)
 }
 
 void RSTracker::startPreview(){
+	qDebug() << "tracker_rs: starting preview";
     mPreviewUpdateTimer.start(kPreviewUpdateInterval);
 }
 
 void RSTracker::updatePreview(){
-    if(mImageWidget!=nullptr && mImageWidget->isEnabled())
-        mImageWidget->setImage(mTrackerWorkerThread.getPreview());
+	if (mImageWidget != nullptr && mImageWidget->isEnabled() && mTrackerWorkerThread.isRunning())
+		mImageWidget->setImage(mTrackerWorkerThread.getPreview());
+	else
+		qDebug() << "tracker_rs: not updating preview. worker thread running: " << mTrackerWorkerThread.isRunning();
 }
 
 void RSTracker::stopPreview(){
-    mPreviewUpdateTimer.stop();
+	mPreviewUpdateTimer.stop();
+	qDebug() << "tracker_rs: stopped preview";
 }
 
 void RSTracker::handleTrackingEnded(int exitCode){
@@ -75,7 +82,7 @@ void RSTracker::handleTrackingEnded(int exitCode){
 
 bool RSTracker::startSdkInstallationProcess()
 {
-    bool pStarted = QProcess::startDetached("contrib\\intel_rs_sdk_runtime_websetup_8.0.24.6528.exe --finstall=core,face3d --fnone=all");
+    bool pStarted = QProcess::startDetached("contrib\\intel_rs_sdk_runtime_websetup_10.0.26.0396.exe --finstall=core,face3d --fnone=all");
     if(!pStarted){
         QMessageBox::warning(0, "Intel® RealSense™ Runtime Installation", "Installation process failed to start.", QMessageBox::Ok);
     }
@@ -87,11 +94,19 @@ 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 {
-        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.");
+
+	switch(exitCode){
+	case -101: //The implementation got an invalid handle from the RealSense SDK session/modules
+        msgBox.setInformativeText("Couldn't initialize RealSense tracking. Please make sure SDK Runtime 2016 R2 is installed.");
+		break;
+	case -301: //RealSense SDK runtime execution aborted.
+		msgBox.setInformativeText("Tracking stopped after the RealSense SDK Runtime execution has aborted.");
+		break;
+	case -601: //RealSense Camera stream configuration has changed.
+		msgBox.setInformativeText("Tracking stopped after another program changed camera streams configuration.");
+		break;
+	default:
+        msgBox.setInformativeText("Status code: " + QString::number(exitCode) + ".\n\nNote that you need the latest camera drivers and the SDK runtime 2016 R2 to be installed.");
     }
 
     QPushButton* triggerSdkInstallation = msgBox.addButton("Install Runtime", QMessageBox::ActionRole);
@@ -108,6 +123,8 @@ void RSTracker::data(double *data)
 }
 
 RSTracker::~RSTracker() {
+	qDebug() << "tracker is being destroyed.";
+
     stopPreview();
 
     if(mImageWidget!=nullptr)
diff --git a/tracker-rs/ftnoir_tracker_rs_controls.ui b/tracker-rs/ftnoir_tracker_rs_controls.ui
index 30d06956..4200302d 100644
--- a/tracker-rs/ftnoir_tracker_rs_controls.ui
+++ b/tracker-rs/ftnoir_tracker_rs_controls.ui
@@ -37,7 +37,7 @@ 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™ 
-F200 or SR300 camera and the RealSense™ SDK 2016 R1 runtime.</string>
+F200 or SR300 camera and the RealSense™ SDK 2016 R2 runtime.</string>
      </property>
     </widget>
    </item>
diff --git a/tracker-rs/ftnoir_tracker_rs_worker.cpp b/tracker-rs/ftnoir_tracker_rs_worker.cpp
index c0ea6749..0e2c86f4 100644
--- a/tracker-rs/ftnoir_tracker_rs_worker.cpp
+++ b/tracker-rs/ftnoir_tracker_rs_worker.cpp
@@ -8,10 +8,11 @@
 #include "ftnoir_tracker_rs_worker.h"
 #include "rs_impl/ftnoir_tracker_rs_impl.h"
 #include <QImage>
+#include <QDebug>
 
 RSTrackerWorkerThread::RSTrackerWorkerThread(): mPose{0,0,0,0,0,0}{
     setObjectName("RSTrackerWorkerThread");
-    mPreviewRawData = (uchar*)malloc(1*kPreviewStreamWidth*kPreviewStreamHeight);
+    mPreviewRawData = (uchar*)malloc(1*kPreviewStreamWidth*kPreviewStreamHeight); // Y8 format
     memset(mPreviewRawData, 125, kPreviewStreamWidth*kPreviewStreamHeight); //start with a gray image.
 }
 
@@ -20,6 +21,8 @@ void RSTrackerWorkerThread::run(){
     int retValue;
 
     retValue = rs_tracker_impl_start();
+	qDebug() << "tracker_rs: tracking has started. retValue: " << retValue;
+
     if(retValue==0)
         emit trackingHasStarted();
     else {
@@ -29,17 +32,23 @@ void RSTrackerWorkerThread::run(){
 
     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;
-        }
+		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;
+		}
+		else
+			qDebug() << "tracker_rs: timeout in pose update.";
     }
 
-    rs_tracker_impl_end();
+	qDebug() << "tracker_rs: tracking is ending. retValue: " << retValue;
+
+	retValue = rs_tracker_impl_end();
+
+	qDebug() << "tracker_rs: tracking has ended. retValue: " << retValue;
 }
 
 void RSTrackerWorkerThread::getPose(double *pose){
@@ -48,7 +57,8 @@ void RSTrackerWorkerThread::getPose(double *pose){
 }
 
 const QImage RSTrackerWorkerThread::getPreview(){
-    rs_tracker_impl_get_preview(mPreviewRawData, kPreviewStreamWidth, kPreviewStreamHeight);
+	if(!isInterruptionRequested() && isRunning())
+	    rs_tracker_impl_get_preview(mPreviewRawData, kPreviewStreamWidth, kPreviewStreamHeight);
     return QImage((const uchar*)mPreviewRawData, kPreviewStreamWidth, kPreviewStreamHeight, QImage::Format_Grayscale8).copy();//TODO: avoid deep copy?
 }
 
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_10.0.26.0396.exe
index 5b3704ed..90000041 100644
--- a/tracker-rs/redist/intel_rs_sdk_runtime_websetup_8.0.24.6528.exe
+++ b/tracker-rs/redist/intel_rs_sdk_runtime_websetup_10.0.26.0396.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
index d6e0a261..3f18f212 100644
--- a/tracker-rs/rs_impl/bin/opentrack-tracker-rs-impl.dll
+++ b/tracker-rs/rs_impl/bin/opentrack-tracker-rs-impl.dll
diff --git a/tracker-rs/rs_impl/bin/opentrack-tracker-rs-impl.lib b/tracker-rs/rs_impl/bin/opentrack-tracker-rs-impl.lib
index 47743c50..7e49ed05 100644
--- a/tracker-rs/rs_impl/bin/opentrack-tracker-rs-impl.lib
+++ 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 799630b2..89b7647e 100644
--- a/tracker-rs/rs_impl/build.bat
+++ b/tracker-rs/rs_impl/build.bat
@@ -1,7 +1,7 @@
 IF DEFINED %VS120COMNTOOLS%] (
-	cd "%VS120COMNTOOLS%\..\..\VC"
+	chdir /d "%VS120COMNTOOLS%\..\..\VC"
 	) ELSE (
-	cd "%VS140COMNTOOLS%\..\..\VC"
+	chdir /d "%VS140COMNTOOLS%\..\..\VC"
 	)
 
-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
+vcvarsall x86 && chdir /d %~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 75499c71..c371e30a 100644
--- a/tracker-rs/rs_impl/ftnoir_tracker_rs_impl.cpp
+++ b/tracker-rs/rs_impl/ftnoir_tracker_rs_impl.cpp
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, Intel Corporation
+ * Copyright (c) 2015-2016, 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.
@@ -7,6 +7,7 @@
 
 #include "ftnoir_tracker_rs_impl.h"
 #include <pxcsensemanager.h>
+#include <pxcfacemodule.h>
 #include <pxcfaceconfiguration.h>
 #include <windows.h>
 
@@ -19,15 +20,16 @@ void* g_previewImage = NULL;
 
 CRITICAL_SECTION g_criticalSection;
 
-pxcStatus set_face_module_configuration(PXCFaceModule *faceModule){
-	pxcStatus retStatus;
+pxcStatus set_face_module_configuration(PXCFaceModule *faceModule) {
+	pxcStatus retStatus = PXC_STATUS_NO_ERROR;
 	PXCFaceConfiguration *faceConfig = NULL;
 
-	if (faceModule == NULL)
+	if (faceModule == NULL) {
 		return PXC_STATUS_HANDLE_INVALID;
+	}
 
 	faceConfig = faceModule->CreateActiveConfiguration();
-	if (faceConfig == NULL){
+	if (faceConfig == NULL) {
 		return PXC_STATUS_HANDLE_INVALID;
 	}
 
@@ -41,101 +43,111 @@ pxcStatus set_face_module_configuration(PXCFaceModule *faceModule){
 	faceConfig->SetTrackingMode(PXCFaceConfiguration::FACE_MODE_COLOR_PLUS_DEPTH);
 
 	retStatus = faceConfig->ApplyChanges();
-	if (retStatus != PXC_STATUS_NO_ERROR){
-		faceConfig->Release();
-		return retStatus;
-	}
 
 	faceConfig->Release();
-	return PXC_STATUS_NO_ERROR;
+	return retStatus;
 }
 
-pxcStatus retrieve_preview_from_frame(){
+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;
-		
+	PXCCapture::Sample* sample = NULL;
+	PXCImage::ImageInfo info;
 	PXCImage::ImageData depthData;
-	retStatus = sample->depth->AcquireAccess(PXCImage::Access::ACCESS_READ, PXCImage::PIXEL_FORMAT_RGB32, &depthData);
 
-	if(retStatus == PXC_STATUS_NO_ERROR){
+	if (g_senseManager == NULL) {
+		return PXC_STATUS_HANDLE_INVALID;
+	}
+
+	sample = g_senseManager->QuerySample();
+	if (sample == NULL || sample->depth == NULL) {
+		return PXC_STATUS_DATA_UNAVAILABLE;
+	}
+
+	info = sample->depth->QueryInfo();
+	if (info.width > kPreviewStreamWidth || info.height > kPreviewStreamHeight) {
+		return PXC_STATUS_PARAM_UNSUPPORTED;
+	}
+
+	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.
-				
+
+		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(){
+int rs_tracker_impl_start() {
+	pxcStatus retStatus = PXC_STATUS_NO_ERROR;
+	PXCFaceModule *faceModule = NULL;
+
 	InitializeCriticalSection(&g_criticalSection);
 	g_previewImage = malloc(kPreviewStreamWidth*kPreviewStreamHeight);
-    memset(g_previewImage, 0, kPreviewStreamWidth*kPreviewStreamHeight);
-
-	pxcStatus retStatus;
-	PXCFaceModule *faceModule = NULL;
+	memset(g_previewImage, 0, kPreviewStreamWidth*kPreviewStreamHeight);
 
 	g_senseManager = PXCSenseManager::CreateInstance();
-	if (g_senseManager == NULL){
+	if (g_senseManager == NULL) {
 		rs_tracker_impl_end();
 		return PXC_STATUS_HANDLE_INVALID;
 	}
 
 	retStatus = g_senseManager->EnableFace();
-	if (retStatus != PXC_STATUS_NO_ERROR){
+	if (retStatus != PXC_STATUS_NO_ERROR) {
 		rs_tracker_impl_end();
 		return retStatus;
 	}
 
 	faceModule = g_senseManager->QueryFace();
-	if (faceModule == NULL){
+	if (faceModule == NULL) {
 		rs_tracker_impl_end();
 		return PXC_STATUS_HANDLE_INVALID;
 	}
 
 	retStatus = set_face_module_configuration(faceModule);
-	if (retStatus != PXC_STATUS_NO_ERROR){
+	if (retStatus != PXC_STATUS_NO_ERROR) {
 		rs_tracker_impl_end();
-		return PXC_STATUS_HANDLE_INVALID;
+		return retStatus;
 	}
 
 	retStatus = g_senseManager->Init();
-	if (retStatus != PXC_STATUS_NO_ERROR){
+	if (retStatus != PXC_STATUS_NO_ERROR) {
 		rs_tracker_impl_end();
 		return retStatus;
 	}
 
 	g_faceData = faceModule->CreateOutput();
-	if (g_faceData == NULL){
+	if (g_faceData == NULL) {
 		rs_tracker_impl_end();
 		return PXC_STATUS_HANDLE_INVALID;
 	}
 
-	return PXC_STATUS_NO_ERROR;
+	return retStatus;
 }
 
-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.
+int rs_tracker_impl_get_preview(void* previewImageOut, int width, int height) {
+	if (width != kPreviewStreamWidth || height != kPreviewStreamHeight)
+		return PXC_STATUS_PARAM_UNSUPPORTED;
+
+	if (g_previewImage == NULL)
+		return PXC_STATUS_DATA_UNAVAILABLE;
 
 	EnterCriticalSection(&g_criticalSection);
-	memcpy(previewImageOut, g_previewImage, kPreviewStreamWidth*kPreviewStreamHeight);
+	if (g_previewImage != NULL)
+		memcpy(previewImageOut, g_previewImage, kPreviewStreamWidth*kPreviewStreamHeight);
 	LeaveCriticalSection(&g_criticalSection);
 
-	return PXC_STATUS_HANDLE_INVALID;
+	return PXC_STATUS_NO_ERROR;
 }
 
-int rs_tracker_impl_update_pose(double *data){//TODO: add bool preview activated.
-	pxcStatus retStatus;
+int rs_tracker_impl_update_pose(double *data) {//TODO: add bool preview activated.
+	pxcStatus retStatus = PXC_STATUS_NO_ERROR;
 	PXCFaceData::PoseEulerAngles angles;
 	PXCFaceData::HeadPosition headPosition;
 	PXCFaceData::Face *face = NULL;
@@ -144,18 +156,18 @@ int rs_tracker_impl_update_pose(double *data){//TODO: add bool preview activated
 	bool headPositionAvailable = false;
 
 	if (g_senseManager != NULL && g_faceData != NULL && g_previewImage != NULL
-                && (retStatus = g_senseManager->AcquireFrame(true, 16)) == PXC_STATUS_NO_ERROR){
-				
+		&& (retStatus = g_senseManager->AcquireFrame(true, 30)) == PXC_STATUS_NO_ERROR) {
+
 		retrieve_preview_from_frame();
-		
+
 		retStatus = g_faceData->Update();
-		if (retStatus != PXC_STATUS_NO_ERROR){
+		if (retStatus != PXC_STATUS_NO_ERROR) {
 			rs_tracker_impl_end();
 			return retStatus;
 		}
 
 		pxcI32 numberOfDetectedFaces = g_faceData->QueryNumberOfDetectedFaces();
-		for(int i=0; i<numberOfDetectedFaces; ++i) {
+		for (int i = 0; i < numberOfDetectedFaces; ++i) {
 			face = g_faceData->QueryFaceByIndex(i);
 			if (face == NULL) continue;
 
@@ -168,7 +180,7 @@ int rs_tracker_impl_update_pose(double *data){//TODO: add bool preview activated
 			headPositionAvailable = pose->QueryHeadPosition(&headPosition);
 			if (!headPositionAvailable) continue;
 
-			//TODO: use pxcI32 pose->QueryConfidence(); ? for data[6] or to filter here ?
+			//TODO: use pxcI32 pose->QueryConfidence() ?
 
 			//x, y, z: cm
 			data[0] = headPosition.headCenter.x / 10.;
@@ -179,30 +191,31 @@ int rs_tracker_impl_update_pose(double *data){//TODO: add bool preview activated
 			data[3] = -angles.yaw;
 			data[4] = angles.pitch;
 			data[5] = angles.roll;
-			
+
 			break;
 		}
 
 		g_senseManager->ReleaseFrame();
 	}
 
-    return retStatus;
+	return retStatus;
 }
 
-int rs_tracker_impl_end(){
-	if (g_faceData != NULL){
+int rs_tracker_impl_end() {
+	if (g_faceData != NULL) {
 		g_faceData->Release();
 		g_faceData = NULL;
 	}
 
-	if (g_senseManager != NULL){
+	if (g_senseManager != NULL) {
 		g_senseManager->Release();
 		g_senseManager = NULL;
 	}
-	
-	DeleteCriticalSection(&g_criticalSection);
-	
+
+	EnterCriticalSection(&g_criticalSection);
 	free(g_previewImage);
-	
+	g_previewImage = NULL;
+	DeleteCriticalSection(&g_criticalSection);
+
 	return PXC_STATUS_NO_ERROR;
 }