From 06760892a76c91f8253784acbb2bc8471b9a698c Mon Sep 17 00:00:00 2001
From: Victor Prisacariu <victor@robots.ox.ac.uk>
Date: Mon, 16 Jan 2017 18:04:52 +0000
Subject: [PATCH 1/3] support for LibRoyale (PMD PicoFlexx) also fixed horrible
 UI bug that saved scene when resetting :)

---
 InfiniTAM/Apps/InfiniTAM/CMakeLists.txt   |   2 +
 InfiniTAM/Apps/InfiniTAM/InfiniTAM.cpp    |  12 +
 InfiniTAM/Apps/InfiniTAM/UIEngine.cpp     |   1 +
 InfiniTAM/InputSource/CMakeLists.txt      |   3 +
 InfiniTAM/InputSource/PicoFlexxEngine.cpp | 261 ++++++++++++++++++++++
 InfiniTAM/InputSource/PicoFlexxEngine.h   |  27 +++
 InfiniTAM/cmake/FindLibRoyale.cmake       |  18 ++
 InfiniTAM/cmake/LinkLibRoyale.cmake       |  11 +
 InfiniTAM/cmake/UseLibRoyale.cmake        |  11 +
 9 files changed, 346 insertions(+)
 create mode 100644 InfiniTAM/InputSource/PicoFlexxEngine.cpp
 create mode 100644 InfiniTAM/InputSource/PicoFlexxEngine.h
 create mode 100644 InfiniTAM/cmake/FindLibRoyale.cmake
 create mode 100644 InfiniTAM/cmake/LinkLibRoyale.cmake
 create mode 100644 InfiniTAM/cmake/UseLibRoyale.cmake

diff --git a/InfiniTAM/Apps/InfiniTAM/CMakeLists.txt b/InfiniTAM/Apps/InfiniTAM/CMakeLists.txt
index a1cef9a5e..e9bef643b 100644
--- a/InfiniTAM/Apps/InfiniTAM/CMakeLists.txt
+++ b/InfiniTAM/Apps/InfiniTAM/CMakeLists.txt
@@ -15,6 +15,7 @@ SET(targetname InfiniTAM)
 INCLUDE(${PROJECT_SOURCE_DIR}/cmake/UseCUDA.cmake)
 INCLUDE(${PROJECT_SOURCE_DIR}/cmake/UseFFmpeg.cmake)
 INCLUDE(${PROJECT_SOURCE_DIR}/cmake/UseGLUT.cmake)
+INCLUDE(${PROJECT_SOURCE_DIR}/cmake/UseLibRoyale.cmake)
 INCLUDE(${PROJECT_SOURCE_DIR}/cmake/UseOpenGL.cmake)
 INCLUDE(${PROJECT_SOURCE_DIR}/cmake/UseOpenMP.cmake)
 INCLUDE(${PROJECT_SOURCE_DIR}/cmake/UseOpenNI.cmake)
@@ -54,6 +55,7 @@ INCLUDE(${PROJECT_SOURCE_DIR}/cmake/SetCUDAAppTarget.cmake)
 TARGET_LINK_LIBRARIES(${targetname} InputSource ITMLib MiniSlamGraphLib ORUtils FernRelocLib)
 INCLUDE(${PROJECT_SOURCE_DIR}/cmake/LinkFFmpeg.cmake)
 INCLUDE(${PROJECT_SOURCE_DIR}/cmake/LinkGLUT.cmake)
+INCLUDE(${PROJECT_SOURCE_DIR}/cmake/LinkLibRoyale.cmake)
 INCLUDE(${PROJECT_SOURCE_DIR}/cmake/LinkOpenGL.cmake)
 INCLUDE(${PROJECT_SOURCE_DIR}/cmake/LinkOpenNI.cmake)
 INCLUDE(${PROJECT_SOURCE_DIR}/cmake/LinkPNG.cmake)
diff --git a/InfiniTAM/Apps/InfiniTAM/InfiniTAM.cpp b/InfiniTAM/Apps/InfiniTAM/InfiniTAM.cpp
index ab3dc9d0f..3269ff8f8 100644
--- a/InfiniTAM/Apps/InfiniTAM/InfiniTAM.cpp
+++ b/InfiniTAM/Apps/InfiniTAM/InfiniTAM.cpp
@@ -8,6 +8,7 @@
 #include "../../InputSource/OpenNIEngine.h"
 #include "../../InputSource/Kinect2Engine.h"
 #include "../../InputSource/LibUVCEngine.h"
+#include "../../InputSource/PicoFlexxEngine.h"
 #include "../../InputSource/RealSenseEngine.h"
 #include "../../InputSource/LibUVCEngine.h"
 #include "../../InputSource/RealSenseEngine.h"
@@ -124,6 +125,17 @@ static void CreateDefaultImageSource(ImageSourceEngine* & imageSource, IMUSource
 			imageSource = NULL;
 		}
 	}
