tango: fixed api19 build with latest master

* Added Kinect for Azure menu option

* Reintegrated Kinect for Azure initialization code, fixed previous GUI commit

* Added ifdefs to fix build

* Added preliminary K4A device support

* Edits to K4A camera implementation

* K4A minor initialization fix

* Typo

* Another initialization fix

* Update CameraRGBD tool to display K4A camera output

* K4A added model parameter so we can create maps

* Added combo boxes for K4A options

* Functionalized combo boxes for K4A options

* Finished adding K4A preferences

.travis.yml

@@ -52,6 +52,21 @@ matrix:
         - mkdir -p build && cd build
         - cmake .. 
         - make
+        
+    - dist: focal
+      install:
+        - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu focal main" > /etc/apt/sources.list.d/ros-latest.list'
+        - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
+        - sudo apt-get update
+        - sudo apt-get update && sudo apt-get install dpkg
+        - sudo apt-get -y install ros-noetic-rtabmap-ros
+        - sudo apt-get -y remove ros-noetic-rtabmap
+
+      script: 
+        - source /opt/ros/noetic/setup.bash
+        - mkdir -p build && cd build
+        - cmake .. 
+        - make
 
 notifications:
   email:

CMakeLists.txt

@@ -21,7 +21,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 20)
-SET(RTABMAP_PATCH_VERSION 0)
+SET(RTABMAP_PATCH_VERSION 2)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   
@@ -471,7 +471,7 @@ ENDIF(WITH_REALSENSE)
 IF(WITH_REALSENSE2)
     IF(WIN32)
         FIND_PACKAGE(RealSense2 QUIET)
-	ELSE()
+    ELSE()
         FIND_PACKAGE(realsense2 QUIET)
     ENDIF()
     IF(realsense2_FOUND)
@@ -689,16 +689,6 @@ ELSEIF(OpenCV_VERSION VERSION_GREATER "3.4.2")
   ENDIF(${matchres} EQUAL -1)
 ENDIF()
 
-# check if version status is "-dev" (SIFT compatibility issue between 4.3.0 vs 4.3.0-dev)
-FIND_FILE(OpenCV_VERSION_HPP opencv2/core/version.hpp
-  PATHS ${OpenCV_INCLUDE_DIRS}
-  NO_DEFAULT_PATH)
-FILE(READ ${OpenCV_VERSION_HPP} TMPTXT)
-STRING(FIND "${TMPTXT}" "-dev" matchres)
-IF(${matchres} EQUAL -1)
-  SET(OPENCV_DEV "//")
-ENDIF(${matchres} EQUAL -1)
-
 IF(NOT G2O_FOUND)
    SET(G2O "//")
 ELSE()

README.md

@@ -1,4 +1,4 @@
-rtabmap ![Analytics](https://ga-beacon.appspot.com/UA-56986679-3/github-main?pixel) 
+rtabmap ![Analytics](https://ga-beacon-279122.nn.r.appspot.com/UA-56986679-3/github-main?pixel) 
 =======
 
 [![RTAB-Map Logo](https://raw.githubusercontent.com/introlab/rtabmap/master/guilib/src/images/RTAB-Map100.png)](http://introlab.github.io/rtabmap)
@@ -7,7 +7,7 @@ rtabmap ![Analytics](https://ga-beacon.appspot.com/UA-56986679-3/github-main?pix
 [![License][license-image]][license]
 Linux: [![Build Status](https://travis-ci.org/introlab/rtabmap.svg?branch=master)](https://travis-ci.org/introlab/rtabmap) Windows: [![Build status](https://ci.appveyor.com/api/projects/status/hr73xspix9oqa26h/branch/master?svg=true)](https://ci.appveyor.com/project/matlabbe/rtabmap/branch/master)
 
-[release-image]: https://img.shields.io/badge/release-0.18.0-green.svg?style=flat
+[release-image]: https://img.shields.io/badge/release-0.20.2-green.svg?style=flat
 [releases]: https://github.com/introlab/rtabmap/releases
 
 [license-image]: https://img.shields.io/badge/license-BSD-green.svg?style=flat

Version.h.in

@@ -38,7 +38,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define RTABMAP_VERSION_COMPARE(major, minor, patch)  (major>=@PROJECT_VERSION_MAJOR@ || (major==@PROJECT_VERSION_MAJOR@ && minor>=@PROJECT_VERSION_MINOR@) || (major==@PROJECT_VERSION_MAJOR@ && minor==@PROJECT_VERSION_MINOR@ && patch >=@PROJECT_VERSION_PATCH@))
 
 @NONFREE@#define RTABMAP_NONFREE
-@OPENCV_DEV@#define RTABMAP_OPENCV_DEV
 @TORO@#define RTABMAP_TORO
 @G2O@#define RTABMAP_G2O
 @G2O_CPP_CONF@#define RTABMAP_G2O_CPP11

app/android/AndroidManifest.xml.in

@@ -57,7 +57,7 @@
             android:excludeFromRecents="true"
             android:exported="false"
             android:launchMode="singleTop"
-            android:theme="@android:style/Theme.Material.Light.Dialog.Alert" />
+            android:theme="@style/ThemeApp" />
             
         <provider
             android:name="android.support.v4.content.FileProvider"

app/android/jni/CMakeLists.txt

@@ -22,6 +22,7 @@ set(sources
   scene.cpp
   point_cloud_drawable.cpp
   graph_drawable.cpp
+  background_renderer.cc
   tango-gl/axis.cpp 
   tango-gl/camera.cpp 
   tango-gl/conversions.cpp 

app/android/jni/CameraARCore.cpp

@@ -34,47 +34,31 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
-#ifdef DEPTH_TEST
-// Camera Callbacks
-static void CameraDeviceOnDisconnected(void* context, ACameraDevice* device) {
-  LOGE("Camera(id: %s) is disconnected.\n", ACameraDevice_getId(device));
-}
-static void CameraDeviceOnError(void* context, ACameraDevice* device,
-                                int error) {
-  LOGE("Error(code: %d) on Camera(id: %s).\n", error,
-       ACameraDevice_getId(device));
-}
-// Capture Callbacks
-bool g_captureSessionReady = false;
-static void CaptureSessionOnReady(void* context,
-                                  ACameraCaptureSession* session) {
-  LOGI("Session is ready.\n");
-  g_captureSessionReady = true;
-}
-static void CaptureSessionOnActive(void* context,
-                                   ACameraCaptureSession* session) {
-  LOGI("Session is activated.\n");
-}
-#endif // DEPTH_TEST
-
 //////////////////////////////
 // CameraARCore
 //////////////////////////////
-CameraARCore::CameraARCore(void* env, void* context, void* activity, bool smoothing):
+CameraARCore::CameraARCore(void* env, void* context, void* activity, bool depthFromMotion, bool smoothing):
 	CameraMobile(smoothing),
 	env_(env),
 	context_(context),
 	activity_(activity),
-	arInstallRequested_(false)
+	arInstallRequested_(false),
+	textureId_(9999),
+	uvs_initialized_(false),
+	updateOcclusionImage_(false),
+	depthFromMotion_(depthFromMotion)
 {
-	glGenTextures(1, &textureId_);
 }
 
 CameraARCore::~CameraARCore() {
 	// Disconnect ARCore service
 	close();
 
-	glDeleteTextures(1, &textureId_);
+	if(textureId_ != 9999)
+	{
+		glDeleteTextures(1, &textureId_);
+		textureId_ = 9999;
+	}
 }
 
 
@@ -146,132 +130,10 @@ std::string CameraARCore::getSerial() const
 	return "ARCore";
 }
 
-#ifdef DEPTH_TEST
-void OnImageCallback(void *ctx, AImageReader *reader) {
-  reinterpret_cast<CameraARCore *>(ctx)->imageCallback(reader);
-}
-void CameraARCore::imageCallback(AImageReader *reader) {
-	int32_t format;
-	media_status_t status = AImageReader_getFormat(reader, &format);
-	UWARN("format=%d", format);
-	UASSERT_MSG(status == AMEDIA_OK, "Failed to get the media format");
-
-	if (format == AIMAGE_FORMAT_DEPTH16) {
-		// Create a thread and write out the jpeg files
-		AImage *image = nullptr;
-		media_status_t status = AImageReader_acquireNextImage(reader, &image);
-		UASSERT_MSG(status == AMEDIA_OK && image, "Image is not available");
-
-		int planeCount;
-		status = AImage_getNumberOfPlanes(image, &planeCount);
-		UASSERT_MSG(status == AMEDIA_OK && planeCount == 1,
-				uFormat("Error: getNumberOfPlanes() planceCount = %d", planeCount).c_str());
-		uint8_t *data = nullptr;
-		int len = 0;
-		int stride;
-		int width;
-		int height;
-		AImage_getWidth(image, &width);
-		AImage_getHeight(image, &height);
-		AImage_getPlaneRowStride(image, 0, &stride);
-		AImage_getPlaneData(image, 0, &data, &len);
-
-		cv::Mat output(height, width, CV_16UC1);
-		uint16_t *dataShort = (uint16_t *)data;
-		uint16_t max=0x0;
-		for (int y = 0; y < output.rows; ++y)
-		{
-			for (int x = 0; x < output.cols; ++x)
-			{
-				uint16_t depthSample = dataShort[y*output.cols + x];
-				uint16_t depthRange = (depthSample & 0x1FFF); // first 3 bits are confidence
-				output.at<uint16_t>(y,x) = depthRange;
-				if(depthRange > max)
-				{
-					max = depthRange;
-				}
-			}
-		}
-		UWARN("width=%d, height=%d, bytes=%d stride=%d max=%dmm",
-				width, height, len, stride, (int)max);
-
-		std::string path = "/storage/emulated/0/RTAB-Map/depth.png";
-		cv::imwrite(path, output);
-		UWARN("depth image saved to %s", path.c_str());
-
-		AImage_delete(image);
-	}
-}
-#endif // DEPTH_TEST
-
 bool CameraARCore::init(const std::string & calibrationFolder, const std::string & cameraName)
 {
 	close();
 
-#ifdef DEPTH_TEST
-	///////////////////////////
-	// Depth image using camera2 API
-	/////////////////////////////
-	camera_status_t cameraStatus = ACAMERA_OK;
-
-	cameraManager_ = ACameraManager_create();
-
-	deviceStateCallbacks_.onDisconnected = CameraDeviceOnDisconnected;
-	deviceStateCallbacks_.onError = CameraDeviceOnError;
-
-	const char * cameraId = "0";
-	cameraStatus = ACameraManager_openCamera(cameraManager_, cameraId, &deviceStateCallbacks_, &cameraDevice_);
-	UASSERT_MSG(cameraStatus == ACAMERA_OK, uFormat("Failed to open camera device (id: %s)",
-			cameraId).c_str());
-
-	// Currently only working resolution on Huawei P30 Pro
-	cv::Size size(240, 180);
-	int format = AIMAGE_FORMAT_DEPTH16;
-
-	media_status_t mediaStatus = AImageReader_new(size.width, size.height, format, 2, &imageReader_);
-	UASSERT_MSG(imageReader_ && mediaStatus == AMEDIA_OK, uFormat("Failed to create AImageReader %dx%d format=%d",
-			size.width, size.height, format).c_str());
-
-	AImageReader_ImageListener listener{
-	  .context = this,
-	  .onImageAvailable = OnImageCallback,
-	};
-	AImageReader_setImageListener(imageReader_, &listener);
-
-	//
-	ANativeWindow *nativeWindow;
-	mediaStatus = AImageReader_getWindow(imageReader_, &nativeWindow);
-	UASSERT_MSG(mediaStatus == AMEDIA_OK, "Could not get ANativeWindow");
-
-	outputNativeWindow_ = nativeWindow;
-	ACaptureSessionOutputContainer_create(&captureSessionOutputContainer_);
-	ANativeWindow_acquire(outputNativeWindow_);
-	ACaptureSessionOutput_create(outputNativeWindow_, &sessionOutput_);
-	ACaptureSessionOutputContainer_add(captureSessionOutputContainer_, sessionOutput_);
-	ACameraOutputTarget_create(outputNativeWindow_, &cameraOutputTarget_);
-
-	cameraStatus = ACameraDevice_createCaptureRequest(cameraDevice_, TEMPLATE_RECORD, &captureRequest_);
-	UASSERT_MSG(cameraStatus == ACAMERA_OK,
-			uFormat("Failed to create preview capture request (id: %s, status=%d)",
-					cameraId, cameraStatus).c_str());
-
-	ACaptureRequest_addTarget(captureRequest_, cameraOutputTarget_);
-
-	captureSessionStateCallbacks_.onReady = CaptureSessionOnReady;
-	captureSessionStateCallbacks_.onActive = CaptureSessionOnActive;
-	ACameraDevice_createCaptureSession(
-			cameraDevice_,
-			captureSessionOutputContainer_, // outputs
-			&captureSessionStateCallbacks_, // callbacks
-			&captureSession_);
-
-	ACameraCaptureSession_setRepeatingRequest(captureSession_, nullptr, 1,
-			&captureRequest_, nullptr);
-
-	// Don't start ARCore as we cannot use both at the same time
-	return true;
-#endif // DEPTH_TEST
-
 	UScopeMutex lock(arSessionMutex_);
 
 	ArInstallStatus install_status;
@@ -302,10 +164,19 @@ bool CameraARCore::init(const std::string & calibrationFolder, const std::string
 	UASSERT(ArSession_create(env_, context_, &arSession_) == AR_SUCCESS);
 	UASSERT(arSession_);
 
+	int32_t is_depth_supported = 0;
+	ArSession_isDepthModeSupported(arSession_, AR_DEPTH_MODE_AUTOMATIC, &is_depth_supported);
+
 	ArConfig_create(arSession_, &arConfig_);
 	UASSERT(arConfig_);
 
-	ArConfig_setFocusMode(arSession_, arConfig_, AR_FOCUS_MODE_FIXED);
+	if (is_depth_supported!=0) {
+	  ArConfig_setDepthMode(arSession_, arConfig_, AR_DEPTH_MODE_AUTOMATIC);
+	} else {
+	  ArConfig_setDepthMode(arSession_, arConfig_, AR_DEPTH_MODE_DISABLED);
+	}
+
+	ArConfig_setFocusMode(arSession_, arConfig_, AR_FOCUS_MODE_AUTO);
 	UASSERT(ArSession_configure(arSession_, arConfig_) == AR_SUCCESS);
 
 	ArFrame_create(arSession_, &arFrame_);
@@ -361,9 +232,6 @@ bool CameraARCore::init(const std::string & calibrationFolder, const std::string
 
 	deviceTColorCamera_ = opticalRotation;
 
-	// Required as ArSession_update does some off-screen OpenGL stuff...
-	ArSession_setCameraTextureName(arSession_, textureId_);
-
 	if (ArSession_resume(arSession_) != ArStatus::AR_SUCCESS)
 	{
 		UERROR("Cannot resume camera!");
@@ -410,45 +278,8 @@ void CameraARCore::close()
 	}
 	arPose_ = nullptr;
 
-#ifdef DEPTH_TEST
-
-	if(captureSession_!=nullptr)
-	{
-		g_captureSessionReady = false;
-		ACameraCaptureSession_stopRepeating(captureSession_);
-		double start = UTimer::now();
-		while(g_captureSessionReady != true && UTimer::now()-start < 2.0){
-			uSleep(100);
-			UWARN("Waiting session to close.... max 2 seconds");
-		}
-		//ACameraCaptureSession_close(captureSession_); // FIXME: this crashes?!
-		captureSession_ = nullptr;
-
-		ACaptureRequest_removeTarget(captureRequest_, cameraOutputTarget_);
-		ACaptureRequest_free(captureRequest_);
-		ACameraOutputTarget_free(cameraOutputTarget_);
-		captureRequest_ = nullptr;
-		cameraOutputTarget_ = nullptr;
-
-		ACaptureSessionOutputContainer_remove(captureSessionOutputContainer_, sessionOutput_);
-		ANativeWindow_release(outputNativeWindow_);
-		ACaptureSessionOutputContainer_free(captureSessionOutputContainer_);
-		ACaptureSessionOutput_free(sessionOutput_);
-		captureSessionOutputContainer_ = nullptr;
-		sessionOutput_ = nullptr;
-
-		ACameraDevice_close(cameraDevice_);
-		cameraDevice_ = nullptr;
-
-		ACameraManager_delete(cameraManager_);
-		cameraManager_ = nullptr;
-
-		AImageReader_delete(imageReader_);
-		imageReader_ = nullptr;
-	}
-#endif
-
 	CameraMobile::close();
+	occlusionImage_ = cv::Mat();
 }
 
 LaserScan CameraARCore::scanFromPointCloudData(
@@ -499,6 +330,20 @@ LaserScan CameraARCore::scanFromPointCloudData(
 	return LaserScan();
 }
 
+void CameraARCore::setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height)
+{
+	CameraMobile::setScreenRotationAndSize(colorCameraToDisplayRotation, width, height);
+	if(arSession_)
+	{
+		int ret = static_cast<int>(colorCameraToDisplayRotation) + 1; // remove 90deg camera rotation
+		  if (ret > 3) {
+		    ret -= 4;
+		  }
+
+		ArSession_setDisplayGeometry(arSession_, ret, width, height);
+	}
+}
+
 SensorData CameraARCore::captureImage(CameraInfo * info)
 {
 	UScopeMutex lock(arSessionMutex_);
@@ -510,15 +355,44 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 		return data;
 	}
 
+	if(textureId_ == 9999)
+	{
+		glGenTextures(1, &textureId_);
+		glBindTexture(GL_TEXTURE_EXTERNAL_OES, textureId_);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+	}
+	ArSession_setCameraTextureName(arSession_, textureId_);
+
 	// Update session to get current frame and render camera background.
 	if (ArSession_update(arSession_, arFrame_) != AR_SUCCESS) {
 		LOGE("CameraARCore::captureImage() ArSession_update error");
 		return data;
 	}
 
+	// If display rotation changed (also includes view size change), we need to
+	// re-query the uv coordinates for the on-screen portion of the camera image.
+	int32_t geometry_changed = 0;
+	ArFrame_getDisplayGeometryChanged(arSession_, arFrame_, &geometry_changed);
+	if (geometry_changed != 0 || !uvs_initialized_) {
+		ArFrame_transformCoordinates2d(
+				arSession_, arFrame_, AR_COORDINATES_2D_OPENGL_NORMALIZED_DEVICE_COORDINATES,
+				BackgroundRenderer::kNumVertices, BackgroundRenderer_kVertices, AR_COORDINATES_2D_TEXTURE_NORMALIZED,
+				transformed_uvs_);
+		UASSERT(transformed_uvs_);
+		uvs_initialized_ = true;
+	}
+
 	ArCamera* ar_camera;
 	ArFrame_acquireCamera(arSession_, arFrame_, &ar_camera);
 
+	ArCamera_getViewMatrix(arSession_, ar_camera, glm::value_ptr(viewMatrix_));
+	ArCamera_getProjectionMatrix(arSession_, ar_camera,
+							   /*near=*/0.1f, /*far=*/100.f,
+							   glm::value_ptr(projectionMatrix_));
+
 	ArTrackingState camera_tracking_state;
 	ArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state);
 
@@ -551,17 +425,55 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 			ArPointCloud * pointCloud = nullptr;
 			ArFrame_acquirePointCloud(arSession_, arFrame_, &pointCloud);
 
+			int32_t is_depth_supported = 0;
+			ArSession_isDepthModeSupported(arSession_, AR_DEPTH_MODE_AUTOMATIC, &is_depth_supported);
+
 			ArImage * image = nullptr;
 			ArStatus status = ArFrame_acquireCameraImage(arSession_, arFrame_, &image);
 			if(status == AR_SUCCESS)
 			{
+				if(is_depth_supported && (updateOcclusionImage_||depthFromMotion_))
+				{
+					LOGD("Acquire depth image!");
+					ArImage * depthImage = nullptr;
+					ArFrame_acquireDepthImage(arSession_, arFrame_, &depthImage);
+
+					ArImageFormat format;
+					ArImage_getFormat(arSession_, depthImage, &format);
+					if(format == AR_IMAGE_FORMAT_DEPTH16)
+					{
+						LOGD("Depth format detected!");
+						int planeCount;
+						ArImage_getNumberOfPlanes(arSession_, depthImage, &planeCount);
+						LOGD("planeCount=%d", planeCount);
+						UASSERT_MSG(planeCount == 1, uFormat("Error: getNumberOfPlanes() planceCount = %d", planeCount).c_str());
+						const uint8_t *data = nullptr;
+						int len = 0;
+						int stride;
+						int depth_width;
+						int depth_height;
+						ArImage_getWidth(arSession_, depthImage, &depth_width);
+						ArImage_getHeight(arSession_, depthImage, &depth_height);
+						ArImage_getPlaneRowStride(arSession_, depthImage, 0, &stride);
+						ArImage_getPlaneData(arSession_, depthImage, 0, &data, &len);
+
+						LOGD("width=%d, height=%d, bytes=%d stride=%d", depth_width, depth_height, len, stride);
+
+						occlusionImage_ = cv::Mat(depth_height, depth_width, CV_16UC1, (void*)data).clone();
+
+						float scaleX = (float)depth_width / (float)width;
+						float scaleY = (float)depth_height / (float)height;
+						occlusionModel_ = CameraModel(fx*scaleX, fy*scaleY, cx*scaleX, cy*scaleY, pose*deviceTColorCamera_, 0, cv::Size(depth_width, depth_height));
+					}
+					ArImage_release(depthImage);
+				}
+
 				int64_t timestamp_ns;
 				ArImageFormat format;
 				ArImage_getTimestamp(arSession_, image, &timestamp_ns);
 				ArImage_getFormat(arSession_, image, &format);
 				if(format == AR_IMAGE_FORMAT_YUV_420_888)
 				{
-
 #ifndef DISABLE_LOG
 					int32_t num_planes;
 					ArImage_getNumberOfPlanes(arSession_, image, &num_planes);
@@ -623,7 +535,7 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 							LOGI("pointCloud empty");
 						}
 
-						data = SensorData(scan, rgb, cv::Mat(), model, 0, stamp);
+						data = SensorData(scan, rgb, depthFromMotion_?occlusionImage_:cv::Mat(), model, 0, stamp);
 						data.setFeatures(kpts,  kpts3, cv::Mat());
 					}
 				}
@@ -662,21 +574,49 @@ void CameraARCore::capturePoseOnly()
 	UScopeMutex lock(arSessionMutex_);
 	//LOGI("Capturing image...");
 
-	SensorData data;
 	if(!arSession_)
 	{
 		return;
 	}
 
+	if(textureId_ == 9999)
+	{
+		glGenTextures(1, &textureId_);
+		glBindTexture(GL_TEXTURE_EXTERNAL_OES, textureId_);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+	}
+	ArSession_setCameraTextureName(arSession_, textureId_);
+
 	// Update session to get current frame and render camera background.
 	if (ArSession_update(arSession_, arFrame_) != AR_SUCCESS) {
-		LOGE("CameraARCore::captureImage() ArSession_update error");
+		LOGE("CameraARCore::capturePoseOnly() ArSession_update error");
 		return;
 	}
 
+	// If display rotation changed (also includes view size change), we need to
+	// re-query the uv coordinates for the on-screen portion of the camera image.
+	int32_t geometry_changed = 0;
+	ArFrame_getDisplayGeometryChanged(arSession_, arFrame_, &geometry_changed);
+	if (geometry_changed != 0 || !uvs_initialized_) {
+		ArFrame_transformCoordinates2d(
+				arSession_, arFrame_, AR_COORDINATES_2D_OPENGL_NORMALIZED_DEVICE_COORDINATES,
+				BackgroundRenderer::kNumVertices, BackgroundRenderer_kVertices, AR_COORDINATES_2D_TEXTURE_NORMALIZED,
+				transformed_uvs_);
+		UASSERT(transformed_uvs_);
+		uvs_initialized_ = true;
+	}
+
 	ArCamera* ar_camera;
 	ArFrame_acquireCamera(arSession_, arFrame_, &ar_camera);
 
+	ArCamera_getViewMatrix(arSession_, ar_camera, glm::value_ptr(viewMatrix_));
+	ArCamera_getProjectionMatrix(arSession_, ar_camera,
+							   /*near=*/0.1f, /*far=*/100.f,
+							   glm::value_ptr(projectionMatrix_));
+
 	ArTrackingState camera_tracking_state;
 	ArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state);
 
@@ -694,6 +634,52 @@ void CameraARCore::capturePoseOnly()
 			pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
 			this->poseReceived(pose);
 		}
+
+		int32_t is_depth_supported = 0;
+		ArSession_isDepthModeSupported(arSession_, AR_DEPTH_MODE_AUTOMATIC, &is_depth_supported);
+
+		if(is_depth_supported && updateOcclusionImage_)
+		{
+			LOGD("Acquire depth image!");
+			ArImage * depthImage = nullptr;
+			ArFrame_acquireDepthImage(arSession_, arFrame_, &depthImage);
+
+			ArImageFormat format;
+			ArImage_getFormat(arSession_, depthImage, &format);
+			if(format == AR_IMAGE_FORMAT_DEPTH16)
+			{
+				LOGD("Depth format detected!");
+				int planeCount;
+				ArImage_getNumberOfPlanes(arSession_, depthImage, &planeCount);
+				LOGD("planeCount=%d", planeCount);
+				UASSERT_MSG(planeCount == 1, uFormat("Error: getNumberOfPlanes() planceCount = %d", planeCount).c_str());
+				const uint8_t *data = nullptr;
+				int len = 0;
+				int stride;
+				int width;
+				int height;
+				ArImage_getWidth(arSession_, depthImage, &width);
+				ArImage_getHeight(arSession_, depthImage, &height);
+				ArImage_getPlaneRowStride(arSession_, depthImage, 0, &stride);
+				ArImage_getPlaneData(arSession_, depthImage, 0, &data, &len);
+
+				LOGD("width=%d, height=%d, bytes=%d stride=%d", width, height, len, stride);
+
+				occlusionImage_ = cv::Mat(height, width, CV_16UC1, (void*)data).clone();
+
+				float fx,fy, cx, cy;
+				int32_t rgb_width, rgb_height;
+				ArCamera_getImageIntrinsics(arSession_, ar_camera, arCameraIntrinsics_);
+				ArCameraIntrinsics_getFocalLength(arSession_, arCameraIntrinsics_, &fx, &fy);
+				ArCameraIntrinsics_getPrincipalPoint(arSession_, arCameraIntrinsics_, &cx, &cy);
+				ArCameraIntrinsics_getImageDimensions(arSession_, arCameraIntrinsics_, &rgb_width, &rgb_height);
+
+				float scaleX = (float)width / (float)rgb_width;
+				float scaleY = (float)height / (float)rgb_height;
+				occlusionModel_ = CameraModel(fx*scaleX, fy*scaleY, cx*scaleX, cy*scaleY, pose*deviceTColorCamera_, 0, cv::Size(width, height));
+			}
+			ArImage_release(depthImage);
+		}
 	}
 
 	ArCamera_release(ar_camera);

app/android/jni/CameraARCore.h

@@ -38,14 +38,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UEvent.h>
 #include <rtabmap/utilite/UTimer.h>
 #include <boost/thread/mutex.hpp>
+#include <background_renderer.h>
 
 #include <arcore_c_api.h>
-#ifdef DEPTH_TEST
 #include <camera/NdkCameraDevice.h>
 #include <camera/NdkCameraManager.h>
 #include <media/NdkImageReader.h>
 #include <android/native_window.h>
-#endif
 
 namespace rtabmap {
 
@@ -61,19 +60,28 @@ public:
 			std::vector<cv::Point3f> * kpts3D = 0);
 
 public:
-	CameraARCore(void* env, void* context, void* activity, bool smoothing = false);
+	CameraARCore(void* env, void* context, void* activity, bool depthFromMotion = false, bool smoothing = false);
 	virtual ~CameraARCore();
 
+	bool uvsInitialized() const {return uvs_initialized_;}
+	const float* uvsTransformed() const {return transformed_uvs_;}
+	void getVPMatrices(glm::mat4 & view, glm::mat4 & projection) const {view=viewMatrix_; projection=projectionMatrix_;}
+
+	void updateOcclusionImage(bool enabled) {updateOcclusionImage_ = enabled;}
+	const cv::Mat & getOcclusionImage(CameraModel * model=0) const {if(model)*model=occlusionModel_; return occlusionImage_; }
+
+	virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height);
+
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	void setupGL();
 	virtual void close(); // close Tango connection
 	virtual std::string getSerial() const;
+	GLuint getTextureId() const {return textureId_;}
 
-#ifdef DEPTH_TEST
 	void imageCallback(AImageReader *reader);
-#endif // DEPTH_TEST
 
 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(CameraInfo * info = 0); // should be called in opengl thread
 	virtual void capturePoseOnly();
 
 private:
@@ -92,22 +100,15 @@ private:
 	GLuint textureId_;
 	UMutex arSessionMutex_;
 
-#ifdef DEPTH_TEST
-	// Camera variables
-	ACameraDevice* cameraDevice_ = nullptr;
-	ACaptureRequest* captureRequest_ = nullptr;
-	ACameraOutputTarget* cameraOutputTarget_ = nullptr;
-	ACaptureSessionOutput* sessionOutput_ = nullptr;
-	ACaptureSessionOutputContainer* captureSessionOutputContainer_ = nullptr;
-	ACameraCaptureSession* captureSession_ = nullptr;
-	ANativeWindow *outputNativeWindow_ = nullptr;
+	float transformed_uvs_[BackgroundRenderer::kNumVertices*2];
+	bool uvs_initialized_ = false;
+	glm::mat4 viewMatrix_;
+	glm::mat4 projectionMatrix_;
 
-	ACameraDevice_StateCallbacks deviceStateCallbacks_;
-	ACameraCaptureSession_stateCallbacks captureSessionStateCallbacks_;
-
-	ACameraManager* cameraManager_ = nullptr;
-	AImageReader* imageReader_ = nullptr;
-#endif // DEPTH_TEST
+	bool updateOcclusionImage_;
+	cv::Mat occlusionImage_;
+	CameraModel occlusionModel_;
+	bool depthFromMotion_;
 };
 
 } /* namespace rtabmap */

app/android/jni/CameraMobile.cpp

@@ -207,6 +207,7 @@ void CameraMobile::mainLoop()
 		// Rotate image depending on the camera orientation
 		if(colorCameraToDisplayRotation_ == ROTATION_90)
 		{
+			UDEBUG("ROTATION_90");
 			cv::Mat rgb, depth;
 			cv::Mat rgbt(data.imageRaw().cols, data.imageRaw().rows, data.imageRaw().type());
 			cv::flip(data.imageRaw(),rgb,1);
@@ -226,9 +227,18 @@ void CameraMobile::mainLoop()
 					model.localTransform()*rtabmap::Transform(0,-1,0,0, 1,0,0,0, 0,0,1,0));
 			model.setImageSize(sizet);
 			data.setRGBDImage(rgb, depth, model);
+
+			std::vector<cv::KeyPoint> keypoints = data.keypoints();
+			for(size_t i=0; i<keypoints.size(); ++i)
+			{
+				keypoints[i].pt.x = data.keypoints()[i].pt.y;
+				keypoints[i].pt.y = rgb.rows - data.keypoints()[i].pt.x;
+			}
+			data.setFeatures(keypoints, data.keypoints3D(), cv::Mat());
 		}
 		else if(colorCameraToDisplayRotation_ == ROTATION_180)
 		{
+			UDEBUG("ROTATION_180");
 			cv::Mat rgb, depth;
 			cv::flip(data.imageRaw(),rgb,1);
 			cv::flip(rgb,rgb,0);
@@ -244,9 +254,18 @@ void CameraMobile::mainLoop()
 					model.localTransform()*rtabmap::Transform(0,0,0,0,0,1,0));
 			model.setImageSize(sizet);
 			data.setRGBDImage(rgb, depth, model);
+
+			std::vector<cv::KeyPoint> keypoints = data.keypoints();
+			for(size_t i=0; i<keypoints.size(); ++i)
+			{
+				keypoints[i].pt.x = rgb.cols - data.keypoints()[i].pt.x;
+				keypoints[i].pt.y = rgb.rows - data.keypoints()[i].pt.y;
+			}
+			data.setFeatures(keypoints, data.keypoints3D(), cv::Mat());
 		}
 		else if(colorCameraToDisplayRotation_ == ROTATION_270)
 		{
+			UDEBUG("ROTATION_270");
 			cv::Mat rgb(data.imageRaw().cols, data.imageRaw().rows, data.imageRaw().type());
 			cv::transpose(data.imageRaw(),rgb);
 			cv::flip(rgb,rgb,1);
@@ -263,6 +282,14 @@ void CameraMobile::mainLoop()
 					model.localTransform()*rtabmap::Transform(0,1,0,0, -1,0,0,0, 0,0,1,0));
 			model.setImageSize(sizet);
 			data.setRGBDImage(rgb, depth, model);
+
+			std::vector<cv::KeyPoint> keypoints = data.keypoints();
+			for(size_t i=0; i<keypoints.size(); ++i)
+			{
+				keypoints[i].pt.x = rgb.cols - data.keypoints()[i].pt.y;
+				keypoints[i].pt.y = data.keypoints()[i].pt.x;
+			}
+			data.setFeatures(keypoints, data.keypoints3D(), cv::Mat());
 		}
 
 		rtabmap::Transform pose = info.odomPose;

app/android/jni/CameraMobile.h

@@ -94,7 +94,7 @@ public:
 	const CameraModel & getCameraModel() const {return model_;}
 	const Transform & getDeviceTColorCamera() const {return deviceTColorCamera_;}
 	void setSmoothing(bool enabled) {smoothing_ = enabled;}
-	void setScreenRotation(ScreenRotation colorCameraToDisplayRotation) {colorCameraToDisplayRotation_ = colorCameraToDisplayRotation;}
+	virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height) {colorCameraToDisplayRotation_ = colorCameraToDisplayRotation;}
 	void setGPS(const GPS & gps);
 	void addEnvSensor(int type, float value);
 	void setData(const SensorData & data, const Transform & pose);

app/android/jni/RTABMapApp.cpp

@@ -69,6 +69,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/surface/poisson.h>
 #include <pcl/surface/vtk_smoothing/vtk_mesh_quadric_decimation.h>
 
+
 #define LOW_RES_PIX 2
 //#define DEBUG_RENDERING_PERFORMANCE
 
@@ -263,7 +264,7 @@ void RTABMapApp::setScreenRotation(int displayRotation, int cameraRotation)
 	boost::mutex::scoped_lock  lock(cameraMutex_);
 	if(camera_)
 	{
-		camera_->setScreenRotation(rotation);
+		camera_->setScreenRotationAndSize(main_scene_.getScreenRotation(), main_scene_.getViewPortWidth(), main_scene_.getViewPortHeight());
 	}
 }
 
@@ -657,6 +658,10 @@ bool RTABMapApp::isBuiltWith(int cameraDriver) const
 
 bool RTABMapApp::startCamera(JNIEnv* env, jobject iBinder, jobject context, jobject activity, int driver)
 {
+	//ccapp = new computer_vision::ComputerVisionApplication();
+	//ccapp->OnResume(env, context, activity);
+	//return true;
+
 	cameraDriver_ = driver;
 	LOGW("startCamera() camera driver=%d", cameraDriver_);
 	boost::mutex::scoped_lock  lock(cameraMutex_);
@@ -685,8 +690,7 @@ bool RTABMapApp::startCamera(JNIEnv* env, jobject iBinder, jobject context, jobj
 	else if(cameraDriver_ == 1)
 	{
 #ifdef RTABMAP_ARCORE
-		camera_ = new rtabmap::CameraARCore(env, context, activity, smoothing_);
-
+		camera_ = new rtabmap::CameraARCore(env, context, activity, depthFromMotion_, smoothing_);
 #else
 		UERROR("RTAB-Map is not built with ARCore support!");
 #endif
@@ -712,7 +716,7 @@ bool RTABMapApp::startCamera(JNIEnv* env, jobject iBinder, jobject context, jobj
 
 	if(camera_->init())
 	{
-		camera_->setScreenRotation(main_scene_.getScreenRotation());
+		camera_->setScreenRotationAndSize(main_scene_.getScreenRotation(), main_scene_.getViewPortWidth(), main_scene_.getViewPortHeight());
 
 		//update mesh decimation based on camera calibration
 		LOGI("Cloud density level %d", cloudDensityLevel_);
@@ -937,6 +941,11 @@ void RTABMapApp::SetViewPort(int width, int height)
 {
 	UINFO("");
 	main_scene_.SetupViewPort(width, height);
+	boost::mutex::scoped_lock  lock(cameraMutex_);
+	if(camera_)
+	{
+		camera_->setScreenRotationAndSize(main_scene_.getScreenRotation(), main_scene_.getViewPortWidth(), main_scene_.getViewPortHeight());
+	}
 }
 
 class PostRenderEvent : public UEvent
@@ -1101,12 +1110,59 @@ int RTABMapApp::Render()
 		}
 
 		// ARCore and AREngine capture should be done in opengl thread!
+		const float* uvsTransformed = 0;
+		glm::mat4 arProjectionMatrix(0);
+		glm::mat4 arViewMatrix(0);
+		rtabmap::Mesh occlusionMesh;
 		if((cameraDriver_ == 1 || cameraDriver_ == 2) && camera_!=0)
 		{
 			boost::mutex::scoped_lock  lock(cameraMutex_);
 			if(camera_!=0)
 			{
+#ifdef RTABMAP_ARCORE
+				if(cameraDriver_ == 1)
+				{
+					((rtabmap::CameraARCore*)camera_)->updateOcclusionImage(!visualizingMesh_ && main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson);
+				}
+#endif
+
 				camera_->spinOnce();
+
+#ifdef RTABMAP_ARCORE
+				if(cameraDriver_ == 1)
+				{
+					if(main_scene_.background_renderer_ == 0)
+					{
+						main_scene_.background_renderer_ = new BackgroundRenderer();
+						main_scene_.background_renderer_->InitializeGlContent(((rtabmap::CameraARCore*)camera_)->getTextureId());
+					}
+					if(((rtabmap::CameraARCore*)camera_)->uvsInitialized())
+					{
+						uvsTransformed = ((rtabmap::CameraARCore*)camera_)->uvsTransformed();
+						((rtabmap::CameraARCore*)camera_)->getVPMatrices(arViewMatrix, arProjectionMatrix);
+					}
+					if(!visualizingMesh_ && main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson)
+					{
+						rtabmap::CameraModel occlusionModel;
+						cv::Mat occlusionImage = ((rtabmap::CameraARCore*)camera_)->getOcclusionImage(&occlusionModel);
+
+						if(occlusionModel.isValidForProjection())
+						{
+							pcl::IndicesPtr indices(new std::vector<int>);
+							pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::cloudFromDepth(occlusionImage, occlusionModel, 1, 0, 0, indices.get());
+							cloud = rtabmap::util3d::transformPointCloud(cloud, rtabmap::opengl_world_T_rtabmap_world*occlusionModel.localTransform());
+							occlusionMesh.cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>());
+							pcl::copyPointCloud(*cloud, *occlusionMesh.cloud);
+							occlusionMesh.indices = indices;
+							occlusionMesh.polygons = rtabmap::util3d::organizedFastMesh(cloud, 1.0*M_PI/180.0, false, meshTrianglePix_);
+						}
+						else
+						{
+							UERROR("invalid occlusionModel: %f %f %f %f %dx%d", occlusionModel.fx(), occlusionModel.fy(), occlusionModel.cx(), occlusionModel.cy(), occlusionModel.imageWidth(), occlusionModel.imageHeight());
+						}
+					}
+				}
+#endif
 			}
 		}
 
@@ -1792,7 +1848,7 @@ int RTABMapApp::Render()
 
 			fpsTime.restart();
 			main_scene_.setFrustumVisible(camera_!=0);
-			lastDrawnCloudsCount_ = main_scene_.Render();
+			lastDrawnCloudsCount_ = main_scene_.Render(uvsTransformed, arViewMatrix, arProjectionMatrix, occlusionMesh);
 			if(renderingTime_ < fpsTime.elapsed())
 			{
 				renderingTime_ = fpsTime.elapsed();
@@ -2078,6 +2134,14 @@ void RTABMapApp::setSmoothing(bool enabled)
 	}
 }
 
+void RTABMapApp::setDepthFromMotion(bool enabled)
+{
+	if(depthFromMotion_ != enabled)
+	{
+		depthFromMotion_ = enabled;
+	}
+}
+
 void RTABMapApp::setAppendMode(bool enabled)
 {
 	if(appendMode_ != enabled)

app/android/jni/RTABMapApp.h

@@ -44,6 +44,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/pcl_base.h>
 #include <pcl/TextureMesh.h>
 
+
 // RTABMapApp handles the application lifecycle and resources.
 class RTABMapApp : public UEventsHandler {
  public:
@@ -109,6 +110,7 @@ class RTABMapApp : public UEventsHandler {
   void setCameraColor(bool enabled);
   void setFullResolution(bool enabled);
   void setSmoothing(bool enabled);
+  void setDepthFromMotion(bool enabled);
   void setAppendMode(bool enabled);
   void setDataRecorderMode(bool enabled);
   void setMaxCloudDepth(float value);
@@ -182,6 +184,7 @@ class RTABMapApp : public UEventsHandler {
   bool trajectoryMode_;
   bool rawScanSaved_;
   bool smoothing_;
+  bool depthFromMotion_;
   bool cameraColor_;
   bool fullResolution_;
   bool appendMode_;

app/android/jni/background_renderer.cc

@@ -0,0 +1,92 @@
+/*
+ * Copyright 2018 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+// This modules handles drawing the passthrough camera image into the OpenGL
+// scene.
+
+#include "background_renderer.h"
+
+#include <type_traits>
+
+namespace {
+
+const std::string kVertexShader =
+		"attribute vec4 a_Position;\n"
+		"attribute vec2 a_TexCoord;\n"
+
+		"varying vec2 v_TexCoord;\n"
+
+		"void main() {\n"
+		"   gl_Position = a_Position;\n"
+		"   v_TexCoord = a_TexCoord;\n"
+		"}\n";
+
+const std::string kFragmentShader =
+	"#extension GL_OES_EGL_image_external : require\n"
+
+	"precision mediump float;\n"
+	"varying vec2 v_TexCoord;\n"
+	"uniform samplerExternalOES sTexture;\n"
+
+	"void main() {\n"
+	"    vec4 sample = texture2D(sTexture, v_TexCoord);\n"
+	"    float grey = 0.21 * sample.r + 0.71 * sample.g + 0.07 * sample.b;\n"
+	"    gl_FragColor = vec4(grey, grey, grey, 0.5);\n"
+	"}\n";
+
+}  // namespace
+
+void BackgroundRenderer::InitializeGlContent(GLuint textureId)
+{
+  texture_id_ = textureId;
+
+  shader_program_ = tango_gl::util::CreateProgram(kVertexShader.c_str(), kFragmentShader.c_str());
+  if (!shader_program_) {
+    LOGE("Could not create program.");
+  }
+  glUseProgram(shader_program_);
+  attribute_vertices_ = glGetAttribLocation(shader_program_, "a_Position");
+  attribute_uvs_ = glGetAttribLocation(shader_program_, "a_TexCoord");
+  glUseProgram(0);
+}
+
+void BackgroundRenderer::Draw(const float * transformed_uvs) {
+  static_assert(std::extent<decltype(BackgroundRenderer_kVertices)>::value == kNumVertices * 2, "Incorrect kVertices length");
+
+  glUseProgram(shader_program_);
+  glDepthMask(GL_FALSE);
+  glEnable (GL_BLEND);
+
+  glActiveTexture(GL_TEXTURE0);
+  glBindTexture(GL_TEXTURE_EXTERNAL_OES, texture_id_);
+
+  glVertexAttribPointer(attribute_vertices_, 2, GL_FLOAT, GL_FALSE, 0, BackgroundRenderer_kVertices);
+  glVertexAttribPointer(attribute_uvs_, 2, GL_FLOAT, GL_FALSE, 0, transformed_uvs);
+
+  glEnableVertexAttribArray(attribute_vertices_);
+  glEnableVertexAttribArray(attribute_uvs_);
+
+  glDrawArrays(GL_TRIANGLE_STRIP, 0, 4);
+
+  glDisableVertexAttribArray(attribute_vertices_);
+  glDisableVertexAttribArray(attribute_uvs_);
+
+  glUseProgram(0);
+  glDepthMask(GL_TRUE);
+  glDisable (GL_BLEND);
+  tango_gl::util::CheckGlError("BackgroundRenderer::Draw() error");
+}
+

app/android/jni/background_renderer.h

@@ -0,0 +1,58 @@
+/*
+ * Copyright 2018 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef C_ARCORE_AUGMENTED_IMAGE_BACKGROUND_RENDERER_H_
+#define C_ARCORE_AUGMENTED_IMAGE_BACKGROUND_RENDERER_H_
+
+#include <GLES2/gl2.h>
+#include <GLES2/gl2ext.h>
+#include <cstdlib>
+
+#include "util.h"
+
+static const GLfloat BackgroundRenderer_kVertices[] = {
+	-1.0f, -1.0f, +1.0f, -1.0f, -1.0f, +1.0f, +1.0f, +1.0f,
+};
+
+// This class renders the passthrough camera image into the OpenGL frame.
+class BackgroundRenderer {
+public:
+	// Positions of the quad vertices in clip space (X, Y).
+
+	static constexpr int kNumVertices = 4;
+
+ public:
+  BackgroundRenderer() = default;
+  ~BackgroundRenderer() = default;
+
+  // Sets up OpenGL state.  Must be called on the OpenGL thread and before any
+  // other methods below.
+  void InitializeGlContent(GLuint textureId);
+
+  // Draws the background image.  This methods must be called for every ArFrame
+  // returned by ArSession_update() to catch display geometry change events.
+  void Draw(const float * transformed_uvs);
+
+ private:
+
+  GLuint shader_program_;
+  GLuint texture_id_;
+
+  GLuint attribute_vertices_;
+  GLuint attribute_uvs_;
+};
+
+#endif  // C_ARCORE_AUGMENTED_IMAGE_BACKGROUND_RENDERER_H_

app/android/jni/jni_interface.cpp

@@ -512,6 +512,19 @@ Java_com_introlab_rtabmap_RTABMapLib_setSmoothing(
 	}
 }
 JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setDepthFromMotion(
+		JNIEnv*, jclass, jlong native_application, bool enabled)
+{
+	if(native_application)
+	{
+		return native(native_application)->setDepthFromMotion(enabled);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setCameraColor(
 		JNIEnv*, jclass, jlong native_application, bool enabled)
 {

app/android/jni/scene.cpp

@@ -69,6 +69,7 @@ const std::string kGraphFragmentShader =
 
 
 Scene::Scene() :
+		background_renderer_(0),
 		gesture_camera_(0),
 		axis_(0),
 		frustum_(0),
@@ -160,6 +161,8 @@ void Scene::DeleteResources() {
 		delete trace_;
 		delete grid_;
 		delete box_;
+		delete background_renderer_;
+		background_renderer_ = 0;
 	}
 
 	PointCloudDrawable::releaseShaderPrograms();
@@ -364,7 +367,7 @@ bool intersectFrustumAABB(
 }
 
 //Should only be called in OpenGL thread!
-int Scene::Render() {
+int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat4 arProjectionMatrix, const rtabmap::Mesh & occlusionMesh) {
 	UASSERT(gesture_camera_ != 0);
 
 	if(currentPose_ == 0)
@@ -395,6 +398,17 @@ int Scene::Render() {
 	glm::mat4 projectionMatrix = gesture_camera_->GetProjectionMatrix();
 	glm::mat4 viewMatrix = gesture_camera_->GetViewMatrix();
 
+	bool renderBackgroundCamera =
+			background_renderer_ &&
+			gesture_camera_->GetCameraType() == tango_gl::GestureCamera::kFirstPerson &&
+			!rtabmap::glmToTransform(arProjectionMatrix).isNull() &&
+			uvsTransformed;
+	if(renderBackgroundCamera)
+	{
+		projectionMatrix = arProjectionMatrix;
+		viewMatrix = arViewMatrix;
+	}
+
 	rtabmap::Transform openglCamera = GetOpenGLCameraPose();//*rtabmap::Transform(0.0f, 0.0f, 3.0f, 0.0f, 0.0f, 0.0f);
 	// transform in same coordinate as frustum filtering
 	openglCamera *= rtabmap::Transform(
@@ -444,7 +458,7 @@ int Scene::Render() {
 
 	UTimer timer;
 
-	bool onlineBlending = blending_ && gesture_camera_->GetCameraType()!=tango_gl::GestureCamera::kTopOrtho && mapRendering_ && meshRendering_ && cloudsToDraw.size()>1;
+	bool onlineBlending = (renderBackgroundCamera && occlusionMesh.cloud.get() && occlusionMesh.cloud->size()) || (blending_ && gesture_camera_->GetCameraType()!=tango_gl::GestureCamera::kTopOrtho && mapRendering_ && meshRendering_ && cloudsToDraw.size()>1);
 	if(onlineBlending && fboId_)
 	{
 		// set the rendering destination to FBO
@@ -454,11 +468,19 @@ int Scene::Render() {
 		glClearColor(1, 1, 1, 1);
 		glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
 
-		// Draw scene
-		for(std::vector<PointCloudDrawable*>::const_iterator iter=cloudsToDraw.begin(); iter!=cloudsToDraw.end(); ++iter)
+		if(renderBackgroundCamera)
 		{
-			// set large distance to cam to use low res polygons for fast processing
-			(*iter)->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, false, false, 999.0f);
+			PointCloudDrawable drawable(occlusionMesh);
+			drawable.Render(projectionMatrix, viewMatrix, true, pointSize_, false, false, 999.0f);
+		}
+		else
+		{
+			// Draw scene
+			for(std::vector<PointCloudDrawable*>::const_iterator iter=cloudsToDraw.begin(); iter!=cloudsToDraw.end(); ++iter)
+			{
+				// set large distance to cam to use low res polygons for fast processing
+				(*iter)->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, false, false, 999.0f);
+			}
 		}
 
 		// back to normal window-system-provided framebuffer
@@ -495,6 +517,15 @@ int Scene::Render() {
 	glClearColor(r_, g_, b_, 1.0f);
 	glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
 
+	if(renderBackgroundCamera)
+	{
+		background_renderer_->Draw(uvsTransformed);
+
+		//To debug occlusion image:
+		//PointCloudDrawable drawable(occlusionMesh);
+		//drawable.Render(projectionMatrix, viewMatrix, true, pointSize_, false, false, 999.0f);
+	}
+
 	if(!currentPose_->isNull())
 	{
 		if (frustumVisible_ && gesture_camera_->GetCameraType() != tango_gl::GestureCamera::kFirstPerson)
@@ -523,7 +554,7 @@ int Scene::Render() {
 		}
 	}
 
-	if(gridVisible_)
+	if(gridVisible_ && !renderBackgroundCamera)
 	{
 		grid_->Render(projectionMatrix, viewMatrix);
 	}

app/android/jni/scene.h

@@ -38,6 +38,7 @@
 #include <point_cloud_drawable.h>
 #include <graph_drawable.h>
 #include <bounding_box_drawable.h>
+#include <background_renderer.h>
 
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
@@ -71,13 +72,14 @@ class Scene {
   //         frame's timestamp.
   // @param: point_cloud_vertices, point cloud's vertices of the current point
   //         frame.
-  int Render();
+  int Render(const float * uvsTransformed = 0, glm::mat4 arViewMatrix = glm::mat4(0), glm::mat4 arProjectionMatrix=glm::mat4(0), const rtabmap::Mesh & occlusionMesh=rtabmap::Mesh());
 
   // Set render camera's viewing angle, first person, third person or top down.
   //
   // @param: camera_type, camera type includes first person, third person and
   //         top down
   void SetCameraType(tango_gl::GestureCamera::CameraType camera_type);
+  tango_gl::GestureCamera::CameraType GetCameraType() const {return gesture_camera_->GetCameraType();}
 
   void SetCameraPose(const rtabmap::Transform & pose); // opengl camera
   rtabmap::Transform GetCameraPose() const {return currentPose_!=0?*currentPose_:rtabmap::Transform();}
@@ -152,6 +154,8 @@ class Scene {
   bool isLighting() const {return lighting_;}
   bool isBackfaceCulling() const {return backfaceCulling_;}
 
+  BackgroundRenderer * background_renderer_;
+
  private:
   // Camera object that allows user to use touch input to interact with.
   tango_gl::GestureCamera* gesture_camera_;

app/android/jni/tango-gl/gesture_camera.cpp

@@ -199,8 +199,8 @@ void GestureCamera::SetCameraType(CameraType camera_index) {
       SetRotation(glm::quat(1.0f, 0.0f, 0.0f, 0.0f));
       cam_cur_dist_ = kThirdPersonFollow?kThirdPersonFollowCameraDist:kThirdPersonCameraDist;
       anchor_offset_ = glm::vec3(0.0f,0.0f,0.0f);
-      cam_cur_angle_.x = -M_PI / 6.0f;
-      cam_cur_angle_.y = kThirdPersonFollow?0:M_PI / 4.0f;
+      cam_cur_angle_.x = -M_PI / 12.0f;
+      cam_cur_angle_.y = kThirdPersonFollow?0:M_PI / 2.0f;
       cam_cur_target_rot_ = glm::quat(1,0,0,0);
       StartCameraToCurrentTransform();
       break;

app/android/jni/util.h

@@ -252,7 +252,7 @@ inline ScreenRotation GetAndroidRotationFromColorCameraToDisplay(
 // @param display: integer value of display orientation, values available
 // are 0, 1, 2 ,3. Followed by Android display orientation standard:
 // https://developer.android.com/reference/android/view/Display.html#getRotation()
-// @param color_camera: integer value of color camera oreintation, values
+// @param color_camera: integer value of color camera orientation, values
 // available are 0, 90, 180, 270. Followed by Android camera orientation
 // standard:
 // https://developer.android.com/reference/android/hardware/Camera.CameraInfo.html#orientation

app/android/res/layout/activity_rtabmap.xml

@@ -163,6 +163,7 @@
         android:layout_height="100dp"
         android:layout_alignLeft="@+id/button_library"
         android:layout_below="@+id/button_library"
+        android:layout_marginTop="20dp"
         android:text="@string/new_scan" />
 
 </RelativeLayout>        

app/android/res/layout/activity_settings.xml

@@ -7,7 +7,7 @@
    			android:entries="@array/pref_camera_driver_keys"
    			android:entryValues="@array/pref_camera_driver_values"
    			android:defaultValue="@string/pref_default_camera_driver"/>
-    
+        
     <PreferenceCategory
         android:title="@string/pref_title_rendering">
         <ListPreference 
@@ -97,6 +97,11 @@
 	            android:title="@string/pref_title_resolution"
 	            android:summary="@string/pref_summary_resolution"
 	            android:defaultValue="@string/pref_default_resolution"/>
+		     <com.introlab.rtabmap.CustomSwitchPreference
+                android:key="@string/pref_key_depth_from_motion"
+                android:title="@string/pref_title_depth_from_motion"
+                android:summary="@string/pref_summary_depth_from_motion"
+                android:defaultValue="@string/pref_default_depth_from_motion"/>
 		    <com.introlab.rtabmap.CustomSwitchPreference
 		        android:key="@string/pref_key_smoothing"
 		        android:title="@string/pref_title_smoothing"

app/android/res/values/strings.xml

@@ -81,6 +81,8 @@
     
     <string name="pref_key_camera_driver">pref_key_camera_driver</string>
     <string name="pref_default_camera_driver">0</string>
+    <string name="pref_key_depth_from_motion">pref_key_depth_from_motion</string>
+    <string name="pref_default_depth_from_motion">false</string>
     <string name="pref_key_update_rate">pref_key_update_rate</string>
     <string name="pref_default_update_rate">1</string>
     <string name="pref_key_max_speed">pref_key_max_speed</string>
@@ -323,6 +325,8 @@
     <string name="pref_title_mapping_database">Database</string>
     <string name="pref_title_camera_driver">Camera Driver</string>
     <string name="pref_summary_camera_driver">AR sdk use for capturing 6DoF poses and images. A TOF camera is required to record a 3D model.</string>
+    <string name="pref_title_depth_from_motion">Depth From Motion</string>
+    <string name="pref_summary_depth_from_motion">Use ARCore\'s depth API to compute depth image from motion. If the phone has a TOF camera and is supported by ARCore, results should be better. Currently supported only with ARCore NDK driver.</string>
     <string name="pref_title_append">Append Mode</string>
     <string name="pref_summary_append">When resuming mapping, wait for a relocalization on the current map before starting a new map.</string>
     <string name="pref_title_resolution">HD Mode</string>

app/android/src/com/introlab/rtabmap/ARCoreSharedCamera.java

@@ -1,588 +0,0 @@
-package com.introlab.rtabmap;
-
-import java.nio.ByteBuffer;
-import java.nio.FloatBuffer;
-import java.util.ArrayList;
-import java.util.Arrays;
-import java.util.EnumSet;
-import java.util.List;
-import java.util.concurrent.atomic.AtomicBoolean;
-
-import com.google.ar.core.Camera;
-import com.google.ar.core.CameraIntrinsics;
-import com.google.ar.core.Config;
-import com.google.ar.core.Frame;
-import com.google.ar.core.ImageMetadata;
-import com.google.ar.core.PointCloud;
-import com.google.ar.core.Pose;
-import com.google.ar.core.Session;
-import com.google.ar.core.SharedCamera;
-import com.google.ar.core.TrackingState;
-import com.google.ar.core.exceptions.CameraNotAvailableException;
-import com.google.ar.core.exceptions.NotYetAvailableException;
-import com.google.ar.core.exceptions.UnavailableException;
-
-import android.content.Context;
-import android.graphics.ImageFormat;
-import android.hardware.camera2.CameraAccessException;
-import android.hardware.camera2.CameraCaptureSession;
-import android.hardware.camera2.CameraCharacteristics;
-import android.hardware.camera2.CameraDevice;
-import android.hardware.camera2.CameraManager;
-import android.hardware.camera2.CaptureFailure;
-import android.hardware.camera2.CaptureRequest;
-import android.hardware.camera2.TotalCaptureResult;
-import android.media.Image;
-import android.opengl.GLES20;
-import android.opengl.GLSurfaceView;
-import android.os.Handler;
-import android.os.HandlerThread;
-import android.support.annotation.NonNull;
-import android.util.Log;
-import android.view.Surface;
-
-public class ARCoreSharedCamera {
-
-	public static final String TAG = ARCoreSharedCamera.class.getSimpleName();
-
-
-	private static RTABMapActivity mActivity;
-	public ARCoreSharedCamera(RTABMapActivity c) {
-		mActivity = c;
-	}
-
-	// Depth TOF Image.
-	// Use 240 * 180 for now, hardcoded for Huawei P30 Pro
-	private static final int DEPTH_WIDTH = 240;
-	private static final int DEPTH_HEIGHT = 180;
-
-	// GL Surface used to draw camera preview image.
-	public GLSurfaceView surfaceView;
-
-	// ARCore session that supports camera sharing.
-	private Session sharedSession;
-
-	// Camera capture session. Used by both non-AR and AR modes.
-	private CameraCaptureSession captureSession;
-
-	// Reference to the camera system service.
-	private CameraManager cameraManager;
-
-	// Camera device. Used by both non-AR and AR modes.
-	private CameraDevice cameraDevice;
-
-	// Looper handler thread.
-	private HandlerThread backgroundThread;
-	// Looper handler.
-	private Handler backgroundHandler;
-
-	// ARCore shared camera instance, obtained from ARCore session that supports sharing.
-	private SharedCamera sharedCamera;
-
-	// Camera ID for the camera used by ARCore.
-	private String cameraId;
-	
-	private AtomicBoolean mReady = new AtomicBoolean(false);
-
-	// Camera preview capture request builder
-	private CaptureRequest.Builder previewCaptureRequestBuilder;
-
-	private int cameraTextureId = -1;
-
-	// Image reader that continuously processes CPU images.
-	public TOF_ImageReader mTOFImageReader = new TOF_ImageReader();
-	private boolean mTOFAvailable = false;
-	
-	public boolean isDepthSupported() {return mTOFAvailable;}
-
-	// Camera device state callback.
-	private final CameraDevice.StateCallback cameraDeviceCallback =
-			new CameraDevice.StateCallback() {
-		@Override
-		public void onOpened(@NonNull CameraDevice cameraDevice) {
-			Log.d(TAG, "Camera device ID " + cameraDevice.getId() + " opened.");
-			ARCoreSharedCamera.this.cameraDevice = cameraDevice;
-			createCameraPreviewSession();
-		}
-
-		@Override
-		public void onClosed(@NonNull CameraDevice cameraDevice) {
-			Log.d(TAG, "Camera device ID " + cameraDevice.getId() + " closed.");
-			ARCoreSharedCamera.this.cameraDevice = null;
-		}
-
-		@Override
-		public void onDisconnected(@NonNull CameraDevice cameraDevice) {
-			Log.w(TAG, "Camera device ID " + cameraDevice.getId() + " disconnected.");
-			cameraDevice.close();
-			ARCoreSharedCamera.this.cameraDevice = null;
-		}
-
-		@Override
-		public void onError(@NonNull CameraDevice cameraDevice, int error) {
-			Log.e(TAG, "Camera device ID " + cameraDevice.getId() + " error " + error);
-			cameraDevice.close();
-			ARCoreSharedCamera.this.cameraDevice = null;
-		}
-	};
-
-	// Repeating camera capture session state callback.
-	CameraCaptureSession.StateCallback cameraCaptureCallback =
-			new CameraCaptureSession.StateCallback() {
-
-		// Called when the camera capture session is first configured after the app
-		// is initialized, and again each time the activity is resumed.
-		@Override
-		public void onConfigured(@NonNull CameraCaptureSession session) {
-			Log.d(TAG, "Camera capture session configured.");
-			captureSession = session;
-			setRepeatingCaptureRequest();
-		}
-
-		@Override
-		public void onSurfacePrepared(
-				@NonNull CameraCaptureSession session, @NonNull Surface surface) {
-			Log.d(TAG, "Camera capture surface prepared.");
-		}
-
-		@Override
-		public void onReady(@NonNull CameraCaptureSession session) {
-			Log.d(TAG, "Camera capture session ready.");
-		}
-
-		@Override
-		public void onActive(@NonNull CameraCaptureSession session) {
-			Log.d(TAG, "Camera capture session active.");
-			resumeARCore();
-		}
-
-		@Override
-		public void onClosed(@NonNull CameraCaptureSession session) {
-			Log.d(TAG, "Camera capture session closed.");
-		}
-
-		@Override
-		public void onConfigureFailed(@NonNull CameraCaptureSession session) {
-			Log.e(TAG, "Failed to configure camera capture session.");
-		}
-	};
-
-	// Repeating camera capture session capture callback.
-	private final CameraCaptureSession.CaptureCallback captureSessionCallback =
-			new CameraCaptureSession.CaptureCallback() {
-
-		@Override
-		public void onCaptureCompleted(
-				@NonNull CameraCaptureSession session,
-				@NonNull CaptureRequest request,
-				@NonNull TotalCaptureResult result) {
-			Log.i(TAG, "onCaptureCompleted");
-		}
-
-		//@Override // android 23 
-		public void onCaptureBufferLost(
-				@NonNull CameraCaptureSession session,
-				@NonNull CaptureRequest request,
-				@NonNull Surface target,
-				long frameNumber) {
-			Log.e(TAG, "onCaptureBufferLost: " + frameNumber);
-		}
-
-		@Override
-		public void onCaptureFailed(
-				@NonNull CameraCaptureSession session,
-				@NonNull CaptureRequest request,
-				@NonNull CaptureFailure failure) {
-			Log.e(TAG, "onCaptureFailed: " + failure.getFrameNumber() + " " + failure.getReason());
-		}
-
-		@Override
-		public void onCaptureSequenceAborted(
-				@NonNull CameraCaptureSession session, int sequenceId) {
-			Log.e(TAG, "onCaptureSequenceAborted: " + sequenceId + " " + session);
-		}
-	};
-
-	private void resumeARCore() {
-		// Ensure that session is valid before triggering ARCore resume. Handles the case where the user
-		// manually uninstalls ARCore while the app is paused and then resumes.
-		if (sharedSession == null) {
-			return;
-		}
-
-		try {
-			Log.i(TAG, "Resume ARCore.");
-			// Resume ARCore.
-			sharedSession.resume();
-			// Set capture session callback while in AR mode.
-			sharedCamera.setCaptureCallback(captureSessionCallback, backgroundHandler);
-		} catch (CameraNotAvailableException e) {
-			Log.e(TAG, "Failed to resume ARCore session", e);
-			return;
-		}
-
-	}
-
-
-	// Called when starting non-AR mode or switching to non-AR mode.
-	// Also called when app starts in AR mode, or resumes in AR mode.
-	private void setRepeatingCaptureRequest() {
-		try {
-			captureSession.setRepeatingRequest(
-					previewCaptureRequestBuilder.build(), captureSessionCallback, backgroundHandler);
-		} catch (CameraAccessException e) {
-			Log.e(TAG, "Failed to set repeating request", e);
-		}
-	}
-
-
-	private void createCameraPreviewSession() {
-		Log.e(TAG, "createCameraPreviewSession: " + "starting camera preview session.");
-		try {
-			// Note that isGlAttached will be set to true in AR mode in onDrawFrame().
-			sharedSession.setCameraTextureName(cameraTextureId);
-
-			// Create an ARCore compatible capture request using `TEMPLATE_RECORD`.
-			previewCaptureRequestBuilder = cameraDevice.createCaptureRequest(CameraDevice.TEMPLATE_RECORD);
-
-			// Build surfaces list, starting with ARCore provided surfaces.
-			List<Surface> surfaceList = sharedCamera.getArCoreSurfaces();
-			Log.e(TAG, " createCameraPreviewSession: " + "surfaceList: sharedCamera.getArCoreSurfaces(): " + surfaceList.size());
-
-			// Add a CPU image reader surface. On devices that don't support CPU image access, the image
-			// may arrive significantly later, or not arrive at all.
-			if (mTOFAvailable) surfaceList.add(mTOFImageReader.imageReader.getSurface());
-			// Surface list should now contain three surfacemReadymReadys:
-			// 0. sharedCamera.getSurfaceTexture()
-			// 1. …
-			// 2. depthImageReader.getSurface()
-
-			// Add ARCore surfaces and CPU image surface targets.
-			for (Surface surface : surfaceList) {
-				previewCaptureRequestBuilder.addTarget(surface);
-			}
-
-			// Wrap our callback in a shared camera callback.
-			CameraCaptureSession.StateCallback wrappedCallback = sharedCamera.createARSessionStateCallback(cameraCaptureCallback, backgroundHandler);
-
-			// Create camera capture session for camera preview using ARCore wrapped callback.
-			cameraDevice.createCaptureSession(surfaceList, wrappedCallback, backgroundHandler);
-			
-			mReady.set(true);
-		} catch (CameraAccessException e) {
-			Log.e(TAG, "CameraAccessException", e);
-		}
-
-
-	}
-
-	// Start background handler thread, used to run callbacks without blocking UI thread.
-	private void startBackgroundThread() {
-		backgroundThread = new HandlerThread("sharedCameraBackground");
-		backgroundThread.start();
-		backgroundHandler = new Handler(backgroundThread.getLooper());
-		mTOFImageReader.startBackgroundThread();
-	}
-
-	// Stop background handler thread.
-	private void stopBackgroundThread() {
-		if (backgroundThread != null) {
-			backgroundThread.quitSafely();
-			try {
-				backgroundThread.join();
-				backgroundThread = null;
-				backgroundHandler = null;
-			} catch (InterruptedException e) {
-				Log.e(TAG, "Interrupted while trying to join background handler thread", e);
-			}
-		}
-		mTOFImageReader.stopBackgroundThread();
-	}
-	
-	private long mPreviousTime = 0;
-
-	// Perform various checks, then open camera device and create CPU image reader.
-	public boolean openCamera() {
-
-		close();
-		
-		startBackgroundThread();
-		
-		mPreviousTime = System.currentTimeMillis();
-
-		if(cameraTextureId == -1)
-		{
-			int[] textures = new int[1];
-			GLES20.glGenTextures(1, textures, 0);
-			cameraTextureId = textures[0];
-		}
-
-		Log.v(TAG + " opencamera: ", "Perform various checks, then open camera device and create CPU image reader.");
-		// Don't open camera if already opened.
-		if (cameraDevice != null) {
-			return false;
-		}
-
-		if (sharedSession == null) {
-			try {
-				// Create ARCore session that supports camera sharing.
-				sharedSession = new Session(mActivity, EnumSet.of(Session.Feature.SHARED_CAMERA));
-			} catch (UnavailableException e) {
-				Log.e(TAG, "Failed to create ARCore session that supports camera sharing", e);
-				return false;
-			}
-
-			// Enable auto focus mode while ARCore is running.
-			Config config = sharedSession.getConfig();
-			config.setFocusMode(Config.FocusMode.FIXED);
-			config.setUpdateMode(Config.UpdateMode.LATEST_CAMERA_IMAGE);
-			config.setPlaneFindingMode(Config.PlaneFindingMode.DISABLED);
-			config.setLightEstimationMode(Config.LightEstimationMode.DISABLED);
-			//config.setCloudAnchorMode(Config.CloudAnchorMode.ENABLED);
-			sharedSession.configure(config);
-
-		}
-
-		// Store the ARCore shared camera reference.
-		sharedCamera = sharedSession.getSharedCamera();
-		// Store the ID of the camera used by ARCore.
-		cameraId = sharedSession.getCameraConfig().getCameraId();
-		initCamera(mActivity, cameraId, 1);
-		ArrayList<String> resolutions;
-
-		mTOFAvailable = false;
-
-		resolutions = getResolutions(mActivity, cameraId, ImageFormat.DEPTH16);
-		if (resolutions != null) {
-			for( String temp : resolutions) {
-				Log.e(TAG + "DEPTH16 resolution: ", temp);
-			};
-			if (resolutions.size()>0) mTOFAvailable = true;
-		}
-
-		// Color CPU Image.
-		// Use the currently configured CPU image size.
-		//Size desiredCPUImageSize = sharedSession.getCameraConfig().getImageSize();
-
-		if (mTOFAvailable) mTOFImageReader.createImageReader(DEPTH_WIDTH, DEPTH_HEIGHT);
-
-		// When ARCore is running, make sure it also updates our CPU image surface.
-		if (mTOFAvailable) {
-			sharedCamera.setAppSurfaces(this.cameraId, Arrays.asList(mTOFImageReader.imageReader.getSurface()));
-		}
-
-		try {
-
-			// Wrap our callback in a shared camera callback.
-			CameraDevice.StateCallback wrappedCallback = sharedCamera.createARDeviceStateCallback(cameraDeviceCallback, backgroundHandler);
-
-			// Store a reference to the camera system service.
-			cameraManager = (CameraManager) mActivity.getSystemService(Context.CAMERA_SERVICE);
-
-			// Get the characteristics for the ARCore camera.
-			//CameraCharacteristics characteristics = cameraManager.getCameraCharacteristics(this.cameraId);
-
-			// Open the camera device using the ARCore wrapped callback.
-			cameraManager.openCamera(cameraId, wrappedCallback, backgroundHandler);
-			
-		} catch (CameraAccessException e) {
-			Log.e(TAG, "Failed to open camera", e);
-			return false;
-		} catch (IllegalArgumentException e) {
-			Log.e(TAG, "Failed to open camera", e);
-			return false;
-		} catch (SecurityException e) {
-			Log.e(TAG, "Failed to open camera", e);
-			return false;
-		} 
-
-		Log.i(TAG, " opencamera: TOF_available: " + mTOFAvailable);
-		return true;
-	}
-
-
-	// Close the camera device.
-	public void close() {
-		
-		if (sharedSession != null)  {
-			sharedSession.pause();
-		}
-
-		if (captureSession != null) {
-			captureSession.close();
-			captureSession = null;
-		}
-		if (cameraDevice != null) {
-			cameraDevice.close();
-		}
-
-		if (mTOFImageReader.imageReader != null) {
-			mTOFImageReader.imageReader.close();
-			mTOFImageReader.imageReader = null;
-		}
-
-		if(cameraTextureId>=0)
-		{
-			GLES20.glDeleteTextures(1, new int[] {cameraTextureId}, 0);
-		}
-		
-		stopBackgroundThread();
-	}
-
-	/*************************************************** ONDRAWFRAME ARCORE ************************************************************* */
-
-	// Draw frame when in AR mode. Called on the GL thread.
-	public void updateGL() throws CameraNotAvailableException {
-
-		if(!mReady.get())
-		{
-			return;
-		}
-		
-		if (mTOFAvailable && mTOFImageReader.frameCount == 0) return;
-		
-		// Perform ARCore per-frame update.
-		Frame frame = null;
-		try {
-			frame = sharedSession.update();
-		} catch (Exception e) {
-			e.printStackTrace();
-			return;
-		}
-
-		Camera camera = null;
-		if (frame != null) {
-			camera = frame.getCamera();
-		}else
-		{
-			return;
-		}
-
-		if (camera == null) return;
-		// If not tracking, don't draw 3D objects.
-		if (camera.getTrackingState() == TrackingState.PAUSED) return;
-
-		if (frame.getTimestamp() != 0) {
-			
-			Pose pose = camera.getPose();
-			if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("pose=%f %f %f q=%f %f %f %f", pose.tx(), pose.ty(), pose.tz(), pose.qx(), pose.qy(), pose.qz(), pose.qw()));
-			RTABMapLib.postCameraPoseEvent(RTABMapActivity.nativeApplication, pose.tx(), pose.ty(), pose.tz(), pose.qx(), pose.qy(), pose.qz(), pose.qw());
-			
-			int rateMs = 100; // send images at most 10 Hz
-			if(System. currentTimeMillis() - mPreviousTime < rateMs)
-			{
-				return;
-			}
-			mPreviousTime = System. currentTimeMillis();
-			
-			CameraIntrinsics intrinsics = camera.getImageIntrinsics();
-			try{
-				Image image = frame.acquireCameraImage();
-				PointCloud cloud = frame.acquirePointCloud();
-				FloatBuffer points = cloud.getPoints();
-
-				if (image.getFormat() != ImageFormat.YUV_420_888) {
-					throw new IllegalArgumentException(
-							"Expected image in YUV_420_888 format, got format " + image.getFormat());
-				}
-				
-				if(!RTABMapActivity.DISABLE_LOG)
-				{
-					for(int i =0;i<image.getPlanes().length;++i)
-					{
-						Log.d(TAG, String.format("Plane[%d] pixel stride = %d,  row stride = %d", i, image.getPlanes()[i].getPixelStride(), image.getPlanes()[i].getRowStride()));
-					}
-				}
-				
-				float[] fl = intrinsics.getFocalLength();
-				float[] pp = intrinsics.getPrincipalPoint();
-				if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("fx=%f fy=%f cx=%f cy=%f", fl[0], fl[1], pp[0], pp[1]));
-				
-				ByteBuffer y = image.getPlanes()[0].getBuffer().asReadOnlyBuffer();
-				ByteBuffer u = image.getPlanes()[1].getBuffer().asReadOnlyBuffer();
-				ByteBuffer v = image.getPlanes()[2].getBuffer().asReadOnlyBuffer();
-
-				double stamp = (double)image.getTimestamp()/10e8;
-				if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("RGB %dx%d len=%dbytes format=%d =%f",
-						image.getWidth(), image.getHeight(), y.limit(), image.getFormat(), stamp));
-
-				if(mTOFAvailable)
-				{
-					if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("Depth %dx%d len=%dbytes format=%d stamp=%f",
-							mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, mTOFImageReader.depth16_raw.limit(), ImageFormat.DEPTH16, (double)mTOFImageReader.timestamp/10e9));
-					
-					RTABMapLib.postOdometryEvent(
-							RTABMapActivity.nativeApplication,
-							pose.tx(), pose.ty(), pose.tz(), pose.qx(), pose.qy(), pose.qz(), pose.qw(), 
-							fl[0], fl[1], pp[0], pp[1], stamp, 
-							y, u, v, y.limit(), image.getWidth(), image.getHeight(), image.getFormat(), 
-							mTOFImageReader.depth16_raw, mTOFImageReader.depth16_raw.limit(), mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, ImageFormat.DEPTH16,
-							points, points.limit()/4);
-				}
-				else
-				{
-					ByteBuffer bb = ByteBuffer.allocate(0);
-					RTABMapLib.postOdometryEvent(
-							RTABMapActivity.nativeApplication,
-							pose.tx(), pose.ty(), pose.tz(), pose.qx(), pose.qy(), pose.qz(), pose.qw(), 
-							fl[0], fl[1], pp[0], pp[1], stamp, 
-							y, u, v, y.limit(), image.getWidth(), image.getHeight(), image.getFormat(), 
-							bb, 0, 0, 0, ImageFormat.DEPTH16,
-							points, points.limit()/4);
-				}
-				
-				image.close();
-				cloud.close();
-				
-			} catch (NotYetAvailableException e) {
-
-			}
-		}
-	}
-
-	/********************************************************************************************************************* */
-	/*************************************************** End ************************************************************* */
-	/********************************************************************************************************************* */
-
-
-	public ArrayList<String> getResolutions (Context context, String cameraId,int imageFormat){
-		Log.v(TAG + "getResolutions:", " cameraId:" + cameraId + " imageFormat: " + imageFormat);
-
-		ArrayList<String> output = new ArrayList<String>();
-		try {
-			CameraManager manager = (CameraManager) context.getSystemService(Context.CAMERA_SERVICE);
-			CameraCharacteristics characteristics = manager.getCameraCharacteristics(cameraId);
-
-			for (android.util.Size s : characteristics.get(CameraCharacteristics.SCALER_STREAM_CONFIGURATION_MAP).getOutputSizes(imageFormat)) {
-				output.add(s.getWidth() + "x" + s.getHeight());
-			}
-		} catch (Exception e) {
-			e.printStackTrace();
-		}
-		return output;
-	}
-
-
-	public void initCamera (Context context, String cameraId,int index){
-		boolean ok = false;
-		try {
-			int current = 0;
-			CameraManager manager = (CameraManager) context.getSystemService(Context.CAMERA_SERVICE);
-			CameraCharacteristics characteristics = manager.getCameraCharacteristics(cameraId);
-			for (android.util.Size s : characteristics.get(CameraCharacteristics.SCALER_STREAM_CONFIGURATION_MAP).getOutputSizes(ImageFormat.DEPTH16)) {
-				ok = true;
-				if (current == index)
-					break;
-				else ;
-				current++;
-			}
-		} catch (Exception e) {
-			e.printStackTrace();
-		}
-		if (!ok) {
-			Log.e(TAG + " initCamera", "Depth sensor not found!");
-		}
-	}
-
-
-}

app/android/src/com/introlab/rtabmap/RTABMapActivity.java

@@ -90,9 +90,9 @@ import android.widget.SeekBar.OnSeekBarChangeListener;
 import android.widget.Toast;
 import android.widget.ToggleButton;
 
-import com.google.ar.core.ArCoreApk;
+//import com.google.ar.core.ArCoreApk;
 import com.google.atap.tangoservice.Tango;
-import com.huawei.hiar.AREnginesApk;
+//import com.huawei.hiar.AREnginesApk;
 
 
 // The main activity of the application. This activity shows debug information
@@ -254,7 +254,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 
 	GestureDetector mGesDetect = null;
 
-	ARCoreSharedCamera mArCoreCamera = null;
+	//ARCoreSharedCamera mArCoreCamera = null;
 	int mCameraDriver = 0;
 
 	//Tango Service connection.
@@ -586,8 +586,8 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 		String cameraDriverStr = sharedPref.getString(getString(R.string.pref_key_camera_driver), getString(R.string.pref_default_camera_driver));
 		mCameraDriver = Integer.parseInt(cameraDriverStr);
 		
-		isArCoreAvailable();
-		isArEngineAvailable();
+		//isArCoreAvailable();
+		//isArEngineAvailable();
 	}
 	
 	// Should be called only if read/write permissions are granted!
@@ -613,7 +613,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 		Log.i(TAG, String.format("updateCameraDriverSettings() mCameraDriver=%d RTABMapLib.isBuiltWith(%d)=%d", mCameraDriver, mCameraDriver, RTABMapLib.isBuiltWith(nativeApplication, mCameraDriver)?1:0));
 		
 		SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(this);
-		
+		/*
 		if(mCameraDriver == 0 && (!CheckTangoCoreVersion(MIN_TANGO_CORE_VERSION) || !RTABMapLib.isBuiltWith(nativeApplication, 0)))
 		{
 			if(mIsAREngineAvailable && RTABMapLib.isBuiltWith(nativeApplication, 2))
@@ -659,9 +659,9 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 				editor.putString(getString(R.string.pref_key_camera_driver), "3");
 				editor.commit();
 			}
-		}
+		}*/
 	}
-
+/*
 	private void isArCoreAvailable() {
 		ArCoreApk.Availability availability = ArCoreApk.getInstance().checkAvailability(this);
 		if (availability.isTransient()) {
@@ -713,7 +713,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 		}
 	}
 
-
+*/
 	@Override
 	public void onDestroy() {
 		super.onDestroy();
@@ -992,6 +992,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 			RTABMapLib.setRawScanSaved(nativeApplication, sharedPref.getBoolean(getString(R.string.pref_key_raw_scan_saved), Boolean.parseBoolean(getString(R.string.pref_default_raw_scan_saved))));
 			RTABMapLib.setFullResolution(nativeApplication, sharedPref.getBoolean(getString(R.string.pref_key_resolution), Boolean.parseBoolean(getString(R.string.pref_default_resolution))));
 			RTABMapLib.setSmoothing(nativeApplication, sharedPref.getBoolean(getString(R.string.pref_key_smoothing), Boolean.parseBoolean(getString(R.string.pref_default_smoothing))));
+			RTABMapLib.setDepthFromMotion(nativeApplication, sharedPref.getBoolean(getString(R.string.pref_key_depth_from_motion), Boolean.parseBoolean(getString(R.string.pref_default_depth_from_motion))));
 			RTABMapLib.setCameraColor(nativeApplication, !sharedPref.getBoolean(getString(R.string.pref_key_fisheye), Boolean.parseBoolean(getString(R.string.pref_default_fisheye))));
 			RTABMapLib.setAppendMode(nativeApplication, sharedPref.getBoolean(getString(R.string.pref_key_append), Boolean.parseBoolean(getString(R.string.pref_default_append))));
 			RTABMapLib.setMappingParameter(nativeApplication, "Rtabmap/DetectionRate", mUpdateRate);
@@ -1162,6 +1163,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 
 		SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(this);
 		String cameraDriverStr = sharedPref.getString(getString(R.string.pref_key_camera_driver), getString(R.string.pref_default_camera_driver));
+		final boolean depthFromMotion = sharedPref.getBoolean(getString(R.string.pref_key_depth_from_motion), Boolean.parseBoolean(getString(R.string.pref_default_depth_from_motion)));
 		mCameraDriver = Integer.parseInt(cameraDriverStr);
 
 		if(!DISABLE_LOG) Log.i(TAG, String.format("startCamera() driver=%d", mCameraDriver));
@@ -1197,7 +1199,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 					mToast.makeText(this, "Current camera driver selected is Tango, but Tango service binding failed. Abort scanning...", mToast.LENGTH_LONG).show();
 				}
 			}
-		}
+		}/*
 		else if(mCameraDriver == 1 || mCameraDriver == 2 || mCameraDriver == 3)
 		{
 			if((mCameraDriver == 1 || mCameraDriver == 3) && !mIsARCoreAvailable)
@@ -1218,7 +1220,8 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 			}
 			Thread bindThread = new Thread(new Runnable() {
 				public void run() {
-					if(mCameraDriver==1)
+					
+					if(mCameraDriver==1 && !depthFromMotion)
 					{
 						RTABMapLib.setMeshRendering(
 								nativeApplication, 
@@ -1263,9 +1266,9 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 							}
 							else
 							{
-								if((mState==State.STATE_IDLE || mState==State.STATE_WELCOME) && mCameraDriver == 1)
+								if((mState==State.STATE_IDLE || mState==State.STATE_WELCOME) && mCameraDriver == 1 && !depthFromMotion)
 								{
-									mToast.makeText(getApplicationContext(), "Currently ARCore NDK driver doesn't support depth, only poses and RGB images can be recorded.", mToast.LENGTH_LONG).show();
+									mToast.makeText(getApplicationContext(), "Currently ARCore NDK driver doesn't support depth, only poses, RGB images and 3d features can be recorded.", mToast.LENGTH_LONG).show();
 								}
 								updateState(mState==State.STATE_VISUALIZING?State.STATE_VISUALIZING_CAMERA:State.STATE_CAMERA);	
 								if(mState==State.STATE_VISUALIZING_CAMERA && mItemLocalizationMode.isChecked())
@@ -1278,7 +1281,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 				}
 			});
 			bindThread.start();
-		}
+		}*/
 		else
 		{
 			mToast.makeText(this, "Supported camera driver not found! Cannot start a new scan.", mToast.LENGTH_LONG).show();
@@ -2271,14 +2274,14 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 			}
 		}
 
-		if(mArCoreCamera != null)
+/*		if(mArCoreCamera != null)
 		{
 			synchronized (this) {
 				mRenderer.setCamera(null);
 				mArCoreCamera.close();
 				mArCoreCamera = null;
 			}
-		}
+		}*/
 
 		Thread stopThread = new Thread(new Runnable() {
 			public void run() {
@@ -2311,6 +2314,15 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 
 		updateState(State.STATE_IDLE);
 
+		/*if(mArCoreCamera != null)
+		{
+			synchronized (this) {
+				mRenderer.setCamera(null);
+				mArCoreCamera.close();
+				mArCoreCamera = null;
+			}
+		}*/
+		
 		Thread stopThread = new Thread(new Runnable() {
 			public void run() {
 				if(!DISABLE_LOG) Log.i(TAG, String.format("setPausedMapping()"));

app/android/src/com/introlab/rtabmap/RTABMapLib.java

@@ -72,6 +72,7 @@ public class RTABMapLib
     public static native void setRawScanSaved(long nativeApplication, boolean enabled);
     public static native void setFullResolution(long nativeApplication, boolean enabled);
     public static native void setSmoothing(long nativeApplication, boolean enabled);
+    public static native void setDepthFromMotion(long nativeApplication, boolean enabled);
     public static native void setCameraColor(long nativeApplication, boolean enabled);
     public static native void setAppendMode(long nativeApplication, boolean enabled);
     public static native void setDataRecorderMode(long nativeApplication, boolean enabled);

app/android/src/com/introlab/rtabmap/Renderer.java

@@ -43,7 +43,7 @@ public class Renderer implements GLSurfaceView.Renderer {
 	private float mSurfaceHeight = 0.0f;
 	private float mTextColor = 1.0f;
 	private int mOffset = 0;
-	private ARCoreSharedCamera mCamera = null;
+	//private ARCoreSharedCamera mCamera = null;
 	
 	private Vector<TextObject> mTexts;
 
@@ -73,10 +73,10 @@ public class Renderer implements GLSurfaceView.Renderer {
 		mOffset = offset;
 	}
 	
-	public void setCamera(ARCoreSharedCamera camera)
-	{
-		mCamera = camera;
-	}
+	//public void setCamera(ARCoreSharedCamera camera)
+	//{
+	//	mCamera = camera;
+	//}
 
 	// Render loop of the Gl context.
 	public void onDrawFrame(GL10 useGLES20instead) {
@@ -86,10 +86,10 @@ public class Renderer implements GLSurfaceView.Renderer {
 			{
 				try
 				{
-					if(mCamera!=null)
-					{
-						mCamera.updateGL();
-					}
+				//	if(mCamera!=null)
+				//	{
+				//		mCamera.updateGL();
+				//	}
 					
 					final int value = RTABMapLib.render(mActivity.nativeApplication);
 		

app/android/src/com/introlab/rtabmap/SettingsActivity.java

@@ -361,7 +361,7 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
 		ed.commit(); //save it.
     }
     
-    @Override
+    
 	public void onRequestPermissionsResult(int requestCode, String[] permissions, int[] results) {
 
 		switch (requestCode) {

app/android/src/com/introlab/rtabmap/TOF_ImageReader.java

@@ -1,87 +0,0 @@
-package com.introlab.rtabmap;
-
-
-import android.graphics.ImageFormat;
-import android.media.Image;
-import android.media.ImageReader;
-import android.os.Handler;
-import android.os.HandlerThread;
-import android.util.Log;
-
-import java.nio.ByteBuffer;
-
-public class TOF_ImageReader implements ImageReader.OnImageAvailableListener {
-
-    public int WIDTH;
-    public int HEIGHT;
-    public ImageReader imageReader;
-    public int frameCount = 0;
-    public long timestamp;
-    
-    // Looper handler thread.
-    private HandlerThread backgroundThread;
-    // Looper handler.
-    private Handler backgroundHandler;
-
-    public ByteBuffer depth16_raw;
-
-    TOF_ImageReader(){
-    }
-
-    public void createImageReader(int width, int height){
-        this.WIDTH = width;
-        this.HEIGHT = height;
-        this.imageReader =
-                ImageReader.newInstance(
-                        width,
-                        height,
-                        ImageFormat.DEPTH16,
-                        2);
-        this.imageReader.setOnImageAvailableListener(this, this.backgroundHandler);
-    }
-
-    // CPU image reader callback.
-    @Override
-    public void onImageAvailable(ImageReader imageReader) {
-        Image image  = imageReader.acquireLatestImage();
-        if (image == null) {
-            Log.w("RTABMapActivity", "onImageAvailable: Skipping null image.");
-            return;
-        }
-        else{
-            if(image.getFormat() == ImageFormat.DEPTH16){
-                this.timestamp = image.getTimestamp();
-                depth16_raw = image.getPlanes()[0].getBuffer().asReadOnlyBuffer();
-                // copy raw undecoded DEPTH16 format depth data to NativeBuffer
-                frameCount++;
-            }
-            else{
-                Log.w("RTABMapActivity", "onImageAvailable: depth image not in DEPTH16 format, skipping image");
-            }
-        }
-        image.close();
-    }
-    
- // Start background handler thread, used to run callbacks without blocking UI thread.
-    public void startBackgroundThread() {
-        this.backgroundThread = new HandlerThread("DepthDecoderThread");
-        this.backgroundThread.start();
-        this.backgroundHandler = new Handler(backgroundThread.getLooper());
-    }
-
-    // Stop background handler thread.
-    public void stopBackgroundThread() {
-        if (this.backgroundThread != null) {
-            this.backgroundThread.quitSafely();
-            try {
-                this.backgroundThread.join();
-                this.backgroundThread = null;
-                this.backgroundHandler = null;
-            } catch (InterruptedException e) {
-                Log.e("RTABMapActivity", "Interrupted while trying to join depth background handler thread", e);
-            }
-        }
-    }
-
-}
-

app/src/CMakeLists.txt

@@ -137,6 +137,16 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
           DESTINATION ${openni2_dest_dir} 
           COMPONENT runtime)
   ENDIF(OpenNI2_FOUND)
+  
+  IF(k4a_FOUND)
+	# Install needed depthengine_2_0.dll
+    IF(WIN32)
+		file(TO_CMAKE_PATH "$ENV{K4A_ROOT_DIR}" ENV_K4A_ROOT_DIR)
+		INSTALL(FILES "${ENV_K4A_ROOT_DIR}/tools/depthengine_2_0.dll" 
+          DESTINATION ${plugin_dest_dir} 
+          COMPONENT runtime)
+	ENDIF(WIN32)
+  ENDIF(k4a_FOUND)
 
   # Install needed Qt plugins by copying directories from the qt installation
   # One can cull what gets copied by using 'REGEX "..." EXCLUDE'
@@ -156,26 +166,28 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
 		list(GET loc_list 1 plugin_type)
 		IF(NOT plugin_root)
 			get_filename_component(plugin_root ${plugin_dir} DIRECTORY)
-		ENDIF(NOT plugin_root)
-	    #MESSAGE(STATUS "Qt5 plugin \"${plugin_loc}\" installed in \"${plugin_dest_dir}/plugins${plugin_type}\"")
+		ENDIF(NOT plugin_root)
+		#MESSAGE(STATUS "Qt5 plugin \"${plugin_loc}\" installed in \"${plugin_dest_dir}/plugins${plugin_type}\"")
 		INSTALL(FILES ${plugin_loc}
           DESTINATION ${plugin_dest_dir}/plugins${plugin_type}
           COMPONENT runtime)
-	endforeach()
-	IF(WIN32)
-		IF(NOT Qt5Widgets_VERSION VERSION_LESS 5.10.0)
-			SET(plugin_loc "${plugin_root}/styles/qwindowsvistastyle.dll")
-			IF(EXISTS ${plugin_loc})
-				get_filename_component(plugin_dir ${plugin_loc} DIRECTORY)
-				string(REPLACE "plugins" ";" loc_list ${plugin_dir})
-				list(GET loc_list 1 plugin_type)
-				INSTALL(FILES ${plugin_loc}
-					DESTINATION ${plugin_dest_dir}/plugins${plugin_type}
-					COMPONENT runtime)
-				#MESSAGE(STATUS "Qt5 plugin \"${plugin_loc}\" installed in \"${plugin_dest_dir}/plugins${plugin_type}\"")
-			ENDIF(EXISTS ${plugin_loc})
-		ENDIF(NOT Qt5Widgets_VERSION VERSION_LESS 5.10.0)
-	ENDIF(WIN32)
+	endforeach()
+	IF(NOT Qt5Widgets_VERSION VERSION_LESS 5.10.0)
+		IF(WIN32)
+			SET(plugin_loc "${plugin_root}/styles/qwindowsvistastyle.dll")
+		ELSEIF(APPLE)
+			SET(plugin_loc "${plugin_root}/styles/libqmacstyle.dylib")
+		ENDIF()
+		IF(EXISTS ${plugin_loc})
+			get_filename_component(plugin_dir ${plugin_loc} DIRECTORY)
+			string(REPLACE "plugins" ";" loc_list ${plugin_dir})
+			list(GET loc_list 1 plugin_type)
+			INSTALL(FILES ${plugin_loc}
+				DESTINATION ${plugin_dest_dir}/plugins${plugin_type}
+				COMPONENT runtime)
+			#MESSAGE(STATUS "Qt5 plugin \"${plugin_loc}\" installed in \"${plugin_dest_dir}/plugins${plugin_type}\"")
+		ENDIF(EXISTS ${plugin_loc})
+	ENDIF(NOT Qt5Widgets_VERSION VERSION_LESS 5.10.0)
   ENDIF()
   
   # install a qt.conf file

corelib/include/rtabmap/core/Camera.h

@@ -53,6 +53,7 @@ public:
 	virtual ~Camera();
 	SensorData takeImage(CameraInfo * info = 0);
 
+	bool initFromFile(const std::string & calibrationPath);
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "") = 0;
 	virtual bool isCalibrated() const = 0;
 	virtual std::string getSerial() const = 0;
@@ -73,7 +74,7 @@ protected:
 	 *
 	 * @param imageRate : image/second , 0 for fast as the camera can
 	 */
-	Camera(float imageRate = 0, const Transform & localTransform = Transform::getIdentity());
+	Camera(float imageRate = 0, const Transform & localTransform = CameraModel::opticalRotation());
 
 	/**
 	 * returned rgb and depth images should be already rectified if calibration was loaded

corelib/include/rtabmap/core/CameraModel.h

@@ -37,6 +37,13 @@ namespace rtabmap {
 
 class RTABMAP_EXP CameraModel
 {
+public:
+	/**
+	 * Optical rotation used to transform image coordinate frame (x->right, y->down, z->forward)
+	 * to robot coordinate frame (x->forward, y->left, z->up).
+	 */
+	static Transform opticalRotation() {return Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0);}
+
 public:
 	CameraModel();
 	// K is the camera intrinsic 3x3 CV_64FC1
@@ -50,7 +57,7 @@ public:
 			const cv::Mat & D,
 			const cv::Mat & R,
 			const cv::Mat & P,
-			const Transform & localTransform = Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0));
+			const Transform & localTransform = opticalRotation());
 
 	// minimal
 	CameraModel(
@@ -58,7 +65,7 @@ public:
 			double fy,
 			double cx,
 			double cy,
-			const Transform & localTransform = Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0),
+			const Transform & localTransform = opticalRotation(),
 			double Tx = 0.0f,
 			const cv::Size & imageSize = cv::Size(0,0));
 	// minimal to be saved
@@ -68,7 +75,7 @@ public:
 			double fy,
 			double cx,
 			double cy,
-			const Transform & localTransform = Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0),
+			const Transform & localTransform = opticalRotation(),
 			double Tx = 0.0f,
 			const cv::Size & imageSize = cv::Size(0,0));
 
@@ -113,9 +120,12 @@ public:
 	int imageWidth() const {return imageSize_.width;}
 	int imageHeight() const {return imageSize_.height;}
 
-	double fovX() const {return imageSize_.width>0 && fx()>0?2.0*atan(imageSize_.width/(fx()*2.0)):0.0;}
-	double fovY() const {return imageSize_.height>0 && fy()>0?2.0*atan(imageSize_.height/(fy()*2.0)):0.0;}
+	double fovX() const; // in radians
+	double fovY() const; // in radians
+	double horizontalFOV() const; // in degrees
+	double verticalFOV() const;   // in degrees
 
+	bool load(const std::string & filePath);
 	bool load(const std::string & directory, const std::string & cameraName);
 	bool save(const std::string & directory) const;
 	std::vector<unsigned char> serialize() const;
@@ -125,9 +135,6 @@ public:
 	CameraModel scaled(double scale) const;
 	CameraModel roi(const cv::Rect & roi) const;
 
-	double horizontalFOV() const; // in degrees
-	double verticalFOV() const;   // in degrees
-
 	// For depth images, your should use cv::INTER_NEAREST
 	cv::Mat rectifyImage(const cv::Mat & raw, int interpolation = cv::INTER_LINEAR) const;
 	cv::Mat rectifyDepth(const cv::Mat & raw) const;

corelib/include/rtabmap/core/Features2d.h

@@ -62,7 +62,7 @@ namespace cv{
 namespace xfeatures2d {
 class FREAK;
 class BriefDescriptorExtractor;
-#if CV_MAJOR_VERSION < 4 || (CV_MAJOR_VERSION == 4 && (CV_MINOR_VERSION < 3 || (CV_MINOR_VERSION==3 && !defined(RTABMAP_OPENCV_DEV))))
+#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <= 3) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION < 4 || (CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<11)))
 class SIFT;
 #endif
 class SURF;
@@ -73,10 +73,10 @@ class ORB;
 class SURF_CUDA;
 }
 }
-#if CV_MAJOR_VERSION < 4 || (CV_MAJOR_VERSION == 4 && (CV_MINOR_VERSION < 3 || (CV_MINOR_VERSION==3 && !defined(RTABMAP_OPENCV_DEV))))
+#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <= 3) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION < 4 || (CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<11)))
 typedef cv::xfeatures2d::SIFT CV_SIFT;
 #else
-typedef cv::SIFT CV_SIFT; // SIFT is back in features2d since 4.3.0-dev
+typedef cv::SIFT CV_SIFT; // SIFT is back in features2d since 4.4.0 / 3.4.11
 #endif
 typedef cv::xfeatures2d::SURF CV_SURF;
 typedef cv::FastFeatureDetector CV_FAST;

corelib/include/rtabmap/core/Rtabmap.h

@@ -86,8 +86,32 @@ public:
 			const cv::Mat & image,
 			int id=0, const std::map<std::string, float> & externalStats = std::map<std::string, float>());
 
-	void init(const ParametersMap & parameters, const std::string & databasePath = "");
-	void init(const std::string & configFile = "", const std::string & databasePath = "");
+	/**
+	 * Initialize Rtabmap with parameters and a database
+	 * @param parameters Parameters overriding default parameters and database parameters
+	 *                   (@see loadDatabaseParameters)
+	 * @param databasePath The database input/output path. If not set, an
+	 *                     empty database is used in RAM. If set and the file doesn't exist,
+	 *                     it will be created empty. If the database exists, nodes and
+	 *                     vocabulary will be loaded in working memory.
+	 * @param loadDatabaseParameters If an existing database is used (@see databasePath),
+	 *                               the parameters inside are loaded and set to current
+	 *                               Rtabmap instance.
+	 */
+	void init(const ParametersMap & parameters, const std::string & databasePath = "", bool loadDatabaseParameters = false);
+	/**
+	 * Initialize Rtabmap with parameters from a configuration file and a database
+	 * @param configFile Configuration file (*.ini) overriding default parameters and database parameters
+	 *                   (@see loadDatabaseParameters)
+	 * @param databasePath The database input/output path. If not set, an
+	 *                     empty database is used in RAM. If set and the file doesn't exist,
+	 *                     it will be created empty. If the database exists, nodes and
+	 *                     vocabulary will be loaded in working memory.
+	 * @param loadDatabaseParameters If an existing database is used (@see databasePath),
+	 *                               the parameters inside are loaded and set to current
+	 *                               Rtabmap instance.
+	 */
+	void init(const std::string & configFile = "", const std::string & databasePath = "", bool loadDatabaseParameters = false);
 
 	/**
 	 * Close rtabmap. This will delete rtabmap object if set.
@@ -156,7 +180,7 @@ public:
 	void rejectLastLoopClosure();
 	void deleteLastLocation();
 	void setOptimizedPoses(const std::map<int, Transform> & poses);
-	Signature getSignatureCopy(int id, bool images, bool scan, bool userData, bool occupancyGrid) const;
+	Signature getSignatureCopy(int id, bool images, bool scan, bool userData, bool occupancyGrid, bool withWords, bool withGlobalDescriptors) const;
 	RTABMAP_DEPRECATED(
 		void get3DMap(std::map<int, Signature> & signatures,
 				std::map<int, Transform> & poses,
@@ -171,7 +195,9 @@ public:
 			bool withImages = false,
 			bool withScan = false,
 			bool withUserData = false,
-			bool withGrid = false) const;
+			bool withGrid = false,
+			bool withWords = true,
+			bool withGlobalDescriptors = true) const;
 	int detectMoreLoopClosures(
 			float clusterRadius = 0.5f,
 			float clusterAngle = M_PI/6.0f,
@@ -315,6 +341,7 @@ private:
 	std::map<int, Transform> _odomCachePoses;       // used in localization mode to reject loop closures
 	std::multimap<int, Link> _odomCacheConstraints; // used in localization mode to reject loop closures
 	std::map<int, Transform> _odomCacheAddLink; // used in localization mode when adding external link
+	std::vector<float> _odomCorrectionAcc;
 
 	// Planning stuff
 	int _pathStatus;

corelib/include/rtabmap/core/Statistics.h

@@ -52,6 +52,7 @@ namespace rtabmap {
 
 class RTABMAP_EXP Statistics
 {
+	RTABMAP_STATS(Loop, Id,); // Combined loop or proximity detection
 	RTABMAP_STATS(Loop, RejectedHypothesis,);
 	RTABMAP_STATS(Loop, Accepted_hypothesis_id,);
 	RTABMAP_STATS(Loop, Suppressed_hypothesis_id,);
@@ -61,6 +62,7 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Loop, Reactivate_id,);
 	RTABMAP_STATS(Loop, Hypothesis_ratio,);
 	RTABMAP_STATS(Loop, Hypothesis_reactivated,);
+	RTABMAP_STATS(Loop, Map_id,);
 	RTABMAP_STATS(Loop, Visual_words,);
 	RTABMAP_STATS(Loop, Visual_inliers,);
 	RTABMAP_STATS(Loop, Visual_matches,);
@@ -84,6 +86,15 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Loop, Odom_correction_roll, deg);
 	RTABMAP_STATS(Loop, Odom_correction_pitch, deg);
 	RTABMAP_STATS(Loop, Odom_correction_yaw, deg);
+	//Odom correction
+	RTABMAP_STATS(Loop, Odom_correction_acc_norm, m);
+	RTABMAP_STATS(Loop, Odom_correction_acc_angle, deg);
+	RTABMAP_STATS(Loop, Odom_correction_acc_x, m);
+	RTABMAP_STATS(Loop, Odom_correction_acc_y, m);
+	RTABMAP_STATS(Loop, Odom_correction_acc_z, m);
+	RTABMAP_STATS(Loop, Odom_correction_acc_roll, deg);
+	RTABMAP_STATS(Loop, Odom_correction_acc_pitch, deg);
+	RTABMAP_STATS(Loop, Odom_correction_acc_yaw, deg);
 	// Map to Odom
 	RTABMAP_STATS(Loop, MapToOdom_norm, m);
 	RTABMAP_STATS(Loop, MapToOdom_angle, deg);

corelib/include/rtabmap/core/Transform.h

@@ -140,6 +140,15 @@ public:
 	static Transform fromEigen3f(const Eigen::Isometry3f & matrix);
 	static Transform fromEigen3d(const Eigen::Isometry3d & matrix);
 
+	static Transform opengl_T_rtabmap() {return Transform(
+			 0.0f, -1.0f, 0.0f, 0.0f,
+			 0.0f,  0.0f, 1.0f, 0.0f,
+			-1.0f,  0.0f, 0.0f, 0.0f);}
+	static Transform rtabmap_T_opengl() {return Transform(
+			 0.0f,  0.0f,-1.0f, 0.0f,
+			-1.0f,  0.0f, 0.0f, 0.0f,
+			 0.0f,  1.0f, 0.0f, 0.0f);}
+
 	/**
 	 * Format (3 values): x y z
 	 * Format (6 values): x y z roll pitch yaw

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

@@ -53,7 +53,7 @@ public:
 	CameraFreenect(int deviceId= 0,
 					Type type = kTypeColorDepth,
 					float imageRate=0.0f,
-					const Transform & localTransform = Transform::getIdentity());
+					const Transform & localTransform = CameraModel::opticalRotation());
 	virtual ~CameraFreenect();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");

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

@@ -65,7 +65,7 @@ public:
 	CameraFreenect2(int deviceId= 0,
 					Type type = kTypeDepth2ColorSD,
 					float imageRate=0.0f,
-					const Transform & localTransform = Transform::getIdentity(),
+					const Transform & localTransform = CameraModel::opticalRotation(),
 					float minDepth = 0.3f,
 					float maxDepth = 12.0f,
 					bool bilateralFiltering = true,

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

@@ -46,7 +46,7 @@ public:
 	CameraImages(
 			const std::string & path,
 			float imageRate = 0,
-			const Transform & localTransform = Transform::getIdentity());
+			const Transform & localTransform = CameraModel::opticalRotation());
 	virtual ~CameraImages();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");

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

@@ -50,10 +50,10 @@ public:
 public:
 	CameraK4A(int deviceId = 0,
 		float imageRate = 0.0f,
-		const Transform & localTransform = Transform::getIdentity());
+		const Transform & localTransform = CameraModel::opticalRotation());
 	CameraK4A(const std::string & fileName,
 		float imageRate = 0.0f,
-		const Transform & localTransform = Transform::getIdentity());
+		const Transform & localTransform = CameraModel::opticalRotation());
 	virtual ~CameraK4A();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
@@ -61,6 +61,7 @@ public:
 	virtual std::string getSerial() const;
 
 	void setIRDepthFormat(bool enabled);
+	void setPreferences(int rgb_resolution, int framerate, int depth_resolution);
 
 protected:
 	virtual SensorData captureImage(CameraInfo * info = 0);
@@ -83,9 +84,13 @@ private:
 	CameraModel model_;
 	int deviceId_;
 	std::string fileName_;
+	int rgb_resolution_;
+	int framerate_;
+	int depth_resolution_;
 	bool ir_;
 	double previousStamp_;
 	UTimer timer_;
+	Transform imuLocalTransform_;
 #endif
 
 };

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

@@ -66,7 +66,7 @@ public:
 	CameraK4W2(int deviceId = 0, // not used
 		Type type = kTypeDepth2ColorSD,
 		float imageRate = 0.0f,
-		const Transform & localTransform = Transform::getIdentity());
+		const Transform & localTransform = CameraModel::opticalRotation());
 	virtual ~CameraK4W2();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");

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

@@ -52,7 +52,7 @@ public:
 	static bool available();
 
 public:
-	CameraMyntEye(const std::string & device = "", bool apiRectification = false, bool apiDepth = false, float imageRate = 0, const Transform & localTransform = Transform::getIdentity());
+	CameraMyntEye(const std::string & device = "", bool apiRectification = false, bool apiDepth = false, float imageRate = 0, const Transform & localTransform = CameraModel::opticalRotation());
 	virtual ~CameraMyntEye();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");

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

@@ -54,7 +54,7 @@ public:
 	CameraOpenNI2(const std::string & deviceId = "",
 					Type type = kTypeColorDepth,
 					float imageRate = 0,
-					const Transform & localTransform = Transform::getIdentity());
+					const Transform & localTransform = CameraModel::opticalRotation());
 	virtual ~CameraOpenNI2();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");

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

@@ -45,7 +45,7 @@ public:
 public:
 	CameraOpenNICV(bool asus = false,
 					float imageRate = 0,
-					const Transform & localTransform = Transform::getIdentity());
+					const Transform & localTransform = CameraModel::opticalRotation());
 	virtual ~CameraOpenNICV();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");

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

@@ -66,7 +66,7 @@ public:
 	// default local transform z in, x right, y down));
 	CameraOpenni(const std::string & deviceId="",
 			float imageRate = 0,
-			const Transform & localTransform = Transform::getIdentity());
+			const Transform & localTransform = CameraModel::opticalRotation());
 	virtual ~CameraOpenni();
 #ifdef RTABMAP_OPENNI
 #if PCL_VERSION_COMPARE(>=, 1, 10, 0)

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

@@ -44,7 +44,7 @@ public:
 			const std::string & pathDepthImages,
 			float depthScaleFactor = 1.0f,
 			float imageRate=0.0f,
-			const Transform & localTransform = Transform::getIdentity());
+			const Transform & localTransform = CameraModel::opticalRotation());
 	virtual ~CameraRGBDImages();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");

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

@@ -64,7 +64,7 @@ public:
 		int presetDepth = 0, // 0=best quality, 1=largest image, 2=highest framerate
 		bool computeOdometry = false,
 		float imageRate = 0,
-		const Transform & localTransform = Transform::getIdentity());
+		const Transform & localTransform = CameraModel::opticalRotation());
 	virtual ~CameraRealSense();
 
 	void setDepthScaledToRGBSize(bool enabled);

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

@@ -62,7 +62,7 @@ public:
 	CameraRealSense2(
 		const std::string & deviceId = "",
 		float imageRate = 0,
-		const Transform & localTransform = Transform::getIdentity());
+		const Transform & localTransform = CameraModel::opticalRotation());
 	virtual ~CameraRealSense2();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
@@ -134,6 +134,7 @@ private:
 	bool dualMode_;
 	Transform dualExtrinsics_;
 	std::string jsonConfig_;
+	bool closing_;
 
 	static Transform realsense2PoseRotation_;
 	static Transform realsense2PoseRotationInv_;

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

@@ -45,7 +45,7 @@ public:
 	static bool available();
 
 public:
-	CameraStereoDC1394( float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity());
+	CameraStereoDC1394( float imageRate=0.0f, const Transform & localTransform = CameraModel::opticalRotation());
 	virtual ~CameraStereoDC1394();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");

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

@@ -47,7 +47,7 @@ public:
 	static bool available();
 
 public:
-	CameraStereoFlyCapture2( float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity());
+	CameraStereoFlyCapture2( float imageRate=0.0f, const Transform & localTransform = CameraModel::opticalRotation());
 	virtual ~CameraStereoFlyCapture2();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");

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

@@ -49,12 +49,12 @@ public:
 			const std::string & pathRightImages,
 			bool rectifyImages = false,
 			float imageRate=0.0f,
-			const Transform & localTransform = Transform::getIdentity());
+			const Transform & localTransform = CameraModel::opticalRotation());
 	CameraStereoImages(
 			const std::string & pathLeftRightImages,
 			bool rectifyImages = false,
 			float imageRate=0.0f,
-			const Transform & localTransform = Transform::getIdentity());
+			const Transform & localTransform = CameraModel::opticalRotation());
 	virtual ~CameraStereoImages();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");

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

@@ -53,7 +53,7 @@ public:
 			int device,
 			bool rectifyImages = false,
 			float imageRate = 0.0f,
-			const Transform & localTransform = Transform::getIdentity());
+			const Transform & localTransform = CameraModel::opticalRotation());
 
 	virtual ~CameraStereoTara();
 

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

@@ -46,24 +46,24 @@ public:
 			const std::string & pathSideBySide,
 			bool rectifyImages = false,
 			float imageRate=0.0f,
-			const Transform & localTransform = Transform::getIdentity());
+			const Transform & localTransform = CameraModel::opticalRotation());
 	CameraStereoVideo(
 			const std::string & pathLeft,
 			const std::string & pathRight,
 			bool rectifyImages = false,
 			float imageRate=0.0f,
-			const Transform & localTransform = Transform::getIdentity());
+			const Transform & localTransform = CameraModel::opticalRotation());
 	CameraStereoVideo(
 			int device,
 			bool rectifyImages = false,
 			float imageRate = 0.0f,
-			const Transform & localTransform = Transform::getIdentity());
+			const Transform & localTransform = CameraModel::opticalRotation());
 	CameraStereoVideo(
 			int deviceLeft,
 			int deviceRight,
 			bool rectifyImages = false,
 			float imageRate = 0.0f,
-			const Transform & localTransform = Transform::getIdentity());
+			const Transform & localTransform = CameraModel::opticalRotation());
 	virtual ~CameraStereoVideo();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");

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

@@ -57,7 +57,7 @@ public:
 			int confidenceThr = 100,
 			bool computeOdometry = false,
 			float imageRate=0.0f,
-			const Transform & localTransform = Transform::getIdentity(),
+			const Transform & localTransform = CameraModel::opticalRotation(),
 			bool selfCalibration = true,
 			bool odomForce3DoF = false,
 			int texturenessConfidenceThr = 90); // introduced with ZED SDK 3
@@ -68,7 +68,7 @@ public:
 			int confidenceThr = 100,
 			bool computeOdometry = false,
 			float imageRate=0.0f,
-			const Transform & localTransform = Transform::getIdentity(),
+			const Transform & localTransform = CameraModel::opticalRotation(),
 			bool selfCalibration = true,
 			bool odomForce3DoF = false,
 			int texturenessConfidenceThr = 90); // introduced with ZED SDK 3

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

@@ -45,11 +45,11 @@ public:
 	CameraVideo(int usbDevice = 0,
 			bool rectifyImages = false,
 			float imageRate = 0,
-			const Transform & localTransform = Transform::getIdentity());
+			const Transform & localTransform = CameraModel::opticalRotation());
 	CameraVideo(const std::string & filePath,
 			bool rectifyImages = false,
 			float imageRate = 0,
-			const Transform & localTransform = Transform::getIdentity());
+			const Transform & localTransform = CameraModel::opticalRotation());
 	virtual ~CameraVideo();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");

corelib/include/rtabmap/core/impl/util3d_mapping.hpp

@@ -97,11 +97,28 @@ void segmentObstaclesFromGround(
 			// cluster all surfaces for which the centroid is in the Z-range of the bigger surface
 			if(clusteredFlatSurfaces.size())
 			{
-				ground = clusteredFlatSurfaces.at(biggestFlatSurfaceIndex);
 				Eigen::Vector4f min,max;
-				pcl::getMinMax3D(*cloud, *clusteredFlatSurfaces.at(biggestFlatSurfaceIndex), min, max);
+				if(maxGroundHeight != 0.0f)
+				{
+					// Search for biggest surface under max ground height
+					size_t points = 0;
+					for(size_t i=0;i<clusteredFlatSurfaces.size();++i)
+					{
+						pcl::getMinMax3D(*cloud, *clusteredFlatSurfaces.at(i), min, max);
+						if(min[2]<maxGroundHeight && clusteredFlatSurfaces.size() > points)
+						{
+							points = clusteredFlatSurfaces.at(i)->size();
+							biggestFlatSurfaceIndex = i;
+						}
+					}
+				}
+				else
+				{
+					pcl::getMinMax3D(*cloud, *clusteredFlatSurfaces.at(biggestFlatSurfaceIndex), min, max);
+				}
+				ground = clusteredFlatSurfaces.at(biggestFlatSurfaceIndex);
 
-				if(maxGroundHeight == 0.0f || min[2] < maxGroundHeight)
+				if(!ground->empty() && (maxGroundHeight == 0.0f || min[2] < maxGroundHeight))
 				{
 					for(unsigned int i=0; i<clusteredFlatSurfaces.size(); ++i)
 					{

corelib/include/rtabmap/core/stereo/stereoRectifyFisheye.h

@@ -1189,8 +1189,8 @@ void stereoRectifyFisheye( cv::InputArray _cameraMatrix1, cv::InputArray _distCo
     cv::Mat cameraMatrix1 = _cameraMatrix1.getMat(), cameraMatrix2 = _cameraMatrix2.getMat();
     cv::Mat distCoeffs1 = _distCoeffs1.getMat(), distCoeffs2 = _distCoeffs2.getMat();
     cv::Mat Rmat = _Rmat.getMat(), Tmat = _Tmat.getMat();
-
-#if CV_MAJOR_VERSION > 3 or (CV_MAJOR_VERSION >= 3 and (CV_MINOR_VERSION>4 or CV_MINOR_VERSION>=4  and CV_SUBMINOR_VERSION>=4))
+	
+#if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION >= 3 && (CV_MINOR_VERSION>4 || (CV_MINOR_VERSION>=4 && CV_SUBMINOR_VERSION>=4)))
     CvMat c_cameraMatrix1 = cvMat(cameraMatrix1);
     CvMat c_cameraMatrix2 = cvMat(cameraMatrix2);
     CvMat c_distCoeffs1 = cvMat(distCoeffs1);
@@ -1210,7 +1210,7 @@ void stereoRectifyFisheye( cv::InputArray _cameraMatrix1, cv::InputArray _distCo
     _Pmat1.create(3, 4, rtype);
     _Pmat2.create(3, 4, rtype);
     cv::Mat R1 = _Rmat1.getMat(), R2 = _Rmat2.getMat(), P1 = _Pmat1.getMat(), P2 = _Pmat2.getMat(), Q;
-#if CV_MAJOR_VERSION > 3 or (CV_MAJOR_VERSION >= 3 and (CV_MINOR_VERSION>4 or CV_MINOR_VERSION>=4  and CV_SUBMINOR_VERSION>=4))
+#if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION >= 3 && (CV_MINOR_VERSION>4 || (CV_MINOR_VERSION>=4 && CV_SUBMINOR_VERSION>=4)))
     CvMat c_R1 = cvMat(R1), c_R2 = cvMat(R2), c_P1 = cvMat(P1), c_P2 = cvMat(P2);
 #else
     CvMat c_R1 = CvMat(R1), c_R2 = CvMat(R2), c_P1 = CvMat(P1), c_P2 = CvMat(P2);
@@ -1220,7 +1220,7 @@ void stereoRectifyFisheye( cv::InputArray _cameraMatrix1, cv::InputArray _distCo
     if( _Qmat.needed() )
     {
         _Qmat.create(4, 4, rtype);
-#if CV_MAJOR_VERSION > 3 or (CV_MAJOR_VERSION >= 3 and (CV_MINOR_VERSION>4 or CV_MINOR_VERSION>=4  and CV_SUBMINOR_VERSION>=4))
+#if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION >= 3 && (CV_MINOR_VERSION>4 || (CV_MINOR_VERSION>=4 && CV_SUBMINOR_VERSION>=4)))
         p_Q = &(c_Q = cvMat(Q = _Qmat.getMat()));
 #else
         p_Q = &(c_Q = CvMat(Q = _Qmat.getMat()));
@@ -1230,7 +1230,7 @@ void stereoRectifyFisheye( cv::InputArray _cameraMatrix1, cv::InputArray _distCo
     CvMat *p_distCoeffs1 = distCoeffs1.empty() ? NULL : &c_distCoeffs1;
     CvMat *p_distCoeffs2 = distCoeffs2.empty() ? NULL : &c_distCoeffs2;
     cvStereoRectifyFisheye( &c_cameraMatrix1, &c_cameraMatrix2, p_distCoeffs1, p_distCoeffs2,
-#if CV_MAJOR_VERSION > 3 or (CV_MAJOR_VERSION >= 3 and (CV_MINOR_VERSION>4 or CV_MINOR_VERSION>=4  and CV_SUBMINOR_VERSION>=4))
+#if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION >= 3 && (CV_MINOR_VERSION>4 || (CV_MINOR_VERSION>=4 && CV_SUBMINOR_VERSION>=4)))
         cvSize(imageSize), &c_R, &c_T, &c_R1, &c_R2, &c_P1, &c_P2, p_Q, flags, alpha,
         cvSize(newImageSize));
 #else

corelib/src/CMakeLists.txt

@@ -293,6 +293,11 @@ IF(realsense2_FOUND)
 			${LIBRARIES}
 			${RealSense2_LIBRARIES}
 		)
+	ELSEIF(APPLE)
+	    SET(LIBRARIES
+			${LIBRARIES}
+			${realsense2_LIBRARIES}
+		)
 	ELSE()
 		SET(LIBRARIES
 			${LIBRARIES}

corelib/src/Camera.cpp

@@ -64,6 +64,11 @@ void Camera::resetTimer()
 	_frameRateTimer->start();
 }
 
+bool Camera::initFromFile(const std::string & calibrationPath)
+{
+	return init(UDirectory::getDir(calibrationPath), uSplit(UFile::getName(calibrationPath), '.').front());
+}
+
 SensorData Camera::takeImage(CameraInfo * info)
 {
 	bool warnFrameRateTooHigh = false;

corelib/src/CameraModel.cpp

@@ -211,7 +211,7 @@ void CameraModel::setImageSize(const cv::Size & size)
 	}
 }
 
-bool CameraModel::load(const std::string & directory, const std::string & cameraName)
+bool CameraModel::load(const std::string & filePath)
 {
 	K_ = cv::Mat();
 	D_ = cv::Mat();
@@ -222,7 +222,6 @@ bool CameraModel::load(const std::string & directory, const std::string & camera
 	name_.clear();
 	imageSize_ = cv::Size();
 
-	std::string filePath = directory+"/"+cameraName+".yaml";
 	if(UFile::exists(filePath))
 	{
 		try
@@ -361,6 +360,11 @@ bool CameraModel::load(const std::string & directory, const std::string & camera
 	return false;
 }
 
+bool CameraModel::load(const std::string & directory, const std::string & cameraName)
+{
+	return load(directory+"/"+cameraName+".yaml");
+}
+
 bool CameraModel::save(const std::string & directory) const
 {
 	std::string filePath = directory+"/"+name_+".yaml";
@@ -636,22 +640,23 @@ CameraModel CameraModel::roi(const cv::Rect & roi) const
 	return roiModel;
 }
 
+double CameraModel::fovX() const
+{
+	return imageSize_.width>0 && fx()>0?2.0*atan(imageSize_.width/(fx()*2.0)):0.0;
+}
+double CameraModel::fovY() const
+{
+	return imageSize_.height>0 && fy()>0?2.0*atan(imageSize_.height/(fy()*2.0)):0.0;
+}
+
 double CameraModel::horizontalFOV() const
 {
-	if(imageWidth() > 0 && fx() > 0.0)
-	{
-		return atan((double(imageWidth())/2.0)/fx())*2.0*180.0/CV_PI;
-	}
-	return 0.0;
+	return fovX()*180.0/CV_PI;
 }
 
 double CameraModel::verticalFOV() const
 {
-	if(imageHeight() > 0 && fy() > 0.0)
-	{
-		return atan((double(imageHeight())/2.0)/fy())*2.0*180.0/CV_PI;
-	}
-	return 0.0;
+	return fovY()*180.0/CV_PI;
 }
 
 cv::Mat CameraModel::rectifyImage(const cv::Mat & raw, int interpolation) const

corelib/src/CameraThread.cpp

@@ -74,7 +74,6 @@ CameraThread::CameraThread(Camera * camera, const ParametersMap & parameters) :
 
 CameraThread::~CameraThread()
 {
-	UDEBUG("");
 	join(true);
 	delete _camera;
 	delete _distortionModel;
@@ -139,7 +138,6 @@ void CameraThread::mainLoopBegin()
 void CameraThread::mainLoop()
 {
 	UTimer totalTime;
-	UDEBUG("");
 	CameraInfo info;
 	SensorData data = _camera->takeImage(&info);
 
@@ -161,7 +159,6 @@ void CameraThread::mainLoop()
 
 void CameraThread::mainLoopKill()
 {
-	UDEBUG("");
 	if(dynamic_cast<CameraFreenect2*>(_camera) != 0)
 	{
 		int i=20;

corelib/src/DBDriver.cpp

@@ -690,6 +690,22 @@ void DBDriver::getNodeData(
 			 (!occupancyGrid || s->sensorData().gridCellSize() != 0.0f))))
 		{
 			data = (SensorData)s->sensorData();
+			if(!images)
+			{
+				data.setRGBDImage(cv::Mat(), cv::Mat(), std::vector<CameraModel>());
+			}
+			if(!scan)
+			{
+				data.setLaserScan(LaserScan());
+			}
+			if(!userData)
+			{
+				data.setUserData(cv::Mat());
+			}
+			if(!occupancyGrid)
+			{
+				data.setOccupancyGrid(cv::Mat(), cv::Mat(), cv::Mat(), 0, cv::Point3f());
+			}
 			found = true;
 		}
 	}

corelib/src/Features2d.cpp

@@ -509,7 +509,7 @@ Feature2D * Feature2D::create(const ParametersMap & parameters)
 Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parameters)
 {
 
-#if CV_MAJOR_VERSION < 4 || (CV_MAJOR_VERSION == 4 && (CV_MINOR_VERSION < 3 || (CV_MINOR_VERSION==3 && !defined(RTABMAP_OPENCV_DEV))))
+#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <= 3) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION < 4 || (CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<11)))
 #ifndef RTABMAP_NONFREE
 	if(type == Feature2D::kFeatureSurf || type == Feature2D::kFeatureSift)
 	{
@@ -532,7 +532,7 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame
 #endif
 #endif
 
-#else // >= 4.3.0-dev
+#else // >= 4.4.0 >= 3.4.11
 
 #ifndef RTABMAP_NONFREE
 	if(type == Feature2D::kFeatureSurf)
@@ -542,7 +542,7 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame
 	}
 #endif
 
-#endif // 4.3.0-dev
+#endif // >= 4.4.0 >= 3.4.11
 
 #if CV_MAJOR_VERSION < 3
 	if(type == Feature2D::kFeatureKaze)
@@ -963,7 +963,7 @@ void SIFT::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kSIFTSigma(), sigma_);
 	Parameters::parse(parameters, Parameters::kSIFTRootSIFT(), rootSIFT_);
 
-#if CV_MAJOR_VERSION < 4 || (CV_MAJOR_VERSION == 4 && (CV_MINOR_VERSION < 3 || (CV_MINOR_VERSION==3 && !defined(RTABMAP_OPENCV_DEV))))
+#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <= 3) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION < 4 || (CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<11)))
 #ifdef RTABMAP_NONFREE
 #if CV_MAJOR_VERSION < 3
 	_sift = cv::Ptr<CV_SIFT>(new CV_SIFT(this->getMaxFeatures(), nOctaveLayers_, contrastThreshold_, edgeThreshold_, sigma_));

corelib/src/Memory.cpp

@@ -3086,6 +3086,8 @@ Transform Memory::computeIcpTransformMulti(
 		pcl::PointCloud<pcl::PointNormal>::Ptr assembledToNormalClouds(new pcl::PointCloud<pcl::PointNormal>);
 		pcl::PointCloud<pcl::PointXYZI>::Ptr assembledToIClouds(new pcl::PointCloud<pcl::PointXYZI>);
 		pcl::PointCloud<pcl::PointXYZINormal>::Ptr assembledToNormalIClouds(new pcl::PointCloud<pcl::PointXYZINormal>);
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledToRGBClouds(new pcl::PointCloud<pcl::PointXYZRGB>);
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr assembledToNormalRGBClouds(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 		UDEBUG("maxPoints from(%d) = %d", fromId, maxPoints);
 		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
 		{
@@ -3111,6 +3113,19 @@ Transform Memory::computeIcpTransformMulti(
 										toPoseInv * iter->second * scan.localTransform());
 							}
 						}
+						else if(scan.hasRGB())
+						{
+							if(scan.hasNormals())
+							{
+								*assembledToNormalRGBClouds += *util3d::laserScanToPointCloudRGBNormal(scan,
+										toPoseInv * iter->second * scan.localTransform());
+							}
+							else
+							{
+								*assembledToRGBClouds += *util3d::laserScanToPointCloudRGB(scan,
+										toPoseInv * iter->second * scan.localTransform());
+							}
+						}
 						else
 						{
 							if(scan.hasNormals())
@@ -3160,6 +3175,28 @@ Transform Memory::computeIcpTransformMulti(
 		{
 			assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToIClouds):util3d::laserScanFromPointCloud(*assembledToIClouds);
 		}
+		else if(assembledToNormalRGBClouds->size())
+		{
+			if(fromScan.is2d())
+			{
+				UERROR("Cannot handle 2d scan with RGB format.");
+			}
+			else
+			{
+				assembledScan = util3d::laserScanFromPointCloud(*assembledToNormalRGBClouds);
+			}
+		}
+		else if(assembledToRGBClouds->size())
+		{
+			if(fromScan.is2d())
+			{
+				UERROR("Cannot handle 2d scan with RGB format.");
+			}
+			else
+			{
+				assembledScan = util3d::laserScanFromPointCloud(*assembledToRGBClouds);
+			}
+		}
 		UDEBUG("assembledScan=%d points", assembledScan.cols);
 
 		// scans are in base frame but for 2d scans, set the height so that correspondences matching works
@@ -3847,6 +3884,22 @@ SensorData Memory::getNodeData(int locationId, bool images, bool scan, bool user
 			 (!occupancyGrid || s->sensorData().gridCellSize() != 0.0f))))
 	{
 		r = s->sensorData();
+		if(!images)
+		{
+			r.setRGBDImage(cv::Mat(), cv::Mat(), std::vector<CameraModel>());
+		}
+		if(!scan)
+		{
+			r.setLaserScan(LaserScan());
+		}
+		if(!userData)
+		{
+			r.setUserData(cv::Mat());
+		}
+		if(!occupancyGrid)
+		{
+			r.setOccupancyGrid(cv::Mat(), cv::Mat(), cv::Mat(), 0, cv::Point3f());
+		}
 	}
 	else if(_dbDriver)
 	{
@@ -4244,29 +4297,48 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 				}
 			}
 
-			int oldMaxFeatures = _feature2D->getMaxFeatures();
-			UDEBUG("rawDescriptorsKept=%d, pose=%d, maxFeatures=%d, visMaxFeatures=%d", _rawDescriptorsKept?1:0, pose.isNull()?0:1, _feature2D->getMaxFeatures(), _visMaxFeatures);
-			ParametersMap tmpMaxFeatureParameter;
-			if(_rawDescriptorsKept&&!pose.isNull()&&_feature2D->getMaxFeatures()>0&&_feature2D->getMaxFeatures()<_visMaxFeatures)
+			bool useProvided3dPoints = false;
+			if(_useOdometryFeatures && !data.keypoints().empty())
 			{
-				// The total extracted features should match the number of features used for transformation estimation
-				UDEBUG("Changing temporary max features from %d to %d", _feature2D->getMaxFeatures(), _visMaxFeatures);
-				tmpMaxFeatureParameter.insert(ParametersPair(Parameters::kKpMaxFeatures(), uNumber2Str(_visMaxFeatures)));
-				_feature2D->parseParameters(tmpMaxFeatureParameter);
+				UDEBUG("Using provided keypoints (%d)", (int)data.keypoints().size());
+				keypoints = data.keypoints();
+
+				// In case we provided corresponding 3D features
+				if(keypoints.size() == data.keypoints3D().size())
+				{
+					for(size_t i=0; i<keypoints.size(); ++i)
+					{
+						keypoints[i].class_id = i;
+					}
+					useProvided3dPoints = true;
+				}
 			}
-
-			keypoints = _feature2D->generateKeypoints(
-					imageMono,
-					depthMask);
-
-			if(tmpMaxFeatureParameter.size())
+			else
 			{
-				tmpMaxFeatureParameter.at(Parameters::kKpMaxFeatures()) = uNumber2Str(oldMaxFeatures);
-				_feature2D->parseParameters(tmpMaxFeatureParameter); // reset back
+				int oldMaxFeatures = _feature2D->getMaxFeatures();
+				UDEBUG("rawDescriptorsKept=%d, pose=%d, maxFeatures=%d, visMaxFeatures=%d", _rawDescriptorsKept?1:0, pose.isNull()?0:1, _feature2D->getMaxFeatures(), _visMaxFeatures);
+				ParametersMap tmpMaxFeatureParameter;
+				if(_rawDescriptorsKept&&!pose.isNull()&&_feature2D->getMaxFeatures()>0&&_feature2D->getMaxFeatures()<_visMaxFeatures)
+				{
+					// The total extracted features should match the number of features used for transformation estimation
+					UDEBUG("Changing temporary max features from %d to %d", _feature2D->getMaxFeatures(), _visMaxFeatures);
+					tmpMaxFeatureParameter.insert(ParametersPair(Parameters::kKpMaxFeatures(), uNumber2Str(_visMaxFeatures)));
+					_feature2D->parseParameters(tmpMaxFeatureParameter);
+				}
+
+				keypoints = _feature2D->generateKeypoints(
+						imageMono,
+						depthMask);
+
+				if(tmpMaxFeatureParameter.size())
+				{
+					tmpMaxFeatureParameter.at(Parameters::kKpMaxFeatures()) = uNumber2Str(oldMaxFeatures);
+					_feature2D->parseParameters(tmpMaxFeatureParameter); // reset back
+				}
+				t = timer.ticks();
+				if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f);
+				UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), t);
 			}
-			t = timer.ticks();
-			if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f);
-			UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), t);
 
 			descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
 			t = timer.ticks();
@@ -4398,7 +4470,22 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 					UDEBUG("time rectification = %fs", t);
 				}
 
-				if((!decimatedData.depthRaw().empty() && decimatedData.cameraModels().size() && decimatedData.cameraModels()[0].isValidForProjection()) ||
+				if(useProvided3dPoints && keypoints.size() != data.keypoints3D().size())
+				{
+					UDEBUG("Using provided 3d points (%d->%d)", (int)data.keypoints3D().size(), (int)keypoints.size());
+					keypoints3D.resize(keypoints.size());
+					for(size_t i=0; i<keypoints.size(); ++i)
+					{
+						UASSERT(keypoints[i].class_id < (int)data.keypoints3D().size());
+						keypoints3D[i] = data.keypoints3D()[keypoints[i].class_id];
+					}
+				}
+				else if(keypoints.size() == data.keypoints3D().size())
+				{
+					UDEBUG("Using provided 3d points (%d)", (int)data.keypoints3D().size());
+					keypoints3D = data.keypoints3D();
+				}
+				else if((!decimatedData.depthRaw().empty() && decimatedData.cameraModels().size() && decimatedData.cameraModels()[0].isValidForProjection()) ||
 				   (!decimatedData.rightRaw().empty() && decimatedData.stereoCameraModel().isValidForProjection()))
 				{
 					keypoints3D = _feature2D->generateKeypoints3D(decimatedData, keypoints);

corelib/src/Odometry.cpp

@@ -592,50 +592,58 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 					updateKalmanFilter(vx,vy,vz,vroll,vpitch,vyaw);
 				}
 			}
-			else if(particleFilters_.size())
+			else
 			{
-				// Particle filtering
-				UASSERT(particleFilters_.size()==6);
-				if(velocityGuess_.isNull())
+				if(particleFilters_.size())
 				{
-					particleFilters_[0]->init(vx);
-					particleFilters_[1]->init(vy);
-					particleFilters_[2]->init(vz);
-					particleFilters_[3]->init(vroll);
-					particleFilters_[4]->init(vpitch);
-					particleFilters_[5]->init(vyaw);
-				}
-				else
-				{
-					vx = particleFilters_[0]->filter(vx);
-					vy = particleFilters_[1]->filter(vy);
-					vyaw = particleFilters_[5]->filter(vyaw);
-
-					if(!_holonomic)
+					// Particle filtering
+					UASSERT(particleFilters_.size()==6);
+					if(velocityGuess_.isNull())
 					{
-						// arc trajectory around ICR
-						float tmpY = vyaw!=0.0f ? vx / tan((CV_PI-vyaw)/2.0f) : 0.0f;
-						if(fabs(tmpY) < fabs(vy) || (tmpY<=0 && vy >=0) || (tmpY>=0 && vy<=0))
+						particleFilters_[0]->init(vx);
+						particleFilters_[1]->init(vy);
+						particleFilters_[2]->init(vz);
+						particleFilters_[3]->init(vroll);
+						particleFilters_[4]->init(vpitch);
+						particleFilters_[5]->init(vyaw);
+					}
+					else
+					{
+						vx = particleFilters_[0]->filter(vx);
+						vy = particleFilters_[1]->filter(vy);
+						vyaw = particleFilters_[5]->filter(vyaw);
+
+						if(!_holonomic)
 						{
-							vy = tmpY;
+							// arc trajectory around ICR
+							float tmpY = vyaw!=0.0f ? vx / tan((CV_PI-vyaw)/2.0f) : 0.0f;
+							if(fabs(tmpY) < fabs(vy) || (tmpY<=0 && vy >=0) || (tmpY>=0 && vy<=0))
+							{
+								vy = tmpY;
+							}
+							else
+							{
+								vyaw = (atan(vx/vy)*2.0f-CV_PI)*-1;
+							}
 						}
-						else
+
+						if(!_force3DoF)
 						{
-							vyaw = (atan(vx/vy)*2.0f-CV_PI)*-1;
+							vz = particleFilters_[2]->filter(vz);
+							vroll = particleFilters_[3]->filter(vroll);
+							vpitch = particleFilters_[4]->filter(vpitch);
 						}
 					}
 
-					if(!_force3DoF)
+					if(info)
 					{
-						vz = particleFilters_[2]->filter(vz);
-						vroll = particleFilters_[3]->filter(vroll);
-						vpitch = particleFilters_[4]->filter(vpitch);
+						info->timeParticleFiltering = time.ticks();
 					}
 				}
-
-				if(info)
+				else if(!_holonomic)
 				{
-					info->timeParticleFiltering = time.ticks();
+					// arc trajectory around ICR
+					vy = vyaw!=0.0f ? vx / tan((CV_PI-vyaw)/2.0f) : 0.0f;
 				}
 
 				if(_force3DoF)
@@ -645,17 +653,6 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 					vpitch = 0.0f;
 				}
 			}
-			else if(!_holonomic)
-			{
-				// arc trajectory around ICR
-				vy = vyaw!=0.0f ? vx / tan((CV_PI-vyaw)/2.0f) : 0.0f;
-				if(_force3DoF)
-				{
-					vz = 0.0f;
-					vroll = 0.0f;
-					vpitch = 0.0f;
-				}
-			}
 
 			if(dt)
 			{

corelib/src/Rtabmap.cpp

@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/core/EpipolarGeometry.h"
 
+#include "rtabmap/core/DBDriver.h"
 #include "rtabmap/core/Memory.h"
 #include "rtabmap/core/VWDictionary.h"
 #include "rtabmap/core/BayesFilter.h"
@@ -144,6 +145,7 @@ Rtabmap::Rtabmap() :
 	_mapCorrection(Transform::getIdentity()),
 	_lastLocalizationNodeId(0),
 	_currentSessionHasGPS(false),
+	_odomCorrectionAcc(6,0),
 	_pathStatus(0),
 	_pathCurrentIndex(0),
 	_pathGoalIndex(0),
@@ -286,15 +288,9 @@ void Rtabmap::flushStatisticLogs()
 	}
 }
 
-void Rtabmap::init(const ParametersMap & parameters, const std::string & databasePath)
+void Rtabmap::init(const ParametersMap & parameters, const std::string & databasePath, bool loadDatabaseParameters)
 {
 	UDEBUG("path=%s", databasePath.c_str());
-	ParametersMap::const_iterator iter;
-	if((iter=parameters.find(Parameters::kRtabmapWorkingDirectory())) != parameters.end())
-	{
-		this->setWorkingDirectory(iter->second.c_str());
-	}
-
 	_databasePath = databasePath;
 	if(!_databasePath.empty())
 	{
@@ -308,15 +304,35 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
 
 	bool newDatabase = _databasePath.empty() || !UFile::exists(_databasePath);
 
-	// If not exist, create a memory
+	ParametersMap allParameters;
+	if(!newDatabase && loadDatabaseParameters)
+	{
+		DBDriver * driver = DBDriver::create();
+		if(driver->openConnection(_databasePath, false))
+		{
+			allParameters = driver->getLastParameters();
+			// ignore working directory (we may be on a different computer)
+			allParameters.erase(Parameters::kRtabmapWorkingDirectory());
+		}
+		delete driver;
+	}
+
+	uInsert(allParameters, parameters);
+	ParametersMap::const_iterator iter;
+	if((iter=allParameters.find(Parameters::kRtabmapWorkingDirectory())) != allParameters.end())
+	{
+		this->setWorkingDirectory(iter->second.c_str());
+	}
+
+	// If doesn't exist, create a memory
 	if(!_memory)
 	{
-		_memory = new Memory(parameters);
-		_memory->init(_databasePath, false, parameters, true);
+		_memory = new Memory(allParameters);
+		_memory->init(_databasePath, false, allParameters, true);
 	}
 
 	// Parse all parameters
-	this->parseParameters(parameters);
+	this->parseParameters(allParameters);
 
 	Transform lastPose;
 	_optimizedPoses = _memory->loadOptimizedPoses(&lastPose);
@@ -347,7 +363,7 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
 	setupLogFiles(newDatabase);
 }
 
-void Rtabmap::init(const std::string & configFile, const std::string & databasePath)
+void Rtabmap::init(const std::string & configFile, const std::string & databasePath, bool loadDatabaseParameters)
 {
 	// fill ctrl struct with values from the configuration file
 	ParametersMap param;// = Parameters::defaultParameters;
@@ -358,7 +374,7 @@ void Rtabmap::init(const std::string & configFile, const std::string & databaseP
 		Parameters::readINI(configFile, param);
 	}
 
-	this->init(param, databasePath);
+	this->init(param, databasePath, loadDatabaseParameters);
 }
 
 void Rtabmap::close(bool databaseSaved, const std::string & ouputDatabasePath)
@@ -375,6 +391,7 @@ void Rtabmap::close(bool databaseSaved, const std::string & ouputDatabasePath)
 	_lastLocalizationNodeId = 0;
 	_odomCachePoses.clear();
 	_odomCacheConstraints.clear();
+	_odomCorrectionAcc = std::vector<float>(6,0);
 	_distanceTravelled = 0.0f;
 	_optimizeFromGraphEndChanged = false;
 	this->clearPath(0);
@@ -707,6 +724,7 @@ void Rtabmap::setInitialPose(const Transform & initialPose)
 			_lastLocalizationNodeId = 0;
 			_odomCachePoses.clear();
 			_odomCacheConstraints.clear();
+			_odomCorrectionAcc = std::vector<float>(6,0);
 			_mapCorrection.setIdentity();
 			_mapCorrectionBackup.setNull();
 
@@ -729,11 +747,18 @@ int Rtabmap::triggerNewMap()
 	int mapId = -1;
 	if(_memory)
 	{
+		_lastLocalizationNodeId = 0;
+		_odomCachePoses.clear();
+		_odomCacheConstraints.clear();
+		_odomCorrectionAcc = std::vector<float>(6,0);
+
 		if(!_memory->isIncremental())
 		{
-			UWARN("Memory is not incremental (%s=false), ignoring creating a new map as we "
-				  "should be already processing new nodes in a new session.",
-				  Parameters::kMemIncrementalMemory().c_str());
+			if(_savedLocalizationIgnored)
+			{
+				_mapCorrection.setIdentity();
+				_lastLocalizationPose.setIdentity();
+			}
 			return mapId;
 		}
 		std::map<int, int> reducedIds;
@@ -741,9 +766,6 @@ int Rtabmap::triggerNewMap()
 		UINFO("New map triggered, new map = %d", mapId);
 		_optimizedPoses.clear();
 		_constraints.clear();
-		_lastLocalizationNodeId = 0;
-		_odomCachePoses.clear();
-		_odomCacheConstraints.clear();
 
 		if(_bayesFilter)
 		{
@@ -893,6 +915,7 @@ void Rtabmap::resetMemory()
 	_lastLocalizationNodeId = 0;
 	_odomCachePoses.clear();
 	_odomCacheConstraints.clear();
+	_odomCorrectionAcc = std::vector<float>(6,0);
 	_distanceTravelled = 0.0f;
 	_optimizeFromGraphEndChanged = false;
 	this->clearPath(0);
@@ -1053,36 +1076,34 @@ bool Rtabmap::process(
 	bool fakeOdom = false;
 	if(_rgbdSlamMode)
 	{
-		if(!_memory->isIncremental() && !odomPose.isNull())
+		if(!_memory->isIncremental() &&
+			!odomPose.isNull() &&
+			_optimizedPoses.size() &&
+			_mapCorrection.isIdentity() &&
+			!_lastLocalizationPose.isNull() &&
+			!_lastLocalizationPose.isIdentity() &&
+			_lastLocalizationNodeId == 0)
 		{
-			if(!_mapCorrectionBackup.isNull())
+			// Localization mode
+			if(!_optimizeFromGraphEnd)
 			{
-				_mapCorrection = _mapCorrectionBackup;
-				_mapCorrectionBackup.setNull();
+				//set map->odom so that odom is moved back to last saved localization
+				_mapCorrection = _lastLocalizationPose * odomPose.inverse();
+				std::map<int, Transform> nodesOnly(_optimizedPoses.lower_bound(1), _optimizedPoses.end());
+				_lastLocalizationNodeId = graph::findNearestNode(nodesOnly, _lastLocalizationPose);
+				UWARN("Update map correction based on last localization saved in database! correction = %s, nearest id = %d of last pose = %s, odom = %s",
+						_mapCorrection.prettyPrint().c_str(),
+						_lastLocalizationNodeId,
+						_lastLocalizationPose.prettyPrint().c_str(),
+						odomPose.prettyPrint().c_str());
 			}
-			else if(_optimizedPoses.size() && _mapCorrection.isIdentity() && !_lastLocalizationPose.isNull() && _lastLocalizationNodeId == 0)
+			else
 			{
-				// Localization mode
-				if(!_optimizeFromGraphEnd)
+				//move optimized poses accordingly to last saved localization
+				Transform mapCorrectionInv = odomPose * _lastLocalizationPose.inverse();
+				for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
 				{
-					//set map->odom so that odom is moved back to last saved localization
-					_mapCorrection = _lastLocalizationPose * odomPose.inverse();
-					std::map<int, Transform> nodesOnly(_optimizedPoses.lower_bound(1), _optimizedPoses.end());
-					_lastLocalizationNodeId = graph::findNearestNode(nodesOnly, _lastLocalizationPose);
-					UWARN("Update map correction based on last localization saved in database! correction = %s, nearest id = %d of last pose = %s, odom = %s",
-							_mapCorrection.prettyPrint().c_str(),
-							_lastLocalizationNodeId,
-							_lastLocalizationPose.prettyPrint().c_str(),
-							odomPose.prettyPrint().c_str());
-				}
-				else
-				{
-					//move optimized poses accordingly to last saved localization
-					Transform mapCorrectionInv = odomPose * _lastLocalizationPose.inverse();
-					for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
-					{
-						iter->second = mapCorrectionInv * iter->second;
-					}
+					iter->second = mapCorrectionInv * iter->second;
 				}
 			}
 		}
@@ -1097,12 +1118,20 @@ bool Rtabmap::process(
 			}
 			else // fake localization
 			{
+				if(!_mapCorrectionBackup.isNull())
+				{
+					_mapCorrection = _mapCorrectionBackup;
+					_mapCorrectionBackup.setNull();
+				}
 				if(_lastLocalizationPose.isNull())
 				{
 					_lastLocalizationPose = Transform::getIdentity();
 				}
 				fakeOdom = true;
 				odomPose = _mapCorrection.inverse() * _lastLocalizationPose;
+				UDEBUG("Map correction = %s", _mapCorrection.prettyPrint().c_str());
+				UDEBUG("Last localization pose: %s", _lastLocalizationPose.prettyPrint().c_str());
+				UDEBUG("Fake odom: %s", odomPose.prettyPrint().c_str());
 			}
 		}
 		else if(_memory->isIncremental()) // only in mapping mode
@@ -1351,6 +1380,7 @@ bool Rtabmap::process(
 				_constraints.insert(std::make_pair(iter->first, iter->second.inverse()));
 			}
 		}
+		// only in mapping mode we add a neighbor link
 		if(signature->getLinks().size() &&
 		   signature->getLinks().begin()->second.type() == Link::kNeighbor)
 		{
@@ -1381,6 +1411,11 @@ bool Rtabmap::process(
 		_lastLocalizationPose = newPose; // keep in cache the latest corrected pose
 		if(!_memory->isIncremental())
 		{
+			if(!_odomCacheAddLink.empty())
+			{
+				float odomDistance = (_odomCacheAddLink.rbegin()->second.inverse() * signature->getPose()).getNorm();
+				_distanceTravelled += odomDistance;
+			}
 			_odomCacheAddLink.insert(std::make_pair(signature->id(), signature->getPose()));
 			while(!_odomCacheAddLink.empty() && (int)_odomCacheAddLink.size() > _maxOdomCacheSize+1)
 			{
@@ -1415,7 +1450,6 @@ bool Rtabmap::process(
 										 odomCovariance.inv())));
 					_odomCachePoses.insert(std::make_pair(signature->id(), signature->getPose())); // keep odometry poses
 				}
-
 			}
 		}
 
@@ -2815,10 +2849,12 @@ bool Rtabmap::process(
 				else
 				{
 					Transform newPose = _optimizedPoses.at(localizationLinks.begin()->first) * localizationLinks.begin()->second.transform().inverse();
+					UDEBUG("newPose=%s", newPose.prettyPrint().c_str());
 					if(_graphOptimizer->isSlam2d())
 					{
 						// in case of 3d landmarks, transform constraint to 2D
 						newPose = newPose.to3DoF();
+						UDEBUG("newPose 2D=%s", newPose.prettyPrint().c_str());
 					}
 					else if(_graphOptimizer->gravitySigma() > 0)
 					{
@@ -2851,6 +2887,7 @@ bool Rtabmap::process(
 							transform *= error;
 
 							newPose  = _optimizedPoses.at(loopId) * transform.inverse();
+							UDEBUG("newPose gravity=%s", newPose.prettyPrint().c_str());
 						}
 						else
 						{
@@ -3122,6 +3159,9 @@ bool Rtabmap::process(
 			statistics_.addStatistic(Statistics::kProximitySpace_last_detection_id(), lastProximitySpaceClosureId);
 			statistics_.setProximityDetectionId(lastProximitySpaceClosureId);
 			statistics_.setProximityDetectionMapId(_memory->getMapId(lastProximitySpaceClosureId));
+
+			statistics_.addStatistic(Statistics::kLoopId(), _loopClosureHypothesis.first>0?_loopClosureHypothesis.first:lastProximitySpaceClosureId);
+
 			float x,y,z,roll,pitch,yaw;
 			if(_loopClosureHypothesis.first || lastProximitySpaceClosureId)
 			{
@@ -3132,6 +3172,7 @@ bool Rtabmap::process(
 				UINFO("Set loop closure transform = %s", loopIter->second.transform().prettyPrint().c_str());
 				statistics_.setLoopClosureTransform(loopIter->second.transform());
 
+				statistics_.addStatistic(Statistics::kLoopMap_id(), sLoop->mapId());
 				statistics_.addStatistic(Statistics::kLoopVisual_words(), sLoop->getWords().size());
 
 				// if ground truth exists, compute localization error
@@ -3153,8 +3194,8 @@ bool Rtabmap::process(
 				statistics_.addStatistic(Statistics::kLoopMapToOdom_pitch(),  pitch*180.0f/M_PI);
 				statistics_.addStatistic(Statistics::kLoopMapToOdom_yaw(), yaw*180.0f/M_PI);
 
-				// Odom correction (actual odometry pose change)
-				if(!odomPose.isNull() && !previousMapCorrection.isNull())
+				// Odom correction (actual odometry pose change), ignore correction from first localization
+				if(!odomPose.isNull() && !previousMapCorrection.isNull() && !previousMapCorrection.isIdentity())
 				{
 					Transform odomCorrection = (previousMapCorrection*odomPose).inverse()*_mapCorrection*odomPose;
 					statistics_.addStatistic(Statistics::kLoopOdom_correction_norm(), odomCorrection.getNorm());
@@ -3166,6 +3207,30 @@ bool Rtabmap::process(
 					statistics_.addStatistic(Statistics::kLoopOdom_correction_roll(),  roll*180.0f/M_PI);
 					statistics_.addStatistic(Statistics::kLoopOdom_correction_pitch(),  pitch*180.0f/M_PI);
 					statistics_.addStatistic(Statistics::kLoopOdom_correction_yaw(), yaw*180.0f/M_PI);
+
+					_odomCorrectionAcc[0]+=x;
+					_odomCorrectionAcc[1]+=y;
+					_odomCorrectionAcc[2]+=z;
+					_odomCorrectionAcc[3]+=roll;
+					_odomCorrectionAcc[4]+=pitch;
+					_odomCorrectionAcc[5]+=yaw;
+					Transform odomCorrectionAcc(
+							_odomCorrectionAcc[0],
+							_odomCorrectionAcc[1],
+							_odomCorrectionAcc[2],
+							_odomCorrectionAcc[3],
+							_odomCorrectionAcc[4],
+							_odomCorrectionAcc[5]);
+
+					statistics_.addStatistic(Statistics::kLoopOdom_correction_acc_norm(), odomCorrectionAcc.getNorm());
+					statistics_.addStatistic(Statistics::kLoopOdom_correction_acc_angle(), odomCorrectionAcc.getAngle()*180.0f/M_PI);
+					odomCorrectionAcc.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
+					statistics_.addStatistic(Statistics::kLoopOdom_correction_acc_x(), x);
+					statistics_.addStatistic(Statistics::kLoopOdom_correction_acc_y(), y);
+					statistics_.addStatistic(Statistics::kLoopOdom_correction_acc_z(), z);
+					statistics_.addStatistic(Statistics::kLoopOdom_correction_acc_roll(),  roll*180.0f/M_PI);
+					statistics_.addStatistic(Statistics::kLoopOdom_correction_acc_pitch(),  pitch*180.0f/M_PI);
+					statistics_.addStatistic(Statistics::kLoopOdom_correction_acc_yaw(), yaw*180.0f/M_PI);
 				}
 			}
 			if(!_lastLocalizationPose.isNull() && !_lastLocalizationPose.isIdentity())
@@ -3177,12 +3242,12 @@ bool Rtabmap::process(
 				statistics_.addStatistic(Statistics::kLoopMapToBase_roll(),  roll*180.0f/M_PI);
 				statistics_.addStatistic(Statistics::kLoopMapToBase_pitch(),  pitch*180.0f/M_PI);
 				statistics_.addStatistic(Statistics::kLoopMapToBase_yaw(), yaw*180.0f/M_PI);
+				UINFO("Localization pose = %s", _lastLocalizationPose.prettyPrint().c_str());
 			}
 
 			statistics_.setMapCorrection(_mapCorrection);
 			UINFO("Set map correction = %s", _mapCorrection.prettyPrint().c_str());
 			statistics_.setLocalizationCovariance(localizationCovariance);
-			statistics_.setProximityDetectionId(lastProximitySpaceClosureId);
 
 			// timings...
 			statistics_.addStatistic(Statistics::kTimingMemory_update(), timeMemoryUpdate*1000);
@@ -3410,11 +3475,13 @@ bool Rtabmap::process(
 				_optimizedPoses.erase(lastId);
 				for(std::multimap<int, Link>::iterator iter=_constraints.find(lastId); iter!=_constraints.end() && iter->first==lastId;++iter)
 				{
-					iter->second.to();
-					std::multimap<int, Link>::iterator jter = graph::findLink(_constraints, iter->second.to(), iter->second.from(), false);
-					if(jter != _constraints.end())
+					if(iter->second.to() != iter->second.from())
 					{
-						_constraints.erase(jter);
+						std::multimap<int, Link>::iterator jter = graph::findLink(_constraints, iter->second.to(), iter->second.from(), false);
+						if(jter != _constraints.end())
+						{
+							_constraints.erase(jter);
+						}
 					}
 				}
 				_constraints.erase(lastId);
@@ -4324,7 +4391,7 @@ void Rtabmap::dumpPrediction() const
 	}
 }
 
-Signature Rtabmap::getSignatureCopy(int id, bool images, bool scan, bool userData, bool occupancyGrid) const
+Signature Rtabmap::getSignatureCopy(int id, bool images, bool scan, bool userData, bool occupancyGrid, bool withWords, bool withGlobalDescriptors) const
 {
 	Signature s;
 	if(_memory)
@@ -4345,7 +4412,7 @@ Signature Rtabmap::getSignatureCopy(int id, bool images, bool scan, bool userDat
 		{
 			data = _memory->getNodeData(id, images, scan, userData, occupancyGrid);
 		}
-		if(!images)
+		if(!images && withWords)
 		{
 			std::vector<CameraModel> models;
 			StereoCameraModel stereoModel;
@@ -4353,23 +4420,34 @@ Signature Rtabmap::getSignatureCopy(int id, bool images, bool scan, bool userDat
 			data.setCameraModels(models);
 			data.setStereoCameraModel(stereoModel);
 		}
-		std::multimap<int, cv::KeyPoint> words;
-		std::multimap<int, cv::Point3f> words3;
-		std::multimap<int, cv::Mat> wordsDescriptors;
-		std::vector<rtabmap::GlobalDescriptor> globalDescriptors;
-		_memory->getNodeWordsAndGlobalDescriptors(id, words, words3, wordsDescriptors, globalDescriptors);
+
 		s=Signature(id,
-					mapId,
-					weight,
-					stamp,
-					label,
-					odomPoseLocal,
-					groundTruth,
-					data);
-		s.setWords(words);
-		s.setWords3(words3);
-		s.setWordsDescriptors(wordsDescriptors);
-		s.sensorData().setGlobalDescriptors(globalDescriptors);
+				mapId,
+				weight,
+				stamp,
+				label,
+				odomPoseLocal,
+				groundTruth,
+				data);
+
+		if(withWords || withGlobalDescriptors)
+		{
+			std::multimap<int, cv::KeyPoint> words;
+			std::multimap<int, cv::Point3f> words3;
+			std::multimap<int, cv::Mat> wordsDescriptors;
+			std::vector<rtabmap::GlobalDescriptor> globalDescriptors;
+			_memory->getNodeWordsAndGlobalDescriptors(id, words, words3, wordsDescriptors, globalDescriptors);
+			if(withWords)
+			{
+				s.setWords(words);
+				s.setWords3(words3);
+				s.setWordsDescriptors(wordsDescriptors);
+			}
+			if(withGlobalDescriptors)
+			{
+				s.sensorData().setGlobalDescriptors(globalDescriptors);
+			}
+		}
 		if(velocity.size()==6)
 		{
 			s.setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]);
@@ -4400,7 +4478,9 @@ void Rtabmap::getGraph(
 		bool withImages,
 		bool withScan,
 		bool withUserData,
-		bool withGrid) const
+		bool withGrid,
+		bool withWords,
+		bool withGlobalDescriptors) const
 {
 	if(_memory && _memory->getLastWorkingSignature())
 	{
@@ -4441,7 +4521,7 @@ void Rtabmap::getGraph(
 
 			for(std::set<int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
 			{
-				signatures->insert(std::make_pair(*iter, getSignatureCopy(*iter, withImages, withScan, withUserData, withGrid)));
+				signatures->insert(std::make_pair(*iter, getSignatureCopy(*iter, withImages, withScan, withUserData, withGrid, withWords, withGlobalDescriptors)));
 			}
 		}
 	}
@@ -4825,145 +4905,129 @@ bool Rtabmap::addLink(const Link & link)
 			return false;
 		}
 
-		std::map<int, Transform> poses;
-		std::multimap<int, Link> links;
-		this->getGraph(poses, links, true, false);
-
-		if(_memory->isIncremental())
+		if(_optimizedPoses.find(link.from()) == _optimizedPoses.end() &&
+		   _optimizedPoses.find(link.to()) == _optimizedPoses.end())
 		{
-			if(poses.find(link.from()) == poses.end())
-			{
-				UERROR("Link's \"from id\" %d is not in the graph", link.from());
-				return false;
-			}
-			if(poses.find(link.to()) == poses.end())
-			{
-				UERROR("Link's \"to id\" %d is not in the graph", link.to());
-				return false;
-			}
+			UERROR("Neither nodes %d or %d are in the local graph (size=%d). One of the 2 nodes should be in the local graph.", (int)_optimizedPoses.size(), link.from(), link.to());
+			return false;
 		}
 
-		int from = link.from();
-		int to = link.to();
-
-		if(_optimizationMaxError > 0.0f)
+		// add temporary the link
+		if(!_memory->addLink(link))
 		{
-			//optimize the graph to see if the new constraint is globally valid
-			std::multimap<int, Link> linksIn = links;
-			linksIn.insert(std::make_pair(link.from(), link));
+			UERROR("Cannot add new link %d->%d to memory", link.from(), link.to());
+			return false;
+		}
+
+		// optimize with new link
+		std::map<int, Transform> poses = _optimizedPoses;
+		std::multimap<int, Link> links;
+		cv::Mat covariance;
+		optimizeCurrentMap(this->getLastLocationId(), false, poses, covariance, &links);
+
+		if(poses.find(link.from()) == poses.end())
+		{
+			UERROR("Link's \"from id\" %d is not in the graph (size=%d)", link.from(), (int)poses.size());
+			_memory->removeLink(link.from(), link.to());
+			return false;
+		}
+		if(poses.find(link.to()) == poses.end())
+		{
+			UERROR("Link's \"to id\" %d is not in the graph (size=%d)", link.to(), (int)poses.size());
+			_memory->removeLink(link.from(), link.to());
+			return false;
+		}
+
+		std::string msg;
+		if(poses.empty())
+		{
+			msg = uFormat("Rejecting edge %d->%d because graph optimization has failed!", link.from(), link.to());
+		}
+		else if(_optimizationMaxError > 0.0f)
+		{
+			float maxLinearError = 0.0f;
+			float maxLinearErrorRatio = 0.0f;
+			float maxAngularError = 0.0f;
+			float maxAngularErrorRatio = 0.0f;
 			const Link * maxLinearLink = 0;
 			const Link * maxAngularLink = 0;
-			float maxLinearError = 0.0f;
-			float maxAngularError = 0.0f;
-			float maxLinearErrorRatio = 0.0f;
-			float maxAngularErrorRatio = 0.0f;
-			std::map<int, Transform> optimizedPoses;
-			UASSERT_MSG(poses.find(from) != poses.end(), uFormat("id=%d poses=%d links=%d", from, (int)poses.size(), (int)links.size()).c_str());
-			UASSERT_MSG(poses.find(to) != poses.end(), uFormat("id=%d poses=%d links=%d", to, (int)poses.size(), (int)links.size()).c_str());
-			_graphOptimizer->getConnectedGraph(from, poses, linksIn, optimizedPoses, links);
-			UASSERT_MSG(optimizedPoses.find(from) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", from, (int)optimizedPoses.size(), (int)links.size()).c_str());
-			UASSERT_MSG(optimizedPoses.find(to) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", to, (int)optimizedPoses.size(), (int)links.size()).c_str());
-			UASSERT(graph::findLink(links, from, to) != links.end());
-			int fromId = _optimizeFromGraphEnd?poses.rbegin()->first:poses.begin()->first;
-			optimizedPoses = _graphOptimizer->optimize(fromId, optimizedPoses, links);
-			std::string msg;
-			if(optimizedPoses.size())
+
+			graph::computeMaxGraphErrors(
+					poses,
+					links,
+					maxLinearErrorRatio,
+					maxAngularErrorRatio,
+					maxLinearError,
+					maxAngularError,
+					&maxLinearLink,
+					&maxAngularLink);
+			if(maxLinearLink)
 			{
-				graph::computeMaxGraphErrors(
-						optimizedPoses,
-						links,
-						maxLinearErrorRatio,
-						maxAngularErrorRatio,
-						maxLinearError,
-						maxAngularError,
-						&maxLinearLink,
-						&maxAngularLink);
-				if(maxLinearLink)
+				UINFO("Max optimization linear error = %f m (link %d->%d)", maxLinearError, maxLinearLink->from(), maxLinearLink->to());
+				if(maxLinearErrorRatio > _optimizationMaxError)
 				{
-					UINFO("Max optimization linear error = %f m (link %d->%d)", maxLinearError, maxLinearLink->from(), maxLinearLink->to());
-					if(maxLinearErrorRatio > _optimizationMaxError)
-					{
-						msg = uFormat("Rejecting edge %d->%d because "
-								  "graph error is too large after optimization (%f m for edge %d->%d with ratio %f > std=%f m). "
-								  "\"%s\" is %f.",
-								  from,
-								  to,
-								  maxLinearError,
-								  maxLinearLink->from(),
-								  maxLinearLink->to(),
-								  maxLinearErrorRatio,
-								  sqrt(maxLinearLink->transVariance()),
-								  Parameters::kRGBDOptimizeMaxError().c_str(),
-								  _optimizationMaxError);
-					}
-				}
-				else if(maxAngularLink)
-				{
-					UINFO("Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0f/M_PI, maxAngularLink->from(), maxAngularLink->to());
-					if(maxAngularErrorRatio > _optimizationMaxError)
-					{
-						msg = uFormat("Rejecting edge %d->%d because "
-								  "graph error is too large after optimization (%f deg for edge %d->%d with ratio %f > std=%f deg). "
-								  "\"%s\" is %f m.",
-								  from,
-								  to,
-								  maxAngularError*180.0f/M_PI,
-								  maxAngularLink->from(),
-								  maxAngularLink->to(),
-								  maxAngularErrorRatio,
-								  sqrt(maxAngularLink->rotVariance()),
-								  Parameters::kRGBDOptimizeMaxError().c_str(),
-								  _optimizationMaxError);
-					}
+					msg = uFormat("Rejecting edge %d->%d because "
+							  "graph error is too large after optimization (%f m for edge %d->%d with ratio %f > std=%f m). "
+							  "\"%s\" is %f.",
+							  link.from(),
+							  link.to(),
+							  maxLinearError,
+							  maxLinearLink->from(),
+							  maxLinearLink->to(),
+							  maxLinearErrorRatio,
+							  sqrt(maxLinearLink->transVariance()),
+							  Parameters::kRGBDOptimizeMaxError().c_str(),
+							  _optimizationMaxError);
 				}
 			}
-			else
+			else if(maxAngularLink)
 			{
-				msg = uFormat("Rejecting edge %d->%d because graph optimization has failed!",
-						  from,
-						  to);
-			}
-			if(!msg.empty())
-			{
-				UERROR("%s", msg.c_str());
-				return false;
+				UINFO("Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0f/M_PI, maxAngularLink->from(), maxAngularLink->to());
+				if(maxAngularErrorRatio > _optimizationMaxError)
+				{
+					msg = uFormat("Rejecting edge %d->%d because "
+							  "graph error is too large after optimization (%f deg for edge %d->%d with ratio %f > std=%f deg). "
+							  "\"%s\" is %f m.",
+							  link.from(),
+							  link.to(),
+							  maxAngularError*180.0f/M_PI,
+							  maxAngularLink->from(),
+							  maxAngularLink->to(),
+							  maxAngularErrorRatio,
+							  sqrt(maxAngularLink->rotVariance()),
+							  Parameters::kRGBDOptimizeMaxError().c_str(),
+							  _optimizationMaxError);
+				}
 			}
 		}
-		else
+		if(!msg.empty())
 		{
-			int fromId = _optimizeFromGraphEnd?poses.rbegin()->first:poses.begin()->first;
-			poses = _graphOptimizer->optimize(fromId, poses, links, 0);
-			if(poses.empty())
-			{
-				UERROR("Rejecting edge %d->%d because graph optimization has failed!", from, to);
-				return false;
-			}
+			UERROR("%s", msg.c_str());
+			_memory->removeLink(link.from(), link.to());
+			return false;
 		}
 
-		if(_memory->addLink(link, false))
+		// Update optimized poses
+		for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
 		{
-			// Update optimized poses
-			for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
+			std::map<int, Transform>::iterator jter = poses.find(iter->first);
+			if(jter != poses.end())
 			{
-				std::map<int, Transform>::iterator jter = poses.find(iter->first);
-				if(jter != poses.end())
-				{
-					iter->second = jter->second;
-				}
+				iter->second = jter->second;
 			}
-			if(!_optimizeFromGraphEnd)
-			{
-				_mapCorrection = _optimizedPoses.rbegin()->second * _memory->getSignature(_optimizedPoses.rbegin()->first)->getPose().inverse();
-			}
-
-			std::map<int, Transform> tmp;
-			// Update also the links if some have been added in WM
-			_memory->getMetricConstraints(uKeysSet(_optimizedPoses), tmp, _constraints, false);
-			// This will force rtabmap_ros to regenerate the global occupancy grid if there was one
-			_memory->save2DMap(cv::Mat(), 0, 0, 0);
-
-			return true;
 		}
+		if(!_optimizeFromGraphEnd)
+		{
+			_mapCorrection = _optimizedPoses.rbegin()->second * _memory->getSignature(_optimizedPoses.rbegin()->first)->getPose().inverse();
+		}
+
+		std::map<int, Transform> tmp;
+		// Update also the links if some have been added in WM
+		_memory->getMetricConstraints(uKeysSet(_optimizedPoses), tmp, _constraints, false);
+		// This will force rtabmap_ros to regenerate the global occupancy grid if there was one
+		_memory->save2DMap(cv::Mat(), 0, 0, 0);
+
+		return true;
 	}
 	else // localization mode
 	{
@@ -4978,7 +5042,7 @@ bool Rtabmap::addLink(const Link & link)
 		}
 		if(_optimizedPoses.find(oldestId) == _optimizedPoses.end())
 		{
-			UERROR("Link's id %d is not in the optimized graph", oldestId);
+			UERROR("Link's id %d is not in the optimized graph (_optimizedPoses=%d)", oldestId, (int)_optimizedPoses.size());
 			return false;
 		}
 		if(_optimizeFromGraphEnd)
@@ -5015,7 +5079,7 @@ bool Rtabmap::addLink(const Link & link)
 		{
 			_lastLocalizationPose = _optimizedPoses.at(link.to()) * link.transform().inverse();
 		}
-		UERROR("Set _lastLocalizationPose=%s", _lastLocalizationPose.prettyPrint().c_str());
+		UINFO("Set _lastLocalizationPose=%s", _lastLocalizationPose.prettyPrint().c_str());
 		if(_graphOptimizer->isSlam2d())
 		{
 			// transform constraint to 2D

corelib/src/camera/CameraK4A.cpp

@@ -4,12 +4,12 @@ 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
+ * 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
+ * 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
+ * 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.
 
@@ -22,7 +22,7 @@ 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>
@@ -49,34 +49,47 @@ bool CameraK4A::available()
 }
 
 CameraK4A::CameraK4A(
-	int deviceId,
-	float imageRate,
-	const Transform & localTransform) :
-	Camera(imageRate, localTransform)
+		int deviceId,
+		float imageRate,
+		const Transform & localTransform) :
+			Camera(imageRate, localTransform)
 #ifdef RTABMAP_K4A
-	,deviceId_(deviceId),
-	playbackHandle_(NULL),
-	transformationHandle_(NULL),
-	ir_(false),
-	previousStamp_(0.0)
+			,
+			device_(NULL),
+			config_(K4A_DEVICE_CONFIG_INIT_DISABLE_ALL),
+			transformation_(NULL),
+			capture_(NULL),
+			playbackHandle_(NULL),
+			transformationHandle_(NULL),
+			deviceId_(deviceId),
+			rgb_resolution_(0),
+			framerate_(2),
+			depth_resolution_(2),
+			ir_(false),
+			previousStamp_(0.0)
 #endif
 {
 }
 
 CameraK4A::CameraK4A(
-	const std::string & fileName,
-	float imageRate,
-	const Transform & localTransform) :
-	Camera(imageRate, localTransform)
+		const std::string & fileName,
+		float imageRate,
+		const Transform & localTransform) :
+			Camera(imageRate, localTransform)
 #ifdef RTABMAP_K4A
-	,
-	device_(NULL),
-	playbackHandle_(NULL),
-	transformationHandle_(NULL),
-	deviceId_(-1),
-	fileName_(fileName),
-	ir_(false),
-	previousStamp_(0.0)
+			,
+			device_(NULL),
+			transformation_(NULL),
+			capture_(NULL),
+			playbackHandle_(NULL),
+			transformationHandle_(NULL),
+			deviceId_(-1),
+			fileName_(fileName),
+			rgb_resolution_(0),
+			framerate_(2),
+			depth_resolution_(2),
+			ir_(false),
+			previousStamp_(0.0)
 #endif
 {
 }
@@ -118,11 +131,22 @@ void CameraK4A::close()
 			k4a_device_stop_cameras(device_);
 			k4a_device_close(device_);
 			device_ = NULL;
+			config_ = K4A_DEVICE_CONFIG_INIT_DISABLE_ALL;
 		}
 	}
 #endif
 }
 
+void CameraK4A::setPreferences(int rgb_resolution, int framerate, int depth_resolution)
+{
+#ifdef RTABMAP_K4A
+	rgb_resolution_ = rgb_resolution;
+	framerate_ = framerate;
+	depth_resolution_ = depth_resolution;
+	UINFO("setPreferences(): %i %i %i", rgb_resolution, framerate, depth_resolution);
+#endif
+}
+
 void CameraK4A::setIRDepthFormat(bool enabled)
 {
 #ifdef RTABMAP_K4A
@@ -157,24 +181,24 @@ bool CameraK4A::init(const std::string & calibrationFolder, const std::string &
 		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));
+					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));
+					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);
 		}
@@ -190,13 +214,41 @@ bool CameraK4A::init(const std::string & calibrationFolder, const std::string &
 	}
 	else if (deviceId_ >= 0)
 	{
-		device_ = NULL;
+		if(device_!=NULL)
+		{
+			this->close();
+		}
 
-		config_ = K4A_DEVICE_CONFIG_INIT_DISABLE_ALL;
-		config_.camera_fps = K4A_FRAMES_PER_SECOND_15;
-		config_.depth_mode = K4A_DEPTH_MODE_WFOV_2X2BINNED;
+		switch(rgb_resolution_)
+		{
+		case 0: config_.color_resolution = K4A_COLOR_RESOLUTION_720P; break;
+		case 1: config_.color_resolution = K4A_COLOR_RESOLUTION_1080P; break;
+		case 2: config_.color_resolution = K4A_COLOR_RESOLUTION_1440P; break;
+		case 3: config_.color_resolution = K4A_COLOR_RESOLUTION_1536P; break;
+		case 4: config_.color_resolution = K4A_COLOR_RESOLUTION_2160P; break;
+		case 5:
+		default: config_.color_resolution = K4A_COLOR_RESOLUTION_3072P; break;
+		}
+
+		switch(framerate_)
+		{
+		case 0: config_.camera_fps = K4A_FRAMES_PER_SECOND_5; break;
+		case 1: config_.camera_fps = K4A_FRAMES_PER_SECOND_15; break;
+		case 2:
+		default: config_.camera_fps = K4A_FRAMES_PER_SECOND_30; break;
+		}
+
+		switch(depth_resolution_)
+		{
+		case 0: config_.depth_mode = K4A_DEPTH_MODE_NFOV_2X2BINNED; break;
+		case 1: config_.depth_mode = K4A_DEPTH_MODE_NFOV_UNBINNED; break;
+		case 2: config_.depth_mode = K4A_DEPTH_MODE_WFOV_2X2BINNED; break;
+		case 3:
+		default: config_.depth_mode = K4A_DEPTH_MODE_WFOV_UNBINNED; break;
+		}
+
+		// This is fixed for now
 		config_.color_format = K4A_IMAGE_FORMAT_COLOR_BGRA32;
-		config_.color_resolution = K4A_COLOR_RESOLUTION_720P;
 
 		int device_count = k4a_device_get_installed_count();
 
@@ -205,11 +257,15 @@ bool CameraK4A::init(const std::string & calibrationFolder, const std::string &
 			UERROR("No k4a devices attached!");
 			return false;
 		}
+		else if(deviceId_ > device_count)
+		{
+			UERROR("Cannot select device %d, only %d devices detected.", deviceId_, device_count);
+		}
 
-		UINFO("CameraK4A found k4a device attached");
+		UINFO("CameraK4A found %d k4a device(s) attached", device_count);
 
 		// Open the first plugged in Kinect device
-		if (K4A_FAILED(k4a_device_open(K4A_DEVICE_DEFAULT, &device_)))
+		if (K4A_FAILED(k4a_device_open(deviceId_, &device_)))
 		{
 			UERROR("Failed to open k4a device!");
 			return false;
@@ -231,7 +287,7 @@ bool CameraK4A::init(const std::string & calibrationFolder, const std::string &
 		if (K4A_FAILED(k4a_device_start_cameras(device_, &config_)))
 		{
 			UERROR("Failed to start cameras!");
-			k4a_device_close(device_);
+			close();
 			return false;
 		}
 
@@ -240,35 +296,55 @@ bool CameraK4A::init(const std::string & calibrationFolder, const std::string &
 		if (K4A_FAILED(k4a_device_get_calibration(device_, config_.depth_mode, config_.color_resolution, &calibration_)))
 		{
 			UERROR("k4a_device_get_calibration() failed!");
-			k4a_device_close(device_);
+			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));
-                }
+		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));
+		}
 
 		transformation_ = k4a_transformation_create(&calibration_);
 
+		// Get imu transform
+		k4a_calibration_extrinsics_t* imu_extrinsics;
+		if(ir_)
+		{
+			imu_extrinsics = &calibration_.extrinsics[K4A_CALIBRATION_TYPE_ACCEL][K4A_CALIBRATION_TYPE_DEPTH];
+		}
+		else
+		{
+			imu_extrinsics = &calibration_.extrinsics[K4A_CALIBRATION_TYPE_ACCEL][K4A_CALIBRATION_TYPE_COLOR];
+		}
+		imuLocalTransform_ = Transform(
+			imu_extrinsics->rotation[0], imu_extrinsics->rotation[1], imu_extrinsics->rotation[2], imu_extrinsics->translation[0] / 1000.0f,
+			imu_extrinsics->rotation[3], imu_extrinsics->rotation[4], imu_extrinsics->rotation[5], imu_extrinsics->translation[1] / 1000.0f,
+			imu_extrinsics->rotation[6], imu_extrinsics->rotation[7], imu_extrinsics->rotation[8], imu_extrinsics->translation[2] / 1000.0f);
+
+		UINFO("camera to imu=%s", imuLocalTransform_.prettyPrint().c_str());
+		UINFO("base to camera=%s", this->getLocalTransform().prettyPrint().c_str());
+		imuLocalTransform_ = this->getLocalTransform()*imuLocalTransform_;
+		UINFO("base to imu=%s", imuLocalTransform_.prettyPrint().c_str());
+
 		if (K4A_FAILED(k4a_device_start_imu(device_)))
 		{
 			UERROR("Failed to start K4A IMU");
@@ -285,6 +361,7 @@ bool CameraK4A::init(const std::string & calibrationFolder, const std::string &
 			return true;
 		}
 
+		close();
 		return false;
 	}
 	return true;
@@ -302,10 +379,11 @@ bool CameraK4A::isCalibrated() const
 std::string CameraK4A::getSerial() const
 {
 #ifdef RTABMAP_K4A
-	if(device_ != NULL)
-		return(serial_number_);
-	else
-		return fileName_.empty()?"":fileName_;
+	if(!fileName_.empty())
+	{
+		return fileName_;
+	}
+	return(serial_number_);
 #else
 	return "";
 #endif
@@ -482,7 +560,7 @@ SensorData CameraK4A::captureImage(CameraInfo * info)
 							if (sleepTime > 10000)
 							{
 								UWARN("Detected long delay (%d sec, stamps = %f vs %f). Waiting a maximum of 10 seconds.",
-									sleepTime / 1000, previousStamp_, stamp);
+										sleepTime / 1000, previousStamp_, stamp);
 								sleepTime = 10000;
 							}
 							if (sleepTime > 2)
@@ -520,118 +598,115 @@ SensorData CameraK4A::captureImage(CameraInfo * info)
 	}
 	else
 	{
-		k4a_image_t ir_image_;
-		k4a_image_t depth_image_;
-		k4a_image_t rgb_image_;
+		k4a_image_t ir_image_ = NULL;
+		k4a_image_t rgb_image_ = NULL;
 		k4a_imu_sample_t imu_sample_;
 
-		if (K4A_WAIT_RESULT_SUCCEEDED == k4a_device_get_capture(device_, &capture_, K4A_WAIT_INFINITE))
+		double t = UTimer::now();
+		k4a_wait_result_t result = K4A_WAIT_RESULT_FAILED;
+		while((UTimer::now()-t < 5.0) &&
+				(K4A_WAIT_RESULT_SUCCEEDED != (result=k4a_device_get_capture(device_, &capture_, K4A_WAIT_INFINITE)) ||
+				((ir_ && (ir_image_=k4a_capture_get_ir_image(capture_)) == NULL) || (!ir_ && (rgb_image_=k4a_capture_get_color_image(capture_)) == NULL))))
+		{
+			k4a_capture_release(capture_);
+			// the first frame may be null, just retry for 5 seconds
+		}
+
+		if (result == K4A_WAIT_RESULT_SUCCEEDED && (rgb_image_!=NULL || ir_image_!=NULL))
 		{
 			cv::Mat bgrCV;
 			cv::Mat depthCV;
 			IMU imu;
 
-			if (ir_)
+			if (ir_image_ != NULL)
 			{
-				// Retrieve IR image from capture
-				ir_image_ = k4a_capture_get_ir_image(capture_);
+				// Convert IR image
+				cv::Mat bgrCV16(k4a_image_get_height_pixels(ir_image_),
+						k4a_image_get_width_pixels(ir_image_),
+						CV_16UC1,
+						(void*)k4a_image_get_buffer(ir_image_));
 
-				if(ir_image_ != NULL)
-				{
-					// Convert IR image
-					cv::Mat bgrCV16(k4a_image_get_height_pixels(ir_image_),
-							k4a_image_get_width_pixels(ir_image_),
-							CV_16UC1,
-							(void*)k4a_image_get_buffer(ir_image_));
+				bgrCV16.convertTo(bgrCV, CV_8U);
 
-					bgrCV16.convertTo(bgrCV, CV_8U);
-
-					// Release the image
-					k4a_image_release(ir_image_);
-				}
+				// Release the image
+				k4a_image_release(ir_image_);
 			}
 			else
 			{
-				// Retrieve RGB image from capture
-				rgb_image_ = k4a_capture_get_color_image(capture_);
-
-				if(rgb_image_ != NULL)
+				// Convert RGB image
+				if (k4a_image_get_format(rgb_image_) == K4A_IMAGE_FORMAT_COLOR_MJPG)
 				{
-					// Convert RGB image
-					if (k4a_image_get_format(rgb_image_) == K4A_IMAGE_FORMAT_COLOR_MJPG)
-					{
-						bgrCV = uncompressImage(cv::Mat(1, (int)k4a_image_get_size(rgb_image_),
-									CV_8UC1,
-									(void*)k4a_image_get_buffer(rgb_image_)));
-					}
-					else
-					{
-						cv::Mat bgra(k4a_image_get_height_pixels(rgb_image_),
-							     k4a_image_get_width_pixels(rgb_image_),
-							     CV_8UC4,
-							     (void*)k4a_image_get_buffer(rgb_image_));
-
-						cv::cvtColor(bgra, bgrCV, CV_BGRA2BGR);
-					}
-
-					// Release the image
-					k4a_image_release(rgb_image_);
-				}
-			}
-
-			// Retrieve depth image from capture
-			depth_image_ = k4a_capture_get_depth_image(capture_);
-
-			if (depth_image_ != NULL)
-			{
-				if (ir_)
-				{
-					depthCV = cv::Mat(k4a_image_get_height_pixels(depth_image_),
-							  k4a_image_get_width_pixels(depth_image_),
-							  CV_16UC1,
-							  (void*)k4a_image_get_buffer(depth_image_)).clone();
+					bgrCV = uncompressImage(cv::Mat(1, (int)k4a_image_get_size(rgb_image_),
+							CV_8UC1,
+							(void*)k4a_image_get_buffer(rgb_image_)));
 				}
 				else
 				{
-					k4a_image_t transformedDepth = NULL;
+					cv::Mat bgra(k4a_image_get_height_pixels(rgb_image_),
+							k4a_image_get_width_pixels(rgb_image_),
+							CV_8UC4,
+							(void*)k4a_image_get_buffer(rgb_image_));
 
-					if (k4a_image_create(k4a_image_get_format(depth_image_),
-							     bgrCV.cols, bgrCV.rows, bgrCV.cols * 2, &transformedDepth) == K4A_RESULT_SUCCEEDED)
+					cv::cvtColor(bgra, bgrCV, CV_BGRA2BGR);
+				}
+
+				// Release the image
+				k4a_image_release(rgb_image_);
+			}
+
+			if(!bgrCV.empty())
+			{
+				// Retrieve depth image from capture
+				k4a_image_t depth_image_ = k4a_capture_get_depth_image(capture_);
+
+				if (depth_image_ != NULL)
+				{
+					if (ir_)
 					{
-						if(k4a_transformation_depth_image_to_color_camera(transformation_, depth_image_, 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("K4A failed to register depth image");
-						}
-
-						k4a_image_release(transformedDepth);
+						depthCV = cv::Mat(k4a_image_get_height_pixels(depth_image_),
+								k4a_image_get_width_pixels(depth_image_),
+								CV_16UC1,
+								(void*)k4a_image_get_buffer(depth_image_)).clone();
 					}
 					else
 					{
-						UERROR("K4A failed to allocate registered depth image");
+						k4a_image_t transformedDepth = NULL;
+						if (k4a_image_create(k4a_image_get_format(depth_image_),
+								bgrCV.cols, bgrCV.rows, bgrCV.cols * 2, &transformedDepth) == K4A_RESULT_SUCCEEDED)
+						{
+							if(k4a_transformation_depth_image_to_color_camera(transformation_, depth_image_, 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("K4A failed to register depth image");
+							}
+
+							k4a_image_release(transformedDepth);
+						}
+						else
+						{
+							UERROR("K4A failed to allocate registered depth image");
+						}
 					}
+					k4a_image_release(depth_image_);
 				}
-				k4a_image_release(depth_image_);
-		 	}
+			}
+
 			k4a_capture_release(capture_);
 
 			// Get IMU sample, clear buffer
 			if(K4A_WAIT_RESULT_SUCCEEDED == k4a_device_get_imu_sample(device_, &imu_sample_, 60))
 			{
-				imu = IMU(cv::Vec3d(-1 * imu_sample_.gyro_sample.xyz.x, imu_sample_.gyro_sample.xyz.y, -1 * imu_sample_.gyro_sample.xyz.z),
-					  cv::Mat::eye(3, 3, CV_64FC1),
-					  cv::Vec3d(-1 * imu_sample_.acc_sample.xyz.x, imu_sample_.acc_sample.xyz.y, -1 * imu_sample_.acc_sample.xyz.z),
-					  cv::Mat::eye(3, 3, CV_64FC1),
-					  Transform::getIdentity());
-
-				UINFO("IMU: %f %f %f %f %f %f", imu_sample_.gyro_sample.xyz.x, imu_sample_.gyro_sample.xyz.y, imu_sample_.gyro_sample.xyz.z,
-								imu_sample_.acc_sample.xyz.x, imu_sample_.acc_sample.xyz.y, imu_sample_.acc_sample.xyz.z);
+				imu = IMU(cv::Vec3d(imu_sample_.gyro_sample.xyz.x, imu_sample_.gyro_sample.xyz.y, imu_sample_.gyro_sample.xyz.z),
+						cv::Mat::eye(3, 3, CV_64FC1),
+						cv::Vec3d(imu_sample_.acc_sample.xyz.x, imu_sample_.acc_sample.xyz.y, imu_sample_.acc_sample.xyz.z),
+						cv::Mat::eye(3, 3, CV_64FC1),
+						imuLocalTransform_);
 			}
 			else
 			{

corelib/src/camera/CameraMyntEye.cpp

@@ -33,6 +33,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <mynteye/api.h>
 #include <mynteye/device.h>
 #include <mynteye/context.h>
+
+#ifndef M_PI
+#define M_PI       3.14159265358979323846
+#endif
 #endif
 
 namespace rtabmap

corelib/src/camera/CameraRealSense2.cpp

@@ -77,7 +77,8 @@ CameraRealSense2::CameraRealSense2(
 	cameraHeight_(480),
 	cameraFps_(30),
 	publishInterIMU_(false),
-	dualMode_(false)
+	dualMode_(false),
+	closing_(false)
 #endif
 {
 	UDEBUG("");
@@ -86,12 +87,15 @@ CameraRealSense2::CameraRealSense2(
 CameraRealSense2::~CameraRealSense2()
 {
 #ifdef RTABMAP_REALSENSE2
+	closing_ = true;
 	try
 	{
+		UDEBUG("Closing device(s)...");
 		for(size_t i=0; i<dev_.size(); ++i)
 		{
 			if(dev_[i])
 			{
+				UDEBUG("Closing %d sensor(s) from device %d...", (int)dev_[i]->query_sensors().size(), (int)i);
 				for(rs2::sensor _sensor : dev_[i]->query_sensors())
 				{
 					try
@@ -104,6 +108,7 @@ CameraRealSense2::~CameraRealSense2()
 						UWARN("%s", error.what());
 					}
 				}
+				dev_[i]->hardware_reset(); // To avoid freezing on some Windows computers in the following destructor
 				delete dev_[i];
 			}
 		}
@@ -250,7 +255,7 @@ void CameraRealSense2::getPoseAndIMU(
 		{
 			if(maxWaitTimeMs > 0)
 			{
-				UWARN("Could not find poses to interpolate at time %f after waiting %d ms (last is %f)...", stamp, maxWaitTimeMs, poseBuffer_.rbegin()->first);
+				UWARN("Could not find poses to interpolate at image time %f after waiting %d ms (last is %f)...", stamp, maxWaitTimeMs, poseBuffer_.rbegin()->first);
 			}
 		}
 		else
@@ -303,7 +308,7 @@ void CameraRealSense2::getPoseAndIMU(
 		{
 			if(maxWaitTimeMs>0)
 			{
-				UWARN("Could not find acc data to interpolate at time %f after waiting %d ms (last is %f)...", stamp, maxWaitTimeMs, accBuffer_.rbegin()->first);
+				UWARN("Could not find acc data to interpolate at image time %f after waiting %d ms (last is %f)...", stamp, maxWaitTimeMs, accBuffer_.rbegin()->first);
 			}
 			imuMutex_.unlock();
 			return;
@@ -366,7 +371,7 @@ void CameraRealSense2::getPoseAndIMU(
 		{
 			if(maxWaitTimeMs>0)
 			{
-				UWARN("Could not find gyro data to interpolate at time %f after waiting %d ms (last is %f)...", stamp, maxWaitTimeMs, gyroBuffer_.rbegin()->first);
+				UWARN("Could not find gyro data to interpolate at image time %f after waiting %d ms (last is %f)...", stamp, maxWaitTimeMs, gyroBuffer_.rbegin()->first);
 			}
 			imuMutex_.unlock();
 			return;
@@ -519,7 +524,14 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 			{
 				if (info.was_removed(*dev_[i]))
 				{
-					UERROR("The device has been disconnected!");
+					if (closing_)
+					{
+						UDEBUG("The device %d has been disconnected!", i);
+					}
+					else
+					{
+						UERROR("The device %d has been disconnected!", i);
+					}
 				}
 			}
 		}
@@ -1060,7 +1072,6 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
 		if (frameset.size() == 2)
 		{
 			double now = UTimer::now();
-			UDEBUG("Frameset arrived.");
 			bool is_rgb_arrived = false;
 			bool is_depth_arrived = false;
 			bool is_left_fisheye_arrived = false;
@@ -1119,6 +1130,7 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
 			}
 
 			stamp /= 1000.0; // put in seconds
+			UDEBUG("Frameset arrived. system=%fs frame=%fs", now, stamp);
 			if(stamp - now > 1000000000.0)
 			{
 				if(!clockSyncWarningShown_)

docker/bionic/Dockerfile

@@ -104,7 +104,7 @@ RUN rm /bin/sh && ln -s /bin/bash /bin/sh
 # Build RTAB-Map project
 RUN source /ros_entrypoint.sh && \
     cd rtabmap/build && \
-    cmake .. && \
+    cmake -DWITH_ALICE_VISION=ON .. && \
     make && \
     make install && \
     cd ../.. && \

docker/bionic/android/rtabmap_api23/rtabmap.bash

@@ -31,13 +31,13 @@ wget 'https://docs.google.com/uc?authuser=0&id=1s5iPJ7xiridj9Jj--gCy2XiQFniheVm6
 mv TangoSDK_Ikariotikos_Java.jar rtabmap-tango/app/android/libs/.
 
 # ARCore
-wget 'https://docs.google.com/uc?authuser=0&id=1A4gMviyxHCnA19MTMbitOWoSOyoZcCef&export=download' -O arcore.zip
+wget 'https://docs.google.com/uc?authuser=0&id=1VsibeqRYpS5pjmrG-vYTXyiPg8kbIfVN&export=download' -O arcore.zip
 unzip -qq arcore.zip
 rm arcore.zip
-cp -r arcore/include/* $prefix/arm64-v8a/include/.
-cp -r arcore/arm64-v8a/* $prefix/arm64-v8a/lib/.
-cp arcore/*.jar rtabmap-tango/app/android/libs/.
-rm -r arcore
+cp -r arcore1_18/include/* $prefix/arm64-v8a/include/.
+cp -r arcore1_18/arm64-v8a/* $prefix/arm64-v8a/lib/.
+cp arcore1_18/*.jar rtabmap-tango/app/android/libs/.
+rm -r arcore1_18
 
 # AREngine
 wget 'https://docs.google.com/uc?authuser=0&id=1rdaD2Z1QBv-SUeUy0oBmg3C2odfxTHgR&export=download' -O arengine.zip

docker/bionic/android/rtabmap_api24/rtabmap.bash

@@ -31,13 +31,13 @@ wget 'https://docs.google.com/uc?authuser=0&id=1s5iPJ7xiridj9Jj--gCy2XiQFniheVm6
 mv TangoSDK_Ikariotikos_Java.jar rtabmap-tango/app/android/libs/.
 
 # ARCore
-wget 'https://docs.google.com/uc?authuser=0&id=1A4gMviyxHCnA19MTMbitOWoSOyoZcCef&export=download' -O arcore.zip
+wget 'https://docs.google.com/uc?authuser=0&id=1VsibeqRYpS5pjmrG-vYTXyiPg8kbIfVN&export=download' -O arcore.zip
 unzip -qq arcore.zip
 rm arcore.zip
-cp -r arcore/include/* $prefix/arm64-v8a/include/.
-cp -r arcore/arm64-v8a/* $prefix/arm64-v8a/lib/.
-cp arcore/*.jar rtabmap-tango/app/android/libs/.
-rm -r arcore
+cp -r arcore1_18/include/* $prefix/arm64-v8a/include/.
+cp -r arcore1_18/arm64-v8a/* $prefix/arm64-v8a/lib/.
+cp arcore1_18/*.jar rtabmap-tango/app/android/libs/.
+rm -r arcore1_18
 
 # AREngine
 wget 'https://docs.google.com/uc?authuser=0&id=1rdaD2Z1QBv-SUeUy0oBmg3C2odfxTHgR&export=download' -O arengine.zip

docker/bionic/android/rtabmap_api26/rtabmap.bash

@@ -31,13 +31,13 @@ wget 'https://docs.google.com/uc?authuser=0&id=1s5iPJ7xiridj9Jj--gCy2XiQFniheVm6
 mv TangoSDK_Ikariotikos_Java.jar rtabmap-tango/app/android/libs/.
 
 # ARCore
-wget 'https://docs.google.com/uc?authuser=0&id=1A4gMviyxHCnA19MTMbitOWoSOyoZcCef&export=download' -O arcore.zip
+wget 'https://docs.google.com/uc?authuser=0&id=1VsibeqRYpS5pjmrG-vYTXyiPg8kbIfVN&export=download' -O arcore.zip
 unzip -qq arcore.zip
 rm arcore.zip
-cp -r arcore/include/* $prefix/arm64-v8a/include/.
-cp -r arcore/arm64-v8a/* $prefix/arm64-v8a/lib/.
-cp arcore/*.jar rtabmap-tango/app/android/libs/.
-rm -r arcore
+cp -r arcore1_18/include/* $prefix/arm64-v8a/include/.
+cp -r arcore1_18/arm64-v8a/* $prefix/arm64-v8a/lib/.
+cp arcore1_18/*.jar rtabmap-tango/app/android/libs/.
+rm -r arcore1_18
 
 # AREngine
 wget 'https://docs.google.com/uc?authuser=0&id=1rdaD2Z1QBv-SUeUy0oBmg3C2odfxTHgR&export=download' -O arengine.zip

docker/focal/Dockerfile

@@ -0,0 +1,56 @@
+# Image: introlab3it/rtabmap:focal
+
+FROM ros:noetic-perception
+
+# Install build dependencies
+RUN apt-get update && \
+    apt-get install -y git software-properties-common ros-noetic-rtabmap-ros && \
+    apt-get remove -y ros-noetic-rtabmap && \
+    rm -rf /var/lib/apt/lists/
+
+WORKDIR /root/
+
+# GTSAM
+RUN add-apt-repository ppa:joseluisblancoc/gtsam-develop -y
+RUN apt install libgtsam-dev
+
+# libpointmatcher 
+RUN git clone https://github.com/ethz-asl/libnabo.git
+#commit Apr 25 2018
+RUN cd libnabo && \
+    git checkout 7e378f6765393462357b8b74d8dc8c5554542ae6 && \
+    mkdir build && \
+    cd build && \
+    cmake -DCMAKE_BUILD_TYPE=Release .. && \
+    make -j$(nproc) && \
+    make install && \
+    cd && \
+    rm -r libnabo
+RUN git clone https://github.com/ethz-asl/libpointmatcher.git
+#commit Jan 19 2018
+RUN cd libpointmatcher && \
+    git checkout 00004bd41e44a1cf8de24ad87e4914760717cbcc && \
+    mkdir build && \
+    cd build && \
+    cmake -DCMAKE_BUILD_TYPE=Release .. && \
+    make -j$(nproc) && \
+    make install && \
+    cd && \
+    rm -r libpointmatcher
+
+# Clone source code
+ARG CACHE_DATE=2016-01-01
+RUN git clone https://github.com/introlab/rtabmap.git
+
+RUN rm /bin/sh && ln -s /bin/bash /bin/sh
+
+# Build RTAB-Map project
+RUN source /ros_entrypoint.sh && \
+    cd rtabmap/build && \
+    cmake .. && \
+    make && \
+    make install && \
+    cd ../.. && \
+    rm -rf rtabmap && \
+    ldconfig
+

docker/focal/hooks/build

@@ -0,0 +1,2 @@
+#!/bin/bash 
+docker build --build-arg CACHE_DATE="$(date)" --cache-from $IMAGE_NAME -f $DOCKERFILE_PATH -t $IMAGE_NAME -t $DOCKER_REPO:20.04 .

docker/focal/hooks/post_push

@@ -0,0 +1,2 @@
+#!/bin/bash 
+docker push $DOCKER_REPO:20.04

examples/NoEventsExample/main.cpp

@@ -93,13 +93,11 @@ int main(int argc, char * argv[])
 	std::string pathLeftImages = argv[argIndex++];
 	std::string pathRightImages = argv[argIndex++];
 
-	Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0);
 	CameraStereoImages camera(
 			pathLeftImages,
 			pathRightImages,
 			false, // assume that images are already rectified
-			(float)cameraRate,
-			opticalRotation);
+			(float)cameraRate);
 
 	if(camera.init(calibrationDir, calibrationName))
 	{

examples/RGBDMapping/CMakeLists.txt

@@ -66,6 +66,14 @@ IF(realsense2_FOUND)
 	)
 ENDIF(realsense2_FOUND)
 
+# Hack as CameraK4A.h needs k4a include dir
+IF(k4a_FOUND)
+	SET(INCLUDE_DIRS
+		${INCLUDE_DIRS}
+		${k4a_INCLUDE_DIRS}
+	)
+ENDIF(k4a_FOUND)
+
 INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
 
 IF(QT4_FOUND)

examples/RGBDMapping/main.cpp

@@ -46,7 +46,7 @@ void showUsage()
 {
 	printf("\nUsage:\n"
 			"rtabmap-rgbd_mapping driver\n"
-			"  driver       Driver number to use: 0=OpenNI-PCL, 1=OpenNI2, 2=Freenect, 3=OpenNI-CV, 4=OpenNI-CV-ASUS, 5=Freenect2, 6=ZED SDK, 7=RealSense, 8=RealSense2\n\n");
+			"  driver       Driver number to use: 0=OpenNI-PCL, 1=OpenNI2, 2=Freenect, 3=OpenNI-CV, 4=OpenNI-CV-ASUS, 5=Freenect2, 6=ZED SDK, 7=RealSense, 8=RealSense2 9=Kinect for Azure SDK 10=MYNT EYE S\n\n");
 	exit(1);
 }
 
@@ -64,9 +64,9 @@ int main(int argc, char * argv[])
 	else
 	{
 		driver = atoi(argv[argc-1]);
-		if(driver < 0 || driver > 8)
+		if(driver < 0 || driver > 10)
 		{
-			UERROR("driver should be between 0 and 8.");
+			UERROR("driver should be between 0 and 10.");
 			showUsage();
 		}
 	}
@@ -77,7 +77,6 @@ int main(int argc, char * argv[])
 	// Create the OpenNI camera, it will send a CameraEvent at the rate specified.
 	// Set transform to camera so z is up, y is left and x going forward
 	Camera * camera = 0;
-	Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0);
 	if(driver == 1)
 	{
 		if(!CameraOpenNI2::available())
@@ -85,7 +84,7 @@ int main(int argc, char * argv[])
 			UERROR("Not built with OpenNI2 support...");
 			exit(-1);
 		}
-		camera = new CameraOpenNI2("", CameraOpenNI2::kTypeColorDepth, 0, opticalRotation);
+		camera = new CameraOpenNI2();
 	}
 	else if(driver == 2)
 	{
@@ -94,7 +93,7 @@ int main(int argc, char * argv[])
 			UERROR("Not built with Freenect support...");
 			exit(-1);
 		}
-		camera = new CameraFreenect(0, CameraFreenect::kTypeColorDepth, 0, opticalRotation);
+		camera = new CameraFreenect();
 	}
 	else if(driver == 3)
 	{
@@ -103,7 +102,7 @@ int main(int argc, char * argv[])
 			UERROR("Not built with OpenNI from OpenCV support...");
 			exit(-1);
 		}
-		camera = new CameraOpenNICV(false, 0, opticalRotation);
+		camera = new CameraOpenNICV();
 	}
 	else if(driver == 4)
 	{
@@ -112,7 +111,7 @@ int main(int argc, char * argv[])
 			UERROR("Not built with OpenNI from OpenCV support...");
 			exit(-1);
 		}
-		camera = new CameraOpenNICV(true, 0, opticalRotation);
+		camera = new CameraOpenNICV(true);
 	}
 	else if (driver == 5)
 	{
@@ -121,7 +120,7 @@ int main(int argc, char * argv[])
 			UERROR("Not built with Freenect2 support...");
 			exit(-1);
 		}
-		camera = new CameraFreenect2(0, CameraFreenect2::kTypeColor2DepthSD, 0, opticalRotation);
+		camera = new CameraFreenect2(0, CameraFreenect2::kTypeColor2DepthSD);
 	}
 	else if (driver == 6)
 	{
@@ -130,7 +129,7 @@ int main(int argc, char * argv[])
 			UERROR("Not built with ZED SDK support...");
 			exit(-1);
 		}
-		camera = new CameraStereoZed(0, 2, 1, 1, 100, false, 0, opticalRotation);
+		camera = new CameraStereoZed(0, 2, 1, 1, 100, false);
 	}
 	else if (driver == 7)
 	{
@@ -139,7 +138,7 @@ int main(int argc, char * argv[])
 			UERROR("Not built with RealSense support...");
 			exit(-1);
 		}
-		camera = new CameraRealSense(0, 0, 0, false, 0, opticalRotation);
+		camera = new CameraRealSense();
 	}
 	else if (driver == 8)
 	{
@@ -148,11 +147,29 @@ int main(int argc, char * argv[])
 			UERROR("Not built with RealSense2 support...");
 			exit(-1);
 		}
-		camera = new CameraRealSense2("", 0, opticalRotation);
+		camera = new CameraRealSense2();
+	}
+	else if (driver == 9)
+	{
+		if (!rtabmap::CameraK4A::available())
+		{
+			UERROR("Not built with Kinect for Azure SDK support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraK4A(1);
+	}
+	else if (driver == 10)
+	{
+		if (!rtabmap::CameraMyntEye::available())
+		{
+			UERROR("Not built with Mynt Eye S support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraMyntEye();
 	}
 	else
 	{
-		camera = new rtabmap::CameraOpenni("", 0, opticalRotation);
+		camera = new rtabmap::CameraOpenni();
 	}
 
 	if(!camera->init())

examples/WifiMapping/CMakeLists.txt

@@ -60,6 +60,14 @@ IF(realsense2_FOUND)
 	)
 ENDIF(realsense2_FOUND)
 
+# Hack as CameraK4A.h needs k4a include dir
+IF(k4a_FOUND)
+	SET(INCLUDE_DIRS
+		${INCLUDE_DIRS}
+		${k4a_INCLUDE_DIRS}
+	)
+ENDIF(k4a_FOUND)
+
 INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
 
 IF(QT4_FOUND)

examples/WifiMapping/main.cpp

@@ -47,7 +47,7 @@ void showUsage()
 			"Options:\n"
 			"  -i \"name\"            Wifi interface name (e.g. \"eth0\"). Only required on Linux.\n"
 			"  -m                     Enable mirroring of the camera image.\n"
-			"  -d #                   Driver number to use: 0=OpenNI-PCL, 1=OpenNI2, 2=Freenect, 3=OpenNI-CV, 4=OpenNI-CV-ASUS, 5=Freenect2, 6=ZED SDK, 7=RealSense, 8=RealSense2\n\n");
+			"  -d #                   Driver number to use: 0=OpenNI-PCL, 1=OpenNI2, 2=Freenect, 3=OpenNI-CV, 4=OpenNI-CV-ASUS, 5=Freenect2, 6=ZED SDK, 7=RealSense, 8=RealSense2 9=Kinect for Azure SDK 10=MYNT EYE S\n\n");
 	exit(1);
 }
 
@@ -184,6 +184,24 @@ int main(int argc, char * argv[])
 		}
 		camera = new CameraRealSense2("", 0, opticalRotation);
 	}
+	else if (driver == 9)
+	{
+		if (!rtabmap::CameraK4A::available())
+		{
+			UERROR("Not built with Kinect for Azure SDK support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraK4A(1);
+	}
+	else if (driver == 10)
+	{
+		if (!rtabmap::CameraMyntEye::available())
+		{
+			UERROR("Not built with Mynt Eye S support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraMyntEye();
+	}
 	else
 	{
 		camera = new rtabmap::CameraOpenni("", 0, opticalRotation);

guilib/include/rtabmap/gui/MainWindow.h

@@ -246,7 +246,7 @@ Q_SIGNALS:
 	void timeLimitChanged(float);
 	void mappingModeChanged(bool);
 	void noMoreImagesReceived();
-	void loopClosureThrChanged(float);
+	void loopClosureThrChanged(qreal);
 	void twistReceived(float x, float y, float z, float roll, float pitch, float yaw, int row, int col);
 
 private:

guilib/include/rtabmap/gui/StatsToolBox.h

@@ -46,14 +46,14 @@ class RTABMAPGUI_EXP StatItem : public QWidget
 	Q_OBJECT;
 
 public:
-	StatItem(const QString & name, bool cacheOn, const std::vector<float> & x, const std::vector<float> & y, const QString & unit = QString(), const QMenu * menu = 0, QGridLayout * grid = 0, QWidget * parent = 0);
+	StatItem(const QString & name, bool cacheOn, const std::vector<qreal> & x, const std::vector<qreal> & y, const QString & unit = QString(), const QMenu * menu = 0, QGridLayout * grid = 0, QWidget * parent = 0);
 	virtual ~StatItem();
-	void addValue(float y);
-	void addValue(float x, float y);
-	void setValues(const std::vector<float> & x, const std::vector<float> & y);
+	void addValue(qreal y);
+	void addValue(qreal x, qreal y);
+	void setValues(const std::vector<qreal> & x, const std::vector<qreal> & y);
 	QString value() const;
-	std::vector<float> xValues() const {return _x;}
-	std::vector<float> yValues() const {return _y;}
+	std::vector<qreal> xValues() const {return _x;}
+	std::vector<qreal> yValues() const {return _y;}
 	void setCacheOn(bool on);
 	void clearCache();
 
@@ -61,9 +61,9 @@ public Q_SLOTS:
 	void updateMenu(const QMenu * menu);
 
 Q_SIGNALS:
-	void valueAdded(float);
-	void valueAdded(float, float);
-	void valuesChanged(const std::vector<float> &, const std::vector<float> &);
+	void valueAdded(qreal);
+	void valueAdded(qreal, qreal);
+	void valuesChanged(const std::vector<qreal> &, const std::vector<qreal> &);
 	void plotRequested(const StatItem *, const QString &);
 
 private Q_SLOTS:
@@ -80,8 +80,8 @@ private:
 	QMenu * _menu;
 
 	bool _cacheOn;
-	std::vector<float> _x;
-	std::vector<float> _y;
+	std::vector<qreal> _x;
+	std::vector<qreal> _y;
 };
 
 
@@ -104,9 +104,9 @@ public:
 
 public Q_SLOTS:
 	void updateStat(const QString & statFullName, bool cacheOn);
-	void updateStat(const QString & statFullName, float y, bool cacheOn);
-	void updateStat(const QString & statFullName, float x, float y, bool cacheOn);
-	void updateStat(const QString & statFullName, const std::vector<float> & x, const std::vector<float> & y, bool cacheOn);
+	void updateStat(const QString & statFullName, qreal y, bool cacheOn);
+	void updateStat(const QString & statFullName, qreal x, qreal y, bool cacheOn);
+	void updateStat(const QString & statFullName, const std::vector<qreal> & x, const std::vector<qreal> & y, bool cacheOn);
 	void clear();
 
 Q_SIGNALS:

guilib/include/rtabmap/utilite/UPlot.h

@@ -105,7 +105,7 @@ public:
 	/**
 	 * Constructor 3
 	 */
-	UPlotCurve(const QString & name, const QVector<float> & x, const QVector<float> & y, QObject * parent = 0);
+	UPlotCurve(const QString & name, const QVector<qreal> & x, const QVector<qreal> & y, QObject * parent = 0);
 	virtual ~UPlotCurve();
 
 	/**
@@ -140,8 +140,8 @@ public:
 	QPointF getItemData(int index);
 	bool isVisible() const {return _visible;}
 	void setData(QVector<UPlotItem*> & data); // take the ownership
-	void getData(QVector<float> & x, QVector<float> & y) const; // only call in Qt MainThread
-	void getData(QMap<float,float> & data) const; // only call in Qt MainThread
+	void getData(QVector<qreal> & x, QVector<qreal> & y) const; // only call in Qt MainThread
+	void getData(QMap<qreal,qreal> & data) const; // only call in Qt MainThread
 	void draw(QPainter * painter, const QRect & limits);
 
 public Q_SLOTS:
@@ -159,12 +159,12 @@ public Q_SLOTS:
      *
 	 * Set increment of the x values (when auto-increment is used).
 	 */
-    void setXIncrement(float increment);
+    void setXIncrement(qreal increment);
     /**
      *
 	 * Set starting x value (when auto-increment is used).
 	 */
-    void setXStart(float val);
+    void setXStart(qreal val);
     /**
      *
 	 * Add a single value, using a custom UPlotItem.
@@ -175,12 +175,12 @@ public Q_SLOTS:
 	 * Add a single value y, x is auto-incremented by the increment set with setXIncrement().
 	 * @see setXStart()
 	 */
-	void addValue(float y);
+	void addValue(qreal y);
 	/**
 	 *
 	 * Add a single value y at x.
 	 */
-	void addValue(float x, float y);
+	void addValue(qreal x, qreal y);
 	/**
 	 *
 	 * For convenience...
@@ -198,26 +198,26 @@ public Q_SLOTS:
 	 *
 	 * Add multiple values y at x. Vectors must have the same size.
 	 */
-	void addValues(const QVector<float> & xs, const QVector<float> & ys);
+	void addValues(const QVector<qreal> & xs, const QVector<qreal> & ys);
 	/**
 	 *
 	 * Add multiple values y, x is auto-incremented by the increment set with setXIncrement().
 	 * @see setXStart()
 	 */
-	void addValues(const QVector<float> & ys);
+	void addValues(const QVector<qreal> & ys);
 	void addValues(const QVector<int> & ys); // for convenience
 	/**
 	 *
 	 * Add multiple values y, x is auto-incremented by the increment set with setXIncrement().
 	 * @see setXStart()
 	 */
-	void addValues(const std::vector<float> & ys); // for convenience
+	void addValues(const std::vector<qreal> & ys); // for convenience
 	void addValues(const std::vector<int> & ys); // for convenience
 
-	void setData(const QVector<float> & x, const QVector<float> & y);
-	void setData(const std::vector<float> & x, const std::vector<float> & y);
-	void setData(const QVector<float> & y);
-	void setData(const std::vector<float> & y);
+	void setData(const QVector<qreal> & x, const QVector<qreal> & y);
+	void setData(const std::vector<qreal> & x, const std::vector<qreal> & y);
+	void setData(const QVector<qreal> & y);
+	void setData(const std::vector<qreal> & y);
 
 Q_SIGNALS:
 	/**
@@ -231,11 +231,11 @@ protected:
 	void attach(UPlot * plot);
 	void detach(UPlot * plot);
 	void updateMinMax();
-	const QVector<float> & getMinMax() const {return _minMax;}
+	const QVector<qreal> & getMinMax() const {return _minMax;}
 	int removeItem(int index);
 	void _addValue(UPlotItem * data);;
 	virtual bool isMinMaxValid() const {return _minMax.size();}
-	virtual void update(float scaleX, float scaleY, float offsetX, float offsetY, float xDir, float yDir, int maxItemsKept);
+	virtual void update(qreal scaleX, qreal scaleY, qreal offsetX, qreal offsetY, qreal xDir, qreal yDir, int maxItemsKept);
 	QList<QGraphicsItem *> _items;
 	UPlot * _plot;
 
@@ -246,11 +246,11 @@ private:
 	QString _name;
 	QPen _pen;
 	QBrush _brush;
-	float _xIncrement;
-	float _xStart;
+	qreal _xIncrement;
+	qreal _xStart;
 	bool _visible;
 	bool _valuesShown;
-	QVector<float> _minMax; // minX, maxX, minY, maxY
+	QVector<qreal> _minMax; // minX, maxX, minY, maxY
 	QGraphicsRectItem * _rootItem;
 	QColor _itemsColor;
 };
@@ -267,14 +267,14 @@ public:
 	/**
 	 * Constructor.
 	 */
-	UPlotCurveThreshold(const QString & name, float thesholdValue, Qt::Orientation orientation = Qt::Horizontal, QObject * parent = 0);
+	UPlotCurveThreshold(const QString & name, qreal thesholdValue, Qt::Orientation orientation = Qt::Horizontal, QObject * parent = 0);
 	virtual ~UPlotCurveThreshold();
 
 public Q_SLOTS:
 	/**
 	 * Set threshold value.
 	 */
-	void setThreshold(float threshold);
+	void setThreshold(qreal threshold);
 	/**
 	 * Set orientation (Qt::Horizontal or Qt::Vertical).
 	 */
@@ -282,7 +282,7 @@ public Q_SLOTS:
 
 protected:
 	friend class UPlot;
-	virtual void update(float scaleX, float scaleY, float offsetX, float offsetY, float xDir, float yDir, int maxItemsKept);
+	virtual void update(qreal scaleX, qreal scaleY, qreal offsetX, qreal offsetY, qreal xDir, qreal yDir, int maxItemsKept);
 	virtual bool isMinMaxValid() const {return false;}
 
 private:
@@ -298,7 +298,7 @@ public:
 	/**
 	 * Constructor.
 	 */
-	UPlotAxis(Qt::Orientation orientation = Qt::Horizontal, float min=0, float max=1, QWidget * parent = 0);
+	UPlotAxis(Qt::Orientation orientation = Qt::Horizontal, qreal min=0, qreal max=1, QWidget * parent = 0);
 	virtual ~UPlotAxis();
 
 public:
@@ -306,7 +306,7 @@ public:
 	 * Set axis minimum and maximum values, compute the resulting
 	 * intervals depending on the size of the axis.
 	 */
-	void setAxis(float & min, float & max);
+	void setAxis(qreal & min, qreal & max);
 	/**
 	 * Size of the border between the first line and the beginning of the widget.
 	 */
@@ -329,8 +329,8 @@ protected:
 
 private:
 	Qt::Orientation _orientation;
-	float _min;
-	float _max;
+	qreal _min;
+	qreal _max;
 	int _count;
 	int _step;
 	bool _reversed;
@@ -472,8 +472,8 @@ private:
  *	QApplication app(argc, argv);
  *	UPlot plot;
  *	UPlotCurve * curve = plot.addCurve("My curve");
- *	float y[10] = {0, 1, 2, 3, -3, -2, -1, 0, 1, 2};
- *	curve->addValues(std::vector<float>(y, y+10));
+ *	qreal y[10] = {0, 1, 2, 3, -3, -2, -1, 0, 1, 2};
+ *	curve->addValues(std::vector<qreal>(y, y+10));
  *	plot.showGrid(true);
  *	plot.setGraphicsView(true);
  *	plot.show();
@@ -513,7 +513,7 @@ public:
 	/**
 	 * Add a threshold to the plot.
 	 */
-	UPlotCurveThreshold * addThreshold(const QString & name, float value, Qt::Orientation orientation = Qt::Horizontal);
+	UPlotCurveThreshold * addThreshold(const QString & name, qreal value, Qt::Orientation orientation = Qt::Horizontal);
 	QString title() const {return this->objectName();}
 	QPen getRandomPenColored();
 	void showLegend(bool shown);
@@ -525,8 +525,8 @@ public:
 	void showYAxis(bool shown) {_verticalAxis->setVisible(shown);}
 	void setVariableXAxis() {_fixedAxis[0] = false;}
 	void setVariableYAxis() {_fixedAxis[1] = false;}
-	void setFixedXAxis(float x1, float x2);
-	void setFixedYAxis(float y1, float y2);
+	void setFixedXAxis(qreal x1, qreal x2);
+	void setFixedYAxis(qreal y1, qreal y2);
 	void setMaxVisibleItems(int maxVisibleItems);
 	void setTitle(const QString & text);
 	void setXLabel(const QString & text);
@@ -550,6 +550,8 @@ public Q_SLOTS:
 	 */
 	void clearData();
 
+	void frameData(bool xAxis = true, bool yAxis = false);
+
 private Q_SLOTS:
 	void captureScreen();
 	void updateAxis(const UPlotCurve * curve);
@@ -570,20 +572,20 @@ private:
 
 private:
 	void replot(QPainter * painter);
-	bool updateAxis(float x, float y);
-	bool updateAxis(float x1, float x2, float y1, float y2);
+	bool updateAxis(qreal x, qreal y);
+	bool updateAxis(qreal x1, qreal x2, qreal y1, qreal y2);
 	void setupUi();
 	void createActions();
 	void createMenus();
 	void selectScreenCaptureFormat();
-	bool mousePosToValue(const QPoint & pos, float & x, float & y);
+	bool mousePosToValue(const QPoint & pos, qreal & x, qreal & y);
 
 private:
 	UPlotLegend * _legend;
 	QGraphicsView * _view;
 	QGraphicsItem * _sceneRoot;
 	QWidget * _graphicsViewHolder;
-	float _axisMaximums[4]; // {x1->x2, y1->y2}
+	qreal _axisMaximums[4]; // {x1->x2, y1->y2}
 	bool _axisMaximumsSet[4]; // {x1->x2, y1->y2}
 	bool _fixedAxis[2];
 	UPlotAxis * _verticalAxis;

guilib/src/CMakeLists.txt

@@ -139,6 +139,13 @@ IF(realsense2_FOUND)
 		${realsense2_INCLUDE_DIRS}
 	)
 ENDIF(realsense2_FOUND)
+# Hack as CameraK4A.h needs k4a include dir
+IF(k4a_FOUND)
+	SET(INCLUDE_DIRS
+		${INCLUDE_DIRS}
+		${k4a_INCLUDE_DIRS}
+	)
+ENDIF(k4a_FOUND)
 
 IF(QT4_FOUND)
     INCLUDE(${QT_USE_FILE})

guilib/src/CloudViewer.cpp

@@ -138,7 +138,7 @@ CloudViewer::CloudViewer(QWidget *parent, CloudViewerInteractorStyle * style) :
 	int argc = 0;
 	UASSERT(style!=0);
 	style->setCloudViewer(this);
-	style->SetAutoAdjustCameraClippingRange(true);
+	style->AutoAdjustCameraClippingRangeOff();
 	_visualizer = new pcl::visualization::PCLVisualizer(
 		argc, 
 		0, 
@@ -192,10 +192,10 @@ CloudViewer::CloudViewer(QWidget *parent, CloudViewerInteractorStyle * style) :
 
 	setRenderingRate(_renderingRate);
 
-	_visualizer->setCameraPosition(
+	this->setCameraPosition(
 				-1, 0, 0,
 				0, 0, 0,
-				0, 0, 1, 1);
+				0, 0, 1);
 #ifndef _WIN32
 	// Crash on startup on Windows (vtk issue)
 	this->addOrUpdateCoordinate("reference", Transform::getIdentity(), 0.2);
@@ -286,14 +286,12 @@ void CloudViewer::createMenu()
 	_aSetEDLShading = new QAction("Eye-Dome Lighting Shading", this);
 	_aSetEDLShading->setCheckable(true);
 	_aSetEDLShading->setChecked(false);
-
+#if VTK_MAJOR_VERSION < 7
+	_aSetEDLShading->setEnabled(false);
+#endif
 	_aSetLighting = new QAction("Lighting", this);
 	_aSetLighting->setCheckable(true);
 	_aSetLighting->setChecked(false);
-#if VTK_MAJOR_VERSION < 7
-	_aSetLighting->setEnabled(false);
-#endif
-
 	_aSetFlatShading = new QAction("Flat Shading", this);
 	_aSetFlatShading->setCheckable(true);
 	_aSetFlatShading->setChecked(false);
@@ -417,7 +415,6 @@ void CloudViewer::saveSettings(QSettings & settings, const QString & group) cons
 	settings.setValue("camera_target_follow", this->isCameraTargetFollow());
 	settings.setValue("camera_free", this->isCameraFree());
 	settings.setValue("camera_lockZ", this->isCameraLockZ());
-	settings.setValue("camera_ortho", this->isCameraOrtho());
 
 	settings.setValue("bg_color", this->getDefaultBackgroundColor());
 	settings.setValue("rendering_rate", this->getRenderingRate());
@@ -440,6 +437,7 @@ void CloudViewer::loadSettings(QSettings & settings, const QString & group)
 	pose = settings.value("camera_pose", pose).value<QVector3D>();
 	focal = settings.value("camera_focal", focal).value<QVector3D>();
 	up = settings.value("camera_up", up).value<QVector3D>();
+	_lastCameraOrientation= _lastCameraPose= cv::Vec3f(0,0,0);
 	this->setCameraPosition(pose.x(),pose.y(),pose.z(), focal.x(),focal.y(),focal.z(), up.x(),up.y(),up.z());
 
 	this->setGridShown(settings.value("grid", this->isGridShown()).toBool());
@@ -469,7 +467,6 @@ void CloudViewer::loadSettings(QSettings & settings, const QString & group)
 		this->setCameraFree();
 	}
 	this->setCameraLockZ(settings.value("camera_lockZ", this->isCameraLockZ()).toBool());
-	this->setCameraOrtho(settings.value("camera_ortho", this->isCameraOrtho()).toBool());
 
 	this->setDefaultBackgroundColor(settings.value("bg_color", this->getDefaultBackgroundColor()).value<QColor>());
 	
@@ -492,20 +489,43 @@ bool CloudViewer::updateCloudPose(
 		//UDEBUG("Updating pose %s to %s", id.c_str(), pose.prettyPrint().c_str());
 		bool samePose = _addedClouds.find(id).value() == pose;
 		Eigen::Affine3f posef = pose.toEigen3f();
-		if(samePose ||
-		   _visualizer->updatePointCloudPose(id, posef))
+		if(!samePose)
 		{
-			_addedClouds.find(id).value() = pose;
-			if(!samePose)
+			// PointCloud / Mesh
+			bool updated = _visualizer->updatePointCloudPose(id, posef);
+
+#if VTK_MAJOR_VERSION >= 7
+			if(!updated)
 			{
+				// TextureMesh, cannot use updateShapePose because it searches for vtkLODActor, not a vtkActor
+				pcl::visualization::ShapeActorMap::iterator am_it = _visualizer->getShapeActorMap()->find (id);
+				vtkActor* actor;
+				if (am_it != _visualizer->getShapeActorMap()->end ())
+				{
+					actor = vtkActor::SafeDownCast (am_it->second);
+					if (actor)
+					{
+						vtkSmartPointer<vtkMatrix4x4> matrix = vtkSmartPointer<vtkMatrix4x4>::New ();
+						pcl::visualization::PCLVisualizer::convertToVtkMatrix (pose.toEigen3f().matrix (), matrix);
+						actor->SetUserMatrix (matrix);
+						actor->Modified ();
+						updated = true;
+					}
+				}
+			}
+#endif
+
+			if(updated)
+			{
+				_addedClouds.find(id).value() = pose;
 				std::string idNormals = id+"-normals";
 				if(_addedClouds.find(idNormals)!=_addedClouds.end())
 				{
 					_visualizer->updatePointCloudPose(idNormals, posef);
 					_addedClouds.find(idNormals).value() = pose;
 				}
+				return true;
 			}
-			return true;
 		}
 	}
 	return false;
@@ -834,7 +854,11 @@ bool CloudViewer::addCloudMesh(
 	UDEBUG("Adding %s with %d points and %d polygons", id.c_str(), (int)cloud->size(), (int)polygons.size());
 	if(_visualizer->addPolygonMesh<pcl::PointXYZ>(cloud, polygons, id, 1))
 	{
+#if VTK_MAJOR_VERSION >= 7
+		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetAmbient(0.1);
+#else
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetAmbient(0.5);
+#endif
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetLighting(_aSetLighting->isChecked());
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetInterpolation(_aSetFlatShading->isChecked()?VTK_FLAT:VTK_PHONG);
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked());
@@ -868,7 +892,11 @@ bool CloudViewer::addCloudMesh(
 	UDEBUG("Adding %s with %d points and %d polygons", id.c_str(), (int)cloud->size(), (int)polygons.size());
 	if(_visualizer->addPolygonMesh<pcl::PointXYZRGB>(cloud, polygons, id, 1))
 	{
+#if VTK_MAJOR_VERSION >= 7
+		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetAmbient(0.1);
+#else
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetAmbient(0.5);
+#endif
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetLighting(_aSetLighting->isChecked());
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetInterpolation(_aSetFlatShading->isChecked()?VTK_FLAT:VTK_PHONG);
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked());
@@ -902,7 +930,11 @@ bool CloudViewer::addCloudMesh(
 	UDEBUG("Adding %s with %d points and %d polygons", id.c_str(), (int)cloud->size(), (int)polygons.size());
 	if(_visualizer->addPolygonMesh<pcl::PointXYZRGBNormal>(cloud, polygons, id, 1))
 	{
+#if VTK_MAJOR_VERSION >= 7
+		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetAmbient(0.1);
+#else
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetAmbient(0.5);
+#endif
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetLighting(_aSetLighting->isChecked());
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetInterpolation(_aSetFlatShading->isChecked()?VTK_FLAT:VTK_PHONG);
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked());
@@ -935,7 +967,11 @@ bool CloudViewer::addCloudMesh(
 	UDEBUG("Adding %s with %d polygons", id.c_str(), (int)mesh->polygons.size());
 	if(_visualizer->addPolygonMesh(*mesh, id, 1))
 	{
+#if VTK_MAJOR_VERSION >= 7
+		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetAmbient(0.1);
+#else
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetAmbient(0.5);
+#endif
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetLighting(_aSetLighting->isChecked());
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked());
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetBackfaceCulling(_aBackfaceCulling->isChecked());
@@ -969,25 +1005,26 @@ bool CloudViewer::addCloudTextureMesh(
 	UDEBUG("Adding %s", id.c_str());
 	if(this->addTextureMesh(*textureMesh, texture, id, 1))
 	{
-		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetLighting(_aSetLighting->isChecked());
-		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetInterpolation(_aSetFlatShading->isChecked()?VTK_FLAT:VTK_PHONG);
-		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked());
-		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetBackfaceCulling(_aBackfaceCulling->isChecked());
-		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetFrontfaceCulling(_frontfaceCulling);
+#if VTK_MAJOR_VERSION >= 7
+		vtkActor* actor = vtkActor::SafeDownCast (_visualizer->getShapeActorMap()->find(id)->second);
+#else
+		vtkActor* actor = vtkActor::SafeDownCast (_visualizer->getCloudActorMap()->find(id)->second.actor);
+#endif
+		UASSERT(actor);
 		if(!textureMesh->cloud.is_dense)
 		{
-			_visualizer->getCloudActorMap()->find(id)->second.actor->GetTexture()->SetInterpolate(1);
-			_visualizer->getCloudActorMap()->find(id)->second.actor->GetTexture()->SetBlendingMode(vtkTexture::VTK_TEXTURE_BLENDING_MODE_REPLACE);
+			actor->GetTexture()->SetInterpolate(1);
+			actor->GetTexture()->SetBlendingMode(vtkTexture::VTK_TEXTURE_BLENDING_MODE_REPLACE);
 		}
-		_visualizer->updatePointCloudPose(id, pose.toEigen3f());
 		if(_buildLocator)
 		{
 			vtkSmartPointer<vtkOBBTree> tree = vtkSmartPointer<vtkOBBTree>::New();
-			tree->SetDataSet(_visualizer->getCloudActorMap()->find(id)->second.actor->GetMapper()->GetInput());
+			tree->SetDataSet(actor->GetMapper()->GetInput());
 			tree->BuildLocator();
 			_locators.insert(std::make_pair(id, tree));
 		}
-		_addedClouds.insert(id, pose);
+		_addedClouds.insert(id, Transform::getIdentity());
+		this->updateCloudPose(id, pose);
 		return true;
 	}
 	return false;
@@ -1226,8 +1263,13 @@ bool CloudViewer::addTextureMesh (
 {
 	// Copied from PCL 1.8, modified to ignore vertex color and accept only one material (loaded from memory instead of file)
 
+#if VTK_MAJOR_VERSION >= 7
+  pcl::visualization::ShapeActorMap::iterator am_it = _visualizer->getShapeActorMap()->find (id);
+  if (am_it != _visualizer->getShapeActorMap()->end ())
+#else
   pcl::visualization::CloudActorMap::iterator am_it = _visualizer->getCloudActorMap()->find (id);
   if (am_it != _visualizer->getCloudActorMap()->end ())
+#endif
   {
     PCL_ERROR ("[PCLVisualizer::addTextureMesh] A shape with id <%s> already exists!"
                " Please choose a different id and retry.\n",
@@ -1328,7 +1370,11 @@ bool CloudViewer::addTextureMesh (
     mapper->SetInputData (polydata);
 #endif
 
+#if VTK_MAJOR_VERSION >= 7
+  vtkSmartPointer<vtkActor> actor = vtkSmartPointer<vtkActor>::New ();
+#else
   vtkSmartPointer<vtkLODActor> actor = vtkSmartPointer<vtkLODActor>::New ();
+#endif
   vtkTextureUnitManager* tex_manager = vtkOpenGLRenderWindow::SafeDownCast (_visualizer->getRenderWindow())->GetTextureUnitManager ();
     if (!tex_manager)
       return (false);
@@ -1377,17 +1423,24 @@ bool CloudViewer::addTextureMesh (
 	}
 
   // Save the pointer/ID pair to the global actor map
+#if VTK_MAJOR_VERSION >= 7
+  (*_visualizer->getShapeActorMap())[id] = actor;
+#else
   (*_visualizer->getCloudActorMap())[id].actor = actor;
-
-  // Save the viewpoint transformation matrix to the global actor map
+   // Save the viewpoint transformation matrix to the global actor map
   (*_visualizer->getCloudActorMap())[id].viewpoint_transformation_ = transformation;
-
-  _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetAmbient(0.5);
-  _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetLighting(_aSetLighting->isChecked());
-  _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetInterpolation(_aSetFlatShading->isChecked()?VTK_FLAT:VTK_PHONG);
-  _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked());
-  _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetBackfaceCulling(_aBackfaceCulling->isChecked());
-  _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetFrontfaceCulling(_frontfaceCulling);
+#endif
+
+#if VTK_MAJOR_VERSION >= 7
+  actor->GetProperty()->SetAmbient(0.1);
+#else
+  actor->GetProperty()->SetAmbient(0.5);
+#endif
+  actor->GetProperty()->SetLighting(_aSetLighting->isChecked());
+  actor->GetProperty()->SetInterpolation(_aSetFlatShading->isChecked()?VTK_FLAT:VTK_PHONG);
+  actor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked());
+  actor->GetProperty()->SetBackfaceCulling(_aBackfaceCulling->isChecked());
+  actor->GetProperty()->SetFrontfaceCulling(_frontfaceCulling);
   return true;
 }
 
@@ -1404,10 +1457,17 @@ bool CloudViewer::addOccupancyGridMap(
 	float ySize = float(map8U.rows) * resolution;
 
 	UDEBUG("resolution=%f, xSize=%f, ySize=%f, xMin=%f, yMin=%f", resolution, xSize, ySize, xMin, yMin);
+#if VTK_MAJOR_VERSION >= 7
+	if(_visualizer->getShapeActorMap()->find("map") != _visualizer->getShapeActorMap()->end())
+	{
+		_visualizer->removeShape("map");
+	}
+#else
 	if(_visualizer->getCloudActorMap()->find("map") != _visualizer->getCloudActorMap()->end())
 	{
 		_visualizer->removePointCloud("map");
 	}
+#endif
 
 	if(xSize > 0.0f && ySize > 0.0f)
 	{
@@ -1451,10 +1511,17 @@ bool CloudViewer::addOccupancyGridMap(
 
 void CloudViewer::removeOccupancyGridMap()
 {
+#if VTK_MAJOR_VERSION >= 7
+	if(_visualizer->getShapeActorMap()->find("map") != _visualizer->getShapeActorMap()->end())
+	{
+		_visualizer->removeShape("map");
+	}
+#else
 	if(_visualizer->getCloudActorMap()->find("map") != _visualizer->getCloudActorMap()->end())
 	{
 		_visualizer->removePointCloud("map");
 	}
+#endif
 }
 
 void CloudViewer::addOrUpdateCoordinate(
@@ -2251,54 +2318,64 @@ void CloudViewer::resetCamera()
 		cv::Point3f pt = util3d::transformPoint(cv::Point3f(_lastPose.x(), _lastPose.y(), _lastPose.z()), ( _lastPose.rotation()*Transform(-1, 0, 0)).translation());
 		if(_aCameraOrtho->isChecked())
 		{
-			_visualizer->setCameraPosition(
+			this->setCameraPosition(
 					_lastPose.x(), _lastPose.y(), _lastPose.z()+5,
 					_lastPose.x(), _lastPose.y(), _lastPose.z(),
-					1, 0, 0, 1);
+					1, 0, 0);
 		}
 		else if(_aLockViewZ->isChecked())
 		{
-			_visualizer->setCameraPosition(
+			this->setCameraPosition(
 					pt.x, pt.y, pt.z,
 					_lastPose.x(), _lastPose.y(), _lastPose.z(),
-					0, 0, 1, 1);
+					0, 0, 1);
 		}
 		else
 		{
-			_visualizer->setCameraPosition(
+			this->setCameraPosition(
 					pt.x, pt.y, pt.z,
 					_lastPose.x(), _lastPose.y(), _lastPose.z(),
-					_lastPose.r31(), _lastPose.r32(), _lastPose.r33(), 1);
+					_lastPose.r31(), _lastPose.r32(), _lastPose.r33());
 		}
 	}
 	else if(_aCameraOrtho->isChecked())
 	{
-		_visualizer->setCameraPosition(
+		this->setCameraPosition(
 				0, 0, 5,
 				0, 0, 0,
-				1, 0, 0, 1);
+				1, 0, 0);
 	}
 	else
 	{
-		_visualizer->setCameraPosition(
+		this->setCameraPosition(
 				-1, 0, 0,
 				0, 0, 0,
-				0, 0, 1, 1);
+				0, 0, 1);
 	}
-	this->update();
 }
 
 void CloudViewer::removeAllClouds()
 {
-	_addedClouds.clear();
-	_locators.clear();
-	_visualizer->removeAllPointClouds();
+	QMap<std::string, Transform> addedClouds = _addedClouds;
+	QList<std::string> ids = _addedClouds.keys();
+	for(QList<std::string>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
+	{
+		removeCloud(*iter);
+	}
+	UASSERT(_addedClouds.empty());
+	UASSERT(_locators.empty());
 }
 
 
 bool CloudViewer::removeCloud(const std::string & id)
 {
 	bool success = _visualizer->removePointCloud(id);
+#if VTK_MAJOR_VERSION >= 7
+	if(!success)
+	{
+		success = _visualizer->removeShape(id);
+	}
+#endif
 	_visualizer->removePointCloud(id+"-normals");
 	_addedClouds.remove(id); // remove after visualizer
 	_addedClouds.remove(id+"-normals");
@@ -2445,6 +2522,18 @@ void CloudViewer::setBackfaceCulling(bool enabled, bool frontfaceCulling)
 		iter->second.actor->GetProperty()->SetBackfaceCulling(_aBackfaceCulling->isChecked());
 		iter->second.actor->GetProperty()->SetFrontfaceCulling(_frontfaceCulling);
 	}
+#if VTK_MAJOR_VERSION >= 7
+	pcl::visualization::ShapeActorMapPtr shapeActorMap = _visualizer->getShapeActorMap();
+	for(pcl::visualization::ShapeActorMap::iterator iter=shapeActorMap->begin(); iter!=shapeActorMap->end(); ++iter)
+	{
+		vtkActor* actor = vtkActor::SafeDownCast (iter->second);
+		if(actor)
+		{
+			actor->GetProperty()->SetBackfaceCulling(_aBackfaceCulling->isChecked());
+			actor->GetProperty()->SetFrontfaceCulling(_frontfaceCulling);
+		}
+	}
+#endif
 	this->update();
 }
 
@@ -2519,6 +2608,17 @@ void CloudViewer::setLighting(bool on)
 	{
 		iter->second.actor->GetProperty()->SetLighting(_aSetLighting->isChecked());
 	}
+#if VTK_MAJOR_VERSION >= 7
+	pcl::visualization::ShapeActorMapPtr shapeActorMap = _visualizer->getShapeActorMap();
+	for(pcl::visualization::ShapeActorMap::iterator iter=shapeActorMap->begin(); iter!=shapeActorMap->end(); ++iter)
+	{
+		vtkActor* actor = vtkActor::SafeDownCast (iter->second);
+		if(actor && _addedClouds.contains(iter->first))
+		{
+			actor->GetProperty()->SetLighting(_aSetLighting->isChecked());
+		}
+	}
+#endif
 	this->update();
 }
 
@@ -2530,6 +2630,17 @@ void CloudViewer::setShading(bool on)
 	{
 		iter->second.actor->GetProperty()->SetInterpolation(_aSetFlatShading->isChecked()?VTK_FLAT:VTK_PHONG); // VTK_FLAT - VTK_GOURAUD - VTK_PHONG
 	}
+#if VTK_MAJOR_VERSION >= 7
+	pcl::visualization::ShapeActorMapPtr shapeActorMap = _visualizer->getShapeActorMap();
+	for(pcl::visualization::ShapeActorMap::iterator iter=shapeActorMap->begin(); iter!=shapeActorMap->end(); ++iter)
+	{
+		vtkActor* actor = vtkActor::SafeDownCast (iter->second);
+		if(actor && _addedClouds.contains(iter->first))
+		{
+			actor->GetProperty()->SetInterpolation(_aSetFlatShading->isChecked()?VTK_FLAT:VTK_PHONG); // VTK_FLAT - VTK_GOURAUD - VTK_PHONG
+		}
+	}
+#endif
 	this->update();
 }
 
@@ -2541,6 +2652,17 @@ void CloudViewer::setEdgeVisibility(bool visible)
 	{
 		iter->second.actor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked());
 	}
+#if VTK_MAJOR_VERSION >= 7
+	pcl::visualization::ShapeActorMapPtr shapeActorMap = _visualizer->getShapeActorMap();
+	for(pcl::visualization::ShapeActorMap::iterator iter=shapeActorMap->begin(); iter!=shapeActorMap->end(); ++iter)
+	{
+		vtkActor* actor = vtkActor::SafeDownCast (iter->second);
+		if(actor && _addedClouds.contains(iter->first))
+		{
+			actor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked());
+		}
+	}
+#endif
 	this->update();
 }
 
@@ -2592,8 +2714,42 @@ void CloudViewer::setCameraPosition(
 		float focalX, float focalY, float focalZ,
 		float upX, float upY, float upZ)
 {
-	_lastCameraOrientation= _lastCameraPose= cv::Vec3f(0,0,0);
-	_visualizer->setCameraPosition(x,y,z, focalX,focalY,focalX, upX,upY,upZ, 1);
+	vtkRenderer* renderer = NULL;
+	double boundingBox[6] = {1, -1, 1, -1, 1, -1};
+
+	// compute global bounding box
+	_visualizer->getRendererCollection()->InitTraversal ();
+	while ((renderer = _visualizer->getRendererCollection()->GetNextItem ()) != NULL)
+	{
+		vtkSmartPointer<vtkCamera> cam = renderer->GetActiveCamera ();
+		cam->SetPosition (x, y, z);
+		cam->SetFocalPoint (focalX, focalY, focalZ);
+		cam->SetViewUp (upX, upY, upZ);
+
+		double BB[6];
+		renderer->ComputeVisiblePropBounds(BB);
+		for (int i = 0; i < 6; i++) {
+			if (i % 2 == 0) {
+				// Even Index is Min
+				if (BB[i] < boundingBox[i]) {
+					boundingBox[i] = BB[i];
+				}
+			} else {
+				// Odd Index is Max
+				if (BB[i] > boundingBox[i]) {
+					boundingBox[i] = BB[i];
+				}
+			}
+		}
+	}
+
+	_visualizer->getRendererCollection()->InitTraversal ();
+	while ((renderer = _visualizer->getRendererCollection()->GetNextItem ()) != NULL)
+	{
+		renderer->ResetCameraClippingRange(boundingBox);
+	}
+
+	_visualizer->getRenderWindow()->Render ();
 }
 
 void CloudViewer::updateCameraTargetPosition(const Transform & pose)
@@ -2710,10 +2866,10 @@ void CloudViewer::updateCameraTargetPosition(const Transform & pose)
 				}
 			}
 
-			_visualizer->setCameraPosition(
+			this->setCameraPosition(
 				cameras.front().pos[0], cameras.front().pos[1], cameras.front().pos[2],
 				cameras.front().focal[0], cameras.front().focal[1], cameras.front().focal[2],
-				cameras.front().view[0], cameras.front().view[1], cameras.front().view[2], 1);
+				cameras.front().view[0], cameras.front().view[1], cameras.front().view[2]);
 		}
 	}
 
@@ -2807,6 +2963,19 @@ void CloudViewer::setCloudVisibility(const std::string & id, bool isVisible)
 	}
 	else
 	{
+#if VTK_MAJOR_VERSION >= 7
+		pcl::visualization::ShapeActorMapPtr shapeActorMap = _visualizer->getShapeActorMap();
+		pcl::visualization::ShapeActorMap::iterator iter = shapeActorMap->find(id);
+		if(iter != shapeActorMap->end())
+		{
+			vtkActor* actor = vtkActor::SafeDownCast (iter->second);
+			if(actor)
+			{
+				actor->SetVisibility(isVisible?1:0);
+				return;
+			}
+		}
+#endif
 		UERROR("Cannot find actor named \"%s\".", id.c_str());
 	}
 }
@@ -2821,6 +2990,18 @@ bool CloudViewer::getCloudVisibility(const std::string & id)
 	}
 	else
 	{
+#if VTK_MAJOR_VERSION >= 7
+		pcl::visualization::ShapeActorMapPtr shapeActorMap = _visualizer->getShapeActorMap();
+		pcl::visualization::ShapeActorMap::iterator iter = shapeActorMap->find(id);
+		if(iter != shapeActorMap->end())
+		{
+			vtkActor* actor = vtkActor::SafeDownCast (iter->second);
+			if(actor)
+			{
+				return actor->GetVisibility() != 0;
+			}
+		}
+#endif
 		UERROR("Cannot find actor named \"%s\".", id.c_str());
 	}
 	return false;
@@ -2837,20 +3018,39 @@ void CloudViewer::setCloudColorIndex(const std::string & id, int index)
 void CloudViewer::setCloudOpacity(const std::string & id, double opacity)
 {
 	double lastOpacity;
-	_visualizer->getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, lastOpacity, id);
-	if(lastOpacity != opacity)
+	if(_visualizer->getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, lastOpacity, id))
 	{
-		_visualizer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, id);
+		if(lastOpacity != opacity)
+		{
+			_visualizer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, id);
+		}
 	}
+#if VTK_MAJOR_VERSION >= 7
+	else
+	{
+		pcl::visualization::ShapeActorMap::iterator am_it = _visualizer->getShapeActorMap()->find (id);
+		if (am_it != _visualizer->getShapeActorMap()->end ())
+		{
+			vtkActor* actor = vtkActor::SafeDownCast (am_it->second);
+			if(actor)
+			{
+				actor->GetProperty ()->SetOpacity (opacity);
+				actor->Modified ();
+			}
+		}
+	}
+#endif
 }
 
 void CloudViewer::setCloudPointSize(const std::string & id, int size)
 {
 	double lastSize;
-	_visualizer->getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, lastSize, id);
-	if((int)lastSize != size)
+	if(_visualizer->getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, lastSize, id))
 	{
-		_visualizer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, (double)size, id);
+		if((int)lastSize != size)
+		{
+			_visualizer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, (double)size, id);
+		}
 	}
 }
 
@@ -3025,6 +3225,13 @@ void CloudViewer::addGrid()
 					r, g, b, name, 2);
 			_gridLines.push_back(name);
 		}
+		// this will update clipping planes
+		std::vector<pcl::visualization::Camera> cameras;
+		_visualizer->getCameras(cameras);
+		this->setCameraPosition(
+				cameras.front().pos[0], cameras.front().pos[1], cameras.front().pos[2],
+				cameras.front().focal[0], cameras.front().focal[1], cameras.front().focal[2],
+				cameras.front().view[0], cameras.front().view[1], cameras.front().view[2]);
 	}
 }
 
@@ -3249,12 +3456,10 @@ void CloudViewer::keyPressEvent(QKeyEvent * event)
 		cameras.front().focal[0] += cummulatedDir[0] + cummulatedFocalDir[0];
 		cameras.front().focal[1] += cummulatedDir[1] + cummulatedFocalDir[1];
 		cameras.front().focal[2] += cummulatedDir[2] + cummulatedFocalDir[2];
-		_visualizer->setCameraPosition(
+		this->setCameraPosition(
 			cameras.front().pos[0], cameras.front().pos[1], cameras.front().pos[2],
 			cameras.front().focal[0], cameras.front().focal[1], cameras.front().focal[2],
-			cameras.front().view[0], cameras.front().view[1], cameras.front().view[2], 1);
-
-		update();
+			cameras.front().view[0], cameras.front().view[1], cameras.front().view[2]);
 
 		Q_EMIT configChanged();
 	}
@@ -3280,12 +3485,12 @@ void CloudViewer::mouseMoveEvent(QMouseEvent * event)
 {
 	QVTKWidget::mouseMoveEvent(event);
 
+	std::vector<pcl::visualization::Camera> cameras;
+	_visualizer->getCameras(cameras);
+
 	// camera view up z locked?
 	if(_aLockViewZ->isChecked() && !_aCameraOrtho->isChecked())
 	{
-		std::vector<pcl::visualization::Camera> cameras;
-		_visualizer->getCameras(cameras);
-
 		cv::Vec3d newCameraOrientation = cv::Vec3d(0,0,1).cross(cv::Vec3d(cameras.front().pos)-cv::Vec3d(cameras.front().focal));
 
 		if(	_lastCameraOrientation!=cv::Vec3d(0,0,0) &&
@@ -3317,14 +3522,12 @@ void CloudViewer::mouseMoveEvent(QMouseEvent * event)
 		cameras.front().view[0] = 0;
 		cameras.front().view[1] = 0;
 		cameras.front().view[2] = 1;
+	}
 
-		_visualizer->setCameraPosition(
+	this->setCameraPosition(
 			cameras.front().pos[0], cameras.front().pos[1], cameras.front().pos[2],
 			cameras.front().focal[0], cameras.front().focal[1], cameras.front().focal[2],
-			cameras.front().view[0], cameras.front().view[1], cameras.front().view[2], 1);
-
-	}
-	this->update();
+			cameras.front().view[0], cameras.front().view[1], cameras.front().view[2]);
 
 	Q_EMIT configChanged();
 }
@@ -3332,12 +3535,20 @@ void CloudViewer::mouseMoveEvent(QMouseEvent * event)
 void CloudViewer::wheelEvent(QWheelEvent * event)
 {
 	QVTKWidget::wheelEvent(event);
+
+	std::vector<pcl::visualization::Camera> cameras;
+	_visualizer->getCameras(cameras);
+
 	if(_aLockViewZ->isChecked() && !_aCameraOrtho->isChecked())
 	{
-		std::vector<pcl::visualization::Camera> cameras;
-		_visualizer->getCameras(cameras);
 		_lastCameraPose = cv::Vec3d(cameras.front().pos);
 	}
+
+	this->setCameraPosition(
+		cameras.front().pos[0], cameras.front().pos[1], cameras.front().pos[2],
+		cameras.front().focal[0], cameras.front().focal[1], cameras.front().focal[2],
+		cameras.front().view[0], cameras.front().view[1], cameras.front().view[2]);
+
 	Q_EMIT configChanged();
 }
 

guilib/src/CloudViewerInteractorStyle.cpp

@@ -118,76 +118,79 @@ void CloudViewerInteractorStyle::OnMouseMove()
 		{
 			int pickPosition[2];
 			this->GetInteractor()->GetEventPosition(pickPosition);
-			this->Interactor->GetPicker()->Pick(pickPosition[0], pickPosition[1],
+			int result = this->Interactor->GetPicker()->Pick(pickPosition[0], pickPosition[1],
 					0,  // always zero.
 					this->CurrentRenderer);
-			double picked[3];
-			this->Interactor->GetPicker()->GetPickPosition(picked);
-
-			UDEBUG("Control move! Picked value: %f %f %f", picked[0], picked[1], picked[2]);
-
-			float textSize = 0.05;
-
-			viewer_->removeCloud("interactor_points_alt");
-			pointsHolder_->resize(2);
-			pcl::PointXYZRGB pt;
-			pt.r = 255;
-			pt.x = picked[0];
-			pt.y = picked[1];
-			pt.z = picked[2];
-			pointsHolder_->at(0) = pt;
-
-			viewer_->removeLine("interactor_ray_alt");
-			viewer_->removeText("interactor_ray_text_alt");
-
-			// Intersect the locator with the line
-			double length = 5.0;
-			double pickedNormal[3];
-			cellPicker->GetPickNormal(pickedNormal);
-			double lineP0[3] = {picked[0], picked[1], picked[2]};
-			double lineP1[3] = {picked[0]+pickedNormal[0]*length, picked[1]+pickedNormal[1]*length, picked[2]+pickedNormal[2]*length};
-			vtkSmartPointer<vtkPoints> intersectPoints = vtkSmartPointer<vtkPoints>::New();
-
-			viewer_->getLocators().begin()->second->IntersectWithLine(lineP0, lineP1, intersectPoints, NULL);
-
-			// Display list of intersections
-			double intersection[3];
-			double previous[3] = {picked[0], picked[1], picked[2]};
-			for(int i = 0; i < intersectPoints->GetNumberOfPoints(); i++ )
+			if(result)
 			{
-				intersectPoints->GetPoint(i, intersection);
+				double picked[3];
+				this->Interactor->GetPicker()->GetPickPosition(picked);
 
-				Eigen::Vector3f v(intersection[0]-previous[0], intersection[1]-previous[1], intersection[2]-previous[2]);
-				float n = v.norm();
-				if(n  > 0.01f)
+				UDEBUG("Control move! Picked value: %f %f %f", picked[0], picked[1], picked[2]);
+
+				float textSize = 0.05;
+
+				viewer_->removeCloud("interactor_points_alt");
+				pointsHolder_->resize(2);
+				pcl::PointXYZRGB pt;
+				pt.r = 255;
+				pt.x = picked[0];
+				pt.y = picked[1];
+				pt.z = picked[2];
+				pointsHolder_->at(0) = pt;
+
+				viewer_->removeLine("interactor_ray_alt");
+				viewer_->removeText("interactor_ray_text_alt");
+
+				// Intersect the locator with the line
+				double length = 5.0;
+				double pickedNormal[3];
+				cellPicker->GetPickNormal(pickedNormal);
+				double lineP0[3] = {picked[0], picked[1], picked[2]};
+				double lineP1[3] = {picked[0]+pickedNormal[0]*length, picked[1]+pickedNormal[1]*length, picked[2]+pickedNormal[2]*length};
+				vtkSmartPointer<vtkPoints> intersectPoints = vtkSmartPointer<vtkPoints>::New();
+
+				viewer_->getLocators().begin()->second->IntersectWithLine(lineP0, lineP1, intersectPoints, NULL);
+
+				// Display list of intersections
+				double intersection[3];
+				double previous[3] = {picked[0], picked[1], picked[2]};
+				for(int i = 0; i < intersectPoints->GetNumberOfPoints(); i++ )
 				{
-					v/=n;
-					v *= n/2.0f;
-					pt.r = 125;
-					pt.g = 125;
-					pt.b = 125;
-					pt.x = intersection[0];
-					pt.y = intersection[1];
-					pt.z = intersection[2];
-					pointsHolder_->at(1) = pt;
-					viewer_->addOrUpdateText("interactor_ray_text_alt", uFormat("%.2f m", n),
-							Transform(previous[0]+v[0], previous[1]+v[1],previous[2]+v[2], 0, 0, 0),
-							textSize,
-							Qt::gray);
-					viewer_->addOrUpdateLine("interactor_ray_alt",
-							Transform(previous[0], previous[1], previous[2], 0, 0, 0),
-							Transform(intersection[0], intersection[1], intersection[2], 0, 0, 0),
-							Qt::gray);
+					intersectPoints->GetPoint(i, intersection);
 
-					previous[0] = intersection[0];
-					previous[1] = intersection[1];
-					previous[2] = intersection[2];
-					break;
+					Eigen::Vector3f v(intersection[0]-previous[0], intersection[1]-previous[1], intersection[2]-previous[2]);
+					float n = v.norm();
+					if(n  > 0.01f)
+					{
+						v/=n;
+						v *= n/2.0f;
+						pt.r = 125;
+						pt.g = 125;
+						pt.b = 125;
+						pt.x = intersection[0];
+						pt.y = intersection[1];
+						pt.z = intersection[2];
+						pointsHolder_->at(1) = pt;
+						viewer_->addOrUpdateText("interactor_ray_text_alt", uFormat("%.2f m", n),
+								Transform(previous[0]+v[0], previous[1]+v[1],previous[2]+v[2], 0, 0, 0),
+								textSize,
+								Qt::gray);
+						viewer_->addOrUpdateLine("interactor_ray_alt",
+								Transform(previous[0], previous[1], previous[2], 0, 0, 0),
+								Transform(intersection[0], intersection[1], intersection[2], 0, 0, 0),
+								Qt::gray);
+
+						previous[0] = intersection[0];
+						previous[1] = intersection[1];
+						previous[2] = intersection[2];
+						break;
+					}
 				}
+				viewer_->addCloud("interactor_points_alt", pointsHolder_);
+				viewer_->setCloudPointSize("interactor_points_alt", 15);
+				viewer_->setCloudOpacity("interactor_points_alt", 0.5);
 			}
-			viewer_->addCloud("interactor_points_alt", pointsHolder_);
-			viewer_->setCloudPointSize("interactor_points_alt", 15);
-			viewer_->setCloudOpacity("interactor_points_alt", 0.5);
 		}
 	}
 	// Forward events
@@ -220,14 +223,14 @@ void CloudViewerInteractorStyle::OnLeftButtonDown()
 		if(this->NumberOfClicks >= 2)
 		{
 			this->NumberOfClicks = 0;
-			this->Interactor->GetPicker()->Pick(pickPosition[0], pickPosition[1],
+			int result = this->Interactor->GetPicker()->Pick(pickPosition[0], pickPosition[1],
 					0,  // always zero.
 					this->CurrentRenderer);
-			double picked[3];
-			this->Interactor->GetPicker()->GetPickPosition(picked);
-			UDEBUG("Double clicked! Picked value: %f %f %f", picked[0], picked[1], picked[2]);
-			if(this->GetInteractor()->GetControlKey()==0)
+			if(result && this->GetInteractor()->GetControlKey()==0)
 			{
+				double picked[3];
+				this->Interactor->GetPicker()->GetPickPosition(picked);
+				UDEBUG("Double clicked! Picked value: %f %f %f", picked[0], picked[1], picked[2]);
 				vtkCamera *camera = this->CurrentRenderer->GetActiveCamera();
 				UASSERT(camera);
 				double position[3];
@@ -265,61 +268,64 @@ void CloudViewerInteractorStyle::OnLeftButtonDown()
 		}
 		else if(this->GetInteractor()->GetControlKey() && viewer_)
 		{
-			this->Interactor->GetPicker()->Pick(pickPosition[0], pickPosition[1],
+			int result = this->Interactor->GetPicker()->Pick(pickPosition[0], pickPosition[1],
 					0,  // always zero.
 					this->CurrentRenderer);
-			double picked[3];
-			this->Interactor->GetPicker()->GetPickPosition(picked);
-
-			UDEBUG("Shift clicked! Picked value: %f %f %f", picked[0], picked[1], picked[2]);
-
-			float textSize = 0.05;
-
-			viewer_->removeCloud("interactor_points");
-			pointsHolder_->clear();
-			pcl::PointXYZRGB pt;
-			pt.r = 255;
-			pt.x = picked[0];
-			pt.y = picked[1];
-			pt.z = picked[2];
-			pointsHolder_->push_back(pt);
-
-			viewer_->removeLine("interactor_ray");
-			viewer_->removeText("interactor_ray_text");
-
-			if(	PreviousMeasure[0] != 0.0f && PreviousMeasure[1] != 0.0f && PreviousMeasure[2] != 0.0f &&
-				viewer_->getAddedLines().find("interactor_line") == viewer_->getAddedLines().end())
+			if(result)
 			{
-				viewer_->addOrUpdateLine("interactor_line",
-						Transform(PreviousMeasure[0], PreviousMeasure[1], PreviousMeasure[2], 0, 0, 0),
-						Transform(picked[0], picked[1], picked[2], 0, 0, 0),
-						Qt::red);
-				pt.x = PreviousMeasure[0];
-				pt.y = PreviousMeasure[1];
-				pt.z = PreviousMeasure[2];
+				double picked[3];
+				this->Interactor->GetPicker()->GetPickPosition(picked);
+
+				UDEBUG("Shift clicked! Picked value: %f %f %f", picked[0], picked[1], picked[2]);
+
+				float textSize = 0.05;
+
+				viewer_->removeCloud("interactor_points");
+				pointsHolder_->clear();
+				pcl::PointXYZRGB pt;
+				pt.r = 255;
+				pt.x = picked[0];
+				pt.y = picked[1];
+				pt.z = picked[2];
 				pointsHolder_->push_back(pt);
 
-				Eigen::Vector3f v(picked[0]-PreviousMeasure[0], picked[1]-PreviousMeasure[1], picked[2]-PreviousMeasure[2]);
-				float n = v.norm();
-				v/=n;
-				v *= n/2.0f;
-				viewer_->addOrUpdateText("interactor_pose", uFormat("%.2f m", n),
-						Transform(PreviousMeasure[0]+v[0], PreviousMeasure[1]+v[1],PreviousMeasure[2]+v[2], 0, 0, 0),
-						textSize,
-						Qt::red);
-			}
-			else
-			{
-				viewer_->removeText("interactor_pose");
-				viewer_->removeLine("interactor_line");
-			}
-			PreviousMeasure[0] = picked[0];
-			PreviousMeasure[1] = picked[1];
-			PreviousMeasure[2] = picked[2];
+				viewer_->removeLine("interactor_ray");
+				viewer_->removeText("interactor_ray_text");
 
-			viewer_->addCloud("interactor_points", pointsHolder_);
-			viewer_->setCloudPointSize("interactor_points", 15);
-			viewer_->setCloudOpacity("interactor_points", 0.5);
+				if(	PreviousMeasure[0] != 0.0f && PreviousMeasure[1] != 0.0f && PreviousMeasure[2] != 0.0f &&
+					viewer_->getAddedLines().find("interactor_line") == viewer_->getAddedLines().end())
+				{
+					viewer_->addOrUpdateLine("interactor_line",
+							Transform(PreviousMeasure[0], PreviousMeasure[1], PreviousMeasure[2], 0, 0, 0),
+							Transform(picked[0], picked[1], picked[2], 0, 0, 0),
+							Qt::red);
+					pt.x = PreviousMeasure[0];
+					pt.y = PreviousMeasure[1];
+					pt.z = PreviousMeasure[2];
+					pointsHolder_->push_back(pt);
+
+					Eigen::Vector3f v(picked[0]-PreviousMeasure[0], picked[1]-PreviousMeasure[1], picked[2]-PreviousMeasure[2]);
+					float n = v.norm();
+					v/=n;
+					v *= n/2.0f;
+					viewer_->addOrUpdateText("interactor_pose", uFormat("%.2f m", n),
+							Transform(PreviousMeasure[0]+v[0], PreviousMeasure[1]+v[1],PreviousMeasure[2]+v[2], 0, 0, 0),
+							textSize,
+							Qt::red);
+				}
+				else
+				{
+					viewer_->removeText("interactor_pose");
+					viewer_->removeLine("interactor_line");
+				}
+				PreviousMeasure[0] = picked[0];
+				PreviousMeasure[1] = picked[1];
+				PreviousMeasure[2] = picked[2];
+
+				viewer_->addCloud("interactor_points", pointsHolder_);
+				viewer_->setCloudPointSize("interactor_points", 15);
+				viewer_->setCloudOpacity("interactor_points", 0.5);
+			}
 		}
 	}
 

guilib/src/DatabaseViewer.cpp

@@ -2090,7 +2090,7 @@ void DatabaseViewer::updateStatistics()
 		double firstStamp = 0.0;
 		std::map<int, std::pair<std::map<std::string, float>, double> > allStats = dbDriver_->getAllStatistics();
 
-		std::map<std::string, std::pair<std::vector<float>, std::vector<float> > > allData;
+		std::map<std::string, std::pair<std::vector<qreal>, std::vector<qreal> > > allData;
 		std::map<std::string, int > allDataOi;
 
 		for(int i=0; i<ids_.size(); ++i)
@@ -2111,18 +2111,18 @@ void DatabaseViewer::updateStatistics()
 				if(allData.find(iter->first) == allData.end())
 				{
 					//initialize data vectors
-					allData.insert(std::make_pair(iter->first, std::make_pair(std::vector<float>(ids_.size(), 0.0f), std::vector<float>(ids_.size(), 0.0f) )));
+					allData.insert(std::make_pair(iter->first, std::make_pair(std::vector<qreal>(ids_.size(), 0.0f), std::vector<qreal>(ids_.size(), 0.0f) )));
 					allDataOi.insert(std::make_pair(iter->first, 0));
 				}
 
 				int & oi = allDataOi.at(iter->first);
-				allData.at(iter->first).first[oi] = ui_->checkBox_timeStats->isChecked()?float(stamp-firstStamp):ids_[i];
+				allData.at(iter->first).first[oi] = ui_->checkBox_timeStats->isChecked()?qreal(stamp-firstStamp):ids_[i];
 				allData.at(iter->first).second[oi] = iter->second;
 				++oi;
 			}
 		}
 
-		for(std::map<std::string, std::pair<std::vector<float>, std::vector<float> > >::iterator iter=allData.begin(); iter!=allData.end(); ++iter)
+		for(std::map<std::string, std::pair<std::vector<qreal>, std::vector<qreal> > >::iterator iter=allData.begin(); iter!=allData.end(); ++iter)
 		{
 			int oi = allDataOi.at(iter->first);
 			iter->second.first.resize(oi);
@@ -6922,10 +6922,14 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
 		}
 
 		Transform toPoseInv = filteredScanPoses.at(currentLink.to()).inverse();
-		LaserScan fromScan;
 		dbDriver_->loadNodeData(fromS, !silent, true, !silent, !silent);
 		fromS->sensorData().uncompressData();
+		LaserScan fromScan = fromS->sensorData().laserScanRaw();
 		int maxPoints = fromScan.size();
+		if(maxPoints == 0)
+		{
+			UWARN("From scan %d is empty!", fromS->id());
+		}
 		pcl::PointCloud<pcl::PointXYZ>::Ptr assembledToClouds(new pcl::PointCloud<pcl::PointXYZ>);
 		pcl::PointCloud<pcl::PointNormal>::Ptr assembledToNormalClouds(new pcl::PointCloud<pcl::PointNormal>);
 		pcl::PointCloud<pcl::PointXYZI>::Ptr assembledToIClouds(new pcl::PointCloud<pcl::PointXYZI>);
@@ -6974,6 +6978,10 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
 							maxPoints = scan.size();
 						}
 					}
+					else
+					{
+						UWARN("scan format of %d is not the same than from scan %d: %d vs %d", data.id(), fromS->id(), scan.format(), fromScan.format());
+					}
 				}
 				else
 				{
@@ -6999,6 +7007,10 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
 		{
 			assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToIClouds):util3d::laserScanFromPointCloud(*assembledToIClouds);
 		}
+		else
+		{
+			UWARN("Assembled scan is empty!");
+		}
 		SensorData assembledData;
 		// scans are in base frame but for 2d scans, set the height so that correspondences matching works
 		assembledData.setLaserScan(LaserScan(

guilib/src/MainWindow.cpp

@@ -288,7 +288,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
 	_ui->posteriorPlot->setFixedYAxis(0,1);
 	UPlotCurveThreshold * tc;
 	tc = _ui->posteriorPlot->addThreshold("Loop closure thr", float(_preferencesDialog->getLoopThr()));
-	connect(this, SIGNAL(loopClosureThrChanged(float)), tc, SLOT(setThreshold(float)));
+	connect(this, SIGNAL(loopClosureThrChanged(qreal)), tc, SLOT(setThreshold(qreal)));
 
 	_likelihoodCurve = new PdfPlotCurve("Likelihood", &_cachedSignatures, this);
 	_ui->likelihoodPlot->addCurve(_likelihoodCurve, false);
@@ -1985,9 +1985,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 
 			ULOGGER_DEBUG("");
 			//Adjust thresholds
-			float value;
-			value = float(_preferencesDialog->getLoopThr());
-			Q_EMIT(loopClosureThrChanged(value));
+			Q_EMIT(loopClosureThrChanged(_preferencesDialog->getLoopThr()));
 		}
 		if(!stat.likelihood().empty() && _ui->dockWidget_likelihood->isVisible())
 		{
@@ -4328,9 +4326,7 @@ void MainWindow::applyPrefSettings(const rtabmap::ParametersMap & parameters, bo
 	_ui->doubleSpinBox_stats_timeLimit->setValue(_preferencesDialog->getTimeLimit());
 	_ui->actionSLAM_mode->setChecked(_preferencesDialog->isSLAMMode());
 
-	float value;
-	value = float(_preferencesDialog->getLoopThr());
-	Q_EMIT(loopClosureThrChanged(value));
+	Q_EMIT(loopClosureThrChanged(_preferencesDialog->getLoopThr()));
 }
 
 void MainWindow::drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords, const std::multimap<int, cv::KeyPoint> & loopWords)
@@ -4606,6 +4602,7 @@ void MainWindow::updateSelectSourceMenu()
 	_ui->actionOpenNI2_sense->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI2);
 	_ui->actionFreenect2->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcFreenect2);
 	_ui->actionKinect_for_Windows_SDK_v2->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcK4W2);
+	_ui->actionKinect_for_Azure->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcK4W2);
 	_ui->actionRealSense_R200->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense);
 	_ui->actionRealSense_ZR300->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense);
 	_ui->actionRealSense2_SR300->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense2);
@@ -5218,6 +5215,7 @@ void MainWindow::startDetection()
 				int odomStrategy = Parameters::defaultOdomStrategy();
 				Parameters::parse(odomParameters, Parameters::kOdomStrategy(), odomStrategy);
 				double gravitySigma = _preferencesDialog->getOdomF2MGravitySigma();
+				UDEBUG("Odom gravitySigma=%f", gravitySigma);
 				if(gravitySigma >= 0.0)
 				{
 					uInsert(odomParameters, ParametersPair(Parameters::kOptimizerGravitySigma(), uNumber2Str(gravitySigma)));

guilib/src/PreferencesDialog.cpp

@@ -198,7 +198,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 #endif
 
 // SIFT
-#if CV_MAJOR_VERSION < 4 || (CV_MAJOR_VERSION == 4 && (CV_MINOR_VERSION < 3 || (CV_MINOR_VERSION==3 && !defined(RTABMAP_OPENCV_DEV))))
+#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <= 3) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION < 4 || (CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<11)))
 #ifndef RTABMAP_NONFREE
 	_ui->comboBox_detector_strategy->setItemData(1, 0, Qt::UserRole - 1);
 	_ui->vis_feature_detector->setItemData(1, 0, Qt::UserRole - 1);
@@ -748,6 +748,9 @@ 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->comboBox_k4a_rgb_resolution, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->comboBox_k4a_framerate, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->comboBox_k4a_depth_resolution, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	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()));
@@ -1883,6 +1886,9 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->lineEdit_rs2_jsonFile->clear();
 		_ui->lineEdit_openniOniPath->clear();
 		_ui->lineEdit_openni2OniPath->clear();
+		_ui->comboBox_k4a_rgb_resolution->setCurrentIndex(0);
+		_ui->comboBox_k4a_framerate->setCurrentIndex(2);
+		_ui->comboBox_k4a_depth_resolution->setCurrentIndex(2);
 		_ui->checkbox_k4a_irDepth->setChecked(false);
 		_ui->lineEdit_k4a_mkv->clear();
 		_ui->source_checkBox_useMKVStamps->setChecked(true);
@@ -2306,6 +2312,9 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	settings.endGroup(); // K4W2
 
 	settings.beginGroup("K4A");
+	_ui->comboBox_k4a_rgb_resolution->setCurrentIndex(settings.value("rgb_resolution", _ui->comboBox_k4a_rgb_resolution->currentIndex()).toInt());
+	_ui->comboBox_k4a_framerate->setCurrentIndex(settings.value("framerate", _ui->comboBox_k4a_framerate->currentIndex()).toInt());
+	_ui->comboBox_k4a_depth_resolution->setCurrentIndex(settings.value("depth_resolution", _ui->comboBox_k4a_depth_resolution->currentIndex()).toInt());
 	_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());
 	_ui->source_checkBox_useMKVStamps->setChecked(settings.value("useMkvStamps", _ui->source_checkBox_useMKVStamps->isChecked()).toBool());
@@ -2784,6 +2793,9 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.endGroup(); // K4W2
 
 	settings.beginGroup("K4A");
+	settings.setValue("rgb_resolution", _ui->comboBox_k4a_rgb_resolution->currentIndex());
+	settings.setValue("framerate", _ui->comboBox_k4a_framerate->currentIndex());
+	settings.setValue("depth_resolution", _ui->comboBox_k4a_depth_resolution->currentIndex());
 	settings.setValue("ir", _ui->checkbox_k4a_irDepth->isChecked());
 	settings.setValue("mkvPath", _ui->lineEdit_k4a_mkv->text());
 	settings.setValue("useMkvStamps", _ui->source_checkBox_useMKVStamps->isChecked());
@@ -2945,7 +2957,7 @@ void PreferencesDialog::writeCoreSettings(const QString & filePath) const
 
 bool PreferencesDialog::validateForm()
 {
-#if CV_MAJOR_VERSION < 4 || (CV_MAJOR_VERSION == 4 && (CV_MINOR_VERSION < 3 || (CV_MINOR_VERSION==3 && !defined(RTABMAP_OPENCV_DEV))))
+#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <= 3) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION < 4 || (CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<11)))
 #ifndef RTABMAP_NONFREE
 	// verify that SURF/SIFT cannot be selected if not built with OpenCV nonfree module
 	// BOW dictionary type
@@ -2966,7 +2978,7 @@ bool PreferencesDialog::validateForm()
 		_ui->vis_feature_detector->setCurrentIndex(Feature2D::kFeatureFastBrief);
 	}
 #endif
-#else //>= 4.3.0-dev
+#else //>= 4.4.0 >= 3.4.11
 #ifndef RTABMAP_NONFREE
 	// verify that SURF cannot be selected if not built with OpenCV nonfree module
 	// BOW dictionary type
@@ -4564,8 +4576,8 @@ void PreferencesDialog::updatePredictionPlot()
 				_ui->lineEdit_bayes_predictionLC->text().toStdString().c_str());
 		return;
 	}
-	QVector<float> dataX((values.size()-2)*2 + 1);
-	QVector<float> dataY((values.size()-2)*2 + 1);
+	QVector<qreal> dataX((values.size()-2)*2 + 1);
+	QVector<qreal> dataY((values.size()-2)*2 + 1);
 	double value;
 	double sum = 0;
 	int lvl = 1;
@@ -5634,6 +5646,9 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 		}
 		
 		((CameraK4A*)camera)->setIRDepthFormat(_ui->checkbox_k4a_irDepth->isChecked());
+		((CameraK4A*)camera)->setPreferences(_ui->comboBox_k4a_rgb_resolution->currentIndex(),
+						     _ui->comboBox_k4a_framerate->currentIndex(),
+						     _ui->comboBox_k4a_depth_resolution->currentIndex());
 	}
 	else if (driver == kSrcRealSense)
 	{

guilib/src/StatsToolBox.cpp

@@ -44,7 +44,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
-StatItem::StatItem(const QString & name, bool cacheOn, const std::vector<float> & x, const std::vector<float> & y, const QString & unit, const QMenu * menu, QGridLayout * grid, QWidget * parent) :
+StatItem::StatItem(const QString & name, bool cacheOn, const std::vector<qreal> & x, const std::vector<qreal> & y, const QString & unit, const QMenu * menu, QGridLayout * grid, QWidget * parent) :
 	QWidget(parent),
 	_button(0),
 	_name(0),
@@ -84,7 +84,7 @@ void StatItem::clearCache()
 	_value->clear();
 }
 
-void StatItem::addValue(float y)
+void StatItem::addValue(qreal y)
 {
 	if(_cacheOn)
 	{
@@ -94,7 +94,7 @@ void StatItem::addValue(float y)
 	Q_EMIT valueAdded(y);
 }
 
-void StatItem::addValue(float x, float y)
+void StatItem::addValue(qreal x, qreal y)
 {
 	if(_cacheOn)
 	{
@@ -111,7 +111,7 @@ void StatItem::addValue(float x, float y)
 	Q_EMIT valueAdded(x,y);
 }
 
-void StatItem::setValues(const std::vector<float> & x, const std::vector<float> & y)
+void StatItem::setValues(const std::vector<qreal> & x, const std::vector<qreal> & y)
 {
 	if(_cacheOn)
 	{
@@ -254,30 +254,30 @@ void StatsToolBox::setCacheOn(bool on)
 
 void StatsToolBox::updateStat(const QString & statFullName, bool cacheOn)
 {
-	std::vector<float> vx,vy;
+	std::vector<qreal> vx,vy;
 	updateStat(statFullName, vx, vy, cacheOn);
 }
 
-void StatsToolBox::updateStat(const QString & statFullName, float y, bool cacheOn)
+void StatsToolBox::updateStat(const QString & statFullName, qreal y, bool cacheOn)
 {
-	std::vector<float> vx,vy(1);
+	std::vector<qreal> vx,vy(1);
 	vy[0] = y;
 	updateStat(statFullName, vx, vy, cacheOn);
 }
 
-void StatsToolBox::updateStat(const QString & statFullName, float x, float y, bool cacheOn)
+void StatsToolBox::updateStat(const QString & statFullName, qreal x, qreal y, bool cacheOn)
 {
-	std::vector<float> vx(1),vy(1);
+	std::vector<qreal> vx(1),vy(1);
 	vx[0] = x;
 	vy[0] = y;
 	updateStat(statFullName, vx, vy, cacheOn);
 }
 
-void StatsToolBox::updateStat(const QString & statFullName, const std::vector<float> & x, const std::vector<float> & y, bool cacheOn)
+void StatsToolBox::updateStat(const QString & statFullName, const std::vector<qreal> & x, const std::vector<qreal> & y, bool cacheOn)
 {
-	// round float to max 2 numbers after the dot
-	//x = (float(int(100*x)))/100;
-	//y = (float(int(100*y)))/100;
+	// round qreal to max 2 numbers after the dot
+	//x = (qreal(int(100*x)))/100;
+	//y = (qreal(int(100*y)))/100;
 
 	StatItem * item = _statBox->findChild<StatItem *>(statFullName);
 	if(item)
@@ -393,9 +393,9 @@ void StatsToolBox::plot(const StatItem * stat, const QString & plotName)
 		{
 			UPlotCurve * curve = new UPlotCurve(stat->objectName(), plot);
 			curve->setPen(plot->getRandomPenColored());
-			connect(stat, SIGNAL(valueAdded(float)), curve, SLOT(addValue(float)));
-			connect(stat, SIGNAL(valueAdded(float, float)), curve, SLOT(addValue(float, float)));
-			connect(stat, SIGNAL(valuesChanged(const std::vector<float> &, const std::vector<float> &)), curve, SLOT(setData(const std::vector<float> &, const std::vector<float> &)));
+			connect(stat, SIGNAL(valueAdded(qreal)), curve, SLOT(addValue(qreal)));
+			connect(stat, SIGNAL(valueAdded(qreal, qreal)), curve, SLOT(addValue(qreal, qreal)));
+			connect(stat, SIGNAL(valuesChanged(const std::vector<qreal> &, const std::vector<qreal> &)), curve, SLOT(setData(const std::vector<qreal> &, const std::vector<qreal> &)));
 			if(stat->value().compare("*") == 0)
 			{
 				plot->setMaxVisibleItems(0);
@@ -453,9 +453,9 @@ void StatsToolBox::plot(const StatItem * stat, const QString & plotName)
 		//Add a new curve linked to the statBox
 		UPlotCurve * curve = new UPlotCurve(stat->objectName(), newPlot);
 		curve->setPen(newPlot->getRandomPenColored());
-		connect(stat, SIGNAL(valueAdded(float)), curve, SLOT(addValue(float)));
-		connect(stat, SIGNAL(valueAdded(float, float)), curve, SLOT(addValue(float, float)));
-		connect(stat, SIGNAL(valuesChanged(const std::vector<float> &, const std::vector<float> &)), curve, SLOT(setData(const std::vector<float> &, const std::vector<float> &)));
+		connect(stat, SIGNAL(valueAdded(qreal)), curve, SLOT(addValue(qreal)));
+		connect(stat, SIGNAL(valueAdded(qreal, qreal)), curve, SLOT(addValue(qreal, qreal)));
+		connect(stat, SIGNAL(valuesChanged(const std::vector<qreal> &, const std::vector<qreal> &)), curve, SLOT(setData(const std::vector<qreal> &, const std::vector<qreal> &)));
 		if(stat->value().compare("*") == 0)
 		{
 			newPlot->setMaxVisibleItems(0);

guilib/src/ui/mainWindow.ui

@@ -1555,7 +1555,7 @@
   </action>
   <action name="actionKinect_for_Azure">
    <property name="checkable">
-    <bool>false</bool>
+    <bool>true</bool>
    </property>
    <property name="text">
     <string>Kinect for Azure</string>

guilib/src/ui/preferencesDialog.ui

@@ -63,7 +63,7 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-696</y>
+            <y>-360</y>
             <width>680</width>
             <height>3270</height>
            </rect>
@@ -3109,7 +3109,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                     <item>
                      <widget class="QStackedWidget" name="stackedWidget_src">
                       <property name="currentIndex">
-                       <number>1</number>
+                       <number>0</number>
                       </property>
                       <widget class="QWidget" name="page_41">
                        <layout class="QVBoxLayout" name="verticalLayout_64">
@@ -3220,7 +3220,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">
@@ -4417,17 +4417,31 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                   <string>Kinect for Azure</string>
                                  </property>
                                  <layout class="QGridLayout" name="gridLayout_113">
-                                  <item row="0" column="1" colspan="2">
-                                   <widget class="QLabel" name="label_558">
+                                  <item row="2" column="0">
+                                   <widget class="QCheckBox" name="checkbox_k4a_irDepth">
                                     <property name="text">
-                                     <string>Use IR for RGB image </string>
+                                     <string/>
                                     </property>
-                                    <property name="wordWrap">
-                                     <bool>true</bool>
+                                    <property name="checked">
+                                     <bool>false</bool>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="8" column="0">
+                                   <widget class="QCheckBox" name="source_checkBox_useMKVStamps">
+                                    <property name="text">
+                                     <string/>
                                     </property>
                                    </widget>
                                   </item>
                                   <item row="5" column="0">
+                                   <widget class="QToolButton" name="toolButton_k4a_mkv">
+                                    <property name="text">
+                                     <string>...</string>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="9" column="0">
                                    <spacer name="verticalSpacer_83">
                                     <property name="orientation">
                                      <enum>Qt::Vertical</enum>
@@ -4440,14 +4454,17 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </spacer>
                                   </item>
-                                  <item row="1" column="0">
-                                   <widget class="QToolButton" name="toolButton_k4a_mkv">
+                                  <item row="2" column="1" colspan="2">
+                                   <widget class="QLabel" name="label_558">
                                     <property name="text">
-                                     <string>...</string>
+                                     <string>Use IR for RGB image </string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="1" column="3">
+                                  <item row="5" column="3">
                                    <widget class="QLabel" name="label_556">
                                     <property name="text">
                                      <string>Path to a *.MKV file.</string>
@@ -4460,24 +4477,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
-                                  <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="1" column="1" colspan="2">
-                                   <widget class="QLineEdit" name="lineEdit_k4a_mkv">
-                                    <property name="text">
-                                     <string/>
-                                    </property>
-                                   </widget>
-                                  </item>
-                                  <item row="4" column="1">
+                                  <item row="8" column="1">
                                    <widget class="QLabel" name="label_557">
                                     <property name="text">
                                      <string>Use MKV file stamps as input rate.</string>
@@ -4490,8 +4490,161 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="4" column="0">
-                                   <widget class="QCheckBox" name="source_checkBox_useMKVStamps">
+                                  <item row="0" column="0" colspan="3">
+                                   <widget class="QFrame" name="frame">
+                                    <property name="minimumSize">
+                                     <size>
+                                      <width>200</width>
+                                      <height>90</height>
+                                     </size>
+                                    </property>
+                                    <property name="frameShape">
+                                     <enum>QFrame::NoFrame</enum>
+                                    </property>
+                                    <property name="frameShadow">
+                                     <enum>QFrame::Plain</enum>
+                                    </property>
+                                    <property name="lineWidth">
+                                     <number>0</number>
+                                    </property>
+                                    <widget class="QLabel" name="label_600">
+                                     <property name="geometry">
+                                      <rect>
+                                       <x>80</x>
+                                       <y>0</y>
+                                       <width>161</width>
+                                       <height>21</height>
+                                      </rect>
+                                     </property>
+                                     <property name="text">
+                                      <string>RGB camera resolution</string>
+                                     </property>
+                                    </widget>
+                                    <widget class="QComboBox" name="comboBox_k4a_rgb_resolution">
+                                     <property name="geometry">
+                                      <rect>
+                                       <x>0</x>
+                                       <y>0</y>
+                                       <width>71</width>
+                                       <height>25</height>
+                                      </rect>
+                                     </property>
+                                     <item>
+                                      <property name="text">
+                                       <string>720p</string>
+                                      </property>
+                                     </item>
+                                     <item>
+                                      <property name="text">
+                                       <string>1080p</string>
+                                      </property>
+                                     </item>
+                                     <item>
+                                      <property name="text">
+                                       <string>1440p</string>
+                                      </property>
+                                     </item>
+                                     <item>
+                                      <property name="text">
+                                       <string>1536p</string>
+                                      </property>
+                                     </item>
+                                     <item>
+                                      <property name="text">
+                                       <string>2160p</string>
+                                      </property>
+                                     </item>
+                                     <item>
+                                      <property name="text">
+                                       <string>3072p</string>
+                                      </property>
+                                     </item>
+                                    </widget>
+                                    <widget class="QComboBox" name="comboBox_k4a_framerate">
+                                     <property name="geometry">
+                                      <rect>
+                                       <x>0</x>
+                                       <y>60</y>
+                                       <width>51</width>
+                                       <height>25</height>
+                                      </rect>
+                                     </property>
+                                     <item>
+                                      <property name="text">
+                                       <string>5</string>
+                                      </property>
+                                     </item>
+                                     <item>
+                                      <property name="text">
+                                       <string>15</string>
+                                      </property>
+                                     </item>
+                                     <item>
+                                      <property name="text">
+                                       <string>30</string>
+                                      </property>
+                                     </item>
+                                    </widget>
+                                    <widget class="QLabel" name="label_601">
+                                     <property name="geometry">
+                                      <rect>
+                                       <x>60</x>
+                                       <y>60</y>
+                                       <width>131</width>
+                                       <height>21</height>
+                                      </rect>
+                                     </property>
+                                     <property name="text">
+                                      <string>Frames per second</string>
+                                     </property>
+                                    </widget>
+                                    <widget class="QComboBox" name="comboBox_k4a_depth_resolution">
+                                     <property name="geometry">
+                                      <rect>
+                                       <x>0</x>
+                                       <y>30</y>
+                                       <width>111</width>
+                                       <height>25</height>
+                                      </rect>
+                                     </property>
+                                     <item>
+                                      <property name="text">
+                                       <string>320x288</string>
+                                      </property>
+                                     </item>
+                                     <item>
+                                      <property name="text">
+                                       <string>640x576</string>
+                                      </property>
+                                     </item>
+                                     <item>
+                                      <property name="text">
+                                       <string>512x512</string>
+                                      </property>
+                                     </item>
+                                     <item>
+                                      <property name="text">
+                                       <string>1024x1024</string>
+                                      </property>
+                                     </item>
+                                    </widget>
+                                    <widget class="QLabel" name="label_602">
+                                     <property name="geometry">
+                                      <rect>
+                                       <x>120</x>
+                                       <y>30</y>
+                                       <width>171</width>
+                                       <height>21</height>
+                                      </rect>
+                                     </property>
+                                     <property name="text">
+                                      <string>Depth camera resolution</string>
+                                     </property>
+                                    </widget>
+                                   </widget>
+                                  </item>
+                                  <item row="5" column="1" colspan="2">
+                                   <widget class="QLineEdit" name="lineEdit_k4a_mkv">
                                     <property name="text">
                                      <string/>
                                     </property>
@@ -13523,7 +13676,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                                 <double>0.100000000000000</double>
                                </property>
                                <property name="value">
-                                <double>0.000000000000000</double>
+                                <double>-1.000000000000000</double>
                                </property>
                               </widget>
                              </item>

guilib/src/utilite/UPlot.cpp

@@ -280,7 +280,7 @@ UPlotCurve::UPlotCurve(const QString & name, QVector<UPlotItem *> data, QObject
 	this->setData(data);
 }
 
-UPlotCurve::UPlotCurve(const QString & name, const QVector<float> & x, const QVector<float> & y, QObject * parent) :
+UPlotCurve::UPlotCurve(const QString & name, const QVector<qreal> & x, const QVector<qreal> & y, QObject * parent) :
 	QObject(parent),
 	_plot(0),
 	_name(name),
@@ -338,15 +338,15 @@ void UPlotCurve::detach(UPlot * plot)
 
 void UPlotCurve::updateMinMax()
 {
-	float x,y;
+	qreal x,y;
 	const UPlotItem * item;
 	if(!_items.size())
 	{
-		_minMax = QVector<float>();
+		_minMax = QVector<qreal>();
 	}
 	else
 	{
-		_minMax = QVector<float>(4);
+		_minMax = QVector<qreal>(4);
 	}
 	for(int i=0; i<_items.size(); ++i)
 	{
@@ -378,12 +378,12 @@ void UPlotCurve::_addValue(UPlotItem * data)
 	// add item
 	if(data)
 	{
-		float x = data->data().x();
-		float y = data->data().y();
+		qreal x = data->data().x();
+		qreal y = data->data().y();
 
 		if(_minMax.size() != 4)
 		{
-			_minMax = QVector<float>(4);
+			_minMax = QVector<qreal>(4);
 		}
 		if(_items.size())
 		{
@@ -429,7 +429,7 @@ void UPlotCurve::addValue(UPlotItem * data)
 	}
 }
 
-void UPlotCurve::addValue(float x, float y)
+void UPlotCurve::addValue(qreal x, qreal y)
 {
 	if(_items.size() &&
 		_minMax[0] != _minMax[1] &&
@@ -441,13 +441,13 @@ void UPlotCurve::addValue(float x, float y)
 		this->clear();
 	}
 
-	float width = 2; // TODO warn : hard coded value!
+	qreal width = 2; // TODO warn : hard coded value!
 	this->addValue(new UPlotItem(x,y,width));
 }
 
-void UPlotCurve::addValue(float y)
+void UPlotCurve::addValue(qreal y)
 {
-	float x = 0;
+	qreal x = 0;
 	if(_items.size())
 	{
 		UPlotItem * lastItem = (UPlotItem *)_items.last();
@@ -463,7 +463,7 @@ void UPlotCurve::addValue(float y)
 void UPlotCurve::addValue(const QString & value)
 {
 	bool ok;
-	float v = value.toFloat(&ok);
+	qreal v = value.toDouble(&ok);
 	if(ok)
 	{
 		this->addValue(v);
@@ -483,9 +483,9 @@ void UPlotCurve::addValues(QVector<UPlotItem *> & data)
 	Q_EMIT dataChanged(this);
 }
 
-void UPlotCurve::addValues(const QVector<float> & xs, const QVector<float> & ys)
+void UPlotCurve::addValues(const QVector<qreal> & xs, const QVector<qreal> & ys)
 {
-	float width = 2; // TODO warn : hard coded value!
+	qreal width = 2; // TODO warn : hard coded value!
 	for(int i=0; i<xs.size() && i<ys.size(); ++i)
 	{
 		this->_addValue(new UPlotItem(xs.at(i),ys.at(i),width));
@@ -493,10 +493,10 @@ void UPlotCurve::addValues(const QVector<float> & xs, const QVector<float> & ys)
 	Q_EMIT dataChanged(this);
 }
 
-void UPlotCurve::addValues(const QVector<float> & ys)
+void UPlotCurve::addValues(const QVector<qreal> & ys)
 {
-	float x = 0;
-	float width = 2; // TODO warn : hard coded value!
+	qreal x = 0;
+	qreal width = 2; // TODO warn : hard coded value!
 	for(int i=0; i<ys.size(); ++i)
 	{
 		if(_items.size())
@@ -515,8 +515,8 @@ void UPlotCurve::addValues(const QVector<float> & ys)
 
 void UPlotCurve::addValues(const QVector<int> & ys)
 {
-	float x = 0;
-	float width = 2; // TODO warn : hard coded value!
+	qreal x = 0;
+	qreal width = 2; // TODO warn : hard coded value!
 	for(int i=0; i<ys.size(); ++i)
 	{
 		if(_items.size())
@@ -535,8 +535,8 @@ void UPlotCurve::addValues(const QVector<int> & ys)
 
 void UPlotCurve::addValues(const std::vector<int> & ys)
 {
-	float x = 0;
-	float width = 2; // TODO warn : hard coded value!
+	qreal x = 0;
+	qreal width = 2; // TODO warn : hard coded value!
 	for(unsigned int i=0; i<ys.size(); ++i)
 	{
 		if(_items.size())
@@ -553,10 +553,10 @@ void UPlotCurve::addValues(const std::vector<int> & ys)
 	Q_EMIT dataChanged(this);
 }
 
-void UPlotCurve::addValues(const std::vector<float> & ys)
+void UPlotCurve::addValues(const std::vector<qreal> & ys)
 {
-	float x = 0;
-	float width = 2; // TODO warn : hard coded value!
+	qreal x = 0;
+	qreal width = 2; // TODO warn : hard coded value!
 	for(unsigned int i=0; i<ys.size(); ++i)
 	{
 		if(_items.size())
@@ -596,8 +596,8 @@ int UPlotCurve::removeItem(int index)
 				if(_items.size())
 				{
 					UPlotItem * tmp = (UPlotItem *)_items.at(0);
-					float x = tmp->data().x();
-					float y = tmp->data().y();
+					qreal x = tmp->data().x();
+					qreal y = tmp->data().y();
 					_minMax[0]=x;
 					_minMax[1]=x;
 					_minMax[2]=y;
@@ -615,7 +615,7 @@ int UPlotCurve::removeItem(int index)
 				}
 				else
 				{
-					_minMax = QVector<float>();
+					_minMax = QVector<qreal>();
 				}
 			}
 		}
@@ -687,7 +687,7 @@ void UPlotCurve::setItemsColor(const QColor & color)
 	}
 }
 
-void UPlotCurve::update(float scaleX, float scaleY, float offsetX, float offsetY, float xDir, float yDir, int maxItemsKept)
+void UPlotCurve::update(qreal scaleX, qreal scaleY, qreal offsetX, qreal offsetY, qreal xDir, qreal yDir, int maxItemsKept)
 {
 	//ULOGGER_DEBUG("scaleX=%f, scaleY=%f, offsetX=%f, offsetY=%f, xDir=%d, yDir=%d, _plot->scene()->width()=%f, _plot->scene()->height=%f", scaleX, scaleY, offsetX, offsetY, xDir, yDir,_plot->scene()->width(),_plot->scene()->height());
 	//make sure direction values are 1 or -1
@@ -865,12 +865,12 @@ void UPlotCurve::setVisible(bool visible)
 	}
 }
 
-void UPlotCurve::setXIncrement(float increment)
+void UPlotCurve::setXIncrement(qreal increment)
 {
 	_xIncrement = increment;
 }
 
-void UPlotCurve::setXStart(float val)
+void UPlotCurve::setXStart(qreal val)
 {
 	_xStart = val;
 }
@@ -884,7 +884,7 @@ void UPlotCurve::setData(QVector<UPlotItem*> & data)
 	}
 }
 
-void UPlotCurve::setData(const QVector<float> & x, const QVector<float> & y)
+void UPlotCurve::setData(const QVector<qreal> & x, const QVector<qreal> & y)
 {
 	if(x.size() == y.size())
 	{
@@ -904,8 +904,8 @@ void UPlotCurve::setData(const QVector<float> & x, const QVector<float> & y)
 
 		// update values
 		int index = 0;
-		QVector<float>::const_iterator i=x.begin();
-		QVector<float>::const_iterator j=y.begin();
+		QVector<qreal>::const_iterator i=x.begin();
+		QVector<qreal>::const_iterator j=y.begin();
 		for(; i!=x.end() && j!=y.end(); ++i, ++j, index+=2)
 		{
 			((UPlotItem*)_items[index])->setData(QPointF(*i, *j));
@@ -925,7 +925,7 @@ void UPlotCurve::setData(const QVector<float> & x, const QVector<float> & y)
 	}
 }
 
-void UPlotCurve::setData(const std::vector<float> & x, const std::vector<float> & y)
+void UPlotCurve::setData(const std::vector<qreal> & x, const std::vector<qreal> & y)
 {
 	if(x.size() == y.size())
 	{
@@ -945,8 +945,8 @@ void UPlotCurve::setData(const std::vector<float> & x, const std::vector<float>
 
 		// update values
 		int index = 0;
-		std::vector<float>::const_iterator i=x.begin();
-		std::vector<float>::const_iterator j=y.begin();
+		std::vector<qreal>::const_iterator i=x.begin();
+		std::vector<qreal>::const_iterator j=y.begin();
 		for(; i!=x.end() && j!=y.end(); ++i, ++j, index+=2)
 		{
 			((UPlotItem*)_items[index])->setData(QPointF(*i, *j));
@@ -966,12 +966,12 @@ void UPlotCurve::setData(const std::vector<float> & x, const std::vector<float>
 	}
 }
 
-void UPlotCurve::setData(const QVector<float> & y)
+void UPlotCurve::setData(const QVector<qreal> & y)
 {
 	this->setData(y.toStdVector());
 }
 
-void UPlotCurve::setData(const std::vector<float> & y)
+void UPlotCurve::setData(const std::vector<qreal> & y)
 {
 	//match the size of the current data
 	int margin = int((_items.size()+1)/2) - int(y.size());
@@ -989,8 +989,8 @@ void UPlotCurve::setData(const std::vector<float> & y)
 
 	// update values
 	int index = 0;
-	float x = 0;
-	std::vector<float>::const_iterator j=y.begin();
+	qreal x = 0;
+	std::vector<qreal>::const_iterator j=y.begin();
 	for(; j!=y.end(); ++j, index+=2)
 	{
 		((UPlotItem*)_items[index])->setData(QPointF(x++, *j));
@@ -1001,7 +1001,7 @@ void UPlotCurve::setData(const std::vector<float> & y)
 	Q_EMIT dataChanged(this);
 }
 
-void UPlotCurve::getData(QVector<float> & x, QVector<float> & y) const
+void UPlotCurve::getData(QVector<qreal> & x, QVector<qreal> & y) const
 {
 	x.clear();
 	y.clear();
@@ -1018,7 +1018,7 @@ void UPlotCurve::getData(QVector<float> & x, QVector<float> & y) const
 	}
 }
 
-void UPlotCurve::getData(QMap<float,float> & data) const
+void UPlotCurve::getData(QMap<qreal,qreal> & data) const
 {
 	data.clear();
 	if(_items.size())
@@ -1034,7 +1034,7 @@ void UPlotCurve::getData(QMap<float,float> & data) const
 
 
 
-UPlotCurveThreshold::UPlotCurveThreshold(const QString & name, float thesholdValue, Qt::Orientation orientation, QObject * parent) :
+UPlotCurveThreshold::UPlotCurveThreshold(const QString & name, qreal thesholdValue, Qt::Orientation orientation, QObject * parent) :
 	UPlotCurve(name, parent),
 	_orientation(orientation)
 {
@@ -1055,7 +1055,7 @@ UPlotCurveThreshold::~UPlotCurveThreshold()
 
 }
 
-void UPlotCurveThreshold::setThreshold(float threshold)
+void UPlotCurveThreshold::setThreshold(qreal threshold)
 {
 #if PRINT_DEBUG
 	ULOGGER_DEBUG("%f", threshold);
@@ -1104,7 +1104,7 @@ void UPlotCurveThreshold::setOrientation(Qt::Orientation orientation)
 	}
 }
 
-void UPlotCurveThreshold::update(float scaleX, float scaleY, float offsetX, float offsetY, float xDir, float yDir, int maxItemsKept)
+void UPlotCurveThreshold::update(qreal scaleX, qreal scaleY, qreal offsetX, qreal offsetY, qreal xDir, qreal yDir, int maxItemsKept)
 {
 	if(_items.size() == 3)
 	{
@@ -1142,7 +1142,7 @@ void UPlotCurveThreshold::update(float scaleX, float scaleY, float offsetX, floa
 
 
 
-UPlotAxis::UPlotAxis(Qt::Orientation orientation, float min, float max, QWidget * parent) :
+UPlotAxis::UPlotAxis(Qt::Orientation orientation, qreal min, qreal max, QWidget * parent) :
 	QWidget(parent),
 	_orientation(orientation),
 	_min(0),
@@ -1177,14 +1177,14 @@ void UPlotAxis::setReversed(bool reversed)
 {
 	if(_reversed != reversed)
 	{
-		float min = _min;
+		qreal min = _min;
 		_min = _max;
 		_max = min;
 	}
 	_reversed = reversed;
 }
 
-void UPlotAxis::setAxis(float & min, float & max)
+void UPlotAxis::setAxis(qreal & min, qreal & max)
 {
 	int borderMin = 0;
 	int borderMax = 0;
@@ -1241,13 +1241,13 @@ void UPlotAxis::setAxis(float & min, float & max)
 	// Rounding min and max
 	if(min != max)
 	{
-		float mul = 1;
-		float rangef = max - min;
+		qreal mul = 1;
+		qreal rangef = max - min;
 		int countStep = _count/5;
-		float val;
+		qreal val;
 		for(int i=0; i<6; ++i)
 		{
-			val = (rangef/float(countStep)) * mul;
+			val = (rangef/qreal(countStep)) * mul;
 			if( val >= 1.0f && val < 10.0f)
 			{
 				break;
@@ -1264,8 +1264,8 @@ void UPlotAxis::setAxis(float & min, float & max)
 		//ULOGGER_DEBUG("min=%f, max=%f", min, max);
 		int minR = min*mul-0.9;
 		int maxR = max*mul+0.9;
-		min = float(minR)/mul;
-		max = float(maxR)/mul;
+		min = qreal(minR)/mul;
+		max = qreal(maxR)/mul;
 		//ULOGGER_DEBUG("mul=%f, minR=%d, maxR=%d,countStep=%d", mul, minR, maxR, countStep);
 	}
 
@@ -1430,8 +1430,8 @@ void UPlotLegendItem::contextMenuEvent(QContextMenuEvent * event)
 	{
 		if(_curve)
 		{
-			QVector<float> x;
-			QVector<float> y;
+			QVector<qreal> x;
+			QVector<qreal> y;
 			_curve->getData(x, y);
 			QString text;
 			text.append("x");
@@ -1501,11 +1501,11 @@ QPixmap UPlotLegendItem::createSymbol(const QPen & pen, const QBrush & brush)
 
 void UPlotLegendItem::updateStdDevMeanMax()
 {
-	QVector<float> x, y;
+	QVector<qreal> x, y;
 	_curve->getData(x, y);
-	float mean = uMean(y.data(), y.size());
-	float stdDev = std::sqrt(uVariance(y.data(), y.size(), mean));
-	float max = uMax(y.data(), y.size());
+	qreal mean = uMean(y.data(), y.size());
+	qreal stdDev = std::sqrt(uVariance(y.data(), y.size(), mean));
+	qreal max = uMax(y.data(), y.size());
 	QString nameSpaced = _curve->name();
 	nameSpaced.replace('_', ' ');
 	nameSpaced += QString("\n(%1=%2, %3=%4, max=%5, n=%6)").arg(QChar(0xbc, 0x03)).arg(QString::number(mean, 'f', 3)).arg(QChar(0xc3, 0x03)).arg(QString::number(stdDev, 'f', 3)).arg(QString::number(max, 'f', 3)).arg(y.size());
@@ -1691,28 +1691,28 @@ void UPlotLegend::contextMenuEvent(QContextMenuEvent * event)
 		if(items.size())
 		{
 			// create common x-axis
-			QMap<float, float> xAxisMap;
+			QMap<qreal, qreal> xAxisMap;
 			for(int i=0; i<items.size(); ++i)
 			{
-				QMap<float, float> data;
+				QMap<qreal, qreal> data;
 				items.at(i)->curve()->getData(data);
-				for(QMap<float, float>::iterator iter=data.begin(); iter!=data.end(); ++iter)
+				for(QMap<qreal, qreal>::iterator iter=data.begin(); iter!=data.end(); ++iter)
 				{
 					xAxisMap.insert(iter.key(), iter.value());
 				}
 			}
-			QList<float> xAxis = xAxisMap.uniqueKeys();
+			QList<qreal> xAxis = xAxisMap.uniqueKeys();
 
-			QVector<QVector<float> > axes;
+			QVector<QVector<qreal> > axes;
 			for(int i=0; i<items.size(); ++i)
 			{
-				QMap<float, float> data;
+				QMap<qreal, qreal> data;
 				items.at(i)->curve()->getData(data);
 
-				QVector<float> y(xAxis.size(), std::numeric_limits<float>::quiet_NaN());
+				QVector<qreal> y(xAxis.size(), std::numeric_limits<qreal>::quiet_NaN());
 				// just to make sure that we have the same number of data on each curve, set NAN for unknowns
 				int j=0;
-				for(QList<float>::iterator iter=xAxis.begin(); iter!=xAxis.end(); ++iter)
+				for(QList<qreal>::iterator iter=xAxis.begin(); iter!=xAxis.end(); ++iter)
 				{
 					if(data.contains(*iter))
 					{
@@ -2226,7 +2226,7 @@ void UPlot::replot(QPainter * painter)
 		}
 	}
 
-	float axis[4] = {0};
+	qreal axis[4] = {0};
 	for(int i=0; i<4; ++i)
 	{
 		axis[i] = _axisMaximums[i];
@@ -2244,8 +2244,8 @@ void UPlot::replot(QPainter * painter)
 
 	QRectF newRect(0,0, _graphicsViewHolder->size().width(), _graphicsViewHolder->size().height());
 	_view->scene()->setSceneRect(newRect);
-	float borderHor = (float)_horizontalAxis->border();
-	float borderVer = (float)_verticalAxis->border();
+	qreal borderHor = (qreal)_horizontalAxis->border();
+	qreal borderVer = (qreal)_verticalAxis->border();
 
 	//grid
 	qDeleteAll(hGridLines);
@@ -2255,14 +2255,14 @@ void UPlot::replot(QPainter * painter)
 	if(_aShowGrid->isChecked())
 	{
 		// TODO make a PlotGrid class ?
-		float w = newRect.width()-(borderHor*2);
-		float h = newRect.height()-(borderVer*2);
-		float stepH = w / float(_horizontalAxis->count());
-		float stepV = h / float(_verticalAxis->count());
+		qreal w = newRect.width()-(borderHor*2);
+		qreal h = newRect.height()-(borderVer*2);
+		qreal stepH = w / qreal(_horizontalAxis->count());
+		qreal stepV = h / qreal(_verticalAxis->count());
 		QPen dashPen(Qt::DashLine);
 		dashPen.setColor(QColor(255-_bgColor.red(), 255-_bgColor.green(), 255-_bgColor.blue(), 100));
 		QPen pen(dashPen.color());
-		for(float i=0.0f; i*stepV <= h+stepV; i+=5.0f)
+		for(qreal i=0.0f; i*stepV <= h+stepV; i+=5.0f)
 		{
 			//horizontal lines
 			if(!_aGraphicsView->isChecked())
@@ -2291,7 +2291,7 @@ void UPlot::replot(QPainter * painter)
 				hGridLines.last()->setPen(pen);
 			}
 		}
-		for(float i=0; i*stepH < w+stepH; i+=5.0f)
+		for(qreal i=0; i*stepH < w+stepH; i+=5.0f)
 		{
 			//vertical lines
 			if(!_aGraphicsView->isChecked())
@@ -2323,9 +2323,9 @@ void UPlot::replot(QPainter * painter)
 	}
 
 	// curves
-	float scaleX = 1;
-	float scaleY = 1;
-	float den = 0;
+	qreal scaleX = 1;
+	qreal scaleY = 1;
+	qreal den = 0;
 	den = axis[1] - axis[0];
 	if(den != 0)
 	{
@@ -2340,8 +2340,8 @@ void UPlot::replot(QPainter * painter)
 	{
 		if((*i)->isVisible())
 		{
-			float xDir = 1.0f;
-			float yDir = -1.0f;
+			qreal xDir = 1.0f;
+			qreal yDir = -1.0f;
 			(*i)->update(scaleX,
 						scaleY,
 						xDir<0?axis[1]+borderHor/scaleX:-(axis[0]-borderHor/scaleX),
@@ -2359,7 +2359,7 @@ void UPlot::replot(QPainter * painter)
 	// Update refresh rate
 	if(_aShowRefreshRate->isChecked())
 	{
-		int refreshRate = qRound(1000.0f/float(_refreshIntervalTime.restart()));
+		int refreshRate = qRound(1000.0f/qreal(_refreshIntervalTime.restart()));
 		if(refreshRate > 0 && refreshRate < _lowestRefreshRate)
 		{
 			_lowestRefreshRate = refreshRate;
@@ -2374,14 +2374,14 @@ void UPlot::replot(QPainter * painter)
 	}
 }
 
-void UPlot::setFixedXAxis(float x1, float x2)
+void UPlot::setFixedXAxis(qreal x1, qreal x2)
 {
 	_fixedAxis[0] = true;
 	_axisMaximums[0] = x1;
 	_axisMaximums[1] = x2;
 }
 
-void UPlot::setFixedYAxis(float y1, float y2)
+void UPlot::setFixedYAxis(qreal y1, qreal y2)
 {
 	_fixedAxis[1] = true;
 	_axisMaximums[2] = y1;
@@ -2392,7 +2392,7 @@ void UPlot::updateAxis(const UPlotCurve * curve)
 {
 	if(curve && curve->isVisible() && curve->itemsSize() && curve->isMinMaxValid())
 	{
-		const QVector<float> & minMax = curve->getMinMax();
+		const QVector<qreal> & minMax = curve->getMinMax();
 		//ULOGGER_DEBUG("x1=%f, x2=%f, y1=%f, y2=%f", minMax[0], minMax[1], minMax[2], minMax[3]);
 		if(minMax.size() != 4)
 		{
@@ -2404,7 +2404,7 @@ void UPlot::updateAxis(const UPlotCurve * curve)
 	}
 }
 
-bool UPlot::updateAxis(float x1, float x2, float y1, float y2)
+bool UPlot::updateAxis(qreal x1, qreal x2, qreal y1, qreal y2)
 {
 	bool modified = false;
 	modified = updateAxis(x1,y1);
@@ -2419,7 +2419,7 @@ bool UPlot::updateAxis(float x1, float x2, float y1, float y2)
 	return modified;
 }
 
-bool UPlot::updateAxis(float x, float y)
+bool UPlot::updateAxis(qreal x, qreal y)
 {
 	//ULOGGER_DEBUG("x=%f, y=%f", x,y);
 	bool modified = false;
@@ -2470,7 +2470,7 @@ void UPlot::updateAxis()
 	{
 		if(_curves.at(i)->isVisible() && _curves.at(i)->isMinMaxValid())
 		{
-			const QVector<float> & minMax = _curves.at(i)->getMinMax();
+			const QVector<qreal> & minMax = _curves.at(i)->getMinMax();
 			this->updateAxis(minMax[0], minMax[1], minMax[2], minMax[3]);
 		}
 	}
@@ -2561,7 +2561,7 @@ void UPlot::mouseMoveEvent(QMouseEvent * event)
 			_mousePressedPos = _mouseCurrentPos;
 		}
 
-		float x,y;
+		qreal x,y;
 		if(mousePosToValue(event->pos(), x ,y))
 		{
 			if(QApplication::mouseButtons() & Qt::LeftButton)
@@ -2603,7 +2603,7 @@ void UPlot::mouseReleaseEvent(QMouseEvent * event)
 
 		if(right - left > 5 || bottom - top > 5)
 		{
-			float axis[4];
+			qreal axis[4];
 			if(mousePosToValue(QPoint(left, top), axis[0], axis[3]) && mousePosToValue(QPoint(right, bottom), axis[1], axis[2]))
 			{
 #if PRINT_DEBUG
@@ -2631,7 +2631,7 @@ void UPlot::mouseDoubleClickEvent(QMouseEvent * event)
 	QWidget::mouseDoubleClickEvent(event);
 }
 
-bool UPlot::mousePosToValue(const QPoint & pos, float & x, float & y)
+bool UPlot::mousePosToValue(const QPoint & pos, qreal & x, qreal & y)
 {
 	int xPos = pos.x() - _graphicsViewHolder->pos().x() - _horizontalAxis->border();
 	int yPos = pos.y() - _graphicsViewHolder->pos().y() - _verticalAxis->border();
@@ -2662,15 +2662,15 @@ bool UPlot::mousePosToValue(const QPoint & pos, float & x, float & y)
 
 	//UDEBUG("IN");
 	//UDEBUG("x1=%f, x2=%f, y1=%f, y2=%f", _axisMaximums[0], _axisMaximums[1], _axisMaximums[2], _axisMaximums[3]);
-	//UDEBUG("border hor=%f ver=%f", (float)_horizontalAxis->border(), (float)_verticalAxis->border());
+	//UDEBUG("border hor=%f ver=%f", (qreal)_horizontalAxis->border(), (qreal)_verticalAxis->border());
 	//UDEBUG("rect = %d,%d %d,%d", _graphicsViewHolder->pos().x(), _graphicsViewHolder->pos().y(), _graphicsViewHolder->width(), _graphicsViewHolder->height());
 	//UDEBUG("%d,%d", event->pos().x(), event->pos().y());
 	//UDEBUG("x/y %d,%d", x, y);
 	//UDEBUG("max %d,%d", maxX, maxY);
 
 	//UDEBUG("map %f,%f", x, y);
-	x = _axisMaximums[0] + float(xPos)*(_axisMaximums[1] - _axisMaximums[0]) / float(maxX);
-	y = _axisMaximums[2] + float(maxY - yPos)*(_axisMaximums[3] - _axisMaximums[2]) / float(maxY);
+	x = _axisMaximums[0] + qreal(xPos)*(_axisMaximums[1] - _axisMaximums[0]) / qreal(maxX);
+	y = _axisMaximums[2] + qreal(maxY - yPos)*(_axisMaximums[3] - _axisMaximums[2]) / qreal(maxY);
 	return true;
 }
 
@@ -2943,8 +2943,61 @@ void UPlot::clearData()
 	_aGraphicsView->isChecked()?this->replot(0):this->update();
 }
 
+void UPlot::frameData(bool xAxis, bool yAxis)
+{
+	if(!xAxis && !yAxis)
+	{
+		return;
+	}
+	qreal minX = std::numeric_limits<qreal>::max();
+	qreal minY = std::numeric_limits<qreal>::max();
+	for(int i=0; i<_curves.size(); ++i)
+	{
+		if(qobject_cast<UPlotCurveThreshold*>(_curves.at(i)) == 0)
+		{
+			const QVector<qreal> & minMax = _curves.at(i)->getMinMax();
+			if(minMax.size() == 4)
+			{
+				if(minMax[0] < minX)
+				{
+					minX = minMax[0];
+				}
+				if(minMax[2] < minY)
+				{
+					minY = minMax[2];
+				}
+			}
+		}
+	}
+	if(minX != std::numeric_limits<qreal>::max())
+	{
+		for(int i=0; i<_curves.size(); ++i)
+		{
+			if(qobject_cast<UPlotCurveThreshold*>(_curves.at(i)) == 0)
+			{
+				QVector<qreal> x;
+				QVector<qreal> y;
+				_curves.at(i)->getData(x,y);
+				for(int j=0; j<x.size(); ++j)
+				{
+					if(xAxis)
+					{
+						x[j]-=minX;
+					}
+					if(yAxis)
+					{
+						y[j]-=minY;
+					}
+				}
+				_curves.at(i)->setData(x,y);
+			}
+		}
+	}
+	_aGraphicsView->isChecked()?this->replot(0):this->update();
+}
+
 // for convenience...
-UPlotCurveThreshold * UPlot::addThreshold(const QString & name, float value, Qt::Orientation orientation)
+UPlotCurveThreshold * UPlot::addThreshold(const QString & name, qreal value, Qt::Orientation orientation)
 {
 	UPlotCurveThreshold * curve = new UPlotCurveThreshold(name, value, orientation, this);
 	QPen pen = curve->pen();

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>rtabmap</name>
-  <version>0.20.0</version>
+  <version>0.20.2</version>
   <description>RTAB-Map's standalone library. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
   <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>
@@ -12,7 +12,6 @@
 
   <buildtool_depend>cmake</buildtool_depend>
   
-  <build_depend>libvtk-qt</build_depend>
   <build_depend>qt_gui_cpp</build_depend> <!-- libqt4-dev or libqt5-dev -->
   <build_depend>libpcl-all-dev</build_depend>
   <build_depend>libsqlite3-dev</build_depend>
@@ -26,7 +25,6 @@
   <build_depend>octomap</build_depend>
   <build_depend>libg2o</build_depend>
   
-  <run_depend>libvtk-qt</run_depend>
   <run_depend>qt_gui_cpp</run_depend>
   <run_depend>libpcl-all-dev</run_depend>
   <run_depend>libsqlite3-dev</run_depend>

tools/Calibration/CMakeLists.txt

@@ -26,6 +26,14 @@ IF(realsense2_FOUND)
 	)
 ENDIF(realsense2_FOUND)
 
+# Hack as CameraK4A.h needs k4a include dir
+IF(k4a_FOUND)
+	SET(INCLUDE_DIRS
+		${INCLUDE_DIRS}
+		${k4a_INCLUDE_DIRS}
+	)
+ENDIF(k4a_FOUND)
+
 INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
 
 ADD_EXECUTABLE(calibration main.cpp)

tools/CameraRGBD/CMakeLists.txt

@@ -26,6 +26,14 @@ IF(realsense2_FOUND)
 	)
 ENDIF(realsense2_FOUND)
 
+# Hack as CameraK4A.h needs k4a include dir
+IF(k4a_FOUND)
+	SET(INCLUDE_DIRS
+		${INCLUDE_DIRS}
+		${k4a_INCLUDE_DIRS}
+	)
+ENDIF(k4a_FOUND)
+
 INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
 
 ADD_EXECUTABLE(rgbd_camera main.cpp)

tools/CameraRGBD/main.cpp

@@ -303,7 +303,7 @@ int main(int argc, char * argv[])
 			UERROR("Not built with Kinect for Azure SDK support...");
 			exit(-1);
 		}
-		camera = new rtabmap::CameraK4A(1, 0, rtabmap::Transform::getIdentity());
+		camera = new rtabmap::CameraK4A(1);
 	}
 	else if (driver == 13)
 	{
@@ -347,9 +347,6 @@ int main(int argc, char * argv[])
 	{
 		viewer = new pcl::visualization::CloudViewer("cloud");
 	}
-	rtabmap::Transform t(1, 0, 0, 0,
-						 0, -1, 0, 0,
-						 0, 0, -1, 0);
 
 	cv::VideoWriter videoWriter;
 	UDirectory dir;
@@ -418,7 +415,7 @@ int main(int argc, char * argv[])
 				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::cloudFromDepthRGB(
 						rgb, depth,
 						data.cameraModels()[0]);
-				cloud = rtabmap::util3d::transformPointCloud(cloud, t);
+				cloud = rtabmap::util3d::transformPointCloud(cloud, rtabmap::Transform::opengl_T_rtabmap()*data.cameraModels()[0].localTransform());
 				if(viewer)
 					viewer->showCloud(cloud, "cloud");
 			}
@@ -429,7 +426,7 @@ int main(int argc, char * argv[])
 				pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::cloudFromDepth(
 						depth,
 						data.cameraModels()[0]);
-				cloud = rtabmap::util3d::transformPointCloud(cloud, t);
+				cloud = rtabmap::util3d::transformPointCloud(cloud, rtabmap::Transform::opengl_T_rtabmap()*data.cameraModels()[0].localTransform());
 				viewer->showCloud(cloud, "cloud");
 			}
 
@@ -457,7 +454,7 @@ int main(int argc, char * argv[])
 				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::cloudFromStereoImages(
 						rgb, right,
 						data.stereoCameraModel());
-				cloud = rtabmap::util3d::transformPointCloud(cloud, t);
+				cloud = rtabmap::util3d::transformPointCloud(cloud, rtabmap::Transform::opengl_T_rtabmap()*data.stereoCameraModel().localTransform());
 				if(viewer)
 					viewer->showCloud(cloud, "cloud");
 			}

tools/DataRecorder/CMakeLists.txt

@@ -32,6 +32,14 @@ IF(realsense2_FOUND)
 	)
 ENDIF(realsense2_FOUND)
 
+# Hack as CameraK4A.h needs k4a include dir
+IF(k4a_FOUND)
+	SET(INCLUDE_DIRS
+		${INCLUDE_DIRS}
+		${k4a_INCLUDE_DIRS}
+	)
+ENDIF(k4a_FOUND)
+
 INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
 
 IF(MINGW)

tools/DataRecorder/main.cpp

@@ -56,6 +56,12 @@ void showUsage()
 			"                                     5=Freenect2  (Kinect v2)\n"
 			"                                     6=DC1394     (Bumblebee2)\n"
 			"                                     7=FlyCapture2 (Bumblebee2)\n"
+			"                                     8=ZED stereo\n"
+			"                                     9=RealSense\n"
+			"                                     10=Kinect for Windows 2 SDK\n"
+			"                                     11=RealSense2\n"
+			"                                     12=Kinect for Azure SDK\n"
+			"                                     13=MYNT EYE S\n"
 			"  -device ""                Device ID (default \"\")\n");
 	exit(1);
 }
@@ -127,7 +133,7 @@ int main (int argc, char * argv[])
 			if(i < argc)
 			{
 				driver = std::atoi(argv[i]);
-				if(driver < 0 || driver > 7)
+				if(driver < 0 || driver > 13)
 				{
 					showUsage();
 				}
@@ -176,10 +182,9 @@ int main (int argc, char * argv[])
 	signal(SIGINT, &sighandler);
 
 	rtabmap::Camera * camera = 0;
-	rtabmap::Transform t=rtabmap::Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0);
 	if(driver == 0)
 	{
-		camera = new rtabmap::CameraOpenni(deviceId, rate, t);
+		camera = new rtabmap::CameraOpenni(deviceId, rate);
 	}
 	else if(driver == 1)
 	{
@@ -188,7 +193,7 @@ int main (int argc, char * argv[])
 			UERROR("Not built with OpenNI2 support...");
 			exit(-1);
 		}
-		camera = new rtabmap::CameraOpenNI2(deviceId, CameraOpenNI2::kTypeColorDepth, rate, t);
+		camera = new rtabmap::CameraOpenNI2(deviceId, CameraOpenNI2::kTypeColorDepth, rate);
 	}
 	else if(driver == 2)
 	{
@@ -197,7 +202,7 @@ int main (int argc, char * argv[])
 			UERROR("Not built with Freenect support...");
 			exit(-1);
 		}
-		camera = new rtabmap::CameraFreenect(deviceId.size()?atoi(deviceId.c_str()):0, CameraFreenect::kTypeColorDepth, rate, t);
+		camera = new rtabmap::CameraFreenect(deviceId.size()?atoi(deviceId.c_str()):0, CameraFreenect::kTypeColorDepth, rate);
 	}
 	else if(driver == 3)
 	{
@@ -206,7 +211,7 @@ int main (int argc, char * argv[])
 			UERROR("Not built with OpenNI from OpenCV support...");
 			exit(-1);
 		}
-		camera = new rtabmap::CameraOpenNICV(false, rate, t);
+		camera = new rtabmap::CameraOpenNICV(false, rate);
 	}
 	else if(driver == 4)
 	{
@@ -215,7 +220,7 @@ int main (int argc, char * argv[])
 			UERROR("Not built with OpenNI from OpenCV support...");
 			exit(-1);
 		}
-		camera = new rtabmap::CameraOpenNICV(true, rate, t);
+		camera = new rtabmap::CameraOpenNICV(true, rate);
 	}
 	else if(driver == 5)
 	{
@@ -224,7 +229,7 @@ int main (int argc, char * argv[])
 			UERROR("Not built with Freenect2 support...");
 			exit(-1);
 		}
-		camera = new rtabmap::CameraFreenect2(deviceId.size()?atoi(deviceId.c_str()):0, rtabmap::CameraFreenect2::kTypeColor2DepthSD, rate, t);
+		camera = new rtabmap::CameraFreenect2(deviceId.size()?atoi(deviceId.c_str()):0, rtabmap::CameraFreenect2::kTypeColor2DepthSD, rate);
 	}
 	else if(driver == 6)
 	{
@@ -233,7 +238,7 @@ int main (int argc, char * argv[])
 			UERROR("Not built with dc1394 support...");
 			exit(-1);
 		}
-		camera = new rtabmap::CameraStereoDC1394(rate, t);
+		camera = new rtabmap::CameraStereoDC1394(rate);
 	}
 	else if(driver == 7)
 	{
@@ -242,7 +247,61 @@ int main (int argc, char * argv[])
 			UERROR("Not built with FlyCapture2/Triclops support...");
 			exit(-1);
 		}
-		camera = new rtabmap::CameraStereoFlyCapture2(rate, t);
+		camera = new rtabmap::CameraStereoFlyCapture2(rate);
+	}
+	else if(driver == 8)
+	{
+		if(!rtabmap::CameraStereoZed::available())
+		{
+			UERROR("Not built with ZED sdk support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraStereoZed(uStr2Int(deviceId));
+	}
+	else if (driver == 9)
+	{
+		if (!rtabmap::CameraRealSense::available())
+		{
+			UERROR("Not built with RealSense support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraRealSense(uStr2Int(deviceId));
+	}
+	else if (driver == 10)
+	{
+		if (!rtabmap::CameraK4W2::available())
+		{
+			UERROR("Not built with Kinect for Windows 2 SDK support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraK4W2(uStr2Int(deviceId));
+	}
+	else if (driver == 11)
+	{
+		if (!rtabmap::CameraRealSense2::available())
+		{
+			UERROR("Not built with RealSense2 SDK support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraRealSense2(deviceId);
+	}
+	else if (driver == 12)
+	{
+		if (!rtabmap::CameraK4A::available())
+		{
+			UERROR("Not built with Kinect for Azure SDK support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraK4A(1);
+	}
+	else if (driver == 13)
+	{
+		if (!rtabmap::CameraMyntEye::available())
+		{
+			UERROR("Not built with Mynt Eye S support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraMyntEye(deviceId);
 	}
 	else
 	{

tools/Export/main.cpp

@@ -169,7 +169,7 @@ int main(int argc, char * argv[])
 #ifdef RTABMAP_ALICE_VISION
 			multiband = true;
 #else
-			printf("\"--multiband\" option cannot be used vecause RTAB-Map is not built with AliceVision support. Ignoring multiband...\n");
+			printf("\"--multiband\" option cannot be used because RTAB-Map is not built with AliceVision support. Ignoring multiband...\n");
 #endif
 		}
 		else if(std::strcmp(argv[i], "--poisson_depth") == 0)
@@ -487,7 +487,7 @@ int main(int argc, char * argv[])
 							textureMesh->tex_materials[i].tex_file += ".jpg";
 							printf("Saving texture to %s.\n", textureMesh->tex_materials[i].tex_file.c_str());
 							UASSERT(textures.cols % textures.rows == 0);
-							success = cv::imwrite(textureMesh->tex_materials[i].tex_file, cv::Mat(textures, cv::Range::all(), cv::Range(textures.rows*i, textures.rows*(i+1))));
+							success = cv::imwrite(outputDirectory+"/"+textureMesh->tex_materials[i].tex_file, cv::Mat(textures, cv::Range::all(), cv::Range(textures.rows*i, textures.rows*(i+1))));
 							if(!success)
 							{
 								UERROR("Failed saving %s!", textureMesh->tex_materials[i].tex_file.c_str());