Added CameraK4A driver (only MKV playback supported yet). Fixed build with realsense2 support on Windows. Preferences: added imu filtering parameters to ini file.

CMakeLists.txt

@@ -166,6 +166,7 @@ option(WITH_ORB_OCTREE    "Include ORB Octree feature support"   ON)
 option(WITH_FREENECT      "Include Freenect support"             ON)
 option(WITH_FREENECT2     "Include Freenect2 support"            ON)
 option(WITH_K4W2          "Include Kinect for Windows v2 support" ON)
+option(WITH_K4A           "Include Kinect for Azure support"     ON)
 option(WITH_OPENNI2       "Include OpenNI2 support"              ON)
 option(WITH_DC1394        "Include dc1394 support"               ON)
 option(WITH_G2O           "Include g2o support"                  ON)
@@ -323,6 +324,13 @@ IF(WITH_K4W2 AND WIN32)
     ENDIF(KinectSDK2_FOUND)
 ENDIF(WITH_K4W2 AND WIN32)
 
+IF(WITH_K4A)
+    FIND_PACKAGE(K4A QUIET)
+	IF(K4A_FOUND)
+       MESSAGE(STATUS "Found Kinect for Azure: ${K4A_INCLUDE_DIRS}")
+    ENDIF(K4A_FOUND)
+ENDIF(WITH_K4A)
+
 # IF PCL depends on OpenNI2 (already found), ignore WITH_OPENNI2
 IF(WITH_OPENNI2 OR OpenNI2_FOUND)
     FIND_PACKAGE(OpenNI2 QUIET)
@@ -640,6 +648,11 @@ IF(NOT KinectSDK2_FOUND)
 ELSE()
    SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${KinectSDK2_LIBRARIES})
 ENDIF()
