From f98f0654a57335204c8e05639d5324e87481c09d Mon Sep 17 00:00:00 2001
From: ravich <ravich3141@gmail.com>
Date: Tue, 31 Oct 2017 15:16:23 +0530
Subject: [PATCH 1/8] Added ROS as another input source.

---
 InfiniTAM/Apps/InfiniTAM/InfiniTAM_ROS.cpp | 224 +++++++++++++++++++++
 InfiniTAM/InputSource/ROSEngine.cpp        | 130 ++++++++++++
 InfiniTAM/InputSource/ROSEngine.h          |  36 ++++
 3 files changed, 390 insertions(+)
 create mode 100644 InfiniTAM/Apps/InfiniTAM/InfiniTAM_ROS.cpp
 create mode 100644 InfiniTAM/InputSource/ROSEngine.cpp
 create mode 100644 InfiniTAM/InputSource/ROSEngine.h

diff --git a/InfiniTAM/Apps/InfiniTAM/InfiniTAM_ROS.cpp b/InfiniTAM/Apps/InfiniTAM/InfiniTAM_ROS.cpp
new file mode 100644
index 000000000..fef836516
--- /dev/null
+++ b/InfiniTAM/Apps/InfiniTAM/InfiniTAM_ROS.cpp
@@ -0,0 +1,224 @@
+// Copyright 2014-2017 Oxford University Innovation Limited and the authors of InfiniTAM
+
+#include <cstdlib>
+#include <iostream>
+
+#include <ros/ros.h>
+
+#include "UIEngine.h"
+
+#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"
+#include "../../InputSource/FFMPEGReader.h"
+#include "../../ITMLib/ITMLibDefines.h"
+#include "../../ITMLib/Core/ITMBasicEngine.h"
+#include "../../ITMLib/Core/ITMBasicSurfelEngine.h"
+#include "../../ITMLib/Core/ITMMultiEngine.h"
+
+using namespace InfiniTAM::Engine;
+using namespace InputSource;
+using namespace ITMLib;
+
+/** Create a default source of depth images from a list of command line
+    arguments. Typically, @para arg1 would identify the calibration file to
+    use, @para arg2 the colour images, @para arg3 the depth images and
+    @para arg4 the IMU images. If images are omitted, some live sources will
+    be tried.
+*/
+static void CreateDefaultImageSource(ImageSourceEngine* & imageSource, IMUSourceEngine* & imuSource, const char *arg1, const char *arg2, const char *arg3, const char *arg4)
+{
+	const char *calibFile = arg1;
+	const char *filename1 = arg2;
+	const char *filename2 = arg3;
+	const char *filename_imu = arg4;
+
+	if (strcmp(calibFile, "viewer") == 0)
+	{
+		imageSource = new BlankImageGenerator("", Vector2i(640, 480));
+		printf("starting in viewer mode: make sure to press n first to initiliase the views ... \n");
+		return;
+	}
+
+	printf("using calibration file: %s\n", calibFile);
+
+	if ((imageSource == NULL) && (filename2 != NULL))
+	{
+		printf("using rgb images: %s\nusing depth images: %s\n", filename1, filename2);
+		if (filename_imu == NULL)
+		{
+			ImageMaskPathGenerator pathGenerator(filename1, filename2);
+			imageSource = new ImageFileReader<ImageMaskPathGenerator>(calibFile, pathGenerator);
+		}
+		else
+		{
+			printf("using imu data: %s\n", filename_imu);
+			imageSource = new RawFileReader(calibFile, filename1, filename2, Vector2i(320, 240), 0.5f);
+			imuSource = new IMUSourceEngine(filename_imu);
+		}
+
+		if (imageSource->getDepthImageSize().x == 0)
+		{
+			delete imageSource;
+			if (imuSource != NULL) delete imuSource;
+			imuSource = NULL;
+			imageSource = NULL;
+		}
+	}
+
+	if ((imageSource == NULL) && (filename1 != NULL) && (filename_imu == NULL))
+	{
+		imageSource = new InputSource::FFMPEGReader(calibFile, filename1, filename2);
+		if (imageSource->getDepthImageSize().x == 0)
+		{
+			delete imageSource;
+			imageSource = NULL;
+		}
+	}
+
+	if (imageSource == NULL)
+	{
+		// If no calibration file specified, use the factory default calibration
+		bool useInternalCalibration = !calibFile || strlen(calibFile) == 0;
+
+		printf("trying OpenNI device: %s - calibration: %s\n",
+				filename1 ? filename1 : "<OpenNI default device>",
+				useInternalCalibration ? "internal" : "from file");
+		imageSource = new OpenNIEngine(calibFile, filename1, useInternalCalibration);
+		if (imageSource->getDepthImageSize().x == 0)
+		{
+			delete imageSource;
+			imageSource = NULL;
+		}
+	}
+
+	if (imageSource == NULL)
+	{
+		printf("trying UVC device\n");
+		imageSource = new LibUVCEngine(calibFile);
+		if (imageSource->getDepthImageSize().x == 0)
+		{
+			delete imageSource;
+			imageSource = NULL;
+		}
+	}
+
+	if (imageSource == NULL)
+	{
+		printf("trying RealSense device\n");
+		imageSource = new RealSenseEngine(calibFile);
+		if (imageSource->getDepthImageSize().x == 0)
+		{
+			delete imageSource;
+			imageSource = NULL;
+		}
+	}
+
+	if (imageSource == NULL)
+	{
+		printf("trying MS Kinect 2 device\n");
+		imageSource = new Kinect2Engine(calibFile);
+		if (imageSource->getDepthImageSize().x == 0)
+		{
+			delete imageSource;
+			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)
+try
+{
+	ros::init(argc, argv, "infinitam_ros");
+	ros::start();
+	
+	const char *arg1 = "";
+	const char *arg2 = NULL;
+	const char *arg3 = NULL;
+	const char *arg4 = NULL;
+
+	int arg = 1;
+	do {
+		if (argv[arg] != NULL) arg1 = argv[arg]; else break;
+		++arg;
+		if (argv[arg] != NULL) arg2 = argv[arg]; else break;
+		++arg;
+		if (argv[arg] != NULL) arg3 = argv[arg]; else break;
+		++arg;
+		if (argv[arg] != NULL) arg4 = argv[arg]; else break;
+	} while (false);
+
+	if (arg == 1) {
+		printf("usage: %s [<calibfile> [<imagesource>] ]\n"
+		       "  <calibfile>   : path to a file containing intrinsic calibration parameters\n"
+		       "  <imagesource> : either one argument to specify OpenNI device ID\n"
+		       "                  or two arguments specifying rgb and depth file masks\n"
+		       "\n"
+		       "examples:\n"
+		       "  %s ./Files/Teddy/calib.txt ./Files/Teddy/Frames/%%04i.ppm ./Files/Teddy/Frames/%%04i.pgm\n"
+		       "  %s ./Files/Teddy/calib.txt\n\n", argv[0], argv[0], argv[0]);
+	}
+
+	printf("initialising ...\n");
+	ImageSourceEngine *imageSource = NULL;
+	IMUSourceEngine *imuSource = NULL;
+
+	CreateDefaultImageSource(imageSource, imuSource, arg1, arg2, arg3, arg4);
+	if (imageSource==NULL)
+	{
+		std::cout << "failed to open any image stream" << std::endl;
+		return -1;
+	}
+
+	ITMLibSettings *internalSettings = new ITMLibSettings();
+
+	ITMMainEngine *mainEngine = NULL;
+	switch (internalSettings->libMode)
+	{
+	case ITMLibSettings::LIBMODE_BASIC:
+		mainEngine = new ITMBasicEngine<ITMVoxel, ITMVoxelIndex>(internalSettings, imageSource->getCalib(), imageSource->getRGBImageSize(), imageSource->getDepthImageSize());
+		break;
+	case ITMLibSettings::LIBMODE_BASIC_SURFELS:
+		mainEngine = new ITMBasicSurfelEngine<ITMSurfelT>(internalSettings, imageSource->getCalib(), imageSource->getRGBImageSize(), imageSource->getDepthImageSize());
+		break;
+	case ITMLibSettings::LIBMODE_LOOPCLOSURE:
+		mainEngine = new ITMMultiEngine<ITMVoxel, ITMVoxelIndex>(internalSettings, imageSource->getCalib(), imageSource->getRGBImageSize(), imageSource->getDepthImageSize());
+		break;
+	default: 
+		throw std::runtime_error("Unsupported library mode!");
+		break;
+	}
+
+	UIEngine::Instance()->Initialise(argc, argv, imageSource, imuSource, mainEngine, "./Files/Out", internalSettings->deviceType);
+	UIEngine::Instance()->Run();
+	UIEngine::Instance()->Shutdown();
+
+	delete mainEngine;
+	delete internalSettings;
+	delete imageSource;
+	if (imuSource != NULL) delete imuSource;
+
+	ros::shutdown();
+	return 0;
+}
+catch(std::exception& e)
+{
+	std::cerr << e.what() << '\n';
+	return EXIT_FAILURE;
+}
+
diff --git a/InfiniTAM/InputSource/ROSEngine.cpp b/InfiniTAM/InputSource/ROSEngine.cpp
new file mode 100644
index 000000000..118251527
--- /dev/null
+++ b/InfiniTAM/InputSource/ROSEngine.cpp
@@ -0,0 +1,130 @@
+// authored by https://github.com/ravich2-7183
+
+#include "ROSEngine.h"
+
+#include "../ORUtils/FileUtils.h"
+
+#include <cstdio>
+#include <iostream>
+#include <stdexcept>
+#include <string>
+#include <thread>
+#include <mutex>
+#include <functional>
+
+#include <ros/ros.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/time_synchronizer.h>
+#include <sensor_msgs/Image.h>
+#include <image_transport/image_transport.h>
+#include <cv_bridge/cv_bridge.h>
+#include <sensor_msgs/image_encodings.h>
+
+using namespace InputSource;
+using namespace sensor_msgs;
+using namespace message_filters;
+
+ROSEngine::topicListenerThread()
+{
+	// subscribe to rgb and depth topics
+	message_filters::Subscriber<sensor_msgs::Image> rgb_sub_(nh_, "/dtam/rgb", 1); // TODO remove dtam and generalize to: /namespace/rgb, /namespace/depth
+	message_filters::Subscriber<sensor_msgs::Image> depth_sub_(nh_, "/dtam/depth", 1); // uint16 depth image in mm. Native OpenNI format, preferred by InfiniTAM.
+	TimeSynchronizer<Image, Image> sync(rgb_sub_, depth_sub_, 10);
+	sync.registerCallback(std::bind(&ROSEngine::processMessage, _1, _2));
+
+	ros::spin();	
+}
+
+ROSEngine::processMessage(const ImageConstPtr& rgb_image, const ImageConstPtr& depth_image)
+{
+	std::lock_guard<std::mutex> process_message_lock(images_mutex_);
+
+	// TODO document that rgb_image must have an encoding equivalent to RGBA8
+	
+	// copy rgb_image into rgb_image_
+	uint8 *rgb_image_data = rgb_image.data;
+	for (int i = 0; i < rgb_image_->noDims.x * rgb_image_->noDims.y; i++) {
+			Vector4u newPix;
+			newPix.x = rgb_image_data[i*4+0];
+			newPix.y = rgb_image_data[i*4+1];
+			newPix.z = rgb_image_data[i*4+2];
+			newPix.w = 255;
+			
+			rgb_image_[i] = newPix;
+	}
+
+	// copy depth_image into depth_image_
+	const short *depth_image_data = (const short*)depth_image.data;
+	memcpy(depth_image_, depth_image_data, depth_image.height*depth_image.step);
+}
+
+ROSEngine::ROSEngine(const char *calibFilename, const char *deviceURI, const bool useInternalCalibration,
+	Vector2i requested_imageSize_rgb, Vector2i requested_imageSize_d) :
+	BaseImageSourceEngine(calibFilename),
+	nh_(),
+	rgb_image_(requested_imageSize_rgb),
+	depth_image_(requested_imageSize_d)
+{
+	// Start up topic listener thread
+	std::thread topic_listener_thread(&ROSEngine::topicListenerThread, this);
+	topic_listener_thread.join();
+
+	// TODO document that depth images must be in millimeters
+	this->calib.disparityCalib.SetStandard(); // assumes depth is in millimeters
+
+	this->imageSize_d	= Vector2i(0,0);
+	this->imageSize_rgb = Vector2i(0,0);
+
+	if (useInternalCalibration) { // TODO correct this
+		this->calib.trafo_rgb_to_depth = ITMLib::ITMExtrinsics();
+		if (depthAvailable) {
+			float h_fov = data->depthStream.getHorizontalFieldOfView();
+			float v_fov = data->depthStream.getVerticalFieldOfView();
+			this->calib.intrinsics_d.SetFrom(
+				(float)imageSize_d.x / (2.0f * tan(h_fov/2.0f)),
+				(float)imageSize_d.y / (2.0f * tan(v_fov/2.0f)),
+				(float)imageSize_d.x / 2.0f,
+				(float)imageSize_d.y / 2.0f);
+		}
+		if (colorAvailable) {
+			float h_fov = data->colorStream.getHorizontalFieldOfView();
+			float v_fov = data->colorStream.getVerticalFieldOfView();
+			this->calib.intrinsics_rgb.SetFrom(
+				(float)imageSize_rgb.x / (2.0f * tan(h_fov/2.0f)),
+				(float)imageSize_rgb.y / (2.0f * tan(v_fov/2.0f)),
+				(float)imageSize_rgb.x / 2.0f,
+				(float)imageSize_rgb.y / 2.0f);
+		}
+	}
+}
+
+void ROSEngine::getImages(ITMUChar4Image *rgbImage, ITMShortImage *rawDepthImage)
+{
+	std::lock_guard<std::mutex> get_images_lock(images_mutex_);
+
+	rgbImage->SetFrom(&rgb_image_,       MemoryCopyDirection::CPU_TO_CPU);
+	rawDepthImage>SetFrom(&depth_image_, MemoryCopyDirection::CPU_TO_CPU);
+}
+
+bool ROSEngine::hasMoreImages(void) const
+{
+	return ros::ok();
+}
+
+Vector2i ROSEngine::getDepthImageSize(void) const
+{
+	return data ? imageSize_d : Vector2i(0,0);
+}
+
+Vector2i ROSEngine::getRGBImageSize(void) const
+{
+	return data ? imageSize_rgb : Vector2i(0,0);
+}
+
+ROSEngine::~ROSEngine()
+{
+	if (data) {
+		delete data;
+		data = NULL;
+	}
+}
diff --git a/InfiniTAM/InputSource/ROSEngine.h b/InfiniTAM/InputSource/ROSEngine.h
new file mode 100644
index 000000000..d7c4c0d8a
--- /dev/null
+++ b/InfiniTAM/InputSource/ROSEngine.h
@@ -0,0 +1,36 @@
+// Copyright 2014-2017 Oxford University Innovation Limited and the authors of InfiniTAM
+
+#pragma once
+
+#include <mutex>
+
+#include "ImageSourceEngine.h"
+
+#if (!defined USING_CMAKE) && (defined _MSC_VER)
+#pragma comment(lib, "OpenNI2")
+#endif
+
+namespace InputSource {
+
+class ROSEngine : public BaseImageSourceEngine
+{
+private:
+	ros::NodeHandle nh_;
+	ros::Subscriber rgb_sub_, depth_sub_;
+	Vector2i imageSize_rgb_, imageSize_d_;
+	ITMUChar4Image rgb_image_;
+	ITMShortImage depth_image_;
+	std::mutex images_mutex_;
+
+public:
+	ROSEngine(const char *calibFilename, const char *deviceURI = NULL, const bool useInternalCalibration = false,
+		Vector2i imageSize_rgb = Vector2i(640, 480), Vector2i imageSize_d = Vector2i(640, 480));
+	~ROSEngine();
+
+	bool hasMoreImages(void) const;
+	void getImages(ITMUChar4Image *rgb, ITMShortImage *rawDepth);
+	Vector2i getDepthImageSize(void) const;
+	Vector2i getRGBImageSize(void) const;
+};
+
+}

From 5c6d3176b72c69b57740cd77418c857bda9c2dd9 Mon Sep 17 00:00:00 2001
From: ravich <ravich3141@gmail.com>
Date: Thu, 2 Nov 2017 12:42:11 +0530
Subject: [PATCH 2/8] Catkinized InfiniTAM.

---
 InfiniTAM/CMakeLists.txt => CMakeLists.txt    |  38 ++-
 InfiniTAM/Apps/InfiniTAM/CMakeLists.txt       |   2 +-
 InfiniTAM/Apps/InfiniTAM/InfiniTAM.cpp        |  17 ++
 InfiniTAM/Apps/InfiniTAM/InfiniTAM_ROS.cpp    | 224 ------------------
 InfiniTAM/InputSource/CMakeLists.txt          |   2 +
 InfiniTAM/InputSource/ROSEngine.cpp           | 119 ++++------
 InfiniTAM/InputSource/ROSEngine.h             |  25 +-
 .../cmake => cmake}/CUDACheckCompute.cmake    |   0
 {InfiniTAM/cmake => cmake}/FindCSparse.cmake  |   0
 {InfiniTAM/cmake => cmake}/FindFFmpeg.cmake   |   0
 .../cmake => cmake}/FindLibRoyale.cmake       |   0
 {InfiniTAM/cmake => cmake}/FindOpenNI.cmake   |   0
 .../cmake => cmake}/FindRealSense.cmake       |   0
 {InfiniTAM/cmake => cmake}/Flags.cmake        |   0
 {InfiniTAM/cmake => cmake}/LinkFFmpeg.cmake   |   0
 {InfiniTAM/cmake => cmake}/LinkGLUT.cmake     |   0
 .../cmake => cmake}/LinkLibRoyale.cmake       |   0
 {InfiniTAM/cmake => cmake}/LinkOpenGL.cmake   |   0
 {InfiniTAM/cmake => cmake}/LinkOpenNI.cmake   |   0
 {InfiniTAM/cmake => cmake}/LinkPNG.cmake      |   0
 .../cmake => cmake}/LinkRealSense.cmake       |   0
 {InfiniTAM/cmake => cmake}/LinkUVC.cmake      |   0
 .../cmake => cmake}/OfferC++11Support.cmake   |   0
 .../cmake => cmake}/OfferInputMirroring.cmake |   0
 .../cmake => cmake}/SetCUDAAppTarget.cmake    |   0
 .../cmake => cmake}/SetCUDALibTarget.cmake    |   0
 {InfiniTAM/cmake => cmake}/SetLibTarget.cmake |   0
 {InfiniTAM/cmake => cmake}/UseCUDA.cmake      |   0
 {InfiniTAM/cmake => cmake}/UseFFmpeg.cmake    |   0
 {InfiniTAM/cmake => cmake}/UseGLUT.cmake      |   0
 {InfiniTAM/cmake => cmake}/UseKinect2.cmake   |   0
 {InfiniTAM/cmake => cmake}/UseLibRoyale.cmake |   0
 {InfiniTAM/cmake => cmake}/UseOpenGL.cmake    |   0
 {InfiniTAM/cmake => cmake}/UseOpenMP.cmake    |   0
 {InfiniTAM/cmake => cmake}/UseOpenNI.cmake    |   0
 {InfiniTAM/cmake => cmake}/UsePNG.cmake       |   0
 {InfiniTAM/cmake => cmake}/UseRealSense.cmake |   0
 {InfiniTAM/cmake => cmake}/UseUVC.cmake       |   0
 .../cuda_compute_capability.cpp               |   0
 package.xml                                   |  25 ++
 40 files changed, 138 insertions(+), 314 deletions(-)
 rename InfiniTAM/CMakeLists.txt => CMakeLists.txt (61%)
 delete mode 100644 InfiniTAM/Apps/InfiniTAM/InfiniTAM_ROS.cpp
 rename {InfiniTAM/cmake => cmake}/CUDACheckCompute.cmake (100%)
 rename {InfiniTAM/cmake => cmake}/FindCSparse.cmake (100%)
 rename {InfiniTAM/cmake => cmake}/FindFFmpeg.cmake (100%)
 rename {InfiniTAM/cmake => cmake}/FindLibRoyale.cmake (100%)
 rename {InfiniTAM/cmake => cmake}/FindOpenNI.cmake (100%)
 rename {InfiniTAM/cmake => cmake}/FindRealSense.cmake (100%)
 rename {InfiniTAM/cmake => cmake}/Flags.cmake (100%)
 rename {InfiniTAM/cmake => cmake}/LinkFFmpeg.cmake (100%)
 rename {InfiniTAM/cmake => cmake}/LinkGLUT.cmake (100%)
 rename {InfiniTAM/cmake => cmake}/LinkLibRoyale.cmake (100%)
 rename {InfiniTAM/cmake => cmake}/LinkOpenGL.cmake (100%)
 rename {InfiniTAM/cmake => cmake}/LinkOpenNI.cmake (100%)
 rename {InfiniTAM/cmake => cmake}/LinkPNG.cmake (100%)
 rename {InfiniTAM/cmake => cmake}/LinkRealSense.cmake (100%)
 rename {InfiniTAM/cmake => cmake}/LinkUVC.cmake (100%)
 rename {InfiniTAM/cmake => cmake}/OfferC++11Support.cmake (100%)
 rename {InfiniTAM/cmake => cmake}/OfferInputMirroring.cmake (100%)
 rename {InfiniTAM/cmake => cmake}/SetCUDAAppTarget.cmake (100%)
 rename {InfiniTAM/cmake => cmake}/SetCUDALibTarget.cmake (100%)
 rename {InfiniTAM/cmake => cmake}/SetLibTarget.cmake (100%)
 rename {InfiniTAM/cmake => cmake}/UseCUDA.cmake (100%)
 rename {InfiniTAM/cmake => cmake}/UseFFmpeg.cmake (100%)
 rename {InfiniTAM/cmake => cmake}/UseGLUT.cmake (100%)
 rename {InfiniTAM/cmake => cmake}/UseKinect2.cmake (100%)
 rename {InfiniTAM/cmake => cmake}/UseLibRoyale.cmake (100%)
 rename {InfiniTAM/cmake => cmake}/UseOpenGL.cmake (100%)
 rename {InfiniTAM/cmake => cmake}/UseOpenMP.cmake (100%)
 rename {InfiniTAM/cmake => cmake}/UseOpenNI.cmake (100%)
 rename {InfiniTAM/cmake => cmake}/UsePNG.cmake (100%)
 rename {InfiniTAM/cmake => cmake}/UseRealSense.cmake (100%)
 rename {InfiniTAM/cmake => cmake}/UseUVC.cmake (100%)
 rename {InfiniTAM/cmake => cmake}/cuda_compute_capability.cpp (100%)
 create mode 100644 package.xml