+
+	if (imageSource == NULL)
+	{
+		printf("trying PMD PicoFlexx device\n");
+		imageSource = new PicoFlexxEngine(calibFile);
+		if (imageSource->getDepthImageSize().x == 0)
+		{
+			delete imageSource;
+			imageSource = NULL;
+		}
+	}
 }
 
 int main(int argc, char** argv)
diff --git a/InfiniTAM/Apps/InfiniTAM/UIEngine.cpp b/InfiniTAM/Apps/InfiniTAM/UIEngine.cpp
index a1935779d..4e867b5c4 100644
--- a/InfiniTAM/Apps/InfiniTAM/UIEngine.cpp
+++ b/InfiniTAM/Apps/InfiniTAM/UIEngine.cpp
@@ -272,6 +272,7 @@ void UIEngine::glutKeyUpFunction(unsigned char key, int x, int y)
 		ITMBasicEngine<ITMVoxel, ITMVoxelIndex> *basicEngine = dynamic_cast<ITMBasicEngine<ITMVoxel, ITMVoxelIndex>*>(uiEngine->mainEngine);
 		if (basicEngine != NULL) basicEngine->resetAll();
 	}
+	break;
 	case 'k':
 	{
 		printf("saving scene to disk ... ");
diff --git a/InfiniTAM/InputSource/CMakeLists.txt b/InfiniTAM/InputSource/CMakeLists.txt
index 5e64db218..1865e7bb8 100644
--- a/InfiniTAM/InputSource/CMakeLists.txt
+++ b/InfiniTAM/InputSource/CMakeLists.txt
@@ -25,6 +25,7 @@ INCLUDE(${PROJECT_SOURCE_DIR}/cmake/UseKinect2.cmake)
 INCLUDE(${PROJECT_SOURCE_DIR}/cmake/UseOpenGL.cmake)
 INCLUDE(${PROJECT_SOURCE_DIR}/cmake/UseOpenMP.cmake)
 INCLUDE(${PROJECT_SOURCE_DIR}/cmake/UseOpenNI.cmake)
+INCLUDE(${PROJECT_SOURCE_DIR}/cmake/UseLibRoyale.cmake)
 INCLUDE(${PROJECT_SOURCE_DIR}/cmake/UseRealSense.cmake)
 INCLUDE(${PROJECT_SOURCE_DIR}/cmake/UseUVC.cmake)
 
@@ -41,6 +42,7 @@ IMUSourceEngine.cpp
 Kinect2Engine.cpp
 LibUVCEngine.cpp
 OpenNIEngine.cpp
+PicoFlexxEngine.cpp
 RealSenseEngine.cpp
 )
 
@@ -53,6 +55,7 @@ IMUSourceEngine.h
 Kinect2Engine.h
 LibUVCEngine.h
 OpenNIEngine.h
+PicoFlexxEngine.h
 RealSenseEngine.h
 )
 
