package.xml: added explicitly libvtk-qt dependency for debian build error

.travis.yml

@@ -53,21 +53,6 @@ matrix:
         - 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:
     - matlabbe@gmail.com

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 2)
+SET(RTABMAP_PATCH_VERSION 0)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   
@@ -689,6 +689,16 @@ 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-279122.nn.r.appspot.com/UA-56986679-3/github-main?pixel) 
+rtabmap ![Analytics](https://ga-beacon.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-279122.nn.r.appspot.com/UA-56986679-3/git
 [![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.20.2-green.svg?style=flat
+[release-image]: https://img.shields.io/badge/release-0.18.0-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,6 +38,7 @@ 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="@style/ThemeApp" />
+            android:theme="@android:style/Theme.Material.Light.Dialog.Alert" />
             
         <provider
             android:name="android.support.v4.content.FileProvider"

app/android/jni/CMakeLists.txt

@@ -22,7 +22,6 @@ 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,31 +34,47 @@ 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 depthFromMotion, bool smoothing):
+CameraARCore::CameraARCore(void* env, void* context, void* activity, bool smoothing):
 	CameraMobile(smoothing),
 	env_(env),
 	context_(context),
 	activity_(activity),
-	arInstallRequested_(false),
-	textureId_(9999),
-	uvs_initialized_(false),
-	updateOcclusionImage_(false),
-	depthFromMotion_(depthFromMotion)
+	arInstallRequested_(false)
 {
+	glGenTextures(1, &textureId_);
 }
 
 CameraARCore::~CameraARCore() {
 	// Disconnect ARCore service
 	close();
 
-	if(textureId_ != 9999)
-	{
 	glDeleteTextures(1, &textureId_);
-		textureId_ = 9999;
-	}
 }
 
 
@@ -130,10 +146,132 @@ 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;
@@ -164,19 +302,10 @@ 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_);
 
-	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);
+	ArConfig_setFocusMode(arSession_, arConfig_, AR_FOCUS_MODE_FIXED);
 	UASSERT(ArSession_configure(arSession_, arConfig_) == AR_SUCCESS);
 
 	ArFrame_create(arSession_, &arFrame_);
@@ -232,6 +361,9 @@ 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!");
@@ -278,8 +410,45 @@ 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(
@@ -330,20 +499,6 @@ 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_);
@@ -355,44 +510,15 @@ 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);
 
@@ -425,55 +551,17 @@ 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);
@@ -535,7 +623,7 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 							LOGI("pointCloud empty");
 						}
 
-						data = SensorData(scan, rgb, depthFromMotion_?occlusionImage_:cv::Mat(), model, 0, stamp);
+						data = SensorData(scan, rgb, cv::Mat(), model, 0, stamp);
 						data.setFeatures(kpts,  kpts3, cv::Mat());
 					}
 				}
@@ -574,49 +662,21 @@ 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::capturePoseOnly() ArSession_update error");
+		LOGE("CameraARCore::captureImage() 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);
 
@@ -634,52 +694,6 @@ 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,13 +38,14 @@ 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 {
 
@@ -60,28 +61,19 @@ public:
 			std::vector<cv::Point3f> * kpts3D = 0);
 
 public:
-	CameraARCore(void* env, void* context, void* activity, bool depthFromMotion = false, bool smoothing = false);
+	CameraARCore(void* env, void* context, void* activity, 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); // should be called in opengl thread
+	virtual SensorData captureImage(CameraInfo * info = 0);
 	virtual void capturePoseOnly();
 
 private:
@@ -100,15 +92,22 @@ private:
 	GLuint textureId_;
 	UMutex arSessionMutex_;
 
-	float transformed_uvs_[BackgroundRenderer::kNumVertices*2];
-	bool uvs_initialized_ = false;
-	glm::mat4 viewMatrix_;
-	glm::mat4 projectionMatrix_;
+#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;
 
-	bool updateOcclusionImage_;
-	cv::Mat occlusionImage_;
-	CameraModel occlusionModel_;
-	bool depthFromMotion_;
+	ACameraDevice_StateCallbacks deviceStateCallbacks_;
+	ACameraCaptureSession_stateCallbacks captureSessionStateCallbacks_;
+
+	ACameraManager* cameraManager_ = nullptr;
+	AImageReader* imageReader_ = nullptr;
+#endif // DEPTH_TEST
 };
 
 } /* namespace rtabmap */

app/android/jni/CameraMobile.cpp

@@ -207,7 +207,6 @@ 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);
@@ -227,18 +226,9 @@ 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);
@@ -254,18 +244,9 @@ 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);
@@ -282,14 +263,6 @@ 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;}
-	virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height) {colorCameraToDisplayRotation_ = colorCameraToDisplayRotation;}
+	void setScreenRotation(ScreenRotation colorCameraToDisplayRotation) {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,7 +69,6 @@ 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
 
@@ -264,7 +263,7 @@ void RTABMapApp::setScreenRotation(int displayRotation, int cameraRotation)
 	boost::mutex::scoped_lock  lock(cameraMutex_);
 	if(camera_)
 	{
-		camera_->setScreenRotationAndSize(main_scene_.getScreenRotation(), main_scene_.getViewPortWidth(), main_scene_.getViewPortHeight());
+		camera_->setScreenRotation(rotation);
 	}
 }
 