diff --git a/InfiniTAM/CMakeLists.txt b/CMakeLists.txt
similarity index 61%
rename from InfiniTAM/CMakeLists.txt
rename to CMakeLists.txt
index c249e7578..87f69e131 100644
--- a/InfiniTAM/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -12,6 +12,32 @@ PROJECT(InfiniTAM)
 
 SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/")
 
+####################################
+# Find catkin macros and libraries #
+####################################
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  std_msgs
+  message_filters
+)
+
+find_package(Boost REQUIRED
+  system
+)
+
+include_directories(
+  ${Boost_INCLUDE_DIRS}
+  ${catkin_INCLUDE_DIRS}
+)
+
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES using_markers
+#  CATKIN_DEPENDS roscpp visualization_msgs
+#  DEPENDS system_lib
+)
+
 #################################
 # Add additional compiler flags #
 #################################
@@ -38,9 +64,9 @@ ENDIF()
 # Add subdirectories #
 ######################
 
-ADD_SUBDIRECTORY(Apps)
-ADD_SUBDIRECTORY(FernRelocLib)
-ADD_SUBDIRECTORY(InputSource)
-ADD_SUBDIRECTORY(ITMLib)
-ADD_SUBDIRECTORY(MiniSlamGraphLib)
-ADD_SUBDIRECTORY(ORUtils)
+ADD_SUBDIRECTORY(InfiniTAM/Apps)
+ADD_SUBDIRECTORY(InfiniTAM/FernRelocLib)
+ADD_SUBDIRECTORY(InfiniTAM/InputSource)
+ADD_SUBDIRECTORY(InfiniTAM/ITMLib)
+ADD_SUBDIRECTORY(InfiniTAM/MiniSlamGraphLib)
+ADD_SUBDIRECTORY(InfiniTAM/ORUtils)
diff --git a/InfiniTAM/Apps/InfiniTAM/CMakeLists.txt b/InfiniTAM/Apps/InfiniTAM/CMakeLists.txt
index e9bef643b..52b033e59 100644
--- a/InfiniTAM/Apps/InfiniTAM/CMakeLists.txt
+++ b/InfiniTAM/Apps/InfiniTAM/CMakeLists.txt
@@ -52,7 +52,7 @@ INCLUDE(${PROJECT_SOURCE_DIR}/cmake/SetCUDAAppTarget.cmake)
 # Specify the libraries to link #
 #################################
 