diff --git a/InfiniTAM/InputSource/PicoFlexxEngine.cpp b/InfiniTAM/InputSource/PicoFlexxEngine.cpp
new file mode 100644
index 000000000..93354e542
--- /dev/null
+++ b/InfiniTAM/InputSource/PicoFlexxEngine.cpp
@@ -0,0 +1,261 @@
+// Copyright 2017 Akos Maroy
+
+#include "PicoFlexxEngine.h"
+
+#include <cstdio>
+#include <stdexcept>
+#include <algorithm>
+#include <mutex>
+
+#ifdef COMPILE_WITH_LibRoyale
+
+#ifndef WIN32
+#include <unistd.h>
+#define ROYALE_SLEEP_MS(x) usleep(x * 1000)
+#else
+#include <windows.h>
+#undef max
+#define ROYALE_SLEEP_MS(x) Sleep(x)
+#endif
+
+#include <royale.hpp>
+
+using namespace InputSource;
+using namespace royale;
+using namespace std;
+
+class PicoFlexxEngine::PrivateData : public IDepthDataListener 
+{
+public:
+	PicoFlexxEngine *engine;
+	std::unique_ptr<ICameraDevice> cameraDevice;
+	vector<Vector4u> rgbImage;
+	vector<short> depthImage;
+	mutex mtx;
+
+	PrivateData(PicoFlexxEngine *pfe) : engine(pfe), depthImage(0) {};
+	~PrivateData() {}
+
+	void onNewData(const DepthData *data);
+};
+
+
+void PicoFlexxEngine::PrivateData::onNewData(const DepthData *data) 
+{
+	lock_guard<mutex> lock(mtx);
+
+	// handle the grayscale image
+	engine->imageSize_rgb = Vector2i(data->width, data->height);
+
+	rgbImage.clear();
+	rgbImage.reserve(data->points.size());
+
+	// copy grayscale image data into an RGB data set
+	std::for_each(data->points.begin(), data->points.end(), [this](const DepthPoint &dd) {
+		// PicoFlexx seems to return 0 when bright, and up to FF0 when totally dark
+		// convert this into a value into an 8 bit value
+		unsigned char charValue = dd.grayValue >> 4;
+		Vector4u pixel;
+		pixel.x = pixel.y = pixel.z = pixel.w = charValue;
+		this->rgbImage.push_back(pixel);
+	});
+
+
+	// handle the depth image
+	engine->imageSize_d = Vector2i(data->width, data->height);
+
+	depthImage.clear();
+	depthImage.reserve(data->points.size());
+
+	// copy depth image data, converting meters in float to millimeters in short
+	std::for_each(data->points.begin(), data->points.end(), [this](const DepthPoint &dd) {
+		// do not copy if confidence is low. confidence is 0 when bad, 255 when good
+		// it seems there are no intermediate values, still let's cut at 128
+		if (dd.depthConfidence > 128) {
+			this->depthImage.push_back((short)(dd.z * 1000.0));
+		}
+		else {
+			this->depthImage.push_back(0);
+		}
+	});
+}
+
+
+PicoFlexxEngine::PicoFlexxEngine(const char *calibFilename, const char *deviceURI, const bool useInternalCalibration,
+	Vector2i requested_imageSize_rgb, Vector2i requested_imageSize_d)
+	: BaseImageSourceEngine(calibFilename)
+{
+	this->imageSize_d = Vector2i(0, 0);
+	this->imageSize_rgb = Vector2i(0, 0);
+
+	data = new PrivateData(this);
+
+	// the camera manager will query for a connected camera
+	{
+		CameraManager manager;
+
+		auto camlist = manager.getConnectedCameraList();
+		cout << "Detected " << camlist.size() << " camera(s)." << endl;
+		if (!camlist.empty())
+		{
+			cout << "CamID for first device: " << camlist.at(0).c_str() << " with a length of (" << camlist.at(0).length() << ")" << endl;
+			data->cameraDevice = manager.createCamera(camlist[0]);
+		}
+	}
+	// the camera device is now available and CameraManager can be deallocated here
+
+	if (data->cameraDevice == nullptr)
+	{
+		cerr << "Cannot create the camera device" << endl;
+		return;
+	}
+
+	// IMPORTANT: call the initialize method before working with the camera device
+	if (data->cameraDevice->initialize() != CameraStatus::SUCCESS)
+	{
+		cerr << "Cannot initialize the camera device" << endl;
+		return;
+	}
+
+	// set camera parameters
+	LensParameters lensParams;
+	if (data->cameraDevice->getLensParameters(lensParams) != CameraStatus::SUCCESS) {
+		cerr << "Cannot determine lens parameters" << endl;
+		return;
+	}
+	
+	this->calib.intrinsics_d.SetFrom(lensParams.focalLength.first, lensParams.focalLength.second,
+		lensParams.principalPoint.first, lensParams.principalPoint.second);
+	this->calib.intrinsics_rgb.SetFrom(lensParams.focalLength.first, lensParams.focalLength.second,
+		lensParams.principalPoint.first, lensParams.principalPoint.second);
+
+	Vector<String> useCases;
+	auto status = data->cameraDevice->getUseCases(useCases);
+
+	if (status != CameraStatus::SUCCESS || useCases.empty())
+	{
+		cerr << "No use cases are available" << endl;
+		cerr << "getUseCases() returned: " << getErrorString(status) << endl;
+		return;
+	}
+
+	// list the available use cases
+	cout << "Available Pico Flexx use cases:" << endl;
+	for_each(useCases.begin(), useCases.end(), [](const String &s) {
+		cout << s << endl;
+	});
+
+	// register a data listener
+	if (data->cameraDevice->registerDataListener(data) != CameraStatus::SUCCESS)
+	{
+		cerr << "Error registering data listener" << endl;
+		return;
+	}
+
+	// set an operation mode
+	// TODO allow setting this from the command line
+	if (data->cameraDevice->setUseCase("MODE_9_10FPS_1000") != CameraStatus::SUCCESS)
+	{
+		cerr << "Error setting use case" << endl;
+		return;
+	}
+
+	// start capture mode
+	if (data->cameraDevice->startCapture() != CameraStatus::SUCCESS)
+	{
+		cerr << "Error starting the capturing" << endl;
+		return;
+	}
+
+	// waiting for the capture to start and 'data' to be populated
+	ROYALE_SLEEP_MS(1000);
+}
+
+PicoFlexxEngine::~PicoFlexxEngine()
+{
+	if (data) {
+		// stop capture mode
+		if (data->cameraDevice) {
+			if (data->cameraDevice->stopCapture() != CameraStatus::SUCCESS) {
+				cerr << "Error stopping the capturing" << endl;
+			}
+		}
+
+		delete data;
+	}
+}
+
+void PicoFlexxEngine::getImages(ITMUChar4Image *rgbImage, ITMShortImage *rawDepthImage)
+{
+	lock_guard<mutex> lock(data->mtx);
+
+	// copy the color info
+#ifdef PROVIDE_RGB
+	Vector4u *rgb = rgbImage->GetData(MEMORYDEVICE_CPU);
+	if (data->rgbImage.size()) {
+		memcpy(rgb, data->rgbImage.data(), rgbImage->dataSize * sizeof(Vector4u));
+	}
+	else
+		memset(rgb, 0, rgbImage->dataSize * sizeof(Vector4u));
+#else
+	imageSize_rgb = Vector2i(0, 0);
+#endif
+
+	// copy the depth info
+	short *depth = rawDepthImage->GetData(MEMORYDEVICE_CPU);
+	if (data->depthImage.size()) {
+		memcpy(depth, data->depthImage.data(), rawDepthImage->dataSize * sizeof(short));
+	}
+	else memset(depth, 0, rawDepthImage->dataSize * sizeof(short));
+
+#if 0
+	cout << "depthImage size: " << data->depthImage.size() << endl;
+	for (int i = 0; i < imageSize_d[0]; ++i) {
+		cout << hex << data->depthImage.at(i) << " ";
+	}
+	cout << endl;
+
+	cout << "first line raw data:" << endl;
+	for (int i = 0; i < imageSize_d[0]; ++i) {
+		cout << hex << depth[i] << " ";
+	}
+	cout << endl;
+#endif
+
+	return /*true*/;
+}
+
+bool PicoFlexxEngine::hasMoreImages(void) const { return data != NULL; }
+Vector2i PicoFlexxEngine::getDepthImageSize(void) const { return (data != NULL) ? imageSize_d : Vector2i(0, 0); }
+Vector2i PicoFlexxEngine::getRGBImageSize(void) const { return (data != NULL) ? imageSize_rgb : Vector2i(0, 0); }
+
+#else
+
+using namespace InfiniTAM::Engine;
+
+PicoFlexxEngine::PicoFlexxEngine(const char *calibFilename, const char *deviceURI, const bool useInternalCalibration, Vector2i requested_imageSize_rgb, Vector2i requested_imageSize_d)
+	: ImageSourceEngine(calibFilename)
+{
+	printf("compiled without LibRoyale support\n");
+}
+PicoFlexxEngine::~PicoFlexxEngine()
+{}
+void PicoFlexxEngine::getImages(ITMUChar4Image *rgbImage, ITMShortImage *rawDepthImage)
+{
+	return;
+}
+bool PicoFlexxEngine::hasMoreImages(void) const
+{
+	return false;
+}
+Vector2i PicoFlexxEngine::getDepthImageSize(void) const
+{
+	return Vector2i(0, 0);
+}
+Vector2i PicoFlexxEngine::getRGBImageSize(void) const
+{
+	return Vector2i(0, 0);
+}
+
+#endif
+
diff --git a/InfiniTAM/InputSource/PicoFlexxEngine.h b/InfiniTAM/InputSource/PicoFlexxEngine.h
new file mode 100644
index 000000000..8a836af57
--- /dev/null
+++ b/InfiniTAM/InputSource/PicoFlexxEngine.h
@@ -0,0 +1,27 @@
+// Copyright 2017 Akos Maroy
+
+#pragma once
+
+#include "ImageSourceEngine.h"
+
+namespace InputSource
+{
+	class PicoFlexxEngine : public BaseImageSourceEngine
+	{
+	private:
+		class PrivateData;
+		PrivateData *data;
+		Vector2i imageSize_rgb, imageSize_d;
+		bool colorAvailable, depthAvailable;
+
+	public:
+		PicoFlexxEngine(const char *calibFilename, const char *deviceURI = NULL, const bool useInternalCalibration = false,
+			Vector2i imageSize_rgb = Vector2i(224, 171), Vector2i imageSize_d = Vector2i(224, 171));
+		~PicoFlexxEngine();
+
+		bool hasMoreImages(void) const;
+		void getImages(ITMUChar4Image *rgb, ITMShortImage *rawDepth);
+		Vector2i getDepthImageSize(void) const;
+		Vector2i getRGBImageSize(void) const;
+	};
+}
\ No newline at end of file
diff --git a/InfiniTAM/cmake/FindLibRoyale.cmake b/InfiniTAM/cmake/FindLibRoyale.cmake
new file mode 100644
index 000000000..46ffebf0d
--- /dev/null
+++ b/InfiniTAM/cmake/FindLibRoyale.cmake
@@ -0,0 +1,18 @@
+mark_as_advanced(FORCE LibRoyale_INCLUDE_DIR LibRoyale_LIBRARY LibRoyale_ROOT)
+
+find_path(LibRoyale_INCLUDE_DIR royale.hpp 
+	PATHS ${LibRoyale_INCLUDE_DIR} "C:/Program Files/royale/2.3.0.92/include" "/usr/local/include")
+
+find_path(LibRoyale_ROOT royale_license.txt 
+	PATHS ${LibRoyale_ROOT} "C:/Program Files/royale/2.3.0.92" "/usr/local")
+	
+find_library(LibRoyale_LIBRARY
+	NAMES royale
+	PATHS "C:/Program Files/royale/2.3.0.92/lib" "/usr/local/bin" ${CMAKE_LIB_PATH}
+)
+
+if (LibRoyale_LIBRARY AND LibRoyale_INCLUDE_DIR AND LibRoyale_ROOT)
+	set(LibRoyale_FOUND TRUE)
+else ()
+	set(LibRoyale_FOUND FALSE)
+endif()
\ No newline at end of file
diff --git a/InfiniTAM/cmake/LinkLibRoyale.cmake b/InfiniTAM/cmake/LinkLibRoyale.cmake
new file mode 100644
index 000000000..e90684393
--- /dev/null
+++ b/InfiniTAM/cmake/LinkLibRoyale.cmake
@@ -0,0 +1,11 @@
+#######################
+# LinkLibRoyale.cmake #
+#######################
+
+IF(WITH_LIBROYALE)
+  target_link_libraries(${targetname} ${LibRoyale_LIBRARY})
+  
+  IF(MSVC_IDE)
+    ADD_CUSTOM_COMMAND(TARGET ${targetname} POST_BUILD COMMAND ${CMAKE_COMMAND} -E copy_if_different "${LibRoyale_ROOT}/bin/royale.dll" "$<TARGET_FILE_DIR:${targetname}>")
+  ENDIF()
+ENDIF()
\ No newline at end of file
diff --git a/InfiniTAM/cmake/UseLibRoyale.cmake b/InfiniTAM/cmake/UseLibRoyale.cmake
new file mode 100644
index 000000000..1a0a30ed6
--- /dev/null
+++ b/InfiniTAM/cmake/UseLibRoyale.cmake
@@ -0,0 +1,11 @@
+######################
+# UseLibRoyale.cmake #
+######################
+
+OPTION(WITH_LIBROYALE "Build with LibRoyale support?" OFF)
+
+IF(WITH_LIBROYALE)
+  FIND_PACKAGE(LibRoyale REQUIRED)
+  INCLUDE_DIRECTORIES(${LibRoyale_INCLUDE_DIR})
+  ADD_DEFINITIONS(-DCOMPILE_WITH_LibRoyale)
+ENDIF()