@@ -658,10 +657,6 @@ 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_);
@@ -690,7 +685,8 @@ bool RTABMapApp::startCamera(JNIEnv* env, jobject iBinder, jobject context, jobj
 	else if(cameraDriver_ == 1)
 	{
 #ifdef RTABMAP_ARCORE
-		camera_ = new rtabmap::CameraARCore(env, context, activity, depthFromMotion_, smoothing_);
+		camera_ = new rtabmap::CameraARCore(env, context, activity, smoothing_);
+
 #else
 		UERROR("RTAB-Map is not built with ARCore support!");
 #endif
@@ -716,7 +712,7 @@ bool RTABMapApp::startCamera(JNIEnv* env, jobject iBinder, jobject context, jobj
 
 	if(camera_->init())
 	{
-		camera_->setScreenRotationAndSize(main_scene_.getScreenRotation(), main_scene_.getViewPortWidth(), main_scene_.getViewPortHeight());
+		camera_->setScreenRotation(main_scene_.getScreenRotation());
 
 		//update mesh decimation based on camera calibration
 		LOGI("Cloud density level %d", cloudDensityLevel_);
@@ -941,11 +937,6 @@ 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
@@ -1110,59 +1101,12 @@ 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
 			}
 		}
 
@@ -1848,7 +1792,7 @@ int RTABMapApp::Render()
 
 			fpsTime.restart();
 			main_scene_.setFrustumVisible(camera_!=0);
-			lastDrawnCloudsCount_ = main_scene_.Render(uvsTransformed, arViewMatrix, arProjectionMatrix, occlusionMesh);
+			lastDrawnCloudsCount_ = main_scene_.Render();
 			if(renderingTime_ < fpsTime.elapsed())
 			{
 				renderingTime_ = fpsTime.elapsed();
@@ -2134,14 +2078,6 @@ 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,7 +44,6 @@ 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:
@@ -110,7 +109,6 @@ 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);
@@ -184,7 +182,6 @@ class RTABMapApp : public UEventsHandler {
   bool trajectoryMode_;
   bool rawScanSaved_;
   bool smoothing_;
-  bool depthFromMotion_;
   bool cameraColor_;
   bool fullResolution_;
   bool appendMode_;

app/android/jni/background_renderer.cc

@@ -1,92 +0,0 @@
-/*
- * 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

@@ -1,58 +0,0 @@
-/*
- * 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,19 +512,6 @@ 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,7 +69,6 @@ const std::string kGraphFragmentShader =
 
 
 Scene::Scene() :
-		background_renderer_(0),
 		gesture_camera_(0),
 		axis_(0),
 		frustum_(0),
@@ -161,8 +160,6 @@ void Scene::DeleteResources() {
 		delete trace_;
 		delete grid_;
 		delete box_;
-		delete background_renderer_;
-		background_renderer_ = 0;
 	}
 
 	PointCloudDrawable::releaseShaderPrograms();
@@ -367,7 +364,7 @@ bool intersectFrustumAABB(
 }
 
 //Should only be called in OpenGL thread!
-int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat4 arProjectionMatrix, const rtabmap::Mesh & occlusionMesh) {
+int Scene::Render() {
 	UASSERT(gesture_camera_ != 0);
 
 	if(currentPose_ == 0)
@@ -398,17 +395,6 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
 	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(
@@ -458,7 +444,7 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
 
 	UTimer timer;
 
-	bool onlineBlending = (renderBackgroundCamera && occlusionMesh.cloud.get() && occlusionMesh.cloud->size()) || (blending_ && gesture_camera_->GetCameraType()!=tango_gl::GestureCamera::kTopOrtho && mapRendering_ && meshRendering_ && cloudsToDraw.size()>1);
+	bool onlineBlending = blending_ && gesture_camera_->GetCameraType()!=tango_gl::GestureCamera::kTopOrtho && mapRendering_ && meshRendering_ && cloudsToDraw.size()>1;
 	if(onlineBlending && fboId_)
 	{
 		// set the rendering destination to FBO
@@ -468,20 +454,12 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
 		glClearColor(1, 1, 1, 1);
 		glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
 
-		if(renderBackgroundCamera)
-		{
-			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
 		glBindFramebuffer(GL_FRAMEBUFFER, 0); // unbind
@@ -517,15 +495,6 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
 	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)
@@ -554,7 +523,7 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
 		}
 	}
 
-	if(gridVisible_ && !renderBackgroundCamera)
+	if(gridVisible_)
 	{
 		grid_->Render(projectionMatrix, viewMatrix);
 	}

app/android/jni/scene.h

@@ -38,7 +38,6 @@
 #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>
@@ -72,14 +71,13 @@ class Scene {
   //         frame's timestamp.
   // @param: point_cloud_vertices, point cloud's vertices of the current point
   //         frame.
-  int Render(const float * uvsTransformed = 0, glm::mat4 arViewMatrix = glm::mat4(0), glm::mat4 arProjectionMatrix=glm::mat4(0), const rtabmap::Mesh & occlusionMesh=rtabmap::Mesh());
+  int Render();
 
   // 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();}
@@ -154,8 +152,6 @@ 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 / 12.0f;
-      cam_cur_angle_.y = kThirdPersonFollow?0:M_PI / 2.0f;
+      cam_cur_angle_.x = -M_PI / 6.0f;
+      cam_cur_angle_.y = kThirdPersonFollow?0:M_PI / 4.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 orientation, values
+// @param color_camera: integer value of color camera oreintation, 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,7 +163,6 @@
         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

@@ -98,11 +98,6 @@
 	            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"
 		        android:summary="@string/pref_summary_smoothing"

app/android/res/values/strings.xml

@@ -81,8 +81,6 @@
     
     <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>
@@ -325,8 +323,6 @@
     <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

@@ -0,0 +1,588 @@
+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,7 +992,6 @@ 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);
@@ -1163,7 +1162,6 @@ 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));
@@ -1199,7 +1197,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)
@@ -1220,8 +1218,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 			}
 			Thread bindThread = new Thread(new Runnable() {
 				public void run() {
-					
-					if(mCameraDriver==1 && !depthFromMotion)
+					if(mCameraDriver==1)
 					{
 						RTABMapLib.setMeshRendering(
 								nativeApplication, 
@@ -1266,9 +1263,9 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 							}
 							else
 							{
-								if((mState==State.STATE_IDLE || mState==State.STATE_WELCOME) && mCameraDriver == 1 && !depthFromMotion)
+								if((mState==State.STATE_IDLE || mState==State.STATE_WELCOME) && mCameraDriver == 1)
 								{
-									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();
+									mToast.makeText(getApplicationContext(), "Currently ARCore NDK driver doesn't support depth, only poses and RGB images 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())
@@ -1281,7 +1278,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();
@@ -2274,14 +2271,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() {
@@ -2314,15 +2311,6 @@ 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,7 +72,6 @@ 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

@@ -0,0 +1,87 @@
+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

@@ -138,16 +138,6 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
           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'
   # Exclude debug libraries
@@ -172,12 +162,9 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
           DESTINATION ${plugin_dest_dir}/plugins${plugin_type}
           COMPONENT runtime)
 	endforeach()
-	IF(NOT Qt5Widgets_VERSION VERSION_LESS 5.10.0)
 	IF(WIN32)
+		IF(NOT Qt5Widgets_VERSION VERSION_LESS 5.10.0)
 			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})
@@ -188,6 +175,7 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
 				#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)
   ENDIF()
   
   # install a qt.conf file

corelib/include/rtabmap/core/Camera.h

@@ -53,7 +53,6 @@ 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;
@@ -74,7 +73,7 @@ protected:
 	 *
 	 * @param imageRate : image/second , 0 for fast as the camera can
 	 */
-	Camera(float imageRate = 0, const Transform & localTransform = CameraModel::opticalRotation());
+	Camera(float imageRate = 0, const Transform & localTransform = Transform::getIdentity());
 
 	/**
 	 * returned rgb and depth images should be already rectified if calibration was loaded

corelib/include/rtabmap/core/CameraModel.h

@@ -37,13 +37,6 @@ 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
@@ -57,7 +50,7 @@ public:
 			const cv::Mat & D,
 			const cv::Mat & R,
 			const cv::Mat & P,
-			const Transform & localTransform = opticalRotation());
+			const Transform & localTransform = Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0));
 
 	// minimal
 	CameraModel(
@@ -65,7 +58,7 @@ public:
 			double fy,
 			double cx,
 			double cy,
-			const Transform & localTransform = opticalRotation(),
+			const Transform & localTransform = Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0),
 			double Tx = 0.0f,
 			const cv::Size & imageSize = cv::Size(0,0));
 	// minimal to be saved
@@ -75,7 +68,7 @@ public:
 			double fy,
 			double cx,
 			double cy,
-			const Transform & localTransform = opticalRotation(),
+			const Transform & localTransform = Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0),
 			double Tx = 0.0f,
 			const cv::Size & imageSize = cv::Size(0,0));
 
@@ -120,12 +113,9 @@ public:
 	int imageWidth() const {return imageSize_.width;}
 	int imageHeight() const {return imageSize_.height;}
 
-	double fovX() const; // in radians
-	double fovY() const; // in radians
-	double horizontalFOV() const; // in degrees
-	double verticalFOV() const;   // in degrees
+	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;}
 
-	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;
@@ -135,6 +125,9 @@ 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 < 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)))
+#if CV_MAJOR_VERSION < 4 || (CV_MAJOR_VERSION == 4 && (CV_MINOR_VERSION < 3 || (CV_MINOR_VERSION==3 && !defined(RTABMAP_OPENCV_DEV))))
 class SIFT;
 #endif
 class SURF;
@@ -73,10 +73,10 @@ class ORB;
 class SURF_CUDA;
 }
 }
-#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)))
+#if CV_MAJOR_VERSION < 4 || (CV_MAJOR_VERSION == 4 && (CV_MINOR_VERSION < 3 || (CV_MINOR_VERSION==3 && !defined(RTABMAP_OPENCV_DEV))))
 typedef cv::xfeatures2d::SIFT CV_SIFT;
 #else
-typedef cv::SIFT CV_SIFT; // SIFT is back in features2d since 4.4.0 / 3.4.11
+typedef cv::SIFT CV_SIFT; // SIFT is back in features2d since 4.3.0-dev
 #endif
 typedef cv::xfeatures2d::SURF CV_SURF;
 typedef cv::FastFeatureDetector CV_FAST;

corelib/include/rtabmap/core/Rtabmap.h

@@ -86,32 +86,8 @@ public:
 			const cv::Mat & image,
 			int id=0, const std::map<std::string, float> & externalStats = std::map<std::string, float>());
 
-	/**
-	 * 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);
+	void init(const ParametersMap & parameters, const std::string & databasePath = "");
+	void init(const std::string & configFile = "", const std::string & databasePath = "");
 
 	/**
 	 * Close rtabmap. This will delete rtabmap object if set.
@@ -180,7 +156,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, bool withWords, bool withGlobalDescriptors) const;
+	Signature getSignatureCopy(int id, bool images, bool scan, bool userData, bool occupancyGrid) const;
 	RTABMAP_DEPRECATED(
 		void get3DMap(std::map<int, Signature> & signatures,
 				std::map<int, Transform> & poses,
@@ -195,9 +171,7 @@ public:
 			bool withImages = false,
 			bool withScan = false,
 			bool withUserData = false,
-			bool withGrid = false,
-			bool withWords = true,
-			bool withGlobalDescriptors = true) const;
+			bool withGrid = false) const;
 	int detectMoreLoopClosures(
 			float clusterRadius = 0.5f,
 			float clusterAngle = M_PI/6.0f,
@@ -341,7 +315,6 @@ 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,7 +52,6 @@ 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,);
@@ -62,7 +61,6 @@ 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,);
@@ -86,15 +84,6 @@ 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,15 +140,6 @@ 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 = CameraModel::opticalRotation());
+					const Transform & localTransform = Transform::getIdentity());
 	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 = CameraModel::opticalRotation(),
+					const Transform & localTransform = Transform::getIdentity(),
 					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 = CameraModel::opticalRotation());
+			const Transform & localTransform = Transform::getIdentity());
 	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 = CameraModel::opticalRotation());
+		const Transform & localTransform = Transform::getIdentity());
 	CameraK4A(const std::string & fileName,
 		float imageRate = 0.0f,
-		const Transform & localTransform = CameraModel::opticalRotation());
+		const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraK4A();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
@@ -61,7 +61,6 @@ 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);
@@ -84,13 +83,9 @@ 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 = CameraModel::opticalRotation());
+		const Transform & localTransform = Transform::getIdentity());
 	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 = CameraModel::opticalRotation());
+	CameraMyntEye(const std::string & device = "", bool apiRectification = false, bool apiDepth = false, float imageRate = 0, const Transform & localTransform = Transform::getIdentity());
 	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 = CameraModel::opticalRotation());
+					const Transform & localTransform = Transform::getIdentity());
 	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 = CameraModel::opticalRotation());
+					const Transform & localTransform = Transform::getIdentity());
 	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 = CameraModel::opticalRotation());
+			const Transform & localTransform = Transform::getIdentity());
 	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 = CameraModel::opticalRotation());
+			const Transform & localTransform = Transform::getIdentity());
 	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 = CameraModel::opticalRotation());
+		const Transform & localTransform = Transform::getIdentity());
 	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 = CameraModel::opticalRotation());
+		const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraRealSense2();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
@@ -134,7 +134,6 @@ 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 = CameraModel::opticalRotation());
+	CameraStereoDC1394( float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity());
 	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 = CameraModel::opticalRotation());
+	CameraStereoFlyCapture2( float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity());
 	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 = CameraModel::opticalRotation());
+			const Transform & localTransform = Transform::getIdentity());
 	CameraStereoImages(
 			const std::string & pathLeftRightImages,
 			bool rectifyImages = false,
 			float imageRate=0.0f,
-			const Transform & localTransform = CameraModel::opticalRotation());
+			const Transform & localTransform = Transform::getIdentity());
 	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 = CameraModel::opticalRotation());
+			const Transform & localTransform = Transform::getIdentity());
 
 	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 = CameraModel::opticalRotation());
+			const Transform & localTransform = Transform::getIdentity());
 	CameraStereoVideo(
 			const std::string & pathLeft,
 			const std::string & pathRight,
 			bool rectifyImages = false,
 			float imageRate=0.0f,
-			const Transform & localTransform = CameraModel::opticalRotation());
+			const Transform & localTransform = Transform::getIdentity());
 	CameraStereoVideo(
 			int device,
 			bool rectifyImages = false,
 			float imageRate = 0.0f,
-			const Transform & localTransform = CameraModel::opticalRotation());
+			const Transform & localTransform = Transform::getIdentity());
 	CameraStereoVideo(
 			int deviceLeft,
 			int deviceRight,
 			bool rectifyImages = false,
 			float imageRate = 0.0f,
-			const Transform & localTransform = CameraModel::opticalRotation());
+			const Transform & localTransform = Transform::getIdentity());
 	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 = CameraModel::opticalRotation(),
+			const Transform & localTransform = Transform::getIdentity(),
 			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 = CameraModel::opticalRotation(),
+			const Transform & localTransform = Transform::getIdentity(),
 			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 = CameraModel::opticalRotation());
+			const Transform & localTransform = Transform::getIdentity());
 	CameraVideo(const std::string & filePath,
 			bool rectifyImages = false,
 			float imageRate = 0,
-			const Transform & localTransform = CameraModel::opticalRotation());
+			const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraVideo();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");

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

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

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

@@ -1190,7 +1190,7 @@ void stereoRectifyFisheye( cv::InputArray _cameraMatrix1, cv::InputArray _distCo
     cv::Mat distCoeffs1 = _distCoeffs1.getMat(), distCoeffs2 = _distCoeffs2.getMat();
     cv::Mat Rmat = _Rmat.getMat(), Tmat = _Tmat.getMat();
 
-#if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION >= 3 && (CV_MINOR_VERSION>4 || (CV_MINOR_VERSION>=4 && CV_SUBMINOR_VERSION>=4)))
+#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))
     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 || (CV_MAJOR_VERSION >= 3 && (CV_MINOR_VERSION>4 || (CV_MINOR_VERSION>=4 && CV_SUBMINOR_VERSION>=4)))
+#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))
     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 || (CV_MAJOR_VERSION >= 3 && (CV_MINOR_VERSION>4 || (CV_MINOR_VERSION>=4 && CV_SUBMINOR_VERSION>=4)))
+#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))
         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 || (CV_MAJOR_VERSION >= 3 && (CV_MINOR_VERSION>4 || (CV_MINOR_VERSION>=4 && CV_SUBMINOR_VERSION>=4)))
+#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))
         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,11 +293,6 @@ IF(realsense2_FOUND)
 			${LIBRARIES}
 			${RealSense2_LIBRARIES}
 		)
-	ELSEIF(APPLE)
-	    SET(LIBRARIES
-			${LIBRARIES}
-			${realsense2_LIBRARIES}
-		)
 	ELSE()
 		SET(LIBRARIES
 			${LIBRARIES}

corelib/src/Camera.cpp

@@ -64,11 +64,6 @@ 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 & filePath)
+bool CameraModel::load(const std::string & directory, const std::string & cameraName)
 {
 	K_ = cv::Mat();
 	D_ = cv::Mat();
@@ -222,6 +222,7 @@ bool CameraModel::load(const std::string & filePath)
 	name_.clear();
 	imageSize_ = cv::Size();
 
+	std::string filePath = directory+"/"+cameraName+".yaml";
 	if(UFile::exists(filePath))
 	{
 		try
@@ -360,11 +361,6 @@ bool CameraModel::load(const std::string & filePath)
 	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";
@@ -640,23 +636,22 @@ 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
 {
-	return fovX()*180.0/CV_PI;
+	if(imageWidth() > 0 && fx() > 0.0)
+	{
+		return atan((double(imageWidth())/2.0)/fx())*2.0*180.0/CV_PI;
+	}
+	return 0.0;
 }
 
 double CameraModel::verticalFOV() const
 {
-	return fovY()*180.0/CV_PI;
+	if(imageHeight() > 0 && fy() > 0.0)
+	{
+		return atan((double(imageHeight())/2.0)/fy())*2.0*180.0/CV_PI;
+	}
+	return 0.0;
 }
 
 cv::Mat CameraModel::rectifyImage(const cv::Mat & raw, int interpolation) const

corelib/src/CameraThread.cpp

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

corelib/src/DBDriver.cpp

@@ -690,22 +690,6 @@ 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 < 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)))
+#if CV_MAJOR_VERSION < 4 || (CV_MAJOR_VERSION == 4 && (CV_MINOR_VERSION < 3 || (CV_MINOR_VERSION==3 && !defined(RTABMAP_OPENCV_DEV))))
 #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.4.0 >= 3.4.11
+#else // >= 4.3.0-dev
 
 #ifndef RTABMAP_NONFREE
 	if(type == Feature2D::kFeatureSurf)
@@ -542,7 +542,7 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame
 	}
 #endif
 
-#endif // >= 4.4.0 >= 3.4.11
+#endif // 4.3.0-dev
 
 #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 < 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)))
+#if CV_MAJOR_VERSION < 4 || (CV_MAJOR_VERSION == 4 && (CV_MINOR_VERSION < 3 || (CV_MINOR_VERSION==3 && !defined(RTABMAP_OPENCV_DEV))))
 #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,8 +3086,6 @@ 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)
 		{
@@ -3113,19 +3111,6 @@ 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())
@@ -3175,28 +3160,6 @@ 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
@@ -3884,22 +3847,6 @@ 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)
 	{
@@ -4297,24 +4244,6 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 				}
 			}
 
-			bool useProvided3dPoints = false;
-			if(_useOdometryFeatures && !data.keypoints().empty())
-			{
-				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;
-				}
-			}
-			else
-			{
 			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;
@@ -4338,7 +4267,6 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			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();
@@ -4470,22 +4398,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 					UDEBUG("time rectification = %fs", t);
 				}
 
-				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()) ||
+				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,9 +592,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 					updateKalmanFilter(vx,vy,vz,vroll,vpitch,vyaw);
 				}
 			}
-			else
-			{
-				if(particleFilters_.size())
+			else if(particleFilters_.size())
 			{
 				// Particle filtering
 				UASSERT(particleFilters_.size()==6);
@@ -639,13 +637,18 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 				{
 					info->timeParticleFiltering = time.ticks();
 				}
+
+				if(_force3DoF)
+				{
+					vz = 0.0f;
+					vroll = 0.0f;
+					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;

corelib/src/Rtabmap.cpp

@@ -34,7 +34,6 @@ 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"
@@ -145,7 +144,6 @@ Rtabmap::Rtabmap() :
 	_mapCorrection(Transform::getIdentity()),
 	_lastLocalizationNodeId(0),
 	_currentSessionHasGPS(false),
-	_odomCorrectionAcc(6,0),
 	_pathStatus(0),
 	_pathCurrentIndex(0),
 	_pathGoalIndex(0),
@@ -288,9 +286,15 @@ void Rtabmap::flushStatisticLogs()
 	}
 }
 
-void Rtabmap::init(const ParametersMap & parameters, const std::string & databasePath, bool loadDatabaseParameters)
+void Rtabmap::init(const ParametersMap & parameters, const std::string & databasePath)
 {
 	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())
 	{
@@ -304,35 +308,15 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
 
 	bool newDatabase = _databasePath.empty() || !UFile::exists(_databasePath);
 
-	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 not exist, create a memory
 	if(!_memory)
 	{
-		_memory = new Memory(allParameters);
-		_memory->init(_databasePath, false, allParameters, true);
+		_memory = new Memory(parameters);
+		_memory->init(_databasePath, false, parameters, true);
 	}
 
 	// Parse all parameters
-	this->parseParameters(allParameters);
+	this->parseParameters(parameters);
 
 	Transform lastPose;
 	_optimizedPoses = _memory->loadOptimizedPoses(&lastPose);
@@ -363,7 +347,7 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
 	setupLogFiles(newDatabase);
 }
 
-void Rtabmap::init(const std::string & configFile, const std::string & databasePath, bool loadDatabaseParameters)
+void Rtabmap::init(const std::string & configFile, const std::string & databasePath)
 {
 	// fill ctrl struct with values from the configuration file
 	ParametersMap param;// = Parameters::defaultParameters;
@@ -374,7 +358,7 @@ void Rtabmap::init(const std::string & configFile, const std::string & databaseP
 		Parameters::readINI(configFile, param);
 	}
 
-	this->init(param, databasePath, loadDatabaseParameters);
+	this->init(param, databasePath);
 }
 
 void Rtabmap::close(bool databaseSaved, const std::string & ouputDatabasePath)
@@ -391,7 +375,6 @@ 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);
@@ -724,7 +707,6 @@ void Rtabmap::setInitialPose(const Transform & initialPose)
 			_lastLocalizationNodeId = 0;
 			_odomCachePoses.clear();
 			_odomCacheConstraints.clear();
-			_odomCorrectionAcc = std::vector<float>(6,0);
 			_mapCorrection.setIdentity();
 			_mapCorrectionBackup.setNull();
 
@@ -747,18 +729,11 @@ int Rtabmap::triggerNewMap()
 	int mapId = -1;
 	if(_memory)
 	{
-		_lastLocalizationNodeId = 0;
-		_odomCachePoses.clear();
-		_odomCacheConstraints.clear();
-		_odomCorrectionAcc = std::vector<float>(6,0);
-
 		if(!_memory->isIncremental())
 		{
-			if(_savedLocalizationIgnored)
-			{
-				_mapCorrection.setIdentity();
-				_lastLocalizationPose.setIdentity();
-			}
+			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());
 			return mapId;
 		}
 		std::map<int, int> reducedIds;
@@ -766,6 +741,9 @@ int Rtabmap::triggerNewMap()
 		UINFO("New map triggered, new map = %d", mapId);
 		_optimizedPoses.clear();
 		_constraints.clear();
+		_lastLocalizationNodeId = 0;
+		_odomCachePoses.clear();
+		_odomCacheConstraints.clear();
 
 		if(_bayesFilter)
 		{
@@ -915,7 +893,6 @@ void Rtabmap::resetMemory()
 	_lastLocalizationNodeId = 0;
 	_odomCachePoses.clear();
 	_odomCacheConstraints.clear();
-	_odomCorrectionAcc = std::vector<float>(6,0);
 	_distanceTravelled = 0.0f;
 	_optimizeFromGraphEndChanged = false;
 	this->clearPath(0);
@@ -1076,13 +1053,14 @@ bool Rtabmap::process(
 	bool fakeOdom = false;
 	if(_rgbdSlamMode)
 	{
-		if(!_memory->isIncremental() &&
-			!odomPose.isNull() &&
-			_optimizedPoses.size() &&
-			_mapCorrection.isIdentity() &&
-			!_lastLocalizationPose.isNull() &&
-			!_lastLocalizationPose.isIdentity() &&
-			_lastLocalizationNodeId == 0)
+		if(!_memory->isIncremental() && !odomPose.isNull())
+		{
+			if(!_mapCorrectionBackup.isNull())
+			{
+				_mapCorrection = _mapCorrectionBackup;
+				_mapCorrectionBackup.setNull();
+			}
+			else if(_optimizedPoses.size() && _mapCorrection.isIdentity() && !_lastLocalizationPose.isNull() && _lastLocalizationNodeId == 0)
 			{
 				// Localization mode
 				if(!_optimizeFromGraphEnd)
@@ -1107,6 +1085,7 @@ bool Rtabmap::process(
 					}
 				}
 			}
+		}
 
 		if(odomPose.isNull())
 		{
@@ -1118,20 +1097,12 @@ 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
@@ -1380,7 +1351,6 @@ 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)
 		{
@@ -1411,11 +1381,6 @@ 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)
 			{
@@ -1450,6 +1415,7 @@ bool Rtabmap::process(
 										 odomCovariance.inv())));
 					_odomCachePoses.insert(std::make_pair(signature->id(), signature->getPose())); // keep odometry poses
 				}
+
 			}
 		}
 
@@ -2849,12 +2815,10 @@ 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)
 					{
@@ -2887,7 +2851,6 @@ bool Rtabmap::process(
 							transform *= error;
 
 							newPose  = _optimizedPoses.at(loopId) * transform.inverse();
-							UDEBUG("newPose gravity=%s", newPose.prettyPrint().c_str());
 						}
 						else
 						{
@@ -3159,9 +3122,6 @@ 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)
 			{
@@ -3172,7 +3132,6 @@ 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
@@ -3194,8 +3153,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), ignore correction from first localization
-				if(!odomPose.isNull() && !previousMapCorrection.isNull() && !previousMapCorrection.isIdentity())
+				// Odom correction (actual odometry pose change)
+				if(!odomPose.isNull() && !previousMapCorrection.isNull())
 				{
 					Transform odomCorrection = (previousMapCorrection*odomPose).inverse()*_mapCorrection*odomPose;
 					statistics_.addStatistic(Statistics::kLoopOdom_correction_norm(), odomCorrection.getNorm());
@@ -3207,30 +3166,6 @@ 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())
@@ -3242,12 +3177,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);
@@ -3475,15 +3410,13 @@ bool Rtabmap::process(
 				_optimizedPoses.erase(lastId);
 				for(std::multimap<int, Link>::iterator iter=_constraints.find(lastId); iter!=_constraints.end() && iter->first==lastId;++iter)
 				{
-					if(iter->second.to() != iter->second.from())
-					{
+					iter->second.to();
 					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);
 			}
 			else
@@ -4391,7 +4324,7 @@ void Rtabmap::dumpPrediction() const
 	}
 }
 
-Signature Rtabmap::getSignatureCopy(int id, bool images, bool scan, bool userData, bool occupancyGrid, bool withWords, bool withGlobalDescriptors) const
+Signature Rtabmap::getSignatureCopy(int id, bool images, bool scan, bool userData, bool occupancyGrid) const
 {
 	Signature s;
 	if(_memory)
@@ -4412,7 +4345,7 @@ Signature Rtabmap::getSignatureCopy(int id, bool images, bool scan, bool userDat
 		{
 			data = _memory->getNodeData(id, images, scan, userData, occupancyGrid);
 		}
-		if(!images && withWords)
+		if(!images)
 		{
 			std::vector<CameraModel> models;
 			StereoCameraModel stereoModel;
@@ -4420,7 +4353,11 @@ 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,
@@ -4429,25 +4366,10 @@ Signature Rtabmap::getSignatureCopy(int id, bool images, bool scan, bool userDat
 					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]);
@@ -4478,9 +4400,7 @@ void Rtabmap::getGraph(
 		bool withImages,
 		bool withScan,
 		bool withUserData,
-		bool withGrid,
-		bool withWords,
-		bool withGlobalDescriptors) const
+		bool withGrid) const
 {
 	if(_memory && _memory->getLastWorkingSignature())
 	{
@@ -4521,7 +4441,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, withWords, withGlobalDescriptors)));
+				signatures->insert(std::make_pair(*iter, getSignatureCopy(*iter, withImages, withScan, withUserData, withGrid)));
 			}
 		}
 	}
@@ -4905,55 +4825,52 @@ bool Rtabmap::addLink(const Link & link)
 			return false;
 		}
 
-		if(_optimizedPoses.find(link.from()) == _optimizedPoses.end() &&
-		   _optimizedPoses.find(link.to()) == _optimizedPoses.end())
-		{
-			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;
-		}
-
-		// add temporary the link
-		if(!_memory->addLink(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::map<int, Transform> poses;
 		std::multimap<int, Link> links;
-		cv::Mat covariance;
-		optimizeCurrentMap(this->getLastLocationId(), false, poses, covariance, &links);
+		this->getGraph(poses, links, true, false);
 
+		if(_memory->isIncremental())
+		{
 			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());
+				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 (size=%d)", link.to(), (int)poses.size());
-			_memory->removeLink(link.from(), link.to());
+				UERROR("Link's \"to id\" %d is not in the graph", 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)
+
+		int from = link.from();
+		int to = link.to();
+
+		if(_optimizationMaxError > 0.0f)
 		{
-			float maxLinearError = 0.0f;
-			float maxLinearErrorRatio = 0.0f;
-			float maxAngularError = 0.0f;
-			float maxAngularErrorRatio = 0.0f;
+			//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));
 			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,
+						optimizedPoses,
 						links,
 						maxLinearErrorRatio,
 						maxAngularErrorRatio,
@@ -4969,8 +4886,8 @@ bool Rtabmap::addLink(const Link & link)
 						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(),
+								  from,
+								  to,
 								  maxLinearError,
 								  maxLinearLink->from(),
 								  maxLinearLink->to(),
@@ -4988,8 +4905,8 @@ bool Rtabmap::addLink(const Link & link)
 						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(),
+								  from,
+								  to,
 								  maxAngularError*180.0f/M_PI,
 								  maxAngularLink->from(),
 								  maxAngularLink->to(),
@@ -5000,13 +4917,31 @@ bool Rtabmap::addLink(const Link & link)
 					}
 				}
 			}
+			else
+			{
+				msg = uFormat("Rejecting edge %d->%d because graph optimization has failed!",
+						  from,
+						  to);
+			}
 			if(!msg.empty())
 			{
 				UERROR("%s", msg.c_str());
-			_memory->removeLink(link.from(), link.to());
 				return false;
 			}
+		}
+		else
+		{
+			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;
+			}
+		}
 
+		if(_memory->addLink(link, false))
+		{
 			// Update optimized poses
 			for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
 			{
@@ -5029,6 +4964,7 @@ bool Rtabmap::addLink(const Link & link)
 
 			return true;
 		}
+	}
 	else // localization mode
 	{
 		int oldestId = link.from()>link.to()?link.to():link.from();
@@ -5042,7 +4978,7 @@ bool Rtabmap::addLink(const Link & link)
 		}
 		if(_optimizedPoses.find(oldestId) == _optimizedPoses.end())
 		{
-			UERROR("Link's id %d is not in the optimized graph (_optimizedPoses=%d)", oldestId, (int)_optimizedPoses.size());
+			UERROR("Link's id %d is not in the optimized graph", oldestId);
 			return false;
 		}
 		if(_optimizeFromGraphEnd)
@@ -5079,7 +5015,7 @@ bool Rtabmap::addLink(const Link & link)
 		{
 			_lastLocalizationPose = _optimizedPoses.at(link.to()) * link.transform().inverse();
 		}
-		UINFO("Set _lastLocalizationPose=%s", _lastLocalizationPose.prettyPrint().c_str());
+		UERROR("Set _lastLocalizationPose=%s", _lastLocalizationPose.prettyPrint().c_str());
 		if(_graphOptimizer->isSlam2d())
 		{
 			// transform constraint to 2D

corelib/src/camera/CameraK4A.cpp

@@ -54,17 +54,9 @@ CameraK4A::CameraK4A(
 	const Transform & localTransform) :
 	Camera(imageRate, localTransform)
 #ifdef RTABMAP_K4A
-			,
-			device_(NULL),
-			config_(K4A_DEVICE_CONFIG_INIT_DISABLE_ALL),
-			transformation_(NULL),
-			capture_(NULL),
+	,deviceId_(deviceId),
 	playbackHandle_(NULL),
 	transformationHandle_(NULL),
-			deviceId_(deviceId),
-			rgb_resolution_(0),
-			framerate_(2),
-			depth_resolution_(2),
 	ir_(false),
 	previousStamp_(0.0)
 #endif
@@ -79,15 +71,10 @@ CameraK4A::CameraK4A(
 #ifdef RTABMAP_K4A
 	,
 	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
@@ -131,22 +118,11 @@ 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
@@ -214,41 +190,13 @@ bool CameraK4A::init(const std::string & calibrationFolder, const std::string &
 	}
 	else if (deviceId_ >= 0)
 	{
-		if(device_!=NULL)
-		{
-			this->close();
-		}
+		device_ = NULL;
 
-		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_ = K4A_DEVICE_CONFIG_INIT_DISABLE_ALL;
+		config_.camera_fps = K4A_FRAMES_PER_SECOND_15;
+		config_.depth_mode = K4A_DEPTH_MODE_WFOV_2X2BINNED;
 		config_.color_format = K4A_IMAGE_FORMAT_COLOR_BGRA32;
+		config_.color_resolution = K4A_COLOR_RESOLUTION_720P;
 
 		int device_count = k4a_device_get_installed_count();
 
@@ -257,15 +205,11 @@ 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 %d k4a device(s) attached", device_count);
+		UINFO("CameraK4A found k4a device attached");
 
 		// Open the first plugged in Kinect device
-		if (K4A_FAILED(k4a_device_open(deviceId_, &device_)))
+		if (K4A_FAILED(k4a_device_open(K4A_DEVICE_DEFAULT, &device_)))
 		{
 			UERROR("Failed to open k4a device!");
 			return false;
@@ -287,7 +231,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!");
-			close();
+			k4a_device_close(device_);
 			return false;
 		}
 
@@ -296,7 +240,7 @@ 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!");
-			close();
+			k4a_device_close(device_);
 			return false;
 		}
 
@@ -325,26 +269,6 @@ bool CameraK4A::init(const std::string & calibrationFolder, const std::string &
 
 		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");
@@ -361,7 +285,6 @@ bool CameraK4A::init(const std::string & calibrationFolder, const std::string &
 			return true;
 		}
 
-		close();
 		return false;
 	}
 	return true;
@@ -379,11 +302,10 @@ bool CameraK4A::isCalibrated() const
 std::string CameraK4A::getSerial() const
 {
 #ifdef RTABMAP_K4A
-	if(!fileName_.empty())
-	{
-		return fileName_;
-	}
+	if(device_ != NULL)
 		return(serial_number_);
+	else
+		return fileName_.empty()?"":fileName_;
 #else
 	return "";
 #endif
@@ -598,26 +520,22 @@ SensorData CameraK4A::captureImage(CameraInfo * info)
 	}
 	else
 	{
-		k4a_image_t ir_image_ = NULL;
-		k4a_image_t rgb_image_ = NULL;
+		k4a_image_t ir_image_;
+		k4a_image_t depth_image_;
+		k4a_image_t rgb_image_;
 		k4a_imu_sample_t imu_sample_;
 
-		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))
+		if (K4A_WAIT_RESULT_SUCCEEDED == k4a_device_get_capture(device_, &capture_, K4A_WAIT_INFINITE))
 		{
 			cv::Mat bgrCV;
 			cv::Mat depthCV;
 			IMU imu;
 
+			if (ir_)
+			{
+				// Retrieve IR image from capture
+				ir_image_ = k4a_capture_get_ir_image(capture_);
+
 				if(ir_image_ != NULL)
 				{
 					// Convert IR image
@@ -631,7 +549,13 @@ SensorData CameraK4A::captureImage(CameraInfo * info)
 					// 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)
@@ -653,11 +577,10 @@ SensorData CameraK4A::captureImage(CameraInfo * info)
 					// 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_);
+			depth_image_ = k4a_capture_get_depth_image(capture_);
 
 			if (depth_image_ != NULL)
 			{
@@ -671,6 +594,7 @@ SensorData CameraK4A::captureImage(CameraInfo * info)
 				else
 				{
 					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)
 					{
@@ -695,18 +619,19 @@ SensorData CameraK4A::captureImage(CameraInfo * info)
 				}
 				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(imu_sample_.gyro_sample.xyz.x, imu_sample_.gyro_sample.xyz.y, imu_sample_.gyro_sample.xyz.z),
+				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(imu_sample_.acc_sample.xyz.x, imu_sample_.acc_sample.xyz.y, imu_sample_.acc_sample.xyz.z),
+					  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),
-						imuLocalTransform_);
+					  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);
 			}
 			else
 			{

corelib/src/camera/CameraMyntEye.cpp

@@ -33,10 +33,6 @@ 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,8 +77,7 @@ CameraRealSense2::CameraRealSense2(
 	cameraHeight_(480),
 	cameraFps_(30),
 	publishInterIMU_(false),
-	dualMode_(false),
-	closing_(false)
+	dualMode_(false)
 #endif
 {
 	UDEBUG("");
@@ -87,15 +86,12 @@ 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
@@ -108,7 +104,6 @@ CameraRealSense2::~CameraRealSense2()
 						UWARN("%s", error.what());
 					}
 				}
-				dev_[i]->hardware_reset(); // To avoid freezing on some Windows computers in the following destructor
 				delete dev_[i];
 			}
 		}
@@ -255,7 +250,7 @@ void CameraRealSense2::getPoseAndIMU(
 		{
 			if(maxWaitTimeMs > 0)
 			{
-				UWARN("Could not find poses to interpolate at image time %f after waiting %d ms (last is %f)...", stamp, maxWaitTimeMs, poseBuffer_.rbegin()->first);
+				UWARN("Could not find poses to interpolate at time %f after waiting %d ms (last is %f)...", stamp, maxWaitTimeMs, poseBuffer_.rbegin()->first);
 			}
 		}
 		else
@@ -308,7 +303,7 @@ void CameraRealSense2::getPoseAndIMU(
 		{
 			if(maxWaitTimeMs>0)
 			{
-				UWARN("Could not find acc data to interpolate at image time %f after waiting %d ms (last is %f)...", stamp, maxWaitTimeMs, accBuffer_.rbegin()->first);
+				UWARN("Could not find acc data to interpolate at time %f after waiting %d ms (last is %f)...", stamp, maxWaitTimeMs, accBuffer_.rbegin()->first);
 			}
 			imuMutex_.unlock();
 			return;
@@ -371,7 +366,7 @@ void CameraRealSense2::getPoseAndIMU(
 		{
 			if(maxWaitTimeMs>0)
 			{
-				UWARN("Could not find gyro data to interpolate at image time %f after waiting %d ms (last is %f)...", stamp, maxWaitTimeMs, gyroBuffer_.rbegin()->first);
+				UWARN("Could not find gyro data to interpolate at time %f after waiting %d ms (last is %f)...", stamp, maxWaitTimeMs, gyroBuffer_.rbegin()->first);
 			}
 			imuMutex_.unlock();
 			return;
@@ -524,14 +519,7 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 			{
 				if (info.was_removed(*dev_[i]))
 				{
-					if (closing_)
-					{
-						UDEBUG("The device %d has been disconnected!", i);
-					}
-					else
-					{
-						UERROR("The device %d has been disconnected!", i);
-					}
+					UERROR("The device has been disconnected!");
 				}
 			}
 		}
@@ -1072,6 +1060,7 @@ 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;
@@ -1130,7 +1119,6 @@ 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 -DWITH_ALICE_VISION=ON .. && \
+    cmake .. && \
     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=1VsibeqRYpS5pjmrG-vYTXyiPg8kbIfVN&export=download' -O arcore.zip
+wget 'https://docs.google.com/uc?authuser=0&id=1A4gMviyxHCnA19MTMbitOWoSOyoZcCef&export=download' -O arcore.zip
 unzip -qq arcore.zip
 rm arcore.zip
-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
+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
 
 # 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=1VsibeqRYpS5pjmrG-vYTXyiPg8kbIfVN&export=download' -O arcore.zip
+wget 'https://docs.google.com/uc?authuser=0&id=1A4gMviyxHCnA19MTMbitOWoSOyoZcCef&export=download' -O arcore.zip
 unzip -qq arcore.zip
 rm arcore.zip
-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
+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
 
 # 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=1VsibeqRYpS5pjmrG-vYTXyiPg8kbIfVN&export=download' -O arcore.zip
+wget 'https://docs.google.com/uc?authuser=0&id=1A4gMviyxHCnA19MTMbitOWoSOyoZcCef&export=download' -O arcore.zip
 unzip -qq arcore.zip
 rm arcore.zip
-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
+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
 
 # AREngine
 wget 'https://docs.google.com/uc?authuser=0&id=1rdaD2Z1QBv-SUeUy0oBmg3C2odfxTHgR&export=download' -O arengine.zip

docker/focal/Dockerfile

@@ -1,56 +0,0 @@
-# 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

@@ -1,2 +0,0 @@
-#!/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

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

examples/NoEventsExample/main.cpp

@@ -93,11 +93,13 @@ 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);
+			(float)cameraRate,
+			opticalRotation);
 
 	if(camera.init(calibrationDir, calibrationName))
 	{

examples/RGBDMapping/CMakeLists.txt

@@ -66,14 +66,6 @@ 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 9=Kinect for Azure SDK 10=MYNT EYE S\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\n\n");
 	exit(1);
 }
 
@@ -64,9 +64,9 @@ int main(int argc, char * argv[])
 	else
 	{
 		driver = atoi(argv[argc-1]);
-		if(driver < 0 || driver > 10)
+		if(driver < 0 || driver > 8)
 		{
-			UERROR("driver should be between 0 and 10.");
+			UERROR("driver should be between 0 and 8.");
 			showUsage();
 		}
 	}
@@ -77,6 +77,7 @@ 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())
@@ -84,7 +85,7 @@ int main(int argc, char * argv[])
 			UERROR("Not built with OpenNI2 support...");
 			exit(-1);
 		}
-		camera = new CameraOpenNI2();
+		camera = new CameraOpenNI2("", CameraOpenNI2::kTypeColorDepth, 0, opticalRotation);
 	}
 	else if(driver == 2)
 	{
@@ -93,7 +94,7 @@ int main(int argc, char * argv[])
 			UERROR("Not built with Freenect support...");
 			exit(-1);
 		}
-		camera = new CameraFreenect();
+		camera = new CameraFreenect(0, CameraFreenect::kTypeColorDepth, 0, opticalRotation);
 	}
 	else if(driver == 3)
 	{
@@ -102,7 +103,7 @@ int main(int argc, char * argv[])
 			UERROR("Not built with OpenNI from OpenCV support...");
 			exit(-1);
 		}
-		camera = new CameraOpenNICV();
+		camera = new CameraOpenNICV(false, 0, opticalRotation);
 	}
 	else if(driver == 4)
 	{
@@ -111,7 +112,7 @@ int main(int argc, char * argv[])
 			UERROR("Not built with OpenNI from OpenCV support...");
 			exit(-1);
 		}
-		camera = new CameraOpenNICV(true);
+		camera = new CameraOpenNICV(true, 0, opticalRotation);
 	}
 	else if (driver == 5)
 	{
@@ -120,7 +121,7 @@ int main(int argc, char * argv[])
 			UERROR("Not built with Freenect2 support...");
 			exit(-1);
 		}
-		camera = new CameraFreenect2(0, CameraFreenect2::kTypeColor2DepthSD);
+		camera = new CameraFreenect2(0, CameraFreenect2::kTypeColor2DepthSD, 0, opticalRotation);
 	}
 	else if (driver == 6)
 	{
@@ -129,7 +130,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);
+		camera = new CameraStereoZed(0, 2, 1, 1, 100, false, 0, opticalRotation);
 	}
 	else if (driver == 7)
 	{
@@ -138,7 +139,7 @@ int main(int argc, char * argv[])
 			UERROR("Not built with RealSense support...");
 			exit(-1);
 		}
-		camera = new CameraRealSense();
+		camera = new CameraRealSense(0, 0, 0, false, 0, opticalRotation);
 	}
 	else if (driver == 8)
 	{
@@ -147,29 +148,11 @@ int main(int argc, char * argv[])
 			UERROR("Not built with RealSense2 support...");
 			exit(-1);
 		}
-		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();
+		camera = new CameraRealSense2("", 0, opticalRotation);
 	}
 	else
 	{
-		camera = new rtabmap::CameraOpenni();
+		camera = new rtabmap::CameraOpenni("", 0, opticalRotation);
 	}
 
 	if(!camera->init())

examples/WifiMapping/CMakeLists.txt

@@ -60,14 +60,6 @@ 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 9=Kinect for Azure SDK 10=MYNT EYE S\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\n\n");
 	exit(1);
 }
 
@@ -184,24 +184,6 @@ 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(qreal);
+	void loopClosureThrChanged(float);
 	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<qreal> & x, const std::vector<qreal> & y, const QString & unit = QString(), const QMenu * menu = 0, QGridLayout * grid = 0, QWidget * parent = 0);
+	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);
 	virtual ~StatItem();
-	void addValue(qreal y);
-	void addValue(qreal x, qreal y);
-	void setValues(const std::vector<qreal> & x, const std::vector<qreal> & y);
+	void addValue(float y);
+	void addValue(float x, float y);
+	void setValues(const std::vector<float> & x, const std::vector<float> & y);
 	QString value() const;
-	std::vector<qreal> xValues() const {return _x;}
-	std::vector<qreal> yValues() const {return _y;}
+	std::vector<float> xValues() const {return _x;}
+	std::vector<float> 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(qreal);
-	void valueAdded(qreal, qreal);
-	void valuesChanged(const std::vector<qreal> &, const std::vector<qreal> &);
+	void valueAdded(float);
+	void valueAdded(float, float);
+	void valuesChanged(const std::vector<float> &, const std::vector<float> &);
 	void plotRequested(const StatItem *, const QString &);
 
 private Q_SLOTS:
@@ -80,8 +80,8 @@ private:
 	QMenu * _menu;
 
 	bool _cacheOn;
-	std::vector<qreal> _x;
-	std::vector<qreal> _y;
+	std::vector<float> _x;
+	std::vector<float> _y;
 };
 
 
@@ -104,9 +104,9 @@ public:
 
 public Q_SLOTS:
 	void updateStat(const QString & statFullName, 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 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 clear();
 
 Q_SIGNALS:

guilib/include/rtabmap/utilite/UPlot.h

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

guilib/src/CMakeLists.txt

@@ -139,13 +139,6 @@ 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->AutoAdjustCameraClippingRangeOff();
+	style->SetAutoAdjustCameraClippingRange(true);
 	_visualizer = new pcl::visualization::PCLVisualizer(
 		argc, 
 		0, 
@@ -192,10 +192,10 @@ CloudViewer::CloudViewer(QWidget *parent, CloudViewerInteractorStyle * style) :
 
 	setRenderingRate(_renderingRate);
 
-	this->setCameraPosition(
+	_visualizer->setCameraPosition(
 				-1, 0, 0,
 				0, 0, 0,
-				0, 0, 1);
+				0, 0, 1, 1);
 #ifndef _WIN32
 	// Crash on startup on Windows (vtk issue)
 	this->addOrUpdateCoordinate("reference", Transform::getIdentity(), 0.2);
@@ -286,12 +286,14 @@ 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);
@@ -415,6 +417,7 @@ 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());
@@ -437,7 +440,6 @@ 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());
@@ -467,6 +469,7 @@ 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>());
 	
@@ -489,43 +492,20 @@ 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)
-		{
-			// 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)
+		if(samePose ||
+		   _visualizer->updatePointCloudPose(id, posef))
 		{
 			_addedClouds.find(id).value() = pose;
+			if(!samePose)
+			{
 				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;
@@ -854,11 +834,7 @@ 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());
@@ -892,11 +868,7 @@ 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());
@@ -930,11 +902,7 @@ 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());
@@ -967,11 +935,7 @@ 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());
@@ -1005,26 +969,25 @@ bool CloudViewer::addCloudTextureMesh(
 	UDEBUG("Adding %s", id.c_str());
 	if(this->addTextureMesh(*textureMesh, texture, id, 1))
 	{
-#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);
+		_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(!textureMesh->cloud.is_dense)
 		{
-			actor->GetTexture()->SetInterpolate(1);
-			actor->GetTexture()->SetBlendingMode(vtkTexture::VTK_TEXTURE_BLENDING_MODE_REPLACE);
+			_visualizer->getCloudActorMap()->find(id)->second.actor->GetTexture()->SetInterpolate(1);
+			_visualizer->getCloudActorMap()->find(id)->second.actor->GetTexture()->SetBlendingMode(vtkTexture::VTK_TEXTURE_BLENDING_MODE_REPLACE);
 		}
+		_visualizer->updatePointCloudPose(id, pose.toEigen3f());
 		if(_buildLocator)
 		{
 			vtkSmartPointer<vtkOBBTree> tree = vtkSmartPointer<vtkOBBTree>::New();
-			tree->SetDataSet(actor->GetMapper()->GetInput());
+			tree->SetDataSet(_visualizer->getCloudActorMap()->find(id)->second.actor->GetMapper()->GetInput());
 			tree->BuildLocator();
 			_locators.insert(std::make_pair(id, tree));
 		}
-		_addedClouds.insert(id, Transform::getIdentity());
-		this->updateCloudPose(id, pose);
+		_addedClouds.insert(id, pose);
 		return true;
 	}
 	return false;
@@ -1263,13 +1226,8 @@ 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",
@@ -1370,11 +1328,7 @@ 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);
@@ -1423,24 +1377,17 @@ 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
   (*_visualizer->getCloudActorMap())[id].viewpoint_transformation_ = transformation;
-#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);
+  _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);
   return true;
 }
 
@@ -1457,17 +1404,10 @@ 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)
 	{
@@ -1511,17 +1451,10 @@ 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(
@@ -2318,64 +2251,54 @@ 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())
 		{
-			this->setCameraPosition(
+			_visualizer->setCameraPosition(
 					_lastPose.x(), _lastPose.y(), _lastPose.z()+5,
 					_lastPose.x(), _lastPose.y(), _lastPose.z(),
-					1, 0, 0);
+					1, 0, 0, 1);
 		}
 		else if(_aLockViewZ->isChecked())
 		{
-			this->setCameraPosition(
+			_visualizer->setCameraPosition(
 					pt.x, pt.y, pt.z,
 					_lastPose.x(), _lastPose.y(), _lastPose.z(),
-					0, 0, 1);
+					0, 0, 1, 1);
 		}
 		else
 		{
-			this->setCameraPosition(
+			_visualizer->setCameraPosition(
 					pt.x, pt.y, pt.z,
 					_lastPose.x(), _lastPose.y(), _lastPose.z(),
-					_lastPose.r31(), _lastPose.r32(), _lastPose.r33());
+					_lastPose.r31(), _lastPose.r32(), _lastPose.r33(), 1);
 		}
 	}
 	else if(_aCameraOrtho->isChecked())
 	{
-		this->setCameraPosition(
+		_visualizer->setCameraPosition(
 				0, 0, 5,
 				0, 0, 0,
-				1, 0, 0);
+				1, 0, 0, 1);
 	}
 	else
 	{
-		this->setCameraPosition(
+		_visualizer->setCameraPosition(
 				-1, 0, 0,
 				0, 0, 0,
-				0, 0, 1);
+				0, 0, 1, 1);
 	}
+	this->update();
 }
 
 void CloudViewer::removeAllClouds()
 {
-	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());
+	_addedClouds.clear();
+	_locators.clear();
+	_visualizer->removeAllPointClouds();
 }
 
 
 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");
@@ -2522,18 +2445,6 @@ 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();
 }
 
@@ -2608,17 +2519,6 @@ 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();
 }
 
@@ -2630,17 +2530,6 @@ 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();
 }
 
@@ -2652,17 +2541,6 @@ 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();
 }
 
@@ -2714,42 +2592,8 @@ void CloudViewer::setCameraPosition(
 		float focalX, float focalY, float focalZ,
 		float upX, float upY, float upZ)
 {
-	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 ();
+	_lastCameraOrientation= _lastCameraPose= cv::Vec3f(0,0,0);
+	_visualizer->setCameraPosition(x,y,z, focalX,focalY,focalX, upX,upY,upZ, 1);
 }
 
 void CloudViewer::updateCameraTargetPosition(const Transform & pose)
@@ -2866,10 +2710,10 @@ void CloudViewer::updateCameraTargetPosition(const Transform & pose)
 				}
 			}
 
-			this->setCameraPosition(
+			_visualizer->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]);
+				cameras.front().view[0], cameras.front().view[1], cameras.front().view[2], 1);
 		}
 	}
 
@@ -2963,19 +2807,6 @@ 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());
 	}
 }
@@ -2990,18 +2821,6 @@ 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;
@@ -3018,41 +2837,22 @@ void CloudViewer::setCloudColorIndex(const std::string & id, int index)
 void CloudViewer::setCloudOpacity(const std::string & id, double opacity)
 {
 	double lastOpacity;
-	if(_visualizer->getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, lastOpacity, id))
-	{
+	_visualizer->getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, lastOpacity, 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;
-	if(_visualizer->getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, lastSize, id))
-	{
+	_visualizer->getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, lastSize, id);
 	if((int)lastSize != size)
 	{
 		_visualizer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, (double)size, id);
 	}
 }
-}
 
 void CloudViewer::setCameraTargetLocked(bool enabled)
 {
@@ -3225,13 +3025,6 @@ 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]);
 	}
 }
 
@@ -3456,10 +3249,12 @@ 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];
-		this->setCameraPosition(
+		_visualizer->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]);
+			cameras.front().view[0], cameras.front().view[1], cameras.front().view[2], 1);
+
+		update();
 
 		Q_EMIT configChanged();
 	}
@@ -3485,12 +3280,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) &&
@@ -3522,12 +3317,14 @@ void CloudViewer::mouseMoveEvent(QMouseEvent * event)
 		cameras.front().view[0] = 0;
 		cameras.front().view[1] = 0;
 		cameras.front().view[2] = 1;
-	}
 
-	this->setCameraPosition(
+		_visualizer->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]);
+			cameras.front().view[0], cameras.front().view[1], cameras.front().view[2], 1);
+
+	}
+	this->update();
 
 	Q_EMIT configChanged();
 }
@@ -3535,20 +3332,12 @@ 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,11 +118,9 @@ void CloudViewerInteractorStyle::OnMouseMove()
 		{
 			int pickPosition[2];
 			this->GetInteractor()->GetEventPosition(pickPosition);
-			int result = this->Interactor->GetPicker()->Pick(pickPosition[0], pickPosition[1],
+			this->Interactor->GetPicker()->Pick(pickPosition[0], pickPosition[1],
 					0,  // always zero.
 					this->CurrentRenderer);
-			if(result)
-			{
 			double picked[3];
 			this->Interactor->GetPicker()->GetPickPosition(picked);
 
@@ -192,7 +190,6 @@ void CloudViewerInteractorStyle::OnMouseMove()
 			viewer_->setCloudOpacity("interactor_points_alt", 0.5);
 		}
 	}
-	}
 	// Forward events
 	PCLVisualizerInteractorStyle::OnMouseMove();
 }
@@ -223,14 +220,14 @@ void CloudViewerInteractorStyle::OnLeftButtonDown()
 		if(this->NumberOfClicks >= 2)
 		{
 			this->NumberOfClicks = 0;
-			int result = this->Interactor->GetPicker()->Pick(pickPosition[0], pickPosition[1],
+			this->Interactor->GetPicker()->Pick(pickPosition[0], pickPosition[1],
 					0,  // always zero.
 					this->CurrentRenderer);
-			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]);
+			if(this->GetInteractor()->GetControlKey()==0)
+			{
 				vtkCamera *camera = this->CurrentRenderer->GetActiveCamera();
 				UASSERT(camera);
 				double position[3];
@@ -268,11 +265,9 @@ void CloudViewerInteractorStyle::OnLeftButtonDown()
 		}
 		else if(this->GetInteractor()->GetControlKey() && viewer_)
 		{
-			int result = this->Interactor->GetPicker()->Pick(pickPosition[0], pickPosition[1],
+			this->Interactor->GetPicker()->Pick(pickPosition[0], pickPosition[1],
 					0,  // always zero.
 					this->CurrentRenderer);
-			if(result)
-			{
 			double picked[3];
 			this->Interactor->GetPicker()->GetPickPosition(picked);
 
@@ -327,7 +322,6 @@ void CloudViewerInteractorStyle::OnLeftButtonDown()
 			viewer_->setCloudOpacity("interactor_points", 0.5);
 		}
 	}
-	}
 
 	// Forward events
 	PCLVisualizerInteractorStyle::OnLeftButtonDown();

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<qreal>, std::vector<qreal> > > allData;
+		std::map<std::string, std::pair<std::vector<float>, std::vector<float> > > 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<qreal>(ids_.size(), 0.0f), std::vector<qreal>(ids_.size(), 0.0f) )));
+					allData.insert(std::make_pair(iter->first, std::make_pair(std::vector<float>(ids_.size(), 0.0f), std::vector<float>(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()?qreal(stamp-firstStamp):ids_[i];
+				allData.at(iter->first).first[oi] = ui_->checkBox_timeStats->isChecked()?float(stamp-firstStamp):ids_[i];
 				allData.at(iter->first).second[oi] = iter->second;
 				++oi;
 			}
 		}
 
-		for(std::map<std::string, std::pair<std::vector<qreal>, std::vector<qreal> > >::iterator iter=allData.begin(); iter!=allData.end(); ++iter)
+		for(std::map<std::string, std::pair<std::vector<float>, std::vector<float> > >::iterator iter=allData.begin(); iter!=allData.end(); ++iter)
 		{
 			int oi = allDataOi.at(iter->first);
 			iter->second.first.resize(oi);
@@ -6922,14 +6922,10 @@ 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>);
@@ -6978,10 +6974,6 @@ 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
 				{
@@ -7007,10 +6999,6 @@ 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(qreal)), tc, SLOT(setThreshold(qreal)));
+	connect(this, SIGNAL(loopClosureThrChanged(float)), tc, SLOT(setThreshold(float)));
 
 	_likelihoodCurve = new PdfPlotCurve("Likelihood", &_cachedSignatures, this);
 	_ui->likelihoodPlot->addCurve(_likelihoodCurve, false);
@@ -1985,7 +1985,9 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 
 			ULOGGER_DEBUG("");
 			//Adjust thresholds
-			Q_EMIT(loopClosureThrChanged(_preferencesDialog->getLoopThr()));
+			float value;
+			value = float(_preferencesDialog->getLoopThr());
+			Q_EMIT(loopClosureThrChanged(value));
 		}
 		if(!stat.likelihood().empty() && _ui->dockWidget_likelihood->isVisible())
 		{
@@ -4326,7 +4328,9 @@ void MainWindow::applyPrefSettings(const rtabmap::ParametersMap & parameters, bo
 	_ui->doubleSpinBox_stats_timeLimit->setValue(_preferencesDialog->getTimeLimit());
 	_ui->actionSLAM_mode->setChecked(_preferencesDialog->isSLAMMode());
 
-	Q_EMIT(loopClosureThrChanged(_preferencesDialog->getLoopThr()));
+	float value;
+	value = float(_preferencesDialog->getLoopThr());
+	Q_EMIT(loopClosureThrChanged(value));
 }
 
 void MainWindow::drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords, const std::multimap<int, cv::KeyPoint> & loopWords)
@@ -4602,7 +4606,6 @@ 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);
@@ -5215,7 +5218,6 @@ 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 < 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)))
+#if CV_MAJOR_VERSION < 4 || (CV_MAJOR_VERSION == 4 && (CV_MINOR_VERSION < 3 || (CV_MINOR_VERSION==3 && !defined(RTABMAP_OPENCV_DEV))))
 #ifndef RTABMAP_NONFREE
 	_ui->comboBox_detector_strategy->setItemData(1, 0, Qt::UserRole - 1);
 	_ui->vis_feature_detector->setItemData(1, 0, Qt::UserRole - 1);
@@ -748,9 +748,6 @@ 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()));
@@ -1886,9 +1883,6 @@ 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);
@@ -2312,9 +2306,6 @@ 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());
@@ -2793,9 +2784,6 @@ 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());
@@ -2957,7 +2945,7 @@ void PreferencesDialog::writeCoreSettings(const QString & filePath) const
 
 bool PreferencesDialog::validateForm()
 {
-#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)))
+#if CV_MAJOR_VERSION < 4 || (CV_MAJOR_VERSION == 4 && (CV_MINOR_VERSION < 3 || (CV_MINOR_VERSION==3 && !defined(RTABMAP_OPENCV_DEV))))
 #ifndef RTABMAP_NONFREE
 	// verify that SURF/SIFT cannot be selected if not built with OpenCV nonfree module
 	// BOW dictionary type
@@ -2978,7 +2966,7 @@ bool PreferencesDialog::validateForm()
 		_ui->vis_feature_detector->setCurrentIndex(Feature2D::kFeatureFastBrief);
 	}
 #endif
-#else //>= 4.4.0 >= 3.4.11
+#else //>= 4.3.0-dev
 #ifndef RTABMAP_NONFREE
 	// verify that SURF cannot be selected if not built with OpenCV nonfree module
 	// BOW dictionary type
@@ -4576,8 +4564,8 @@ void PreferencesDialog::updatePredictionPlot()
 				_ui->lineEdit_bayes_predictionLC->text().toStdString().c_str());
 		return;
 	}
-	QVector<qreal> dataX((values.size()-2)*2 + 1);
-	QVector<qreal> dataY((values.size()-2)*2 + 1);
+	QVector<float> dataX((values.size()-2)*2 + 1);
+	QVector<float> dataY((values.size()-2)*2 + 1);
 	double value;
 	double sum = 0;
 	int lvl = 1;
@@ -5646,9 +5634,6 @@ 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<qreal> & x, const std::vector<qreal> & y, const QString & unit, const QMenu * menu, QGridLayout * grid, QWidget * parent) :
+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) :
 	QWidget(parent),
 	_button(0),
 	_name(0),
@@ -84,7 +84,7 @@ void StatItem::clearCache()
 	_value->clear();
 }
 
-void StatItem::addValue(qreal y)
+void StatItem::addValue(float y)
 {
 	if(_cacheOn)
 	{
@@ -94,7 +94,7 @@ void StatItem::addValue(qreal y)
 	Q_EMIT valueAdded(y);
 }
 
-void StatItem::addValue(qreal x, qreal y)
+void StatItem::addValue(float x, float y)
 {
 	if(_cacheOn)
 	{
@@ -111,7 +111,7 @@ void StatItem::addValue(qreal x, qreal y)
 	Q_EMIT valueAdded(x,y);
 }
 
-void StatItem::setValues(const std::vector<qreal> & x, const std::vector<qreal> & y)
+void StatItem::setValues(const std::vector<float> & x, const std::vector<float> & y)
 {
 	if(_cacheOn)
 	{
@@ -254,30 +254,30 @@ void StatsToolBox::setCacheOn(bool on)
 
 void StatsToolBox::updateStat(const QString & statFullName, bool cacheOn)
 {
-	std::vector<qreal> vx,vy;
+	std::vector<float> vx,vy;
 	updateStat(statFullName, vx, vy, cacheOn);
 }
 
-void StatsToolBox::updateStat(const QString & statFullName, qreal y, bool cacheOn)
+void StatsToolBox::updateStat(const QString & statFullName, float y, bool cacheOn)
 {
-	std::vector<qreal> vx,vy(1);
+	std::vector<float> vx,vy(1);
 	vy[0] = y;
 	updateStat(statFullName, vx, vy, cacheOn);
 }
 
-void StatsToolBox::updateStat(const QString & statFullName, qreal x, qreal y, bool cacheOn)
+void StatsToolBox::updateStat(const QString & statFullName, float x, float y, bool cacheOn)
 {
-	std::vector<qreal> vx(1),vy(1);
+	std::vector<float> vx(1),vy(1);
 	vx[0] = x;
 	vy[0] = y;
 	updateStat(statFullName, vx, vy, cacheOn);
 }
 
-void StatsToolBox::updateStat(const QString & statFullName, const std::vector<qreal> & x, const std::vector<qreal> & y, bool cacheOn)
+void StatsToolBox::updateStat(const QString & statFullName, const std::vector<float> & x, const std::vector<float> & y, bool cacheOn)
 {
-	// round qreal to max 2 numbers after the dot
-	//x = (qreal(int(100*x)))/100;
-	//y = (qreal(int(100*y)))/100;
+	// round float to max 2 numbers after the dot
+	//x = (float(int(100*x)))/100;
+	//y = (float(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(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> &)));
+			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> &)));
 			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(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> &)));
+		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> &)));
 		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>true</bool>
+    <bool>false</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>-360</y>
+            <y>-696</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>0</number>
+                       <number>1</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>10</number>
+                              <number>9</number>
                              </property>
                              <widget class="QWidget" name="page_32">
                               <layout class="QVBoxLayout" name="verticalLayout_63">
@@ -4417,31 +4417,17 @@ 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="2" column="0">
-                                   <widget class="QCheckBox" name="checkbox_k4a_irDepth">
+                                  <item row="0" column="1" colspan="2">
+                                   <widget class="QLabel" name="label_558">
                                     <property name="text">
-                                     <string/>
+                                     <string>Use IR for RGB image </string>
                                     </property>
-                                    <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 name="wordWrap">
+                                     <bool>true</bool>
                                     </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>
@@ -4454,17 +4440,14 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </spacer>
                                   </item>
-                                  <item row="2" column="1" colspan="2">
-                                   <widget class="QLabel" name="label_558">
+                                  <item row="1" column="0">
+                                   <widget class="QToolButton" name="toolButton_k4a_mkv">
                                     <property name="text">
-                                     <string>Use IR for RGB image </string>
-                                    </property>
-                                    <property name="wordWrap">
-                                     <bool>true</bool>
+                                     <string>...</string>
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="5" column="3">
+                                  <item row="1" column="3">
                                    <widget class="QLabel" name="label_556">
                                     <property name="text">
                                      <string>Path to a *.MKV file.</string>
@@ -4477,7 +4460,24 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="8" column="1">
+                                  <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">
                                    <widget class="QLabel" name="label_557">
                                     <property name="text">
                                      <string>Use MKV file stamps as input rate.</string>
@@ -4490,161 +4490,8 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
-                                  <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">
+                                  <item row="4" column="0">
+                                   <widget class="QCheckBox" name="source_checkBox_useMKVStamps">
                                     <property name="text">
                                      <string/>
                                     </property>
@@ -13676,7 +13523,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>-1.000000000000000</double>
+                                <double>0.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<qreal> & x, const QVector<qreal> & y, QObject * parent) :
+UPlotCurve::UPlotCurve(const QString & name, const QVector<float> & x, const QVector<float> & y, QObject * parent) :
 	QObject(parent),
 	_plot(0),
 	_name(name),
@@ -338,15 +338,15 @@ void UPlotCurve::detach(UPlot * plot)
 
 void UPlotCurve::updateMinMax()
 {
-	qreal x,y;
+	float x,y;
 	const UPlotItem * item;
 	if(!_items.size())
 	{
-		_minMax = QVector<qreal>();
+		_minMax = QVector<float>();
 	}
 	else
 	{
-		_minMax = QVector<qreal>(4);
+		_minMax = QVector<float>(4);
 	}
 	for(int i=0; i<_items.size(); ++i)
 	{
@@ -378,12 +378,12 @@ void UPlotCurve::_addValue(UPlotItem * data)
 	// add item
 	if(data)
 	{
-		qreal x = data->data().x();
-		qreal y = data->data().y();
+		float x = data->data().x();
+		float y = data->data().y();
 
 		if(_minMax.size() != 4)
 		{
-			_minMax = QVector<qreal>(4);
+			_minMax = QVector<float>(4);
 		}
 		if(_items.size())
 		{
@@ -429,7 +429,7 @@ void UPlotCurve::addValue(UPlotItem * data)
 	}
 }
 
-void UPlotCurve::addValue(qreal x, qreal y)
+void UPlotCurve::addValue(float x, float y)
 {
 	if(_items.size() &&
 		_minMax[0] != _minMax[1] &&
@@ -441,13 +441,13 @@ void UPlotCurve::addValue(qreal x, qreal y)
 		this->clear();
 	}
 
-	qreal width = 2; // TODO warn : hard coded value!
+	float width = 2; // TODO warn : hard coded value!
 	this->addValue(new UPlotItem(x,y,width));
 }
 
-void UPlotCurve::addValue(qreal y)
+void UPlotCurve::addValue(float y)
 {
-	qreal x = 0;
+	float x = 0;
 	if(_items.size())
 	{
 		UPlotItem * lastItem = (UPlotItem *)_items.last();
@@ -463,7 +463,7 @@ void UPlotCurve::addValue(qreal y)
 void UPlotCurve::addValue(const QString & value)
 {
 	bool ok;
-	qreal v = value.toDouble(&ok);
+	float v = value.toFloat(&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<qreal> & xs, const QVector<qreal> & ys)
+void UPlotCurve::addValues(const QVector<float> & xs, const QVector<float> & ys)
 {
-	qreal width = 2; // TODO warn : hard coded value!
+	float 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<qreal> & xs, const QVector<qreal> & ys)
 	Q_EMIT dataChanged(this);
 }
 
-void UPlotCurve::addValues(const QVector<qreal> & ys)
+void UPlotCurve::addValues(const QVector<float> & ys)
 {
-	qreal x = 0;
-	qreal width = 2; // TODO warn : hard coded value!
+	float x = 0;
+	float 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<qreal> & ys)
 
 void UPlotCurve::addValues(const QVector<int> & ys)
 {
-	qreal x = 0;
-	qreal width = 2; // TODO warn : hard coded value!
+	float x = 0;
+	float 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)
 {
-	qreal x = 0;
-	qreal width = 2; // TODO warn : hard coded value!
+	float x = 0;
+	float 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<qreal> & ys)
+void UPlotCurve::addValues(const std::vector<float> & ys)
 {
-	qreal x = 0;
-	qreal width = 2; // TODO warn : hard coded value!
+	float x = 0;
+	float 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);
-					qreal x = tmp->data().x();
-					qreal y = tmp->data().y();
+					float x = tmp->data().x();
+					float 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<qreal>();
+					_minMax = QVector<float>();
 				}
 			}
 		}
@@ -687,7 +687,7 @@ void UPlotCurve::setItemsColor(const QColor & color)
 	}
 }
 
-void UPlotCurve::update(qreal scaleX, qreal scaleY, qreal offsetX, qreal offsetY, qreal xDir, qreal yDir, int maxItemsKept)
+void UPlotCurve::update(float scaleX, float scaleY, float offsetX, float offsetY, float xDir, float 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(qreal increment)
+void UPlotCurve::setXIncrement(float increment)
 {
 	_xIncrement = increment;
 }
 
-void UPlotCurve::setXStart(qreal val)
+void UPlotCurve::setXStart(float val)
 {
 	_xStart = val;
 }
@@ -884,7 +884,7 @@ void UPlotCurve::setData(QVector<UPlotItem*> & data)
 	}
 }
 
-void UPlotCurve::setData(const QVector<qreal> & x, const QVector<qreal> & y)
+void UPlotCurve::setData(const QVector<float> & x, const QVector<float> & y)
 {
 	if(x.size() == y.size())
 	{
@@ -904,8 +904,8 @@ void UPlotCurve::setData(const QVector<qreal> & x, const QVector<qreal> & y)
 
 		// update values
 		int index = 0;
-		QVector<qreal>::const_iterator i=x.begin();
-		QVector<qreal>::const_iterator j=y.begin();
+		QVector<float>::const_iterator i=x.begin();
+		QVector<float>::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<qreal> & x, const QVector<qreal> & y)
 	}
 }
 
-void UPlotCurve::setData(const std::vector<qreal> & x, const std::vector<qreal> & y)
+void UPlotCurve::setData(const std::vector<float> & x, const std::vector<float> & y)
 {
 	if(x.size() == y.size())
 	{
@@ -945,8 +945,8 @@ void UPlotCurve::setData(const std::vector<qreal> & x, const std::vector<qreal>
 
 		// update values
 		int index = 0;
-		std::vector<qreal>::const_iterator i=x.begin();
-		std::vector<qreal>::const_iterator j=y.begin();
+		std::vector<float>::const_iterator i=x.begin();
+		std::vector<float>::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<qreal> & x, const std::vector<qreal>
 	}
 }
 
-void UPlotCurve::setData(const QVector<qreal> & y)
+void UPlotCurve::setData(const QVector<float> & y)
 {
 	this->setData(y.toStdVector());
 }
 
-void UPlotCurve::setData(const std::vector<qreal> & y)
+void UPlotCurve::setData(const std::vector<float> & 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<qreal> & y)
 
 	// update values
 	int index = 0;
-	qreal x = 0;
-	std::vector<qreal>::const_iterator j=y.begin();
+	float x = 0;
+	std::vector<float>::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<qreal> & y)
 	Q_EMIT dataChanged(this);
 }
 
-void UPlotCurve::getData(QVector<qreal> & x, QVector<qreal> & y) const
+void UPlotCurve::getData(QVector<float> & x, QVector<float> & y) const
 {
 	x.clear();
 	y.clear();
@@ -1018,7 +1018,7 @@ void UPlotCurve::getData(QVector<qreal> & x, QVector<qreal> & y) const
 	}
 }
 
-void UPlotCurve::getData(QMap<qreal,qreal> & data) const
+void UPlotCurve::getData(QMap<float,float> & data) const
 {
 	data.clear();
 	if(_items.size())
@@ -1034,7 +1034,7 @@ void UPlotCurve::getData(QMap<qreal,qreal> & data) const
 
 
 
-UPlotCurveThreshold::UPlotCurveThreshold(const QString & name, qreal thesholdValue, Qt::Orientation orientation, QObject * parent) :
+UPlotCurveThreshold::UPlotCurveThreshold(const QString & name, float thesholdValue, Qt::Orientation orientation, QObject * parent) :
 	UPlotCurve(name, parent),
 	_orientation(orientation)
 {
@@ -1055,7 +1055,7 @@ UPlotCurveThreshold::~UPlotCurveThreshold()
 
 }
 
-void UPlotCurveThreshold::setThreshold(qreal threshold)
+void UPlotCurveThreshold::setThreshold(float threshold)
 {
 #if PRINT_DEBUG
 	ULOGGER_DEBUG("%f", threshold);
@@ -1104,7 +1104,7 @@ void UPlotCurveThreshold::setOrientation(Qt::Orientation orientation)
 	}
 }
 
-void UPlotCurveThreshold::update(qreal scaleX, qreal scaleY, qreal offsetX, qreal offsetY, qreal xDir, qreal yDir, int maxItemsKept)
+void UPlotCurveThreshold::update(float scaleX, float scaleY, float offsetX, float offsetY, float xDir, float yDir, int maxItemsKept)
 {
 	if(_items.size() == 3)
 	{
@@ -1142,7 +1142,7 @@ void UPlotCurveThreshold::update(qreal scaleX, qreal scaleY, qreal offsetX, qrea
 
 
 
-UPlotAxis::UPlotAxis(Qt::Orientation orientation, qreal min, qreal max, QWidget * parent) :
+UPlotAxis::UPlotAxis(Qt::Orientation orientation, float min, float max, QWidget * parent) :
 	QWidget(parent),
 	_orientation(orientation),
 	_min(0),
@@ -1177,14 +1177,14 @@ void UPlotAxis::setReversed(bool reversed)
 {
 	if(_reversed != reversed)
 	{
-		qreal min = _min;
+		float min = _min;
 		_min = _max;
 		_max = min;
 	}
 	_reversed = reversed;
 }
 
-void UPlotAxis::setAxis(qreal & min, qreal & max)
+void UPlotAxis::setAxis(float & min, float & max)
 {
 	int borderMin = 0;
 	int borderMax = 0;
@@ -1241,13 +1241,13 @@ void UPlotAxis::setAxis(qreal & min, qreal & max)
 	// Rounding min and max
 	if(min != max)
 	{
-		qreal mul = 1;
-		qreal rangef = max - min;
+		float mul = 1;
+		float rangef = max - min;
 		int countStep = _count/5;
-		qreal val;
+		float val;
 		for(int i=0; i<6; ++i)
 		{
-			val = (rangef/qreal(countStep)) * mul;
+			val = (rangef/float(countStep)) * mul;
 			if( val >= 1.0f && val < 10.0f)
 			{
 				break;
@@ -1264,8 +1264,8 @@ void UPlotAxis::setAxis(qreal & min, qreal & max)
 		//ULOGGER_DEBUG("min=%f, max=%f", min, max);
 		int minR = min*mul-0.9;
 		int maxR = max*mul+0.9;
-		min = qreal(minR)/mul;
-		max = qreal(maxR)/mul;
+		min = float(minR)/mul;
+		max = float(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<qreal> x;
-			QVector<qreal> y;
+			QVector<float> x;
+			QVector<float> 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<qreal> x, y;
+	QVector<float> x, y;
 	_curve->getData(x, y);
-	qreal mean = uMean(y.data(), y.size());
-	qreal stdDev = std::sqrt(uVariance(y.data(), y.size(), mean));
-	qreal max = uMax(y.data(), y.size());
+	float mean = uMean(y.data(), y.size());
+	float stdDev = std::sqrt(uVariance(y.data(), y.size(), mean));
+	float 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<qreal, qreal> xAxisMap;
+			QMap<float, float> xAxisMap;
 			for(int i=0; i<items.size(); ++i)
 			{
-				QMap<qreal, qreal> data;
+				QMap<float, float> data;
 				items.at(i)->curve()->getData(data);
-				for(QMap<qreal, qreal>::iterator iter=data.begin(); iter!=data.end(); ++iter)
+				for(QMap<float, float>::iterator iter=data.begin(); iter!=data.end(); ++iter)
 				{
 					xAxisMap.insert(iter.key(), iter.value());
 				}
 			}
-			QList<qreal> xAxis = xAxisMap.uniqueKeys();
+			QList<float> xAxis = xAxisMap.uniqueKeys();
 
-			QVector<QVector<qreal> > axes;
+			QVector<QVector<float> > axes;
 			for(int i=0; i<items.size(); ++i)
 			{
-				QMap<qreal, qreal> data;
+				QMap<float, float> data;
 				items.at(i)->curve()->getData(data);
 
-				QVector<qreal> y(xAxis.size(), std::numeric_limits<qreal>::quiet_NaN());
+				QVector<float> y(xAxis.size(), std::numeric_limits<float>::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<qreal>::iterator iter=xAxis.begin(); iter!=xAxis.end(); ++iter)
+				for(QList<float>::iterator iter=xAxis.begin(); iter!=xAxis.end(); ++iter)
 				{
 					if(data.contains(*iter))
 					{
@@ -2226,7 +2226,7 @@ void UPlot::replot(QPainter * painter)
 		}
 	}
 
-	qreal axis[4] = {0};
+	float 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);
-	qreal borderHor = (qreal)_horizontalAxis->border();
-	qreal borderVer = (qreal)_verticalAxis->border();
+	float borderHor = (float)_horizontalAxis->border();
+	float borderVer = (float)_verticalAxis->border();
 
 	//grid
 	qDeleteAll(hGridLines);
@@ -2255,14 +2255,14 @@ void UPlot::replot(QPainter * painter)
 	if(_aShowGrid->isChecked())
 	{
 		// TODO make a PlotGrid class ?
-		qreal w = newRect.width()-(borderHor*2);
-		qreal h = newRect.height()-(borderVer*2);
-		qreal stepH = w / qreal(_horizontalAxis->count());
-		qreal stepV = h / qreal(_verticalAxis->count());
+		float w = newRect.width()-(borderHor*2);
+		float h = newRect.height()-(borderVer*2);
+		float stepH = w / float(_horizontalAxis->count());
+		float stepV = h / float(_verticalAxis->count());
 		QPen dashPen(Qt::DashLine);
 		dashPen.setColor(QColor(255-_bgColor.red(), 255-_bgColor.green(), 255-_bgColor.blue(), 100));
 		QPen pen(dashPen.color());
-		for(qreal i=0.0f; i*stepV <= h+stepV; i+=5.0f)
+		for(float 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(qreal i=0; i*stepH < w+stepH; i+=5.0f)
+		for(float i=0; i*stepH < w+stepH; i+=5.0f)
 		{
 			//vertical lines
 			if(!_aGraphicsView->isChecked())
@@ -2323,9 +2323,9 @@ void UPlot::replot(QPainter * painter)
 	}
 
 	// curves
-	qreal scaleX = 1;
-	qreal scaleY = 1;
-	qreal den = 0;
+	float scaleX = 1;
+	float scaleY = 1;
+	float den = 0;
 	den = axis[1] - axis[0];
 	if(den != 0)
 	{
@@ -2340,8 +2340,8 @@ void UPlot::replot(QPainter * painter)
 	{
 		if((*i)->isVisible())
 		{
-			qreal xDir = 1.0f;
-			qreal yDir = -1.0f;
+			float xDir = 1.0f;
+			float 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/qreal(_refreshIntervalTime.restart()));
+		int refreshRate = qRound(1000.0f/float(_refreshIntervalTime.restart()));
 		if(refreshRate > 0 && refreshRate < _lowestRefreshRate)
 		{
 			_lowestRefreshRate = refreshRate;
@@ -2374,14 +2374,14 @@ void UPlot::replot(QPainter * painter)
 	}
 }
 
-void UPlot::setFixedXAxis(qreal x1, qreal x2)
+void UPlot::setFixedXAxis(float x1, float x2)
 {
 	_fixedAxis[0] = true;
 	_axisMaximums[0] = x1;
 	_axisMaximums[1] = x2;
 }
 
-void UPlot::setFixedYAxis(qreal y1, qreal y2)
+void UPlot::setFixedYAxis(float y1, float 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<qreal> & minMax = curve->getMinMax();
+		const QVector<float> & 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(qreal x1, qreal x2, qreal y1, qreal y2)
+bool UPlot::updateAxis(float x1, float x2, float y1, float y2)
 {
 	bool modified = false;
 	modified = updateAxis(x1,y1);
@@ -2419,7 +2419,7 @@ bool UPlot::updateAxis(qreal x1, qreal x2, qreal y1, qreal y2)
 	return modified;
 }
 
-bool UPlot::updateAxis(qreal x, qreal y)
+bool UPlot::updateAxis(float x, float 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<qreal> & minMax = _curves.at(i)->getMinMax();
+			const QVector<float> & 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;
 		}
 
-		qreal x,y;
+		float 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)
 		{
-			qreal axis[4];
+			float 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, qreal & x, qreal & y)
+bool UPlot::mousePosToValue(const QPoint & pos, float & x, float & 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, qreal & x, qreal & 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", (qreal)_horizontalAxis->border(), (qreal)_verticalAxis->border());
+	//UDEBUG("border hor=%f ver=%f", (float)_horizontalAxis->border(), (float)_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] + qreal(xPos)*(_axisMaximums[1] - _axisMaximums[0]) / qreal(maxX);
-	y = _axisMaximums[2] + qreal(maxY - yPos)*(_axisMaximums[3] - _axisMaximums[2]) / qreal(maxY);
+	x = _axisMaximums[0] + float(xPos)*(_axisMaximums[1] - _axisMaximums[0]) / float(maxX);
+	y = _axisMaximums[2] + float(maxY - yPos)*(_axisMaximums[3] - _axisMaximums[2]) / float(maxY);
 	return true;
 }
 
@@ -2943,61 +2943,8 @@ 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, qreal value, Qt::Orientation orientation)
+UPlotCurveThreshold * UPlot::addThreshold(const QString & name, float 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.2</version>
+  <version>0.20.0</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,6 +12,7 @@
 
   <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>
@@ -25,6 +26,7 @@
   <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,14 +26,6 @@ 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,14 +26,6 @@ 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);
+		camera = new rtabmap::CameraK4A(1, 0, rtabmap::Transform::getIdentity());
 	}
 	else if (driver == 13)
 	{
@@ -347,6 +347,9 @@ 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;
@@ -415,7 +418,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, rtabmap::Transform::opengl_T_rtabmap()*data.cameraModels()[0].localTransform());
+				cloud = rtabmap::util3d::transformPointCloud(cloud, t);
 				if(viewer)
 					viewer->showCloud(cloud, "cloud");
 			}
@@ -426,7 +429,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, rtabmap::Transform::opengl_T_rtabmap()*data.cameraModels()[0].localTransform());
+				cloud = rtabmap::util3d::transformPointCloud(cloud, t);
 				viewer->showCloud(cloud, "cloud");
 			}
 
@@ -454,7 +457,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, rtabmap::Transform::opengl_T_rtabmap()*data.stereoCameraModel().localTransform());
+				cloud = rtabmap::util3d::transformPointCloud(cloud, t);
 				if(viewer)
 					viewer->showCloud(cloud, "cloud");
 			}

tools/DataRecorder/CMakeLists.txt

@@ -32,14 +32,6 @@ 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,12 +56,6 @@ 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);
 }
@@ -133,7 +127,7 @@ int main (int argc, char * argv[])
 			if(i < argc)
 			{
 				driver = std::atoi(argv[i]);
-				if(driver < 0 || driver > 13)
+				if(driver < 0 || driver > 7)
 				{
 					showUsage();
 				}
@@ -182,9 +176,10 @@ 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);
+		camera = new rtabmap::CameraOpenni(deviceId, rate, t);
 	}
 	else if(driver == 1)
 	{
@@ -193,7 +188,7 @@ int main (int argc, char * argv[])
 			UERROR("Not built with OpenNI2 support...");
 			exit(-1);
 		}
-		camera = new rtabmap::CameraOpenNI2(deviceId, CameraOpenNI2::kTypeColorDepth, rate);
+		camera = new rtabmap::CameraOpenNI2(deviceId, CameraOpenNI2::kTypeColorDepth, rate, t);
 	}
 	else if(driver == 2)
 	{
@@ -202,7 +197,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);
+		camera = new rtabmap::CameraFreenect(deviceId.size()?atoi(deviceId.c_str()):0, CameraFreenect::kTypeColorDepth, rate, t);
 	}
 	else if(driver == 3)
 	{
@@ -211,7 +206,7 @@ int main (int argc, char * argv[])
 			UERROR("Not built with OpenNI from OpenCV support...");
 			exit(-1);
 		}
-		camera = new rtabmap::CameraOpenNICV(false, rate);
+		camera = new rtabmap::CameraOpenNICV(false, rate, t);
 	}
 	else if(driver == 4)
 	{
@@ -220,7 +215,7 @@ int main (int argc, char * argv[])
 			UERROR("Not built with OpenNI from OpenCV support...");
 			exit(-1);
 		}
-		camera = new rtabmap::CameraOpenNICV(true, rate);
+		camera = new rtabmap::CameraOpenNICV(true, rate, t);
 	}
 	else if(driver == 5)
 	{
@@ -229,7 +224,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);
+		camera = new rtabmap::CameraFreenect2(deviceId.size()?atoi(deviceId.c_str()):0, rtabmap::CameraFreenect2::kTypeColor2DepthSD, rate, t);
 	}
 	else if(driver == 6)
 	{
@@ -238,7 +233,7 @@ int main (int argc, char * argv[])
 			UERROR("Not built with dc1394 support...");
 			exit(-1);
 		}
-		camera = new rtabmap::CameraStereoDC1394(rate);
+		camera = new rtabmap::CameraStereoDC1394(rate, t);
 	}
 	else if(driver == 7)
 	{
@@ -247,61 +242,7 @@ int main (int argc, char * argv[])
 			UERROR("Not built with FlyCapture2/Triclops support...");
 			exit(-1);
 		}
-		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);
+		camera = new rtabmap::CameraStereoFlyCapture2(rate, t);
 	}
 	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 because RTAB-Map is not built with AliceVision support. Ignoring multiband...\n");
+			printf("\"--multiband\" option cannot be used vecause 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(outputDirectory+"/"+textureMesh->tex_materials[i].tex_file, cv::Mat(textures, cv::Range::all(), cv::Range(textures.rows*i, textures.rows*(i+1))));
+							success = cv::imwrite(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());