-TARGET_LINK_LIBRARIES(${targetname} InputSource ITMLib MiniSlamGraphLib ORUtils FernRelocLib)
+TARGET_LINK_LIBRARIES(${targetname} InputSource ITMLib MiniSlamGraphLib ORUtils FernRelocLib ${catkin_LIBRARIES})
 INCLUDE(${PROJECT_SOURCE_DIR}/cmake/LinkFFmpeg.cmake)
 INCLUDE(${PROJECT_SOURCE_DIR}/cmake/LinkGLUT.cmake)
 INCLUDE(${PROJECT_SOURCE_DIR}/cmake/LinkLibRoyale.cmake)
diff --git a/InfiniTAM/Apps/InfiniTAM/InfiniTAM.cpp b/InfiniTAM/Apps/InfiniTAM/InfiniTAM.cpp
index 14e329f39..59f037c6e 100644
--- a/InfiniTAM/Apps/InfiniTAM/InfiniTAM.cpp
+++ b/InfiniTAM/Apps/InfiniTAM/InfiniTAM.cpp
@@ -3,8 +3,11 @@
 #include <cstdlib>
 #include <iostream>
 
+#include <ros/ros.h>
+
 #include "UIEngine.h"
 
+#include "../../InputSource/ROSEngine.h"
 #include "../../InputSource/OpenNIEngine.h"
 #include "../../InputSource/Kinect2Engine.h"
 #include "../../InputSource/LibUVCEngine.h"
@@ -78,6 +81,17 @@ static void CreateDefaultImageSource(ImageSourceEngine* & imageSource, IMUSource
 		}
 	}
 