From ba320683414a887b4a14bca4bd95c22aa1dc382f Mon Sep 17 00:00:00 2001
From: Victor Prisacariu <victor@robots.ox.ac.uk>
Date: Mon, 23 Jan 2017 12:02:01 +0000
Subject: [PATCH 2/3] various fixes

---
 InfiniTAM/ITMLib/Utils/ITMLibSettings.cpp |   2 +-
 InfiniTAM/InputSource/CMakeLists.txt      |   2 +-
 InfiniTAM/InputSource/PicoFlexxEngine.cpp | 110 ++++++++--------------
 InfiniTAM/InputSource/PicoFlexxEngine.h   |   2 +-
 InfiniTAM/cmake/FindLibRoyale.cmake       |  13 ++-
 InfiniTAM/cmake/LinkLibRoyale.cmake       |   2 +-
 6 files changed, 47 insertions(+), 84 deletions(-)

diff --git a/InfiniTAM/ITMLib/Utils/ITMLibSettings.cpp b/InfiniTAM/ITMLib/Utils/ITMLibSettings.cpp
index ceea0bdf5..fa078470b 100644
--- a/InfiniTAM/ITMLib/Utils/ITMLibSettings.cpp
+++ b/InfiniTAM/ITMLib/Utils/ITMLibSettings.cpp
@@ -39,7 +39,7 @@ ITMLibSettings::ITMLibSettings(void)
 	useBilateralFilter = false;
 
 	/// what to do on tracker failure: ignore, relocalise or stop integration - not supported in loop closure version