+IF(NOT K4A_FOUND)
+   SET(K4A "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${K4A_LIBRARIES})
+ENDIF()
 IF(NOT OpenNI2_FOUND)
    SET(OPENNI2 "//")
 ELSE()
@@ -999,6 +1012,14 @@ ELSE()
 MESSAGE(STATUS "  With Kinect for Windows 2 = NO (Kinect for Windows 2 SDK not found)")
 ENDIF()
 
+IF(K4A_FOUND)
+MESSAGE(STATUS "  With Kinect for Azure     = YES (License: MIT)")
+ELSEIF(NOT WITH_K4A)
+MESSAGE(STATUS "  With Kinect for Azure     = NO (WITH_K4A=OFF)")
+ELSE()
+MESSAGE(STATUS "  With Kinect for Azure     = NO (Kinect for Azure SDK not found)")
+ENDIF()
+
 IF(DC1394_FOUND)
 MESSAGE(STATUS "  With dc1394               = YES (License: LGPL)")
 ELSEIF(NOT WITH_DC1394)

Version.h.in

@@ -48,6 +48,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 @FREENECT@#define RTABMAP_FREENECT
 @FREENECT2@#define RTABMAP_FREENECT2
 @K4W2@#define RTABMAP_K4W2
+@K4A@#define RTABMAP_K4A
 @CVSBA@#define RTABMAP_CVSBA
 @POINTMATCHER@#define RTABMAP_POINTMATCHER
 @LOAM@#define RTABMAP_LOAM

cmake_modules/FindK4A.cmake

@@ -0,0 +1,29 @@
+# - Find K4A 
+# This module finds an kinect 4 azure SDK
+#
+# It sets the following variables:
+#  K4A_FOUND       - Set to false, or undefined, if FlyCapture2 isn't found.
+#  K4A_INCLUDE_DIRS - The FlyCapture2 include directory.
+#  K4A_LIBRARIES     - The FlyCapture2 library to link against.
+
+find_library(K4A_LIBRARY NAMES k4a NO_DEFAULT_PATH PATHS $ENV{K4A_ROOT_DIR}/sdk/windows-desktop/amd64/release/lib)
+find_library(K4ARECORD_LIBRARY NAMES k4arecord NO_DEFAULT_PATH PATHS $ENV{K4A_ROOT_DIR}/sdk/windows-desktop/amd64/release/lib)
+find_path(K4A_INCLUDE_DIR NAMES k4a/k4a.h PATHS $ENV{K4A_ROOT_DIR}/sdk/include)
+
+IF (K4A_INCLUDE_DIR AND K4A_LIBRARY AND K4ARECORD_LIBRARY)
+   SET(K4A_FOUND TRUE)
+   SET(K4A_INCLUDE_DIRS ${K4A_INCLUDE_DIR})
+   SET(K4A_LIBRARIES ${K4A_LIBRARY} ${K4ARECORD_LIBRARY})
+ENDIF (K4A_INCLUDE_DIR AND K4A_LIBRARY AND K4ARECORD_LIBRARY)
+
+IF (K4A_FOUND)
+   # show which K4A was found only if not quiet
+   IF (NOT K4A_FIND_QUIETLY)
+      MESSAGE(STATUS "Found K4A: ${K4A_LIBRARIES}")
+   ENDIF (NOT K4A_FIND_QUIETLY)
+ELSE (K4A_FOUND)
+   # fatal error if K4A is required but not found
+   IF (K4A_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find K4A (Kinect for Azure SDK)")
+   ENDIF (K4A_FIND_REQUIRED)
+ENDIF (K4A_FOUND)

corelib/include/rtabmap/core/CameraRGBD.h

@@ -36,4 +36,5 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/camera/CameraRealSense.h>
 #include <rtabmap/core/camera/CameraRealSense2.h>
 #include <rtabmap/core/camera/CameraRGBDImages.h>
+#include <rtabmap/core/camera/CameraK4A.h>
 

corelib/include/rtabmap/core/camera/CameraK4A.h

@@ -0,0 +1,80 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#pragma once
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include "rtabmap/core/CameraModel.h"
+#include "rtabmap/core/Camera.h"
+#include "rtabmap/core/Version.h"
+
+namespace rtabmap
+{
+
+class RTABMAP_EXP CameraK4A :
+	public Camera
+{
+public:
+	static bool available();
+
+public:
+	CameraK4A(int deviceId = 0,
+		float imageRate = 0.0f,
+		const Transform & localTransform = Transform::getIdentity());
+	CameraK4A(const std::string & fileName,
+		float imageRate = 0.0f,
+		const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraK4A();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+
+	void setIRDepthFormat(bool enabled) {ir_ = enabled;}
+
+protected:
+	virtual SensorData captureImage(CameraInfo * info = 0);
+
+private:
+	void close();
+
+private:
+	int deviceId_;
+	std::string fileName_;
+
+#ifdef RTABMAP_K4A
+	void* playbackHandle_;
+	void* transformationHandle_;
+	CameraModel model_;
+	bool ir_;
+#endif
+
+};
+
+
+} // namespace rtabmap

corelib/src/CMakeLists.txt

@@ -21,6 +21,7 @@ SET(SRC_FILES
     camera/CameraFreenect2.cpp
     camera/CameraImages.cpp
     camera/CameraK4W2.cpp
+	camera/CameraK4A.cpp
     camera/CameraOpenni.cpp
     camera/CameraOpenNI2.cpp
     camera/CameraOpenNICV.cpp
@@ -218,6 +219,17 @@ IF(KinectSDK2_FOUND)
 	)
 ENDIF(KinectSDK2_FOUND)
 
+IF(K4A_FOUND)
+	SET(INCLUDE_DIRS
+		${INCLUDE_DIRS}
+		${K4A_INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+		${LIBRARIES}
+		${K4A_LIBRARIES}
+	)
+ENDIF(K4A_FOUND)
+
 IF(RealSense_FOUND)
 	SET(INCLUDE_DIRS
 		${INCLUDE_DIRS}

corelib/src/Odometry.cpp

@@ -622,7 +622,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 
 		if(dt)
 		{
-			if(dt >=guessSmoothingDelay_/2.0 || particleFilters_.size() || _filteringStrategy==1)
+			if(dt >= (guessSmoothingDelay_/2.0) || particleFilters_.size() || _filteringStrategy==1)
 			{
 				velocityGuess_ = Transform(vx, vy, vz, vroll, vpitch, vyaw);
 				previousVelocities_.clear();

corelib/src/camera/CameraK4A.cpp

@@ -0,0 +1,404 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/core/camera/CameraK4A.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UThreadC.h>
+#include <rtabmap/core/util2d.h>
+#include <rtabmap/core/Compression.h>
+#include <opencv2/imgproc/types_c.h>
+
+#ifdef RTABMAP_K4A
+#include <k4a/k4a.h>
+#include <k4arecord/playback.h>
+#endif
+
+namespace rtabmap
+{
+
+bool CameraK4A::available()
+{
+#ifdef RTABMAP_K4A
+	return true;
+#else
+	return false;
+#endif
+}
+
+CameraK4A::CameraK4A(
+	int deviceId,
+	float imageRate,
+	const Transform & localTransform) :
+	Camera(imageRate, localTransform),
+	deviceId_(deviceId)
+#ifdef RTABMAP_K4A
+	, playbackHandle_(NULL),
+	transformationHandle_(NULL),
+	ir_(false)
+#endif
+{
+}
+
+CameraK4A::CameraK4A(
+	const std::string & fileName,
+	float imageRate,
+	const Transform & localTransform) :
+	Camera(imageRate, localTransform),
+	deviceId_(-1),
+	fileName_(fileName)
+#ifdef RTABMAP_K4A
+	, playbackHandle_(NULL),
+	transformationHandle_(NULL),
+	ir_(false)
+#endif
+{
+}
+
+CameraK4A::~CameraK4A()
+{
+	close();
+}
+
+void CameraK4A::close()
+{
+	if (playbackHandle_ != NULL)
+	{
+		k4a_playback_close((k4a_playback_t)playbackHandle_);
+	}
+	if (transformationHandle_ != NULL)
+	{
+		k4a_transformation_destroy((k4a_transformation_t)transformationHandle_);
+	}
+	/*
+	// Shut down the camera when finished with application logic
+	k4a_device_stop_cameras(device);
+	k4a_device_close(device);
+	*/
+
+}
+
+bool CameraK4A::init(const std::string & calibrationFolder, const std::string & cameraName)
+{
+#ifdef RTABMAP_K4A
+	
+	close();
+
+	if (!fileName_.empty())
+	{
+		if (k4a_playback_open(fileName_.c_str(), (k4a_playback_t*)&playbackHandle_) != K4A_RESULT_SUCCEEDED)
+		{
+			UERROR("Failed to open recording \"%s\"", fileName_.c_str());
+			return false;
+		}
+
+		uint64_t recording_length = k4a_playback_get_last_timestamp_usec((k4a_playback_t)playbackHandle_);
+		UINFO("Recording is %lld seconds long", recording_length / 1000000);
+
+		k4a_calibration_t calibration;
+		if (k4a_playback_get_calibration((k4a_playback_t)playbackHandle_, &calibration))
+		{
+			UERROR("Failed to get calibration");
+			close();
+			return false;
+		}
+		if (ir_)
+		{
+			model_ = CameraModel(
+				calibration.depth_camera_calibration.intrinsics.parameters.param.fx,
+				calibration.depth_camera_calibration.intrinsics.parameters.param.fy,
+				calibration.depth_camera_calibration.intrinsics.parameters.param.cx,
+				calibration.depth_camera_calibration.intrinsics.parameters.param.cy,
+				this->getLocalTransform(),
+				0,
+				cv::Size(calibration.depth_camera_calibration.resolution_width, calibration.depth_camera_calibration.resolution_height));
+		}
+		else
+		{
+			model_ = CameraModel(
+				calibration.color_camera_calibration.intrinsics.parameters.param.fx,
+				calibration.color_camera_calibration.intrinsics.parameters.param.fy,
+				calibration.color_camera_calibration.intrinsics.parameters.param.cx,
+				calibration.color_camera_calibration.intrinsics.parameters.param.cy,
+				this->getLocalTransform(),
+				0,
+				cv::Size(calibration.color_camera_calibration.resolution_width, calibration.color_camera_calibration.resolution_height));
+
+			transformationHandle_ = k4a_transformation_create(&calibration);
+		}
+
+		k4a_record_configuration_t config;
+		if (k4a_playback_get_record_configuration((k4a_playback_t)playbackHandle_, &config))
+		{
+			UERROR("Failed to getting recording configuration");
+			close();
+			return false;
+		}
+	}
+	else if (deviceId_ >= 0)
+	{
+		UERROR("CameraK4A: Live camera stream is not yet supported, only recorded mkv files are.");
+		return false;
+
+		/*uint32_t count = k4a_device_get_installed_count();
+		if (count == 0)
+		{
+			UERROR("No k4a devices attached!");
+			return false;
+		}
+
+		// Open the first plugged in Kinect device
+		k4a_device_t device = NULL;
+		if (K4A_FAILED(k4a_device_open(K4A_DEVICE_DEFAULT, &device)))
+		{
+			UERROR("Failed to open k4a device!");
+			return false;
+		}
+
+		// Get the size of the serial number
+		size_t serial_size = 0;
+		k4a_device_get_serialnum(device, NULL, &serial_size);
+
+		// Allocate memory for the serial, then acquire it
+		char *serial = (char*)(malloc(serial_size));
+		k4a_device_get_serialnum(device, serial, &serial_size);
+		UINFO("Opened device: %s", serial);
+		free(serial);
+
+		// Configure a stream of 4096x3072 BRGA color data at 15 frames per second
+		k4a_device_configuration_t config = K4A_DEVICE_CONFIG_INIT_DISABLE_ALL;
+		config.camera_fps = K4A_FRAMES_PER_SECOND_15;
+		config.color_format = K4A_IMAGE_FORMAT_COLOR_BGRA32;
+		config.color_resolution = K4A_COLOR_RESOLUTION_3072P;
+
+		// Start the camera with the given configuration
+		if (K4A_FAILED(k4a_device_start_cameras(device, &config)))
+		{
+			UERROR("Failed to start cameras!");
+			k4a_device_close(device);
+			return false;
+		}*/
+	}
+	
+	return true;
+#else
+	UERROR("CameraK4A: RTAB-Map is not built with Kinect for Azure SDK support!");
+	return false;
+#endif
+}
+
+bool CameraK4A::isCalibrated() const
+{
+	return true;
+}
+
+std::string CameraK4A::getSerial() const
+{
+	return fileName_.empty()?"":fileName_;
+}
+
+SensorData CameraK4A::captureImage(CameraInfo * info)
+{
+	SensorData data;
+
+#ifdef RTABMAP_K4A
+
+	if (playbackHandle_ != NULL)
+	{
+		k4a_capture_t capture = NULL;
+		k4a_stream_result_t result = K4A_STREAM_RESULT_SUCCEEDED;
+
+		// wait to get all frames
+		UTimer time;
+		while (result == K4A_STREAM_RESULT_SUCCEEDED && time.elapsed() < 5.0)
+		{
+			result = k4a_playback_get_next_capture((k4a_playback_t)playbackHandle_, &capture);
+
+			if (result == K4A_STREAM_RESULT_SUCCEEDED)
+			{
+				cv::Mat bgrCV;
+				cv::Mat depthCV;
+				double stamp = 0;
+
+				// Process capture here
+				if (ir_)
+				{
+					k4a_image_t ir = k4a_capture_get_ir_image(capture);
+					if (ir != NULL)
+					{
+						/*UDEBUG("ir res:%4dx%4d stride:%5d format:%d stamp=%f",
+							k4a_image_get_height_pixels(ir),
+							k4a_image_get_width_pixels(ir),
+							k4a_image_get_stride_bytes(ir),
+							k4a_image_get_format(ir),
+							double(k4a_image_get_timestamp_usec(ir)) / 1000000.0);*/
+
+						UASSERT(k4a_image_get_format(ir) == K4A_IMAGE_FORMAT_IR16);
+
+						cv::Mat bgrCV16(k4a_image_get_height_pixels(ir), k4a_image_get_width_pixels(ir), CV_16UC1, (void*)k4a_image_get_buffer(ir));
+						bgrCV16.convertTo(bgrCV, CV_8U);
+
+						// Release the image
+						k4a_image_release(ir);
+					}
+				}
+				else
+				{
+					k4a_image_t color = k4a_capture_get_color_image(capture);
+					if (color != NULL)
+					{
+						/*UDEBUG("Color res:%4dx%4d stride:%5d format:%d stamp=%f",
+							k4a_image_get_height_pixels(color),
+							k4a_image_get_width_pixels(color),
+							k4a_image_get_stride_bytes(color),
+							k4a_image_get_format(color),
+							double(k4a_image_get_timestamp_usec(color)) / 1000000.0);*/
+
+						UASSERT(k4a_image_get_format(color) == K4A_IMAGE_FORMAT_COLOR_MJPG || k4a_image_get_format(color) == K4A_IMAGE_FORMAT_COLOR_BGRA32);
+
+						if (k4a_image_get_format(color) == K4A_IMAGE_FORMAT_COLOR_MJPG)
+						{
+							bgrCV = uncompressImage(cv::Mat(1, (int)k4a_image_get_size(color), CV_8UC1, (void*)k4a_image_get_buffer(color)));
+							//UDEBUG("Uncompressed = %d %d %d", bgrCV.rows, bgrCV.cols, bgrCV.channels());
+						}
+						else
+						{
+							cv::Mat bgra(k4a_image_get_height_pixels(color), k4a_image_get_width_pixels(color), CV_8UC4, (void*)k4a_image_get_buffer(color));
+							cv::cvtColor(bgra, bgrCV, CV_BGRA2BGR);
+						}
+
+						// Release the image
+						k4a_image_release(color);
+					}
+				}
+
+				if (!bgrCV.empty())
+				{
+					k4a_image_t depth = k4a_capture_get_depth_image(capture);
+					if (depth != NULL)
+					{
+						/*UDEBUG("Depth16 res:%4dx%4d stride:%5d format:%d stamp=%f",
+							k4a_image_get_height_pixels(depth),
+							k4a_image_get_width_pixels(depth),
+							k4a_image_get_stride_bytes(depth),
+							k4a_image_get_format(depth),
+							double(k4a_image_get_timestamp_usec(depth)) / 1000000.0);*/
+
+						UASSERT(k4a_image_get_format(depth) == K4A_IMAGE_FORMAT_DEPTH16);
+
+						stamp = ((double)k4a_image_get_timestamp_usec(depth)) / 1000000;
+
+						if (ir_)
+						{
+							depthCV = cv::Mat(k4a_image_get_height_pixels(depth), k4a_image_get_width_pixels(depth), CV_16UC1, (void*)k4a_image_get_buffer(depth)).clone();
+						}
+						else
+						{
+							k4a_image_t transformedDepth;
+							if (k4a_image_create(k4a_image_get_format(depth), bgrCV.cols, bgrCV.rows, bgrCV.cols * 2, &transformedDepth) == K4A_RESULT_SUCCEEDED)
+							{
+								if (k4a_transformation_depth_image_to_color_camera((k4a_transformation_t)transformationHandle_, depth, transformedDepth) == K4A_RESULT_SUCCEEDED)
+								{
+									depthCV = cv::Mat(k4a_image_get_height_pixels(transformedDepth), k4a_image_get_width_pixels(transformedDepth), CV_16UC1, (void*)k4a_image_get_buffer(transformedDepth)).clone();
+								}
+								else
+								{
+									UERROR("Failed registration!");
+								}
+								k4a_image_release(transformedDepth);
+							}
+							else
+							{
+								UERROR("Failed allocating depth registered! (%d %d %d)", bgrCV.cols, bgrCV.rows, bgrCV.cols * 2);
+							}
+							
+						}
+
+						// Release the image
+						k4a_image_release(depth);
+					}
+				}
+
+				k4a_capture_release(capture);
+
+				IMU imu;
+				// FIXME: local imu transform missing
+				/*k4a_imu_sample_t imuSample;
+				if (k4a_playback_get_next_imu_sample((k4a_playback_t)playbackHandle_, &imuSample) == K4A_STREAM_RESULT_SUCCEEDED)
+				{
+					// K4A IMU Co-ordinates
+					//  x+ = "backwards"
+					//  y+ = "left"
+					//  z+ = "down"
+					//
+					// ROS Standard co-ordinates:
+					//  x+ = "forward"
+					//  y+ = "left"
+					//  z+ = "up"
+					//
+					// Remap K4A IMU to ROS co-ordinate system:
+					// ROS_X+ = K4A_X-
+					// ROS_Y+ = K4A_Y+
+					// ROS_Z+ = K4A_Z-
+
+					imu = IMU(
+						cv::Vec3d(-1*imuSample.gyro_sample.xyz.x, imuSample.gyro_sample.xyz.y, -1 * imuSample.gyro_sample.xyz.z),
+						cv::Mat::eye(3, 3, CV_64FC1),
+						cv::Vec3d(-1 * imuSample.acc_sample.xyz.x, imuSample.acc_sample.xyz.y, -1 * imuSample.acc_sample.xyz.z),
+						cv::Mat::eye(3, 3, CV_64FC1),
+						Transform::getIdentity());
+				}*/
+
+				if (!bgrCV.empty() && !depthCV.empty())
+				{
+					data = SensorData(bgrCV, depthCV, model_, this->getNextSeqID(), stamp);
+					data.setIMU(imu);
+					break;
+				}
+			}
+		}
+
+		if (result == K4A_STREAM_RESULT_EOF)
+		{
+			// End of file reached
+			UINFO("End of file reached");
+		}
+		else if (result == K4A_STREAM_RESULT_FAILED)
+		{
+			UERROR("Failed to read entire recording");
+		}
+	}
+	else
+	{
+		UERROR("CameraK4A: Live camera stream is not yet supported, only recorded mkv files are.");
+	}
+
+#else
+	UERROR("CameraK4A: RTAB-Map is not built with Kinect for Azure SDK support!");
+#endif
+	return data;
+}
+
+} // namespace rtabmap

examples/RGBDMapping/CMakeLists.txt

@@ -58,6 +58,14 @@ SET(LIBRARIES
 	${PCL_LIBRARIES}
 )
 
+# Hack as CameraRealsense2.h needs realsense2 include dir
+IF(realsense2_FOUND)
+	SET(INCLUDE_DIRS
+		${INCLUDE_DIRS}
+		${realsense2_INCLUDE_DIRS}
+	)
+ENDIF(realsense2_FOUND)
+
 INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
 
 IF(QT4_FOUND)

examples/WifiMapping/CMakeLists.txt

@@ -52,6 +52,14 @@ SET(LIBRARIES
 	${PCL_LIBRARIES}
 )
 
+# Hack as CameraRealsense2.h needs realsense2 include dir
+IF(realsense2_FOUND)
+	SET(INCLUDE_DIRS
+		${INCLUDE_DIRS}
+		${realsense2_INCLUDE_DIRS}
+	)
+ENDIF(realsense2_FOUND)
+
 INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
 
 IF(QT4_FOUND)

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -93,6 +93,7 @@ public:
 		kSrcRGBDImages     = 7,
 		kSrcK4W2           = 8,
 		kSrcRealSense2     = 9,
+		kSrcK4A            = 10,
 
 		kSrcStereo         = 100,
 		kSrcDC1394         = 100,
@@ -353,6 +354,7 @@ private Q_SLOTS:
 	void selectSourceDistortionModel();
 	void selectSourceOniPath();
 	void selectSourceOni2Path();
+	void selectSourceMKVPath();
 	void selectSourceSvoPath();
 	void updateSourceGrpVisibility();
 	void testOdometry();

guilib/src/AboutDialog.cpp

@@ -100,6 +100,8 @@ AboutDialog::AboutDialog(QWidget * parent) :
 	_ui->label_dc1394_license->setEnabled(CameraStereoDC1394::available());
 	_ui->label_flycapture2->setText(CameraStereoFlyCapture2::available()?"Yes":"No");
 	_ui->label_zed->setText(CameraStereoZed::available()?"Yes":"No");
+	_ui->label_k4w2->setText(CameraK4W2::available() ? "Yes" : "No");
+	_ui->label_k4a->setText(CameraK4A::available() ? "Yes" : "No");
 
 	_ui->label_toro->setText(Optimizer::isAvailable(Optimizer::kTypeTORO)?"Yes":"No");
 	_ui->label_toro_license->setEnabled(Optimizer::isAvailable(Optimizer::kTypeTORO)?true:false);

guilib/src/CMakeLists.txt

@@ -129,6 +129,14 @@ SET(INCLUDE_DIRS
 	${PCL_INCLUDE_DIRS}
 )
 
+# Hack as CameraRealsense2.h needs realsense2 include dir
+IF(realsense2_FOUND)
+	SET(INCLUDE_DIRS
+		${INCLUDE_DIRS}
+		${realsense2_INCLUDE_DIRS}
+	)
+ENDIF(realsense2_FOUND)
+
 IF(QT4_FOUND)
     INCLUDE(${QT_USE_FILE})
 ENDIF(QT4_FOUND)

guilib/src/PreferencesDialog.cpp

@@ -296,6 +296,10 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	{
 		_ui->comboBox_cameraRGBD->setItemData(kSrcK4W2 - kSrcRGBD, 0, Qt::UserRole - 1);
 	}
+	if (!CameraK4A::available())
+	{
+		_ui->comboBox_cameraRGBD->setItemData(kSrcK4A - kSrcRGBD, 0, Qt::UserRole - 1);
+	}
 	if (!CameraRealSense::available())
 	{
 		_ui->comboBox_cameraRGBD->setItemData(kSrcRealSense - kSrcRGBD, 0, Qt::UserRole - 1);
@@ -689,10 +693,12 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->pushButton_calibrate_simple, SIGNAL(clicked()), this, SLOT(calibrateSimple()));
 	connect(_ui->toolButton_openniOniPath, SIGNAL(clicked()), this, SLOT(selectSourceOniPath()));
 	connect(_ui->toolButton_openni2OniPath, SIGNAL(clicked()), this, SLOT(selectSourceOni2Path()));
+	connect(_ui->toolButton_k4a_mkv, SIGNAL(clicked()), this, SLOT(selectSourceMKVPath()));
 	connect(_ui->toolButton_source_distortionModel, SIGNAL(clicked()), this, SLOT(selectSourceDistortionModel()));
 	connect(_ui->toolButton_distortionModel, SIGNAL(clicked()), this, SLOT(visualizeDistortionModel()));
 	connect(_ui->lineEdit_openniOniPath, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->lineEdit_openni2OniPath, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->lineEdit_k4a_mkv, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->lineEdit_source_distortionModel, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->groupBox_bilateral, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->doubleSpinBox_bilateral_sigmaS, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
@@ -1780,6 +1786,8 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->spinBox_rs2_rate->setValue(60);
 		_ui->lineEdit_openniOniPath->clear();
 		_ui->lineEdit_openni2OniPath->clear();
+		_ui->checkbox_k4a_irDepth->setChecked(false);
+		_ui->lineEdit_k4a_mkv->clear();
 		_ui->lineEdit_cameraRGBDImages_path_rgb->setText("");
 		_ui->lineEdit_cameraRGBDImages_path_depth->setText("");
 		_ui->doubleSpinBox_cameraRGBDImages_scale->setValue(1.0);
@@ -2174,6 +2182,11 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->comboBox_k4w2Format->setCurrentIndex(settings.value("format", _ui->comboBox_k4w2Format->currentIndex()).toInt());
 	settings.endGroup(); // K4W2
 
+	settings.beginGroup("K4A");
+	_ui->checkbox_k4a_irDepth->setChecked(settings.value("ir", _ui->checkbox_k4a_irDepth->isChecked()).toBool());
+	_ui->lineEdit_k4a_mkv->setText(settings.value("mkvPath", _ui->lineEdit_k4a_mkv->text()).toString());
+	settings.endGroup(); // K4A
+
 	settings.beginGroup("RealSense");
 	_ui->comboBox_realsensePresetRGB->setCurrentIndex(settings.value("presetRGB", _ui->comboBox_realsensePresetRGB->currentIndex()).toInt());
 	_ui->comboBox_realsensePresetDepth->setCurrentIndex(settings.value("presetDepth", _ui->comboBox_realsensePresetDepth->currentIndex()).toInt());
@@ -2258,6 +2271,16 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->source_video_lineEdit_path->setText(settings.value("path", _ui->source_video_lineEdit_path->text()).toString());
 	settings.endGroup(); // video
 
+	settings.beginGroup("IMU");
+	_ui->comboBox_imuFilter_strategy->setCurrentIndex(settings.value("strategy", _ui->comboBox_imuFilter_strategy->currentIndex()).toInt());
+	_ui->doubleSpinBox_imuFilterMadgwickGain->setValue(settings.value("madgwick_gain", _ui->doubleSpinBox_imuFilterMadgwickGain->value()).toDouble());
+	_ui->doubleSpinBox_imuFilterMadgwickZeta->setValue(settings.value("madgwick_zeta", _ui->doubleSpinBox_imuFilterMadgwickZeta->value()).toDouble());
+	_ui->doubleSpinBox_imuFilterComplementaryGainAcc->setValue(settings.value("complementary_gain_acc", _ui->doubleSpinBox_imuFilterComplementaryGainAcc->value()).toDouble());
+	_ui->doubleSpinBox_imuFilterComplementaryBiasAlpha->setValue(settings.value("complementary_bias_alpha", _ui->doubleSpinBox_imuFilterComplementaryBiasAlpha->value()).toDouble());
+	_ui->checkBox_imuFilterComplementaryDoAdaptiveGain->setChecked(settings.value("complementary_adaptive_gain", _ui->checkBox_imuFilterComplementaryDoAdaptiveGain->isChecked()).toBool());
+	_ui->checkBox_imuFilterComplementaryDoBiasEstimation->setChecked(settings.value("complementary_biais_estimation", _ui->checkBox_imuFilterComplementaryDoBiasEstimation->isChecked()).toBool());
+	settings.endGroup();//IMU
+
 	settings.beginGroup("Scan");
 	_ui->checkBox_source_scanFromDepth->setChecked(settings.value("fromDepth", _ui->checkBox_source_scanFromDepth->isChecked()).toBool());
 	_ui->spinBox_source_scanDownsampleStep->setValue(settings.value("downsampleStep", _ui->spinBox_source_scanDownsampleStep->value()).toInt());
@@ -2614,6 +2637,11 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("format",				_ui->comboBox_k4w2Format->currentIndex());
 	settings.endGroup(); // K4W2
 
+	settings.beginGroup("K4A");
+	settings.setValue("ir", _ui->checkbox_k4a_irDepth->isChecked());
+	settings.setValue("mkvPath", _ui->lineEdit_k4a_mkv->text());
+	settings.endGroup(); // K4A
+
 	settings.beginGroup("RealSense");
 	settings.setValue("presetRGB",           _ui->comboBox_realsensePresetRGB->currentIndex());
 	settings.setValue("presetDepth",         _ui->comboBox_realsensePresetDepth->currentIndex());
@@ -2696,6 +2724,16 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("path", 			_ui->source_video_lineEdit_path->text());
 	settings.endGroup(); // video
 
+	settings.beginGroup("IMU");
+	settings.setValue("strategy",                       _ui->comboBox_imuFilter_strategy->currentIndex());
+	settings.setValue("madgwick_gain",                  _ui->doubleSpinBox_imuFilterMadgwickGain->value());
+	settings.setValue("madgwick_zeta",                  _ui->doubleSpinBox_imuFilterMadgwickZeta->value());
+	settings.setValue("complementary_gain_acc",         _ui->doubleSpinBox_imuFilterComplementaryGainAcc->value());
+	settings.setValue("complementary_bias_alpha",       _ui->doubleSpinBox_imuFilterComplementaryBiasAlpha->value());
+	settings.setValue("complementary_adaptive_gain",    _ui->checkBox_imuFilterComplementaryDoAdaptiveGain->isChecked());
+	settings.setValue("complementary_biais_estimation", _ui->checkBox_imuFilterComplementaryDoBiasEstimation->isChecked());
+	settings.endGroup();//IMU
+
 	settings.beginGroup("Scan");
 	settings.setValue("fromDepth", 			_ui->checkBox_source_scanFromDepth->isChecked());
 	settings.setValue("downsampleStep", 	_ui->spinBox_source_scanDownsampleStep->value());
@@ -3444,6 +3482,10 @@ void PreferencesDialog::selectSourceDriver(Src src)
 		{
 			_ui->lineEdit_openni2OniPath->clear();
 		}
+		else if (src == kSrcK4A)
+		{
+			_ui->lineEdit_k4a_mkv->clear();
+		}
 	}
 	else if(src >= kSrcStereo && src<kSrcRGB)
 	{
@@ -3835,6 +3877,20 @@ void PreferencesDialog::selectSourceOni2Path()
 	}
 }
 
+void PreferencesDialog::selectSourceMKVPath()
+{
+	QString dir = _ui->lineEdit_k4a_mkv->text();
+	if (dir.isEmpty())
+	{
+		dir = getWorkingDirectory();
+	}
+	QString path = QFileDialog::getOpenFileName(this, tr("Select file"), _ui->lineEdit_k4a_mkv->text(), tr("K4A recording (*.mkv)"));
+	if (!path.isEmpty())
+	{
+		_ui->lineEdit_k4a_mkv->setText(path);
+	}
+}
+
 void PreferencesDialog::selectSourceSvoPath()
 {
 	QString dir = _ui->lineEdit_zedSvoPath->text();
@@ -4530,6 +4586,7 @@ void PreferencesDialog::updateSourceGrpVisibility()
 			(_ui->comboBox_cameraRGBD->currentIndex() == kSrcOpenNI2-kSrcRGBD ||
 			 _ui->comboBox_cameraRGBD->currentIndex() == kSrcFreenect2-kSrcRGBD ||
 			 _ui->comboBox_cameraRGBD->currentIndex() == kSrcK4W2 - kSrcRGBD ||
+			 _ui->comboBox_cameraRGBD->currentIndex() == kSrcK4A - kSrcRGBD ||
 			 _ui->comboBox_cameraRGBD->currentIndex() == kSrcRealSense - kSrcRGBD ||
 			 _ui->comboBox_cameraRGBD->currentIndex() == kSrcRGBDImages-kSrcRGBD ||
 			 _ui->comboBox_cameraRGBD->currentIndex() == kSrcOpenNI_PCL-kSrcRGBD ||
@@ -4537,10 +4594,11 @@ void PreferencesDialog::updateSourceGrpVisibility()
 	_ui->groupBox_openni2->setVisible(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcOpenNI2-kSrcRGBD);
 	_ui->groupBox_freenect2->setVisible(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcFreenect2-kSrcRGBD);
 	_ui->groupBox_k4w2->setVisible(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcK4W2 - kSrcRGBD);
+	_ui->groupBox_k4a->setVisible(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcK4A - kSrcRGBD);
 	_ui->groupBox_realsense->setVisible(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcRealSense - kSrcRGBD);
 	_ui->groupBox_realsense2->setVisible(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcRealSense2 - kSrcRGBD);
 	_ui->groupBox_cameraRGBDImages->setVisible(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcRGBDImages-kSrcRGBD);
-	_ui->groupBox_openni->setVisible(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcOpenNI_PCL-kSrcRGBD);
+	_ui->groupBox_openni->setVisible(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcOpenNI_PCL - kSrcRGBD);
 
 	_ui->stackedWidget_stereo->setVisible(_ui->comboBox_sourceType->currentIndex() == 1 && 
 		(_ui->comboBox_cameraStereo->currentIndex() == kSrcStereoVideo-kSrcStereo || 
@@ -4576,6 +4634,7 @@ void PreferencesDialog::updateSourceGrpVisibility()
 			(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcRGBDImages-kSrcRGBD) ||
 			(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoImages-kSrcStereo) ||
 			(_ui->comboBox_sourceType->currentIndex() == 2 && _ui->source_comboBox_image_type->currentIndex() == kSrcImages-kSrcRGB) ||
+			(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcK4A - kSrcRGBD) || //K4A
 			(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcRealSense2 - kSrcRGBD) || //D435i
 			(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoRealSense2 - kSrcStereo) || //T265
 			(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoZed - kSrcStereo)); // ZEDm
@@ -5259,6 +5318,26 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 			this->getGeneralInputRate(),
 			this->getSourceLocalTransform());
 	}
+	else if (driver == kSrcK4A)
+	{
+		if (!_ui->lineEdit_k4a_mkv->text().isEmpty())
+		{
+			// playback
+			camera = new CameraK4A(
+				_ui->lineEdit_k4a_mkv->text().toStdString().c_str(),
+				this->getGeneralInputRate(),
+				this->getSourceLocalTransform());
+		}
+		else
+		{
+			camera = new CameraK4A(
+				this->getSourceDevice().isEmpty() ? 0 : atoi(this->getSourceDevice().toStdString().c_str()),
+				this->getGeneralInputRate(),
+				this->getSourceLocalTransform());
+		}
+		
+		((CameraK4A*)camera)->setIRDepthFormat(_ui->checkbox_k4a_irDepth->isChecked());
+	}
 	else if (driver == kSrcRealSense)
 	{
 		if(useRawImages && _ui->comboBox_realsenseRGBSource->currentIndex()!=2)

guilib/src/ui/preferencesDialog.ui

@@ -64,15 +64,24 @@
            <rect>
             <x>0</x>
             <y>0</y>
-            <width>673</width>
+            <width>677</width>
-            <height>3038</height>
+            <height>2974</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
            <property name="spacing">
             <number>0</number>
            </property>
-           <property name="margin">
+           <property name="leftMargin">
+            <number>0</number>
+           </property>
+           <property name="topMargin">
+            <number>0</number>
+           </property>
+           <property name="rightMargin">
+            <number>0</number>
+           </property>
+           <property name="bottomMargin">
             <number>0</number>
            </property>
            <item>
@@ -86,7 +95,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>21</number>
+                <number>5</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -3039,6 +3048,11 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                  <string>RealSense2</string>
                                 </property>
                                </item>
+                               <item>
+                                <property name="text">
+                                 <string>Kinect for Azure</string>
+                                </property>
+                               </item>
                               </widget>
                              </item>
                              <item row="0" column="1">
@@ -3082,7 +3096,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            <item>
                             <widget class="QStackedWidget" name="stackedWidget_rgbd">
                              <property name="currentIndex">
-                              <number>9</number>
+                              <number>10</number>
                              </property>
                              <widget class="QWidget" name="page_32">
                               <layout class="QVBoxLayout" name="verticalLayout_63">
@@ -4179,6 +4193,98 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                </item>
                               </layout>
                              </widget>
+                             <widget class="QWidget" name="page_84">
+                              <layout class="QVBoxLayout" name="verticalLayout_145" stretch="0,1">
+                               <item>
+                                <widget class="QGroupBox" name="groupBox_k4a">
+                                 <property name="sizePolicy">
+                                  <sizepolicy hsizetype="Ignored" vsizetype="Ignored">
+                                   <horstretch>0</horstretch>
+                                   <verstretch>0</verstretch>
+                                  </sizepolicy>
+                                 </property>
+                                 <property name="title">
+                                  <string>Kinect for Azure</string>
+                                 </property>
+                                 <layout class="QGridLayout" name="gridLayout_113">
+                                  <item row="0" column="0">
+                                   <widget class="QCheckBox" name="checkbox_k4a_irDepth">
+                                    <property name="text">
+                                     <string/>
+                                    </property>
+                                    <property name="checked">
+                                     <bool>false</bool>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="0" column="1">
+                                   <widget class="QLabel" name="label_558">
+                                    <property name="text">
+                                     <string>Use IR for RGB image </string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="1" column="0">
+                                   <widget class="QToolButton" name="toolButton_k4a_mkv">
+                                    <property name="text">
+                                     <string>...</string>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="1" column="1">
+                                   <widget class="QLineEdit" name="lineEdit_k4a_mkv">
+                                    <property name="text">
+                                     <string/>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="1" column="2">
+                                   <widget class="QLabel" name="label_556">
+                                    <property name="text">
+                                     <string>Path to a *.MKV file.</string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
+                                    </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="2" column="1">
+                                   <spacer name="verticalSpacer_83">
+                                    <property name="orientation">
+                                     <enum>Qt::Vertical</enum>
+                                    </property>
+                                    <property name="sizeHint" stdset="0">
+                                     <size>
+                                      <width>20</width>
+                                      <height>0</height>
+                                     </size>
+                                    </property>
+                                   </spacer>
+                                  </item>
+                                 </layout>
+                                </widget>
+                               </item>
+                               <item>
+                                <spacer name="verticalSpacer_82">
+                                 <property name="orientation">
+                                  <enum>Qt::Vertical</enum>
+                                 </property>
+                                 <property name="sizeHint" stdset="0">
+                                  <size>
+                                   <width>20</width>
+                                   <height>168</height>
+                                  </size>
+                                 </property>
+                                </spacer>
+                               </item>
+                              </layout>
+                             </widget>
                             </widget>
                            </item>
                           </layout>
@@ -5435,7 +5541,16 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                        <string>Directory of images (optional settings)</string>
                       </property>
                       <layout class="QVBoxLayout" name="verticalLayout_93">
-                       <property name="margin">
+                       <property name="leftMargin">
+                        <number>0</number>
+                       </property>
+                       <property name="topMargin">
+                        <number>0</number>
+                       </property>
+                       <property name="rightMargin">
+                        <number>0</number>
+                       </property>
+                       <property name="bottomMargin">
                         <number>0</number>
                        </property>
                        <item>
@@ -16663,7 +16778,16 @@ Lower the ratio -&gt; higher the precision.</string>
                         <property name="spacing">
                          <number>0</number>
                         </property>
-                        <property name="margin">
+                        <property name="leftMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="topMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="rightMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="bottomMargin">
                          <number>0</number>
                         </property>
                         <item>
@@ -16743,7 +16867,16 @@ Lower the ratio -&gt; higher the precision.</string>
                         <property name="spacing">
                          <number>0</number>
                         </property>
-                        <property name="margin">
+                        <property name="leftMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="topMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="rightMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="bottomMargin">
                          <number>0</number>
                         </property>
                         <item>
@@ -16855,7 +16988,16 @@ Lower the ratio -&gt; higher the precision.</string>
                         <property name="spacing">
                          <number>0</number>
                         </property>
-                        <property name="margin">
+                        <property name="leftMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="topMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="rightMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="bottomMargin">
                          <number>0</number>
                         </property>
                         <item>

tools/Calibration/CMakeLists.txt

@@ -18,6 +18,14 @@ SET(LIBRARIES
 
 add_definitions(${PCL_DEFINITIONS})
 
+# Hack as CameraRealsense2.h needs realsense2 include dir
+IF(realsense2_FOUND)
+	SET(INCLUDE_DIRS
+		${INCLUDE_DIRS}
+		${realsense2_INCLUDE_DIRS}
+	)
+ENDIF(realsense2_FOUND)
+
 INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
 
 ADD_EXECUTABLE(calibration main.cpp)

tools/CameraRGBD/CMakeLists.txt

@@ -18,6 +18,14 @@ SET(LIBRARIES
 
 add_definitions(${PCL_DEFINITIONS})
 
+# Hack as CameraRealsense2.h needs realsense2 include dir
+IF(realsense2_FOUND)
+	SET(INCLUDE_DIRS
+		${INCLUDE_DIRS}
+		${realsense2_INCLUDE_DIRS}
+	)
+ENDIF(realsense2_FOUND)
+
 INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
 
 ADD_EXECUTABLE(rgbd_camera main.cpp)

tools/CameraRGBD/main.cpp

@@ -61,8 +61,10 @@ void showUsage()
 			"                                     9=RealSense\n"
 			"                                     10=Kinect for Windows 2 SDK\n"
 			"                                     11=RealSense2\n"
+			"                                     12=Kinect for Azure SDK\n"
 			"  Options:\n"
 			"      -rate #.#                      Input rate Hz (default 0=inf)\n"
+			"      -device #                      Device ID (number or string)\n"
 			"      -save_stereo \"path\"            Save stereo images in a folder or a video file (side by side *.avi).\n"
 			"      -fourcc \"XXXX\"               Four characters FourCC code (default is \"MJPG\") used\n"
 			"                                       when saving stereo images to a video file.\n"
@@ -89,6 +91,7 @@ int main(int argc, char * argv[])
 	std::string stereoSavePath;
 	float rate = 0.0f;
 	std::string fourcc = "MJPG";
+	std::string deviceId;
 	if(argc < 2)
 	{
 		showUsage();
@@ -114,6 +117,19 @@ int main(int argc, char * argv[])
 				}
 				continue;
 			}
+			if (strcmp(argv[i], "-device") == 0)
+			{
+				++i;
+				if (i < argc)
+				{
+					deviceId = argv[i];
+				}
+				else
+				{
+					showUsage();
+				}
+				continue;
+			}
 			if(strcmp(argv[i], "-save_stereo") == 0)
 			{
 				++i;
@@ -157,14 +173,14 @@ int main(int argc, char * argv[])
 
 			// last
 			driver = atoi(argv[i]);
-			if(driver < 0 || driver > 11)
+			if(driver < 0 || driver > 12)
 			{
-				UERROR("driver should be between 0 and 11.");
+				UERROR("driver should be between 0 and 12.");
 				showUsage();
 			}
 		}
 	}
-	UINFO("Using driver %d", driver);
+	UINFO("Using driver %d (device=%s)", driver, deviceId.empty()?"0": deviceId.c_str());
 
 	rtabmap::Camera * camera = 0;
 	if(driver < 6)
@@ -177,7 +193,7 @@ int main(int argc, char * argv[])
 
 		if(driver == 0)
 		{
-			camera = new rtabmap::CameraOpenni();
+			camera = new rtabmap::CameraOpenni(deviceId);
 		}
 		else if(driver == 1)
 		{
@@ -186,7 +202,7 @@ int main(int argc, char * argv[])
 				UERROR("Not built with OpenNI2 support...");
 				exit(-1);
 			}
-			camera = new rtabmap::CameraOpenNI2();
+			camera = new rtabmap::CameraOpenNI2(deviceId);
 		}
 		else if(driver == 2)
 		{
@@ -195,7 +211,7 @@ int main(int argc, char * argv[])
 				UERROR("Not built with Freenect support...");
 				exit(-1);
 			}
-			camera = new rtabmap::CameraFreenect();
+			camera = new rtabmap::CameraFreenect(uStr2Int(deviceId));
 		}
 		else if(driver == 3)
 		{
@@ -222,7 +238,7 @@ int main(int argc, char * argv[])
 				UERROR("Not built with Freenect2 support...");
 				exit(-1);
 			}
-			camera = new rtabmap::CameraFreenect2(0, rtabmap::CameraFreenect2::kTypeColor2DepthSD);
+			camera = new rtabmap::CameraFreenect2(uStr2Int(deviceId), rtabmap::CameraFreenect2::kTypeColor2DepthSD);
 		}
 	}
 	else if(driver == 6)
@@ -250,7 +266,7 @@ int main(int argc, char * argv[])
 			UERROR("Not built with ZED sdk support...");
 			exit(-1);
 		}
-		camera = new rtabmap::CameraStereoZed(0);
+		camera = new rtabmap::CameraStereoZed(uStr2Int(deviceId));
 	}
 	else if (driver == 9)
 	{
@@ -259,7 +275,7 @@ int main(int argc, char * argv[])
 			UERROR("Not built with RealSense support...");
 			exit(-1);
 		}
-		camera = new rtabmap::CameraRealSense(0);
+		camera = new rtabmap::CameraRealSense(uStr2Int(deviceId));
 	}
 	else if (driver == 10)
 	{
@@ -268,7 +284,7 @@ int main(int argc, char * argv[])
 			UERROR("Not built with Kinect for Windows 2 SDK support...");
 			exit(-1);
 		}
-		camera = new rtabmap::CameraK4W2(0);
+		camera = new rtabmap::CameraK4W2(uStr2Int(deviceId));
 	}
 	else if (driver == 11)
 	{
@@ -277,7 +293,16 @@ int main(int argc, char * argv[])
 			UERROR("Not built with RealSense2 SDK support...");
 			exit(-1);
 		}