+	if (imageSource == NULL)
+	{
+		printf("trying ROS input: /depth/image_raw, /rgb/image_raw \n");
+		imageSource = new ROSEngine(calibFile);
+		if (imageSource->getDepthImageSize().x == 0)
+		{
+			delete imageSource;
+			imageSource = NULL;
+		}
+	}
+
 	if (imageSource == NULL)
 	{
 		// If no calibration file specified, use the factory default calibration
@@ -142,6 +156,8 @@ static void CreateDefaultImageSource(ImageSourceEngine* & imageSource, IMUSource
 int main(int argc, char** argv)
 try
 {
+	ros::init(argc, argv, "infinitam_ros");
+	
 	const char *arg1 = "";
 	const char *arg2 = NULL;
 	const char *arg3 = NULL;
@@ -207,6 +223,7 @@ try
 	delete internalSettings;
 	delete imageSource;
 	if (imuSource != NULL) delete imuSource;
+
 	return 0;
 }
 catch(std::exception& e)
diff --git a/InfiniTAM/Apps/InfiniTAM/InfiniTAM_ROS.cpp b/InfiniTAM/Apps/InfiniTAM/InfiniTAM_ROS.cpp
deleted file mode 100644
index fef836516..000000000
--- a/InfiniTAM/Apps/InfiniTAM/InfiniTAM_ROS.cpp
+++ /dev/null
@@ -1,224 +0,0 @@
-// Copyright 2014-2017 Oxford University Innovation Limited and the authors of InfiniTAM
-
-#include <cstdlib>
-#include <iostream>
-
-#include <ros/ros.h>
-
-#include "UIEngine.h"
-
-#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"
-#include "../../InputSource/FFMPEGReader.h"
-#include "../../ITMLib/ITMLibDefines.h"
-#include "../../ITMLib/Core/ITMBasicEngine.h"
-#include "../../ITMLib/Core/ITMBasicSurfelEngine.h"
-#include "../../ITMLib/Core/ITMMultiEngine.h"
-
-using namespace InfiniTAM::Engine;
-using namespace InputSource;
-using namespace ITMLib;
-
-/** Create a default source of depth images from a list of command line
-    arguments. Typically, @para arg1 would identify the calibration file to
-    use, @para arg2 the colour images, @para arg3 the depth images and
-    @para arg4 the IMU images. If images are omitted, some live sources will
-    be tried.
-*/
-static void CreateDefaultImageSource(ImageSourceEngine* & imageSource, IMUSourceEngine* & imuSource, const char *arg1, const char *arg2, const char *arg3, const char *arg4)
-{
-	const char *calibFile = arg1;
-	const char *filename1 = arg2;
-	const char *filename2 = arg3;
-	const char *filename_imu = arg4;
-
-	if (strcmp(calibFile, "viewer") == 0)
-	{
-		imageSource = new BlankImageGenerator("", Vector2i(640, 480));
-		printf("starting in viewer mode: make sure to press n first to initiliase the views ... \n");
-		return;
-	}
-
-	printf("using calibration file: %s\n", calibFile);
-
-	if ((imageSource == NULL) && (filename2 != NULL))
-	{
-		printf("using rgb images: %s\nusing depth images: %s\n", filename1, filename2);
-		if (filename_imu == NULL)
-		{
-			ImageMaskPathGenerator pathGenerator(filename1, filename2);
-			imageSource = new ImageFileReader<ImageMaskPathGenerator>(calibFile, pathGenerator);
-		}
-		else
-		{
-			printf("using imu data: %s\n", filename_imu);
-			imageSource = new RawFileReader(calibFile, filename1, filename2, Vector2i(320, 240), 0.5f);
-			imuSource = new IMUSourceEngine(filename_imu);
-		}
-
-		if (imageSource->getDepthImageSize().x == 0)
-		{
-			delete imageSource;
-			if (imuSource != NULL) delete imuSource;
-			imuSource = NULL;
-			imageSource = NULL;
-		}
-	}
-
-	if ((imageSource == NULL) && (filename1 != NULL) && (filename_imu == NULL))
-	{
-		imageSource = new InputSource::FFMPEGReader(calibFile, filename1, filename2);
-		if (imageSource->getDepthImageSize().x == 0)
-		{
-			delete imageSource;
-			imageSource = NULL;
-		}
-	}
-
-	if (imageSource == NULL)
-	{
-		// If no calibration file specified, use the factory default calibration
-		bool useInternalCalibration = !calibFile || strlen(calibFile) == 0;
-
-		printf("trying OpenNI device: %s - calibration: %s\n",
-				filename1 ? filename1 : "<OpenNI default device>",
-				useInternalCalibration ? "internal" : "from file");
-		imageSource = new OpenNIEngine(calibFile, filename1, useInternalCalibration);
-		if (imageSource->getDepthImageSize().x == 0)
-		{
-			delete imageSource;
-			imageSource = NULL;
-		}
-	}
-
-	if (imageSource == NULL)
-	{
-		printf("trying UVC device\n");
-		imageSource = new LibUVCEngine(calibFile);
-		if (imageSource->getDepthImageSize().x == 0)
-		{
-			delete imageSource;
-			imageSource = NULL;
-		}
-	}
-
-	if (imageSource == NULL)
-	{
-		printf("trying RealSense device\n");
-		imageSource = new RealSenseEngine(calibFile);
-		if (imageSource->getDepthImageSize().x == 0)
-		{
-			delete imageSource;
-			imageSource = NULL;
-		}
-	}
-
-	if (imageSource == NULL)
-	{
-		printf("trying MS Kinect 2 device\n");
-		imageSource = new Kinect2Engine(calibFile);
-		if (imageSource->getDepthImageSize().x == 0)
-		{
-			delete imageSource;
-			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)
-try
-{
-	ros::init(argc, argv, "infinitam_ros");
-	ros::start();
-	
-	const char *arg1 = "";
-	const char *arg2 = NULL;
-	const char *arg3 = NULL;
-	const char *arg4 = NULL;
-
-	int arg = 1;
-	do {
-		if (argv[arg] != NULL) arg1 = argv[arg]; else break;
-		++arg;
-		if (argv[arg] != NULL) arg2 = argv[arg]; else break;
-		++arg;
-		if (argv[arg] != NULL) arg3 = argv[arg]; else break;
-		++arg;
-		if (argv[arg] != NULL) arg4 = argv[arg]; else break;
-	} while (false);
-
-	if (arg == 1) {
-		printf("usage: %s [<calibfile> [<imagesource>] ]\n"
-		       "  <calibfile>   : path to a file containing intrinsic calibration parameters\n"
-		       "  <imagesource> : either one argument to specify OpenNI device ID\n"
-		       "                  or two arguments specifying rgb and depth file masks\n"
-		       "\n"
-		       "examples:\n"
-		       "  %s ./Files/Teddy/calib.txt ./Files/Teddy/Frames/%%04i.ppm ./Files/Teddy/Frames/%%04i.pgm\n"
-		       "  %s ./Files/Teddy/calib.txt\n\n", argv[0], argv[0], argv[0]);
-	}
-
-	printf("initialising ...\n");
-	ImageSourceEngine *imageSource = NULL;
-	IMUSourceEngine *imuSource = NULL;
-
-	CreateDefaultImageSource(imageSource, imuSource, arg1, arg2, arg3, arg4);
-	if (imageSource==NULL)
-	{
-		std::cout << "failed to open any image stream" << std::endl;
-		return -1;
-	}
-
-	ITMLibSettings *internalSettings = new ITMLibSettings();
-
-	ITMMainEngine *mainEngine = NULL;
-	switch (internalSettings->libMode)
-	{
-	case ITMLibSettings::LIBMODE_BASIC:
-		mainEngine = new ITMBasicEngine<ITMVoxel, ITMVoxelIndex>(internalSettings, imageSource->getCalib(), imageSource->getRGBImageSize(), imageSource->getDepthImageSize());
-		break;
-	case ITMLibSettings::LIBMODE_BASIC_SURFELS:
-		mainEngine = new ITMBasicSurfelEngine<ITMSurfelT>(internalSettings, imageSource->getCalib(), imageSource->getRGBImageSize(), imageSource->getDepthImageSize());
-		break;
-	case ITMLibSettings::LIBMODE_LOOPCLOSURE:
-		mainEngine = new ITMMultiEngine<ITMVoxel, ITMVoxelIndex>(internalSettings, imageSource->getCalib(), imageSource->getRGBImageSize(), imageSource->getDepthImageSize());
-		break;
-	default: 
-		throw std::runtime_error("Unsupported library mode!");
-		break;
-	}
-
-	UIEngine::Instance()->Initialise(argc, argv, imageSource, imuSource, mainEngine, "./Files/Out", internalSettings->deviceType);
-	UIEngine::Instance()->Run();
-	UIEngine::Instance()->Shutdown();
-
-	delete mainEngine;
-	delete internalSettings;
-	delete imageSource;
-	if (imuSource != NULL) delete imuSource;
-
-	ros::shutdown();
-	return 0;
-}
-catch(std::exception& e)
-{
-	std::cerr << e.what() << '\n';
-	return EXIT_FAILURE;
-}
-
diff --git a/InfiniTAM/InputSource/CMakeLists.txt b/InfiniTAM/InputSource/CMakeLists.txt
index 0969aaa36..293653acc 100644
--- a/InfiniTAM/InputSource/CMakeLists.txt
+++ b/InfiniTAM/InputSource/CMakeLists.txt
@@ -44,6 +44,7 @@ LibUVCEngine.cpp
 OpenNIEngine.cpp
 PicoFlexxEngine.cpp
 RealSenseEngine.cpp
+ROSEngine.cpp
 )
 
 SET(headers
@@ -57,6 +58,7 @@ LibUVCEngine.h
 OpenNIEngine.h
 PicoFlexxEngine.h
 RealSenseEngine.h
+ROSEngine.h
 )
 
 #############################
diff --git a/InfiniTAM/InputSource/ROSEngine.cpp b/InfiniTAM/InputSource/ROSEngine.cpp
index 118251527..b70f03c3e 100644
--- a/InfiniTAM/InputSource/ROSEngine.cpp
+++ b/InfiniTAM/InputSource/ROSEngine.cpp
@@ -2,108 +2,72 @@
 
 #include "ROSEngine.h"
 
-#include "../ORUtils/FileUtils.h"
-
-#include <cstdio>
-#include <iostream>
-#include <stdexcept>
-#include <string>
-#include <thread>
-#include <mutex>
-#include <functional>
-
-#include <ros/ros.h>
-#include <message_filters/subscriber.h>
-#include <message_filters/time_synchronizer.h>
-#include <sensor_msgs/Image.h>
-#include <image_transport/image_transport.h>
-#include <cv_bridge/cv_bridge.h>
-#include <sensor_msgs/image_encodings.h>
-
+using namespace ORUtils;
 using namespace InputSource;
 using namespace sensor_msgs;
 using namespace message_filters;
 
-ROSEngine::topicListenerThread()
-{
-	// subscribe to rgb and depth topics
-	message_filters::Subscriber<sensor_msgs::Image> rgb_sub_(nh_, "/dtam/rgb", 1); // TODO remove dtam and generalize to: /namespace/rgb, /namespace/depth
-	message_filters::Subscriber<sensor_msgs::Image> depth_sub_(nh_, "/dtam/depth", 1); // uint16 depth image in mm. Native OpenNI format, preferred by InfiniTAM.
-	TimeSynchronizer<Image, Image> sync(rgb_sub_, depth_sub_, 10);
-	sync.registerCallback(std::bind(&ROSEngine::processMessage, _1, _2));
-
-	ros::spin();	
-}
-
-ROSEngine::processMessage(const ImageConstPtr& rgb_image, const ImageConstPtr& depth_image)
+void ROSEngine::processMessage(const ImageConstPtr& rgb_image, const ImageConstPtr& depth_image)
 {
 	std::lock_guard<std::mutex> process_message_lock(images_mutex_);
 
 	// TODO document that rgb_image must have an encoding equivalent to RGBA8
-	
+
 	// copy rgb_image into rgb_image_
-	uint8 *rgb_image_data = rgb_image.data;
-	for (int i = 0; i < rgb_image_->noDims.x * rgb_image_->noDims.y; i++) {
-			Vector4u newPix;
-			newPix.x = rgb_image_data[i*4+0];
-			newPix.y = rgb_image_data[i*4+1];
-			newPix.z = rgb_image_data[i*4+2];
-			newPix.w = 255;
-			
-			rgb_image_[i] = newPix;
+	Vector4u *rgb = rgb_image_.GetData(MEMORYDEVICE_CPU);
+	for(int i = 0; i < rgb_image_.noDims.x * rgb_image_.noDims.y; i++) {
+		Vector4u newPix;
+		newPix.x = (rgb_image->data)[i*4+0];
+		newPix.y = (rgb_image->data)[i*4+1];
+		newPix.z = (rgb_image->data)[i*4+2];
+		newPix.w = 255;
+
+		rgb[i] = newPix;
 	}
 
 	// copy depth_image into depth_image_
-	const short *depth_image_data = (const short*)depth_image.data;
-	memcpy(depth_image_, depth_image_data, depth_image.height*depth_image.step);
+	short *depth = depth_image_.GetData(MEMORYDEVICE_CPU);
+	for(int i = 0; i < depth_image_.noDims.x * depth_image_.noDims.y; i++) {
+		depth[i] = (depth_image->data)[i*sizeof(short)];
+	}
 }
 
-ROSEngine::ROSEngine(const char *calibFilename, const char *deviceURI, const bool useInternalCalibration,
-	Vector2i requested_imageSize_rgb, Vector2i requested_imageSize_d) :
+void ROSEngine::topicListenerThread()
+{
+	// subscribe to rgb and depth topics
+	message_filters::Subscriber<sensor_msgs::Image> rgb_sub_(nh_, "/dtam/rgb", 1); // TODO remove dtam and generalize to: /namespace/rgb, /namespace/depth
+	message_filters::Subscriber<sensor_msgs::Image> depth_sub_(nh_, "/dtam/depth", 1); // uint16 depth image in mm. Native OpenNI format, preferred by InfiniTAM.
+	TimeSynchronizer<sensor_msgs::Image, sensor_msgs::Image> sync(rgb_sub_, depth_sub_, 10);
+	sync.registerCallback(boost::bind(&ROSEngine::processMessage, this, _1, _2));
+
+	ros::spin();
+}
+
+ROSEngine::ROSEngine(const char *calibFilename,
+					 Vector2i requested_imageSize_rgb,
+					 Vector2i requested_imageSize_d) :
 	BaseImageSourceEngine(calibFilename),
 	nh_(),
-	rgb_image_(requested_imageSize_rgb),
-	depth_image_(requested_imageSize_d)
+	rgb_image_(requested_imageSize_rgb, MEMORYDEVICE_CPU),
+	depth_image_(requested_imageSize_d, MEMORYDEVICE_CPU)
 {
+	// Start ROS
+	ros::start();
+
 	// Start up topic listener thread
 	std::thread topic_listener_thread(&ROSEngine::topicListenerThread, this);
 	topic_listener_thread.join();
 
 	// TODO document that depth images must be in millimeters
 	this->calib.disparityCalib.SetStandard(); // assumes depth is in millimeters
-
-	this->imageSize_d	= Vector2i(0,0);
-	this->imageSize_rgb = Vector2i(0,0);
-
-	if (useInternalCalibration) { // TODO correct this
-		this->calib.trafo_rgb_to_depth = ITMLib::ITMExtrinsics();
-		if (depthAvailable) {
-			float h_fov = data->depthStream.getHorizontalFieldOfView();
-			float v_fov = data->depthStream.getVerticalFieldOfView();
-			this->calib.intrinsics_d.SetFrom(
-				(float)imageSize_d.x / (2.0f * tan(h_fov/2.0f)),
-				(float)imageSize_d.y / (2.0f * tan(v_fov/2.0f)),
-				(float)imageSize_d.x / 2.0f,
-				(float)imageSize_d.y / 2.0f);
-		}
-		if (colorAvailable) {
-			float h_fov = data->colorStream.getHorizontalFieldOfView();
-			float v_fov = data->colorStream.getVerticalFieldOfView();
-			this->calib.intrinsics_rgb.SetFrom(
-				(float)imageSize_rgb.x / (2.0f * tan(h_fov/2.0f)),
-				(float)imageSize_rgb.y / (2.0f * tan(v_fov/2.0f)),
-				(float)imageSize_rgb.x / 2.0f,
-				(float)imageSize_rgb.y / 2.0f);
-		}
-	}
 }
 
 void ROSEngine::getImages(ITMUChar4Image *rgbImage, ITMShortImage *rawDepthImage)
 {
 	std::lock_guard<std::mutex> get_images_lock(images_mutex_);
 
-	rgbImage->SetFrom(&rgb_image_,       MemoryCopyDirection::CPU_TO_CPU);
-	rawDepthImage>SetFrom(&depth_image_, MemoryCopyDirection::CPU_TO_CPU);
+	rgbImage->SetFrom(&rgb_image_,        MemoryBlock<Vector4u>::CPU_TO_CPU); 
+	rawDepthImage->SetFrom(&depth_image_, MemoryBlock<short>::CPU_TO_CPU);
 }
 
 bool ROSEngine::hasMoreImages(void) const
@@ -113,18 +77,15 @@ bool ROSEngine::hasMoreImages(void) const
 
 Vector2i ROSEngine::getDepthImageSize(void) const
 {
-	return data ? imageSize_d : Vector2i(0,0);
+	return Vector2i(depth_image_.noDims.x , depth_image_.noDims.y);
 }
 
 Vector2i ROSEngine::getRGBImageSize(void) const
 {
-	return data ? imageSize_rgb : Vector2i(0,0);
+	return Vector2i(rgb_image_.noDims.x , rgb_image_.noDims.y);
 }
 
 ROSEngine::~ROSEngine()
 {
-	if (data) {
-		delete data;
-		data = NULL;
-	}
+	ros::shutdown();
 }
diff --git a/InfiniTAM/InputSource/ROSEngine.h b/InfiniTAM/InputSource/ROSEngine.h
index d7c4c0d8a..5e99a1b14 100644
--- a/InfiniTAM/InputSource/ROSEngine.h
+++ b/InfiniTAM/InputSource/ROSEngine.h
@@ -2,9 +2,23 @@
 
 #pragma once
 
+#include "../ORUtils/FileUtils.h"
+#include "ImageSourceEngine.h"
+
+#include <cstdio>
+#include <iostream>
+#include <stdexcept>
+#include <string>
+#include <thread>
 #include <mutex>
 
-#include "ImageSourceEngine.h"
+#include <ros/ros.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/time_synchronizer.h>
+#include <sensor_msgs/Image.h>
+#include <image_transport/image_transport.h>
+#include <cv_bridge/cv_bridge.h>
+#include <sensor_msgs/image_encodings.h>
 
 #if (!defined USING_CMAKE) && (defined _MSC_VER)
 #pragma comment(lib, "OpenNI2")
@@ -17,16 +31,19 @@ class ROSEngine : public BaseImageSourceEngine
 private:
 	ros::NodeHandle nh_;
 	ros::Subscriber rgb_sub_, depth_sub_;
-	Vector2i imageSize_rgb_, imageSize_d_;
 	ITMUChar4Image rgb_image_;
 	ITMShortImage depth_image_;
 	std::mutex images_mutex_;
 
 public:
-	ROSEngine(const char *calibFilename, const char *deviceURI = NULL, const bool useInternalCalibration = false,
-		Vector2i imageSize_rgb = Vector2i(640, 480), Vector2i imageSize_d = Vector2i(640, 480));
+	ROSEngine(const char *calibFilename, 
+			  Vector2i imageSize_rgb = Vector2i(640, 480),
+			  Vector2i imageSize_d = Vector2i(640, 480));
 	~ROSEngine();
 
+	void processMessage(const sensor_msgs::ImageConstPtr& rgb_image, const sensor_msgs::ImageConstPtr& depth_image);
+	void topicListenerThread();
+	
 	bool hasMoreImages(void) const;
 	void getImages(ITMUChar4Image *rgb, ITMShortImage *rawDepth);
 	Vector2i getDepthImageSize(void) const;
diff --git a/InfiniTAM/cmake/CUDACheckCompute.cmake b/cmake/CUDACheckCompute.cmake
similarity index 100%
rename from InfiniTAM/cmake/CUDACheckCompute.cmake
rename to cmake/CUDACheckCompute.cmake
diff --git a/InfiniTAM/cmake/FindCSparse.cmake b/cmake/FindCSparse.cmake
similarity index 100%
rename from InfiniTAM/cmake/FindCSparse.cmake
rename to cmake/FindCSparse.cmake
diff --git a/InfiniTAM/cmake/FindFFmpeg.cmake b/cmake/FindFFmpeg.cmake
similarity index 100%
rename from InfiniTAM/cmake/FindFFmpeg.cmake
rename to cmake/FindFFmpeg.cmake
diff --git a/InfiniTAM/cmake/FindLibRoyale.cmake b/cmake/FindLibRoyale.cmake
similarity index 100%
rename from InfiniTAM/cmake/FindLibRoyale.cmake
rename to cmake/FindLibRoyale.cmake
diff --git a/InfiniTAM/cmake/FindOpenNI.cmake b/cmake/FindOpenNI.cmake
similarity index 100%
rename from InfiniTAM/cmake/FindOpenNI.cmake
rename to cmake/FindOpenNI.cmake
diff --git a/InfiniTAM/cmake/FindRealSense.cmake b/cmake/FindRealSense.cmake
similarity index 100%
rename from InfiniTAM/cmake/FindRealSense.cmake
rename to cmake/FindRealSense.cmake
diff --git a/InfiniTAM/cmake/Flags.cmake b/cmake/Flags.cmake
similarity index 100%
rename from InfiniTAM/cmake/Flags.cmake
rename to cmake/Flags.cmake
diff --git a/InfiniTAM/cmake/LinkFFmpeg.cmake b/cmake/LinkFFmpeg.cmake
similarity index 100%
rename from InfiniTAM/cmake/LinkFFmpeg.cmake
rename to cmake/LinkFFmpeg.cmake
diff --git a/InfiniTAM/cmake/LinkGLUT.cmake b/cmake/LinkGLUT.cmake
similarity index 100%
rename from InfiniTAM/cmake/LinkGLUT.cmake
rename to cmake/LinkGLUT.cmake
diff --git a/InfiniTAM/cmake/LinkLibRoyale.cmake b/cmake/LinkLibRoyale.cmake
similarity index 100%
rename from InfiniTAM/cmake/LinkLibRoyale.cmake
rename to cmake/LinkLibRoyale.cmake
diff --git a/InfiniTAM/cmake/LinkOpenGL.cmake b/cmake/LinkOpenGL.cmake
similarity index 100%
rename from InfiniTAM/cmake/LinkOpenGL.cmake
rename to cmake/LinkOpenGL.cmake
diff --git a/InfiniTAM/cmake/LinkOpenNI.cmake b/cmake/LinkOpenNI.cmake
similarity index 100%
rename from InfiniTAM/cmake/LinkOpenNI.cmake
rename to cmake/LinkOpenNI.cmake
diff --git a/InfiniTAM/cmake/LinkPNG.cmake b/cmake/LinkPNG.cmake
similarity index 100%
rename from InfiniTAM/cmake/LinkPNG.cmake
rename to cmake/LinkPNG.cmake
diff --git a/InfiniTAM/cmake/LinkRealSense.cmake b/cmake/LinkRealSense.cmake
similarity index 100%
rename from InfiniTAM/cmake/LinkRealSense.cmake
rename to cmake/LinkRealSense.cmake
diff --git a/InfiniTAM/cmake/LinkUVC.cmake b/cmake/LinkUVC.cmake
similarity index 100%
rename from InfiniTAM/cmake/LinkUVC.cmake
rename to cmake/LinkUVC.cmake
diff --git a/InfiniTAM/cmake/OfferC++11Support.cmake b/cmake/OfferC++11Support.cmake
similarity index 100%
rename from InfiniTAM/cmake/OfferC++11Support.cmake
rename to cmake/OfferC++11Support.cmake
diff --git a/InfiniTAM/cmake/OfferInputMirroring.cmake b/cmake/OfferInputMirroring.cmake
similarity index 100%
rename from InfiniTAM/cmake/OfferInputMirroring.cmake
rename to cmake/OfferInputMirroring.cmake
diff --git a/InfiniTAM/cmake/SetCUDAAppTarget.cmake b/cmake/SetCUDAAppTarget.cmake
similarity index 100%
rename from InfiniTAM/cmake/SetCUDAAppTarget.cmake
rename to cmake/SetCUDAAppTarget.cmake
diff --git a/InfiniTAM/cmake/SetCUDALibTarget.cmake b/cmake/SetCUDALibTarget.cmake
similarity index 100%
rename from InfiniTAM/cmake/SetCUDALibTarget.cmake
rename to cmake/SetCUDALibTarget.cmake
diff --git a/InfiniTAM/cmake/SetLibTarget.cmake b/cmake/SetLibTarget.cmake
similarity index 100%
rename from InfiniTAM/cmake/SetLibTarget.cmake
rename to cmake/SetLibTarget.cmake
diff --git a/InfiniTAM/cmake/UseCUDA.cmake b/cmake/UseCUDA.cmake
similarity index 100%
rename from InfiniTAM/cmake/UseCUDA.cmake
rename to cmake/UseCUDA.cmake
diff --git a/InfiniTAM/cmake/UseFFmpeg.cmake b/cmake/UseFFmpeg.cmake
similarity index 100%
rename from InfiniTAM/cmake/UseFFmpeg.cmake
rename to cmake/UseFFmpeg.cmake
diff --git a/InfiniTAM/cmake/UseGLUT.cmake b/cmake/UseGLUT.cmake
similarity index 100%
rename from InfiniTAM/cmake/UseGLUT.cmake
rename to cmake/UseGLUT.cmake
diff --git a/InfiniTAM/cmake/UseKinect2.cmake b/cmake/UseKinect2.cmake
similarity index 100%
rename from InfiniTAM/cmake/UseKinect2.cmake
rename to cmake/UseKinect2.cmake
diff --git a/InfiniTAM/cmake/UseLibRoyale.cmake b/cmake/UseLibRoyale.cmake
similarity index 100%
rename from InfiniTAM/cmake/UseLibRoyale.cmake
rename to cmake/UseLibRoyale.cmake
diff --git a/InfiniTAM/cmake/UseOpenGL.cmake b/cmake/UseOpenGL.cmake
similarity index 100%
rename from InfiniTAM/cmake/UseOpenGL.cmake
rename to cmake/UseOpenGL.cmake
diff --git a/InfiniTAM/cmake/UseOpenMP.cmake b/cmake/UseOpenMP.cmake
similarity index 100%
rename from InfiniTAM/cmake/UseOpenMP.cmake
rename to cmake/UseOpenMP.cmake
diff --git a/InfiniTAM/cmake/UseOpenNI.cmake b/cmake/UseOpenNI.cmake
similarity index 100%
rename from InfiniTAM/cmake/UseOpenNI.cmake
rename to cmake/UseOpenNI.cmake
diff --git a/InfiniTAM/cmake/UsePNG.cmake b/cmake/UsePNG.cmake
similarity index 100%
rename from InfiniTAM/cmake/UsePNG.cmake
rename to cmake/UsePNG.cmake
diff --git a/InfiniTAM/cmake/UseRealSense.cmake b/cmake/UseRealSense.cmake
similarity index 100%
rename from InfiniTAM/cmake/UseRealSense.cmake
rename to cmake/UseRealSense.cmake
diff --git a/InfiniTAM/cmake/UseUVC.cmake b/cmake/UseUVC.cmake
similarity index 100%
rename from InfiniTAM/cmake/UseUVC.cmake
rename to cmake/UseUVC.cmake
diff --git a/InfiniTAM/cmake/cuda_compute_capability.cpp b/cmake/cuda_compute_capability.cpp
similarity index 100%
rename from InfiniTAM/cmake/cuda_compute_capability.cpp
rename to cmake/cuda_compute_capability.cpp
diff --git a/package.xml b/package.xml
new file mode 100644
index 000000000..f1cdb4869
--- /dev/null
+++ b/package.xml
@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>InfiniTAM</name>
+  <version>3.0.0</version>
+  <description>InfiniTAM ROS package</description>
+
+  <maintainer email="ravich3141@gmail.com">ravich</maintainer>
+
+  <license>TODO</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>message_filters</build_depend>
+
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>message_filters</build_export_depend>
+
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>message_filters</exec_depend>
+
+</package>

From 7d2ea21b8e24d6389e99feb94c2dc13cdd011669 Mon Sep 17 00:00:00 2001
From: ravich <ravich3141@gmail.com>
Date: Thu, 2 Nov 2017 17:33:24 +0530
Subject: [PATCH 3/8] Use ApproximateTime sync instead of ExactTime sync.

---
 InfiniTAM/Apps/InfiniTAM/InfiniTAM.cpp |  5 ++++-
 InfiniTAM/InputSource/ROSEngine.cpp    | 25 +++++++++++--------------
 InfiniTAM/InputSource/ROSEngine.h      |  4 +++-
 3 files changed, 18 insertions(+), 16 deletions(-)

diff --git a/InfiniTAM/Apps/InfiniTAM/InfiniTAM.cpp b/InfiniTAM/Apps/InfiniTAM/InfiniTAM.cpp
index 59f037c6e..00e5894c1 100644
--- a/InfiniTAM/Apps/InfiniTAM/InfiniTAM.cpp
+++ b/InfiniTAM/Apps/InfiniTAM/InfiniTAM.cpp
@@ -83,7 +83,7 @@ static void CreateDefaultImageSource(ImageSourceEngine* & imageSource, IMUSource
 
 	if (imageSource == NULL)
 	{
-		printf("trying ROS input: /depth/image_raw, /rgb/image_raw \n");
+		printf("trying ROS input: /camera/depth/image_raw, /camera/rgb/image_raw \n");
 		imageSource = new ROSEngine(calibFile);
 		if (imageSource->getDepthImageSize().x == 0)
 		{
@@ -156,7 +156,9 @@ static void CreateDefaultImageSource(ImageSourceEngine* & imageSource, IMUSource
 int main(int argc, char** argv)
 try
 {
+	// Start ROS
 	ros::init(argc, argv, "infinitam_ros");
+	ros::start();
 	
 	const char *arg1 = "";
 	const char *arg2 = NULL;
@@ -224,6 +226,7 @@ try
 	delete imageSource;
 	if (imuSource != NULL) delete imuSource;
 
+	ros::shutdown();
 	return 0;
 }
 catch(std::exception& e)
diff --git a/InfiniTAM/InputSource/ROSEngine.cpp b/InfiniTAM/InputSource/ROSEngine.cpp
index b70f03c3e..15632a6ac 100644
--- a/InfiniTAM/InputSource/ROSEngine.cpp
+++ b/InfiniTAM/InputSource/ROSEngine.cpp
@@ -35,9 +35,10 @@ void ROSEngine::processMessage(const ImageConstPtr& rgb_image, const ImageConstP
 void ROSEngine::topicListenerThread()
 {
 	// subscribe to rgb and depth topics
-	message_filters::Subscriber<sensor_msgs::Image> rgb_sub_(nh_, "/dtam/rgb", 1); // TODO remove dtam and generalize to: /namespace/rgb, /namespace/depth
-	message_filters::Subscriber<sensor_msgs::Image> depth_sub_(nh_, "/dtam/depth", 1); // uint16 depth image in mm. Native OpenNI format, preferred by InfiniTAM.
-	TimeSynchronizer<sensor_msgs::Image, sensor_msgs::Image> sync(rgb_sub_, depth_sub_, 10);
+	message_filters::Subscriber<sensor_msgs::Image> rgb_sub_(nh_, "/camera/rgb/image_raw", 1); // TODO remove dtam and generalize to: /namespace/rgb, /namespace/depth
+	message_filters::Subscriber<sensor_msgs::Image> depth_sub_(nh_, "/camera/depth/image_raw", 1); // uint16 depth image in mm. Native OpenNI format, preferred by InfiniTAM.
+	typedef sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> ITAMSyncPolicy;
+	Synchronizer<ITAMSyncPolicy> sync(ITAMSyncPolicy(10), rgb_sub_, depth_sub_);
 	sync.registerCallback(boost::bind(&ROSEngine::processMessage, this, _1, _2));
 
 	ros::spin();
@@ -46,17 +47,15 @@ void ROSEngine::topicListenerThread()
 ROSEngine::ROSEngine(const char *calibFilename,
 					 Vector2i requested_imageSize_rgb,
 					 Vector2i requested_imageSize_d) :
-	BaseImageSourceEngine(calibFilename),
-	nh_(),
-	rgb_image_(requested_imageSize_rgb, MEMORYDEVICE_CPU),
-	depth_image_(requested_imageSize_d, MEMORYDEVICE_CPU)
+			BaseImageSourceEngine(calibFilename),
+			nh_(),
+			rgb_image_(requested_imageSize_rgb, MEMORYDEVICE_CPU),
+			depth_image_(requested_imageSize_d, MEMORYDEVICE_CPU),
+			topic_listener_thread(&ROSEngine::topicListenerThread, this)
 {
-	// Start ROS
-	ros::start();
 
 	// Start up topic listener thread
-	std::thread topic_listener_thread(&ROSEngine::topicListenerThread, this);
-	topic_listener_thread.join();
+	// topic_listener_thread.join();
 
 	// TODO document that depth images must be in millimeters
 	this->calib.disparityCalib.SetStandard(); // assumes depth is in millimeters
@@ -86,6 +85,4 @@ Vector2i ROSEngine::getRGBImageSize(void) const
 }
 
 ROSEngine::~ROSEngine()
-{
-	ros::shutdown();
-}
+{}
diff --git a/InfiniTAM/InputSource/ROSEngine.h b/InfiniTAM/InputSource/ROSEngine.h
index 5e99a1b14..51a948f23 100644
--- a/InfiniTAM/InputSource/ROSEngine.h
+++ b/InfiniTAM/InputSource/ROSEngine.h
@@ -14,7 +14,8 @@
 
 #include <ros/ros.h>
 #include <message_filters/subscriber.h>
-#include <message_filters/time_synchronizer.h>
+#include <message_filters/synchronizer.h>
+#include <message_filters/sync_policies/approximate_time.h>
 #include <sensor_msgs/Image.h>
 #include <image_transport/image_transport.h>
 #include <cv_bridge/cv_bridge.h>
@@ -33,6 +34,7 @@ class ROSEngine : public BaseImageSourceEngine
 	ros::Subscriber rgb_sub_, depth_sub_;
 	ITMUChar4Image rgb_image_;
 	ITMShortImage depth_image_;
+	std::thread topic_listener_thread;
 	std::mutex images_mutex_;
 
 public:

From a381d1ffc1f3f8c795bd2cb10e2a482bcedb7e80 Mon Sep 17 00:00:00 2001
From: ravich2_7183 <ravich3141@gmail.com>
Date: Fri, 3 Nov 2017 16:43:10 +0530
Subject: [PATCH 4/8] Updated README.

---
 README.md | 161 +++++++++++-------------------------------------------
 1 file changed, 33 insertions(+), 128 deletions(-)

diff --git a/README.md b/README.md
index 6b006f49a..21480364c 100644
--- a/README.md
+++ b/README.md
@@ -1,31 +1,12 @@
-# InfiniTAM v3
+# infinitam_ros branch forked from [InfiniTAM v3](https://github.com/victorprad/InfiniTAM)
 
-This is the main branch of the software bundle "InfiniTAM", the current version is actively maintained by:
+This repository is a fork of [InfiniTAM v3](https://github.com/victorprad/InfiniTAM) and adds the infinitam_ros branch that enables ROS as an input source to the InfiniTAM system. 
 
-  Victor Adrian Prisacariu <victor@robots.ox.ac.uk>  
-  Olaf Kaehler <olaf@robots.ox.ac.uk>  
-  Stuart Golodetz <smg@robots.ox.ac.uk>  
-  Michael Sapienza <michael.sapienza@eng.ox.ac.uk>  
-  Tommaso Cavallari <tommaso.cavallari@unibo.it>
-  
-Previous maintainers and contributors are:
+# Building the System
 
-  Carl Yuheng Ren <carl@robots.ox.ac.uk>  
-  Ming Ming Cheng <cmm.thu@gmail.com>  
-  Xin Sun <xin.sun@st-hughs.ox.ac.uk>  
-  Philip H.S. Torr <philip.torr@eng.ox.ac.uk>  
-  Ian D Reid <ian.reid@adelaide.edu.au>  
-  David W Murray <dwm@robots.ox.ac.uk>
+## System Requirements
 
-For more information about InfiniTAM please visit the project website <http://www.infinitam.org>. 
-
-Other related projects can be found in the Oxford Active Vision Library <http://www.oxvisionlib.org>.
-
-# 1. Building the System
-
-### 1.1 Requirements
-
-Several 3rd party libraries are needed for compiling InfiniTAM. The given version numbers are checked and working, but different versions might be fine as well. Some of the libraries are optional, and skipping them will reduce functionality.
+The following 3rd party libraries are needed for compiling InfiniTAM. The given version numbers are checked and working, but different versions might be fine as well. 
 
   - cmake (e.g. version 2.8.10.2 or 3.2.3)
     REQUIRED for Linux, unless you write your own build system
@@ -42,132 +23,56 @@ Several 3rd party libraries are needed for compiling InfiniTAM. The given versio
     at least with cmake it is still possible to compile the CPU part without
     available at https://developer.nvidia.com/cuda-downloads
 
-  - OpenNI (e.g. version 2.2.0.33)
-    OPTIONAL but REQUIRED to get live images from suitable hardware
-    also make sure you have freenect/OpenNI2-FreenectDriver if you need it
-    available at http://structure.io/openni
-
-  - libpng (e.g. version 1.6)
-    OPTIONAL, allows to read PNG input files
-    available at http://libpng.org
-
-  - FFMPEG (e.g. version 2.8.6)
-    OPTIONAL, allows writing and playback of lossless FFV1 encoded videos
-    available at https://www.ffmpeg.org/
+  - ROS (tested on indigo, but should work on other versions too).
 
-  - librealsense (e.g. github version from 2016-MAR-22)
-    OPTIONAL, allows to get live images from Intel Realsense cameras
-    available at https://github.com/IntelRealSense/librealsense
+  - boost
 
-  - libuvc (e.g. github version from 2015-OCT-27)
-    OPTIONAL, deprecated alternative to librealsense
-    currently this works only with branch olafkaehler/master
-    available at https://github.com/olafkaehler/libuvc
+## Build Process (for infinitam_ros branch)
 
-  - doxygen (e.g. version 1.8.2)
-    OPTIONAL, builds a nice reference manual
-    available at http://www.doxygen.org/
+infinitam\_ros is catkinized, so you can clone it into your catkin workspace's src/ directory and run `catkin_make' from the workspace directory. 
 
-###1.2 Build Process
+If you are new to the ROS catkin build system or want to build InfiniTAM and run it separately you can do the following: 
 
-  To compile the system, use the standard cmake approach:
+1. Create a new catkin workspace
 ```
-  $ mkdir build
-  $ cd build
-  $ cmake /path/to/InfiniTAM -DOPEN_NI_ROOT=/path/to/OpenNI2/
-  $ make
+$ source /opt/ros/indigo/setup.bash
 ```
-  To create a doxygen documentation, just run doxygen:
-```
-  $ doxygen Doxyfile
-```
-  This will create a new directory doxygen-html/ containing all the
-documentation.
-
-### 1.3 Odds and Ends
 
-Padding the data structure ITMVoxel in ITMLibDefines.h with one extra byte may or may not improve the overall performance on certain GPUs. On a NVidia GTX 680 it appears to do, on a GTX 780 it does not. Have a try yourself if you need the speed.
+\# replace indigo with your ROS version
 
-On Mac OS X 10.9 there are currently some issues with libc++ vs. libstdc++ in conjunction with CUDA. They eventually manifest in error messages like:
 ```
-Undefined symbols for architecture x86_64: 
-"std::ios_base::Init::Init()", referenced from:
-      __GLOBAL__I_a in libITMLib.a(ITMLib_generated_ITMColorTracker_CUDA.cu.o)
-      __GLOBAL__I_a in libITMLib.a(ITMLib_generated_ITMDepthTracker_CUDA.cu.o)
-     [...]
+$ mkdir -p ./infinitam_ws/src
+$ cd ./infinitam_ws/
+$ catkin_make
+$ source devel/setup.bash
 ```
-In the current version of InfiniTAM these errors are avoided by specifying ```CMAKE_CXX_FLAGS=-stdlib=libstdc++``` whenever clang is detected as complier. However, future versions of CUDA might not require this anymore or even get confused and/or require ```CUDA_HOST_COMPILER=/usr/bin/clang``` instead.
-
-If a version of GLUT other than freeglut is used, the InfiniTAM sample application has problems on exit, as it is currently not explicitly cleaning up CUDA memory or closing the OpenNI device. Use freeglut to avoid this if you experience any problems.
-
-Some sensors may need a small change to work correctly with OpenNI, the changes are described [here](http://com.occipital.openni.s3.amazonaws.com/Structure%20Sensor%20OpenNI2%20Quick%20Start%20Guide.pdf).
 
-
-# 2. Sample Programs
-
-The build process should result in an executable InfiniTAM, which is the main sample program. For a version without visualisation, try InfiniTAM_cli. If compiled with OpenNI support, both should run out-of-the-box without problems for live reconstruction. If you have calibration information for your specific device, you can pass it as the first argument to the program, e.g.:
+2. Clone infinitam_ros into the src/ dir
 ```
-  $ ./InfiniTAM Teddy/calib.txt
+$ cd ~/infinitam_ws/src/
+$ git clone git@github.com:ravich2-7183/InfiniTAM.git
+$ git checkout infinitam_ros \# switch to this branch
 ```
-If no OpenNI support has been compiled in, the program can be used for offline processing:
+
+3. Build infinitam_ros
 ```
-  $ ./InfiniTAM Teddy/calib.txt Teddy/Frames/%04i.ppm Teddy/Frames/%04i.pgm
+$ cd ~/infinitam_ws/
 ```
-The arguments are essentially masks for sprintf and the %04i will be replaced by a running number, accordingly.
-
-
-# 3. Additional Documentation
-
-Apart from the doxygen documentation there should also be a technical report
-shipped along with this package. It is also available from the official project
-website. Further technical information is to be found in:
+$ catkin_make
 
+4. Run infinitam_ros
 ```
-@inproceedings{InfiniTAM_ECCV_2016,
-  author    = {Olaf K{\"{a}}hler and
-               Victor Adrian Prisacariu and
-               David W. Murray},
-  title     = {Real-Time Large-Scale Dense 3D Reconstruction with Loop Closure},
-  booktitle = {Computer Vision - {ECCV} 2016 - 14th European Conference, Amsterdam,
-               The Netherlands, October 11-14, 2016, Proceedings, Part {VIII}},
-  pages     = {500--516},
-  year      = {2016}
-}
+$ rosrun InfiniTAM InfiniTAM
 ```
 
-and
+# Expected input topics
+The InfiniTAM node subscribes to the following topics:
 
-```
-@article{InfiniTAM_ISMAR_2015,
-author = {{K{\"a}hler}, O. and
-		  {Prisacariu}, V.~A. and
-		  {Ren}, C.~Y. and
-		  {Sun}, X. and
-		  {Torr}, P.~H.~S and
-		  {Murray}, D.~W.},
-title = "{Very High Frame Rate Volumetric Integration of Depth Images on Mobile Device}",
-journal = "{IEEE Transactions on Visualization and Computer Graphics 
-	   (Proceedings International Symposium on Mixed and Augmented Reality 2015}",
-volume = {22},
-number = {11},
-year = 2015
-```
+1. /depth/image_raw
 
-and
+of type sensor_msgs/Image with encoding = "short")
 
-```
-@article{InfiniTAM_arXiv_2017,
-author = {V A Prisacariu and O K{\"a}hler and S Golodetz and M Sapienza and T Cavallari and P H S Torr and D W Murray},
-title = {{InfiniTAM v3: A Framework for Large-Scale 3D Reconstruction with Loop Closure}},
-journal = {arXiv pre-print arXiv:1708.00783v1},
-year = {2017}
-}
-```
+2. /rgb/image_raw
 
-------
+of type sensor_msgs/Image with encoding = "RGBA8")
 
-### History:
-- 2017-JUL-23: version 3 release
-- 2016-NOV-18: updated to reflect changes to team and project structure
-- 2015-JUL-10: updated dependencies, added reference to ISMAR paper
-- 2014-OCT-06: initial public release

From cf00bc865707e8b86d3a2c1456553b1c7fd549d2 Mon Sep 17 00:00:00 2001
From: ravich2_7183 <ravich3141@gmail.com>
Date: Fri, 3 Nov 2017 16:46:41 +0530
Subject: [PATCH 5/8] Updated README.

---
 README.md | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/README.md b/README.md
index 21480364c..01b099e8f 100644
--- a/README.md
+++ b/README.md
@@ -38,7 +38,7 @@ If you are new to the ROS catkin build system or want to build InfiniTAM and run
 $ source /opt/ros/indigo/setup.bash
 ```
 
-\# replace indigo with your ROS version
+replace indigo with your ROS version
 
 ```
 $ mkdir -p ./infinitam_ws/src
@@ -51,14 +51,14 @@ $ source devel/setup.bash
 ```
 $ cd ~/infinitam_ws/src/
 $ git clone git@github.com:ravich2-7183/InfiniTAM.git
-$ git checkout infinitam_ros \# switch to this branch
+$ git checkout infinitam_ros
 ```
 
 3. Build infinitam_ros
 ```
 $ cd ~/infinitam_ws/
-```
 $ catkin_make
+```
 
 4. Run infinitam_ros
 ```

From cd9e2ca9fa7c28fc7d48315d9bc65f3c88581ff4 Mon Sep 17 00:00:00 2001
From: ravich <ravich3141@gmail.com>
Date: Sat, 4 Nov 2017 17:18:52 +0530
Subject: [PATCH 6/8] freenect /camera/rgb/image_color is bgr8 and not rgba8.

---
 InfiniTAM/Apps/InfiniTAM/InfiniTAM.cpp |  2 +-
 InfiniTAM/InputSource/ROSEngine.cpp    | 18 +++++++-----------
 2 files changed, 8 insertions(+), 12 deletions(-)

diff --git a/InfiniTAM/Apps/InfiniTAM/InfiniTAM.cpp b/InfiniTAM/Apps/InfiniTAM/InfiniTAM.cpp
index 00e5894c1..d4f5fa5cb 100644
--- a/InfiniTAM/Apps/InfiniTAM/InfiniTAM.cpp
+++ b/InfiniTAM/Apps/InfiniTAM/InfiniTAM.cpp
@@ -83,7 +83,7 @@ static void CreateDefaultImageSource(ImageSourceEngine* & imageSource, IMUSource
 
 	if (imageSource == NULL)
 	{
-		printf("trying ROS input: /camera/depth/image_raw, /camera/rgb/image_raw \n");
+		printf("trying ROS input: /camera/depth/image_raw, /camera/rgb/image_color \n");
 		imageSource = new ROSEngine(calibFile);
 		if (imageSource->getDepthImageSize().x == 0)
 		{
diff --git a/InfiniTAM/InputSource/ROSEngine.cpp b/InfiniTAM/InputSource/ROSEngine.cpp
index 15632a6ac..0775941ef 100644
--- a/InfiniTAM/InputSource/ROSEngine.cpp
+++ b/InfiniTAM/InputSource/ROSEngine.cpp
@@ -11,15 +11,15 @@ void ROSEngine::processMessage(const ImageConstPtr& rgb_image, const ImageConstP
 {
 	std::lock_guard<std::mutex> process_message_lock(images_mutex_);
 
-	// TODO document that rgb_image must have an encoding equivalent to RGBA8
+	// TODO document that rgb_image must have an encoding equivalent to bgr8
 
 	// copy rgb_image into rgb_image_
 	Vector4u *rgb = rgb_image_.GetData(MEMORYDEVICE_CPU);
 	for(int i = 0; i < rgb_image_.noDims.x * rgb_image_.noDims.y; i++) {
 		Vector4u newPix;
-		newPix.x = (rgb_image->data)[i*4+0];
-		newPix.y = (rgb_image->data)[i*4+1];
-		newPix.z = (rgb_image->data)[i*4+2];
+		newPix.x = (rgb_image->data)[i*3+2];
+		newPix.y = (rgb_image->data)[i*3+1];
+		newPix.z = (rgb_image->data)[i*3+0];
 		newPix.w = 255;
 
 		rgb[i] = newPix;
@@ -28,14 +28,14 @@ void ROSEngine::processMessage(const ImageConstPtr& rgb_image, const ImageConstP
 	// copy depth_image into depth_image_
 	short *depth = depth_image_.GetData(MEMORYDEVICE_CPU);
 	for(int i = 0; i < depth_image_.noDims.x * depth_image_.noDims.y; i++) {
-		depth[i] = (depth_image->data)[i*sizeof(short)];
+		depth[i] = static_cast<short>((depth_image->data)[i*sizeof(short)]);
 	}
 }
 
 void ROSEngine::topicListenerThread()
 {
 	// subscribe to rgb and depth topics
-	message_filters::Subscriber<sensor_msgs::Image> rgb_sub_(nh_, "/camera/rgb/image_raw", 1); // TODO remove dtam and generalize to: /namespace/rgb, /namespace/depth
+    message_filters::Subscriber<sensor_msgs::Image> rgb_sub_(nh_, "/camera/rgb/image_color", 1); // TODO remove dtam and generalize to: /namespace/rgb, /namespace/depth
 	message_filters::Subscriber<sensor_msgs::Image> depth_sub_(nh_, "/camera/depth/image_raw", 1); // uint16 depth image in mm. Native OpenNI format, preferred by InfiniTAM.
 	typedef sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> ITAMSyncPolicy;
 	Synchronizer<ITAMSyncPolicy> sync(ITAMSyncPolicy(10), rgb_sub_, depth_sub_);
@@ -51,12 +51,8 @@ ROSEngine::ROSEngine(const char *calibFilename,
 			nh_(),
 			rgb_image_(requested_imageSize_rgb, MEMORYDEVICE_CPU),
 			depth_image_(requested_imageSize_d, MEMORYDEVICE_CPU),
-			topic_listener_thread(&ROSEngine::topicListenerThread, this)
+			topic_listener_thread(&ROSEngine::topicListenerThread, this) // Starts up topic listener thread
 {
-
-	// Start up topic listener thread
-	// topic_listener_thread.join();
-
 	// TODO document that depth images must be in millimeters
 	this->calib.disparityCalib.SetStandard(); // assumes depth is in millimeters
 }

From bd28544d7f15fcba347c01af23c228a7028186e2 Mon Sep 17 00:00:00 2001
From: ravich <ravich3141@gmail.com>
Date: Wed, 8 Nov 2017 21:00:28 +0530
Subject: [PATCH 7/8] Figured out how to correctly copy ros Image message into
 InfiniTAM.

---
 InfiniTAM/InputSource/ROSEngine.cpp | 9 ++++++---
 InfiniTAM/InputSource/ROSEngine.h   | 1 -
 2 files changed, 6 insertions(+), 4 deletions(-)

diff --git a/InfiniTAM/InputSource/ROSEngine.cpp b/InfiniTAM/InputSource/ROSEngine.cpp
index 0775941ef..824a7f5fe 100644
--- a/InfiniTAM/InputSource/ROSEngine.cpp
+++ b/InfiniTAM/InputSource/ROSEngine.cpp
@@ -2,6 +2,7 @@
 
 #include "ROSEngine.h"
 
+using namespace std;
 using namespace ORUtils;
 using namespace InputSource;
 using namespace sensor_msgs;
@@ -27,16 +28,18 @@ void ROSEngine::processMessage(const ImageConstPtr& rgb_image, const ImageConstP
 
 	// copy depth_image into depth_image_
 	short *depth = depth_image_.GetData(MEMORYDEVICE_CPU);
+	const short *depth_msg_data  = reinterpret_cast<const short*>(depth_image->data.data());
 	for(int i = 0; i < depth_image_.noDims.x * depth_image_.noDims.y; i++) {
-		depth[i] = static_cast<short>((depth_image->data)[i*sizeof(short)]);
+		depth[i] = depth_msg_data[i];
 	}
 }
 
 void ROSEngine::topicListenerThread()
 {
 	// subscribe to rgb and depth topics
-    message_filters::Subscriber<sensor_msgs::Image> rgb_sub_(nh_, "/camera/rgb/image_color", 1); // TODO remove dtam and generalize to: /namespace/rgb, /namespace/depth
-	message_filters::Subscriber<sensor_msgs::Image> depth_sub_(nh_, "/camera/depth/image_raw", 1); // uint16 depth image in mm. Native OpenNI format, preferred by InfiniTAM.
+	// depth image must be uint16 in mm, which is the native OpenNI format, preferred by InfiniTAM.
+    message_filters::Subscriber<sensor_msgs::Image> rgb_sub_(nh_, "/camera/rgb/image_color", 1);
+	message_filters::Subscriber<sensor_msgs::Image> depth_sub_(nh_, "/camera/depth/image_raw", 1); 
 	typedef sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> ITAMSyncPolicy;
 	Synchronizer<ITAMSyncPolicy> sync(ITAMSyncPolicy(10), rgb_sub_, depth_sub_);
 	sync.registerCallback(boost::bind(&ROSEngine::processMessage, this, _1, _2));
diff --git a/InfiniTAM/InputSource/ROSEngine.h b/InfiniTAM/InputSource/ROSEngine.h
index 51a948f23..bab73496c 100644
--- a/InfiniTAM/InputSource/ROSEngine.h
+++ b/InfiniTAM/InputSource/ROSEngine.h
@@ -5,7 +5,6 @@
 #include "../ORUtils/FileUtils.h"
 #include "ImageSourceEngine.h"
 
-#include <cstdio>
 #include <iostream>
 #include <stdexcept>
 #include <string>

From 33759df4c44a2d9df63d107f2b0c7e13bb8136e8 Mon Sep 17 00:00:00 2001
From: ravich <ravich3141@gmail.com>
Date: Thu, 9 Nov 2017 12:56:47 +0530
Subject: [PATCH 8/8] Code cleanup.

---
 InfiniTAM/InputSource/ROSEngine.cpp | 20 ++++++++------------
 InfiniTAM/InputSource/ROSEngine.h   |  7 ++++++-
 README.md                           | 25 +++++++++++--------------
 3 files changed, 25 insertions(+), 27 deletions(-)

diff --git a/InfiniTAM/InputSource/ROSEngine.cpp b/InfiniTAM/InputSource/ROSEngine.cpp
index 824a7f5fe..863628fbe 100644
--- a/InfiniTAM/InputSource/ROSEngine.cpp
+++ b/InfiniTAM/InputSource/ROSEngine.cpp
@@ -8,27 +8,25 @@ using namespace InputSource;
 using namespace sensor_msgs;
 using namespace message_filters;
 
-void ROSEngine::processMessage(const ImageConstPtr& rgb_image, const ImageConstPtr& depth_image)
+void ROSEngine::processMessage(const ImageConstPtr& rgb_image_msg, const ImageConstPtr& depth_image_msg)
 {
 	std::lock_guard<std::mutex> process_message_lock(images_mutex_);
 
-	// TODO document that rgb_image must have an encoding equivalent to bgr8
-
-	// copy rgb_image into rgb_image_
+	// copy rgb_image_msg into rgb_image_
 	Vector4u *rgb = rgb_image_.GetData(MEMORYDEVICE_CPU);
 	for(int i = 0; i < rgb_image_.noDims.x * rgb_image_.noDims.y; i++) {
 		Vector4u newPix;
-		newPix.x = (rgb_image->data)[i*3+2];
-		newPix.y = (rgb_image->data)[i*3+1];
-		newPix.z = (rgb_image->data)[i*3+0];
+		newPix.x = (rgb_image_msg->data)[i*3+2];
+		newPix.y = (rgb_image_msg->data)[i*3+1];
+		newPix.z = (rgb_image_msg->data)[i*3+0];
 		newPix.w = 255;
 
 		rgb[i] = newPix;
 	}
 
-	// copy depth_image into depth_image_
+	// copy depth_image_msg into depth_image_
 	short *depth = depth_image_.GetData(MEMORYDEVICE_CPU);
-	const short *depth_msg_data  = reinterpret_cast<const short*>(depth_image->data.data());
+	const short *depth_msg_data  = reinterpret_cast<const short*>(depth_image_msg->data.data());
 	for(int i = 0; i < depth_image_.noDims.x * depth_image_.noDims.y; i++) {
 		depth[i] = depth_msg_data[i];
 	}
@@ -37,7 +35,6 @@ void ROSEngine::processMessage(const ImageConstPtr& rgb_image, const ImageConstP
 void ROSEngine::topicListenerThread()
 {
 	// subscribe to rgb and depth topics
-	// depth image must be uint16 in mm, which is the native OpenNI format, preferred by InfiniTAM.
     message_filters::Subscriber<sensor_msgs::Image> rgb_sub_(nh_, "/camera/rgb/image_color", 1);
 	message_filters::Subscriber<sensor_msgs::Image> depth_sub_(nh_, "/camera/depth/image_raw", 1); 
 	typedef sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> ITAMSyncPolicy;
@@ -54,9 +51,8 @@ ROSEngine::ROSEngine(const char *calibFilename,
 			nh_(),
 			rgb_image_(requested_imageSize_rgb, MEMORYDEVICE_CPU),
 			depth_image_(requested_imageSize_d, MEMORYDEVICE_CPU),
-			topic_listener_thread(&ROSEngine::topicListenerThread, this) // Starts up topic listener thread
+			topic_listener_thread(&ROSEngine::topicListenerThread, this) // Starts up topicListenerThread
 {
-	// TODO document that depth images must be in millimeters
 	this->calib.disparityCalib.SetStandard(); // assumes depth is in millimeters
 }
 
diff --git a/InfiniTAM/InputSource/ROSEngine.h b/InfiniTAM/InputSource/ROSEngine.h
index bab73496c..faa0c2e9c 100644
--- a/InfiniTAM/InputSource/ROSEngine.h
+++ b/InfiniTAM/InputSource/ROSEngine.h
@@ -42,7 +42,12 @@ class ROSEngine : public BaseImageSourceEngine
 			  Vector2i imageSize_d = Vector2i(640, 480));
 	~ROSEngine();
 
-	void processMessage(const sensor_msgs::ImageConstPtr& rgb_image, const sensor_msgs::ImageConstPtr& depth_image);
+	/**
+	   @param rgb_image_msg must have an encoding "bgr8"
+	   @param depth_image_msg must have an encoding "mono16" or "16UC1" which is the depth in mm. 
+	*/
+	void processMessage(const sensor_msgs::ImageConstPtr& rgb_image_msg, const sensor_msgs::ImageConstPtr& depth_image_msg);
+
 	void topicListenerThread();
 	
 	bool hasMoreImages(void) const;
diff --git a/README.md b/README.md
index 01b099e8f..1c39ca445 100644
--- a/README.md
+++ b/README.md
@@ -25,24 +25,20 @@ The following 3rd party libraries are needed for compiling InfiniTAM. The given
 
   - ROS (tested on indigo, but should work on other versions too).
 
-  - boost
+  - Boost
 
 ## Build Process (for infinitam_ros branch)
 
 infinitam\_ros is catkinized, so you can clone it into your catkin workspace's src/ directory and run `catkin_make' from the workspace directory. 
 
-If you are new to the ROS catkin build system or want to build InfiniTAM and run it separately you can do the following: 
+If you are new to the ROS catkin build system or want to build and run InfiniTAM in a separate directory, you can do the following: 
 
-1. Create a new catkin workspace
+1. Create a new catkin workspace (replace indigo with your ROS version)
 ```
 $ source /opt/ros/indigo/setup.bash
-```
-
-replace indigo with your ROS version
 
-```
-$ mkdir -p ./infinitam_ws/src
-$ cd ./infinitam_ws/
+$ mkdir -p ~/infinitam_ws/src
+$ cd ~/infinitam_ws/
 $ catkin_make
 $ source devel/setup.bash
 ```
@@ -62,17 +58,18 @@ $ catkin_make
 
 4. Run infinitam_ros
 ```
+$ cd ~/infinitam_ws/
+$ source devel/setup.bash
 $ rosrun InfiniTAM InfiniTAM
 ```
 
 # Expected input topics
 The InfiniTAM node subscribes to the following topics:
 
-1. /depth/image_raw
-
-of type sensor_msgs/Image with encoding = "short")
+1. /camera/depth/image_raw
 
-2. /rgb/image_raw
+of type sensor_msgs/Image with encoding "mono16" or "16UC1" (depth is in millimeters). 
 
-of type sensor_msgs/Image with encoding = "RGBA8")
+2. /camera/rgb/image_color
 
+of type sensor_msgs/Image with encoding  "bgr8" (a common ROS Image message encoding for color images).