-	behaviourOnFailure = FAILUREMODE_IGNORE;
+	behaviourOnFailure = FAILUREMODE_RELOCALISE;
     
 	/// switch between various library modes - basic, with loop closure, etc.
 	libMode = LIBMODE_BASIC;
diff --git a/InfiniTAM/InputSource/CMakeLists.txt b/InfiniTAM/InputSource/CMakeLists.txt
index 1865e7bb8..0969aaa36 100644
--- a/InfiniTAM/InputSource/CMakeLists.txt
+++ b/InfiniTAM/InputSource/CMakeLists.txt
@@ -22,10 +22,10 @@ INCLUDE(${PROJECT_SOURCE_DIR}/cmake/UseCUDA.cmake)
 INCLUDE(${PROJECT_SOURCE_DIR}/cmake/UseFFmpeg.cmake)
 INCLUDE(${PROJECT_SOURCE_DIR}/cmake/UseGLUT.cmake)
 INCLUDE(${PROJECT_SOURCE_DIR}/cmake/UseKinect2.cmake)
+INCLUDE(${PROJECT_SOURCE_DIR}/cmake/UseLibRoyale.cmake)
 INCLUDE(${PROJECT_SOURCE_DIR}/cmake/UseOpenGL.cmake)
 INCLUDE(${PROJECT_SOURCE_DIR}/cmake/UseOpenMP.cmake)
 INCLUDE(${PROJECT_SOURCE_DIR}/cmake/UseOpenNI.cmake)