-		camera = new rtabmap::CameraRealSense2();
+		camera = new rtabmap::CameraRealSense2(deviceId);
+	}
+	else if (driver == 12)
+	{
+		if (!rtabmap::CameraK4A::available())
+		{
+			UERROR("Not built with Kienct for Azure SDK support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraK4A(deviceId);
 	}
 	else
 	{

tools/DataRecorder/CMakeLists.txt

@@ -24,6 +24,14 @@ SET(LIBRARIES
 
 add_definitions(${PCL_DEFINITIONS})
 
+# Hack as CameraRealsense2.h needs realsense2 include dir
+IF(realsense2_FOUND)
+	SET(INCLUDE_DIRS
+		${INCLUDE_DIRS}
+		${realsense2_INCLUDE_DIRS}
+	)
+ENDIF(realsense2_FOUND)
+
 INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
 
 IF(MINGW)

tools/OdometryViewer/CMakeLists.txt

@@ -24,6 +24,14 @@ SET(SRC_FILES
 
 add_definitions(${PCL_DEFINITIONS})
 
+# Hack as CameraRealsense2.h needs realsense2 include dir
+IF(realsense2_FOUND)
+	SET(INCLUDE_DIRS
+		${INCLUDE_DIRS}
+		${realsense2_INCLUDE_DIRS}
+	)
+ENDIF(realsense2_FOUND)
+
 INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
 
 IF(MINGW)

tools/RgbdDataset/CMakeLists.txt

@@ -26,6 +26,14 @@ SET(LIBRARIES
 	${PCL_LIBRARIES}
 )
 
+# Hack as CameraRealsense2.h needs realsense2 include dir
+IF(realsense2_FOUND)
+	SET(INCLUDE_DIRS
+		${INCLUDE_DIRS}
+		${realsense2_INCLUDE_DIRS}
+	)
+ENDIF(realsense2_FOUND)
+
 INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
 
 ADD_EXECUTABLE(rgbd_dataset main.cpp)