-INCLUDE(${PROJECT_SOURCE_DIR}/cmake/UseLibRoyale.cmake)
 INCLUDE(${PROJECT_SOURCE_DIR}/cmake/UseRealSense.cmake)
 INCLUDE(${PROJECT_SOURCE_DIR}/cmake/UseUVC.cmake)
 
diff --git a/InfiniTAM/InputSource/PicoFlexxEngine.cpp b/InfiniTAM/InputSource/PicoFlexxEngine.cpp
index 93354e542..e54240b5a 100644
--- a/InfiniTAM/InputSource/PicoFlexxEngine.cpp
+++ b/InfiniTAM/InputSource/PicoFlexxEngine.cpp
@@ -2,20 +2,19 @@
 
 #include "PicoFlexxEngine.h"
 
+#ifdef COMPILE_WITH_LibRoyale
+
 #include <cstdio>
 #include <stdexcept>
 #include <algorithm>
 #include <mutex>
 
-#ifdef COMPILE_WITH_LibRoyale
-
-#ifndef WIN32
-#include <unistd.h>
-#define ROYALE_SLEEP_MS(x) usleep(x * 1000)
-#else
+#ifdef WIN32
 #include <windows.h>
-#undef max
 #define ROYALE_SLEEP_MS(x) Sleep(x)
+#else
+#include <unistd.h>
+#define ROYALE_SLEEP_MS(x) usleep(x * 1000)
 #endif
 
 #include <royale.hpp>
@@ -33,7 +32,7 @@ class PicoFlexxEngine::PrivateData : public IDepthDataListener
 	vector<short> depthImage;
 	mutex mtx;
 
-	PrivateData(PicoFlexxEngine *pfe) : engine(pfe), depthImage(0) {};
+	explicit PrivateData(PicoFlexxEngine *pfe) : engine(pfe), depthImage(0) {}
 	~PrivateData() {}
 
 	void onNewData(const DepthData *data);
@@ -51,15 +50,15 @@ void PicoFlexxEngine::PrivateData::onNewData(const DepthData *data)
 	rgbImage.reserve(data->points.size());
 
 	// copy grayscale image data into an RGB data set
-	std::for_each(data->points.begin(), data->points.end(), [this](const DepthPoint &dd) {
+	for (int pointId = 0; pointId < data->points.count(); pointId++)
+	{
 		// PicoFlexx seems to return 0 when bright, and up to FF0 when totally dark
 		// convert this into a value into an 8 bit value
-		unsigned char charValue = dd.grayValue >> 4;
+		unsigned char charValue = data->points[pointId].grayValue >> 4;
 		Vector4u pixel;
 		pixel.x = pixel.y = pixel.z = pixel.w = charValue;
 		this->rgbImage.push_back(pixel);
-	});
-
+	};
 
 	// handle the depth image
 	engine->imageSize_d = Vector2i(data->width, data->height);
@@ -68,19 +67,15 @@ void PicoFlexxEngine::PrivateData::onNewData(const DepthData *data)
 	depthImage.reserve(data->points.size());
 
 	// copy depth image data, converting meters in float to millimeters in short
-	std::for_each(data->points.begin(), data->points.end(), [this](const DepthPoint &dd) {
+	for (int pointId = 0; pointId < data->points.count(); pointId++)
+	{
 		// do not copy if confidence is low. confidence is 0 when bad, 255 when good
 		// it seems there are no intermediate values, still let's cut at 128
-		if (dd.depthConfidence > 128) {
-			this->depthImage.push_back((short)(dd.z * 1000.0));
-		}
-		else {
-			this->depthImage.push_back(0);
-		}
-	});
+		const DepthPoint &dd = data->points[pointId];
+		this->depthImage.push_back(dd.depthConfidence > 128 ? (short)(dd.z * 1000.0) : 0);
+	};
 }
 
-
 PicoFlexxEngine::PicoFlexxEngine(const char *calibFilename, const char *deviceURI, const bool useInternalCalibration,
 	Vector2i requested_imageSize_rgb, Vector2i requested_imageSize_d)
 	: BaseImageSourceEngine(calibFilename)
@@ -94,7 +89,7 @@ PicoFlexxEngine::PicoFlexxEngine(const char *calibFilename, const char *deviceUR
 	{
 		CameraManager manager;
 
-		auto camlist = manager.getConnectedCameraList();
+		royale::Vector<royale::String> camlist = manager.getConnectedCameraList();
 		cout << "Detected " << camlist.size() << " camera(s)." << endl;
 		if (!camlist.empty())
 		{
@@ -129,8 +124,8 @@ PicoFlexxEngine::PicoFlexxEngine(const char *calibFilename, const char *deviceUR
 	this->calib.intrinsics_rgb.SetFrom(lensParams.focalLength.first, lensParams.focalLength.second,
 		lensParams.principalPoint.first, lensParams.principalPoint.second);
 
-	Vector<String> useCases;
-	auto status = data->cameraDevice->getUseCases(useCases);
+	royale::Vector<royale::String> useCases;
+	royale::CameraStatus status = data->cameraDevice->getUseCases(useCases);
 
 	if (status != CameraStatus::SUCCESS || useCases.empty())
 	{
@@ -141,9 +136,10 @@ PicoFlexxEngine::PicoFlexxEngine(const char *calibFilename, const char *deviceUR
 
 	// list the available use cases
 	cout << "Available Pico Flexx use cases:" << endl;
-	for_each(useCases.begin(), useCases.end(), [](const String &s) {
-		cout << s << endl;
-	});
+	for (size_t caseId = 0; caseId < useCases.size(); caseId++)
+	{
+		cout << useCases[caseId] << endl;
+	}
 
 	// register a data listener
 	if (data->cameraDevice->registerDataListener(data) != CameraStatus::SUCCESS)
@@ -173,13 +169,11 @@ PicoFlexxEngine::PicoFlexxEngine(const char *calibFilename, const char *deviceUR
 
 PicoFlexxEngine::~PicoFlexxEngine()
 {
-	if (data) {
+	if (data) 
+	{
 		// stop capture mode
-		if (data->cameraDevice) {
-			if (data->cameraDevice->stopCapture() != CameraStatus::SUCCESS) {
-				cerr << "Error stopping the capturing" << endl;
-			}
-		}
+		if (data->cameraDevice) 
+			if (data->cameraDevice->stopCapture() != CameraStatus::SUCCESS) cerr << "Error stopping the capturing" << endl;
 
 		delete data;
 	}
@@ -207,55 +201,25 @@ void PicoFlexxEngine::getImages(ITMUChar4Image *rgbImage, ITMShortImage *rawDept
 		memcpy(depth, data->depthImage.data(), rawDepthImage->dataSize * sizeof(short));
 	}
 	else memset(depth, 0, rawDepthImage->dataSize * sizeof(short));
-
-#if 0
-	cout << "depthImage size: " << data->depthImage.size() << endl;
-	for (int i = 0; i < imageSize_d[0]; ++i) {
-		cout << hex << data->depthImage.at(i) << " ";
-	}
-	cout << endl;
-
-	cout << "first line raw data:" << endl;
-	for (int i = 0; i < imageSize_d[0]; ++i) {
-		cout << hex << depth[i] << " ";
-	}
-	cout << endl;
-#endif
-
-	return /*true*/;
 }
 
 bool PicoFlexxEngine::hasMoreImages(void) const { return data != NULL; }
-Vector2i PicoFlexxEngine::getDepthImageSize(void) const { return (data != NULL) ? imageSize_d : Vector2i(0, 0); }
-Vector2i PicoFlexxEngine::getRGBImageSize(void) const { return (data != NULL) ? imageSize_rgb : Vector2i(0, 0); }
+Vector2i PicoFlexxEngine::getDepthImageSize(void) const { return data != NULL ? imageSize_d : Vector2i(0, 0); }
+Vector2i PicoFlexxEngine::getRGBImageSize(void) const { return data != NULL ? imageSize_rgb : Vector2i(0, 0); }
 
 #else
 
-using namespace InfiniTAM::Engine;
+using namespace InputSource;
 
 PicoFlexxEngine::PicoFlexxEngine(const char *calibFilename, const char *deviceURI, const bool useInternalCalibration, Vector2i requested_imageSize_rgb, Vector2i requested_imageSize_d)
-	: ImageSourceEngine(calibFilename)
+	: BaseImageSourceEngine(calibFilename)
 {
 	printf("compiled without LibRoyale support\n");
 }
-PicoFlexxEngine::~PicoFlexxEngine()
-{}
-void PicoFlexxEngine::getImages(ITMUChar4Image *rgbImage, ITMShortImage *rawDepthImage)
-{
-	return;
-}
-bool PicoFlexxEngine::hasMoreImages(void) const
-{
-	return false;
-}
-Vector2i PicoFlexxEngine::getDepthImageSize(void) const
-{
-	return Vector2i(0, 0);
-}
-Vector2i PicoFlexxEngine::getRGBImageSize(void) const
-{
-	return Vector2i(0, 0);
-}
-
-#endif
+PicoFlexxEngine::~PicoFlexxEngine() {}
+void PicoFlexxEngine::getImages(ITMUChar4Image *rgbImage, ITMShortImage *rawDepthImage) { }
+bool PicoFlexxEngine::hasMoreImages(void) const { return false; }
+Vector2i PicoFlexxEngine::getDepthImageSize(void) const { return Vector2i(0, 0); }
+Vector2i PicoFlexxEngine::getRGBImageSize(void) const { return Vector2i(0, 0); }
 
+#endif
\ No newline at end of file
diff --git a/InfiniTAM/InputSource/PicoFlexxEngine.h b/InfiniTAM/InputSource/PicoFlexxEngine.h
index 8a836af57..4d8a1ee5e 100644
--- a/InfiniTAM/InputSource/PicoFlexxEngine.h
+++ b/InfiniTAM/InputSource/PicoFlexxEngine.h
@@ -15,7 +15,7 @@ namespace InputSource
 		bool colorAvailable, depthAvailable;
 
 	public:
-		PicoFlexxEngine(const char *calibFilename, const char *deviceURI = NULL, const bool useInternalCalibration = false,
+		explicit PicoFlexxEngine(const char *calibFilename, const char *deviceURI = NULL, const bool useInternalCalibration = false,
 			Vector2i imageSize_rgb = Vector2i(224, 171), Vector2i imageSize_d = Vector2i(224, 171));
 		~PicoFlexxEngine();
 
diff --git a/InfiniTAM/cmake/FindLibRoyale.cmake b/InfiniTAM/cmake/FindLibRoyale.cmake
index 46ffebf0d..ad1461133 100644
--- a/InfiniTAM/cmake/FindLibRoyale.cmake
+++ b/InfiniTAM/cmake/FindLibRoyale.cmake
@@ -1,14 +1,13 @@
-mark_as_advanced(FORCE LibRoyale_INCLUDE_DIR LibRoyale_LIBRARY LibRoyale_ROOT)
-
-find_path(LibRoyale_INCLUDE_DIR royale.hpp 
-	PATHS ${LibRoyale_INCLUDE_DIR} "C:/Program Files/royale/2.3.0.92/include" "/usr/local/include")
-
 find_path(LibRoyale_ROOT royale_license.txt 
 	PATHS ${LibRoyale_ROOT} "C:/Program Files/royale/2.3.0.92" "/usr/local")
-	
+
 find_library(LibRoyale_LIBRARY
 	NAMES royale
-	PATHS "C:/Program Files/royale/2.3.0.92/lib" "/usr/local/bin" ${CMAKE_LIB_PATH}
+	PATHS "${LibRoyale_ROOT}/lib" {CMAKE_LIB_PATH}
+)
+
+find_path(LibRoyale_INCLUDE_DIR royale.hpp 
+	PATHS "${LibRoyale_ROOT}/include"
 )
 
 if (LibRoyale_LIBRARY AND LibRoyale_INCLUDE_DIR AND LibRoyale_ROOT)
diff --git a/InfiniTAM/cmake/LinkLibRoyale.cmake b/InfiniTAM/cmake/LinkLibRoyale.cmake
index e90684393..79eee756f 100644
--- a/InfiniTAM/cmake/LinkLibRoyale.cmake
+++ b/InfiniTAM/cmake/LinkLibRoyale.cmake
@@ -3,7 +3,7 @@
 #######################
 
 IF(WITH_LIBROYALE)
-  target_link_libraries(${targetname} ${LibRoyale_LIBRARY})
+  TARGET_LINK_LIBRARIES(${targetname} ${LibRoyale_LIBRARY})
   
   IF(MSVC_IDE)
     ADD_CUSTOM_COMMAND(TARGET ${targetname} POST_BUILD COMMAND ${CMAKE_COMMAND} -E copy_if_different "${LibRoyale_ROOT}/bin/royale.dll" "$<TARGET_FILE_DIR:${targetname}>")

From bd2f6cef93f6303c360f822024ffda672afbd377 Mon Sep 17 00:00:00 2001
From: Victor Prisacariu <victor@robots.ox.ac.uk>
Date: Mon, 23 Jan 2017 12:07:57 +0000
Subject: [PATCH 3/3] fixed copyright

---
 InfiniTAM/InputSource/PicoFlexxEngine.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/InfiniTAM/InputSource/PicoFlexxEngine.cpp b/InfiniTAM/InputSource/PicoFlexxEngine.cpp
index e54240b5a..eb32d3d99 100644
--- a/InfiniTAM/InputSource/PicoFlexxEngine.cpp
+++ b/InfiniTAM/InputSource/PicoFlexxEngine.cpp
@@ -1,4 +1,4 @@
-// Copyright 2017 Akos Maroy
+// Copyright 2017 Akos Maroy and the authors of InfiniTAM
 
 #include "PicoFlexxEngine.h"