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

.travis.yml

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

CMakeLists.txt

@@ -21,7 +21,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 20)
-SET(RTABMAP_PATCH_VERSION 2)
+SET(RTABMAP_PATCH_VERSION 0)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   
@@ -471,7 +471,7 @@ ENDIF(WITH_REALSENSE)
 IF(WITH_REALSENSE2)
     IF(WIN32)
         FIND_PACKAGE(RealSense2 QUIET)
-    ELSE()
+	ELSE()
         FIND_PACKAGE(realsense2 QUIET)
     ENDIF()
     IF(realsense2_FOUND)
@@ -689,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)

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/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;
-	}
+	glDeleteTextures(1, &textureId_);
 }
 
 
@@ -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,59 +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 <arcore_c_api.h>
-#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,19 +454,11 @@ 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)
+		// Draw scene
+		for(std::vector<PointCloudDrawable*>::const_iterator iter=cloudsToDraw.begin(); iter!=cloudsToDraw.end(); ++iter)
 		{
-			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);
-			}
+			// 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
@@ -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

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

app/android/res/values/strings.xml

@@ -81,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/RTABMapActivity.java

@@ -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));
@@ -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())
@@ -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/src/CMakeLists.txt

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

@@ -1189,8 +1189,8 @@ void stereoRectifyFisheye( cv::InputArray _cameraMatrix1, cv::InputArray _distCo
     cv::Mat cameraMatrix1 = _cameraMatrix1.getMat(), cameraMatrix2 = _cameraMatrix2.getMat();
     cv::Mat distCoeffs1 = _distCoeffs1.getMat(), distCoeffs2 = _distCoeffs2.getMat();
     cv::Mat Rmat = _Rmat.getMat(), Tmat = _Tmat.getMat();
-	
-#if CV_MAJOR_VERSION > 3 || (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

@@ -3847,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)
 	{
@@ -4260,48 +4244,29 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 				}
 			}
 
-			bool useProvided3dPoints = false;
-			if(_useOdometryFeatures && !data.keypoints().empty())
+			int oldMaxFeatures = _feature2D->getMaxFeatures();
+			UDEBUG("rawDescriptorsKept=%d, pose=%d, maxFeatures=%d, visMaxFeatures=%d", _rawDescriptorsKept?1:0, pose.isNull()?0:1, _feature2D->getMaxFeatures(), _visMaxFeatures);
+			ParametersMap tmpMaxFeatureParameter;
+			if(_rawDescriptorsKept&&!pose.isNull()&&_feature2D->getMaxFeatures()>0&&_feature2D->getMaxFeatures()<_visMaxFeatures)
 			{
-				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;
-				}
+				// The total extracted features should match the number of features used for transformation estimation
+				UDEBUG("Changing temporary max features from %d to %d", _feature2D->getMaxFeatures(), _visMaxFeatures);
+				tmpMaxFeatureParameter.insert(ParametersPair(Parameters::kKpMaxFeatures(), uNumber2Str(_visMaxFeatures)));
+				_feature2D->parseParameters(tmpMaxFeatureParameter);
 			}
-			else
+
+			keypoints = _feature2D->generateKeypoints(
+					imageMono,
+					depthMask);
+
+			if(tmpMaxFeatureParameter.size())
 			{
-				int oldMaxFeatures = _feature2D->getMaxFeatures();
-				UDEBUG("rawDescriptorsKept=%d, pose=%d, maxFeatures=%d, visMaxFeatures=%d", _rawDescriptorsKept?1:0, pose.isNull()?0:1, _feature2D->getMaxFeatures(), _visMaxFeatures);
-				ParametersMap tmpMaxFeatureParameter;
-				if(_rawDescriptorsKept&&!pose.isNull()&&_feature2D->getMaxFeatures()>0&&_feature2D->getMaxFeatures()<_visMaxFeatures)
-				{
-					// The total extracted features should match the number of features used for transformation estimation
-					UDEBUG("Changing temporary max features from %d to %d", _feature2D->getMaxFeatures(), _visMaxFeatures);
-					tmpMaxFeatureParameter.insert(ParametersPair(Parameters::kKpMaxFeatures(), uNumber2Str(_visMaxFeatures)));
-					_feature2D->parseParameters(tmpMaxFeatureParameter);
-				}
-
-				keypoints = _feature2D->generateKeypoints(
-						imageMono,
-						depthMask);
-
-				if(tmpMaxFeatureParameter.size())
-				{
-					tmpMaxFeatureParameter.at(Parameters::kKpMaxFeatures()) = uNumber2Str(oldMaxFeatures);
-					_feature2D->parseParameters(tmpMaxFeatureParameter); // reset back
-				}
-				t = timer.ticks();
-				if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f);
-				UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), t);
+				tmpMaxFeatureParameter.at(Parameters::kKpMaxFeatures()) = uNumber2Str(oldMaxFeatures);
+				_feature2D->parseParameters(tmpMaxFeatureParameter); // reset back
 			}
+			t = timer.ticks();
+			if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f);
+			UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), t);
 
 			descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
 			t = timer.ticks();
@@ -4433,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 < 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/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,34 +1053,36 @@ 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())
 		{
-			// Localization mode
-			if(!_optimizeFromGraphEnd)
+			if(!_mapCorrectionBackup.isNull())
 			{
-				//set map->odom so that odom is moved back to last saved localization
-				_mapCorrection = _lastLocalizationPose * odomPose.inverse();
-				std::map<int, Transform> nodesOnly(_optimizedPoses.lower_bound(1), _optimizedPoses.end());
-				_lastLocalizationNodeId = graph::findNearestNode(nodesOnly, _lastLocalizationPose);
-				UWARN("Update map correction based on last localization saved in database! correction = %s, nearest id = %d of last pose = %s, odom = %s",
-						_mapCorrection.prettyPrint().c_str(),
-						_lastLocalizationNodeId,
-						_lastLocalizationPose.prettyPrint().c_str(),
-						odomPose.prettyPrint().c_str());
+				_mapCorrection = _mapCorrectionBackup;
+				_mapCorrectionBackup.setNull();
 			}
-			else
+			else if(_optimizedPoses.size() && _mapCorrection.isIdentity() && !_lastLocalizationPose.isNull() && _lastLocalizationNodeId == 0)
 			{
-				//move optimized poses accordingly to last saved localization
-				Transform mapCorrectionInv = odomPose * _lastLocalizationPose.inverse();
-				for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
+				// Localization mode
+				if(!_optimizeFromGraphEnd)
 				{
-					iter->second = mapCorrectionInv * iter->second;
+					//set map->odom so that odom is moved back to last saved localization
+					_mapCorrection = _lastLocalizationPose * odomPose.inverse();
+					std::map<int, Transform> nodesOnly(_optimizedPoses.lower_bound(1), _optimizedPoses.end());
+					_lastLocalizationNodeId = graph::findNearestNode(nodesOnly, _lastLocalizationPose);
+					UWARN("Update map correction based on last localization saved in database! correction = %s, nearest id = %d of last pose = %s, odom = %s",
+							_mapCorrection.prettyPrint().c_str(),
+							_lastLocalizationNodeId,
+							_lastLocalizationPose.prettyPrint().c_str(),
+							odomPose.prettyPrint().c_str());
+				}
+				else
+				{
+					//move optimized poses accordingly to last saved localization
+					Transform mapCorrectionInv = odomPose * _lastLocalizationPose.inverse();
+					for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
+					{
+						iter->second = mapCorrectionInv * iter->second;
+					}
 				}
 			}
 		}
@@ -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,13 +3410,11 @@ 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())
 					{
-						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(jter);
 					}
 				}
 				_constraints.erase(lastId);
@@ -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,34 +4353,23 @@ Signature Rtabmap::getSignatureCopy(int id, bool images, bool scan, bool userDat
 			data.setCameraModels(models);
 			data.setStereoCameraModel(stereoModel);
 		}
-
+		std::multimap<int, cv::KeyPoint> words;
+		std::multimap<int, cv::Point3f> words3;
+		std::multimap<int, cv::Mat> wordsDescriptors;
+		std::vector<rtabmap::GlobalDescriptor> globalDescriptors;
+		_memory->getNodeWordsAndGlobalDescriptors(id, words, words3, wordsDescriptors, globalDescriptors);
 		s=Signature(id,
-				mapId,
-				weight,
-				stamp,
-				label,
-				odomPoseLocal,
-				groundTruth,
-				data);
-
-		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);
-			}
-		}
+					mapId,
+					weight,
+					stamp,
+					label,
+					odomPoseLocal,
+					groundTruth,
+					data);
+		s.setWords(words);
+		s.setWords3(words3);
+		s.setWordsDescriptors(wordsDescriptors);
+		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,129 +4825,145 @@ 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(poses.find(link.from()) == poses.end())
+		if(_memory->isIncremental())
 		{
-			UERROR("Link's \"from id\" %d is not in the graph (size=%d)", link.from(), (int)poses.size());
-			_memory->removeLink(link.from(), link.to());
-			return false;
-		}
-		if(poses.find(link.to()) == poses.end())
-		{
-			UERROR("Link's \"to id\" %d is not in the graph (size=%d)", link.to(), (int)poses.size());
-			_memory->removeLink(link.from(), link.to());
-			return false;
+			if(poses.find(link.from()) == poses.end())
+			{
+				UERROR("Link's \"from id\" %d is not in the graph", link.from());
+				return false;
+			}
+			if(poses.find(link.to()) == poses.end())
+			{
+				UERROR("Link's \"to id\" %d is not in the graph", link.to());
+				return false;
+			}
 		}
 
-		std::string msg;
-		if(poses.empty())
+		int from = link.from();
+		int to = link.to();
+
+		if(_optimizationMaxError > 0.0f)
 		{
-			msg = uFormat("Rejecting edge %d->%d because graph optimization has failed!", link.from(), link.to());
-		}
-		else if(_optimizationMaxError > 0.0f)
-		{
-			float maxLinearError = 0.0f;
-			float maxLinearErrorRatio = 0.0f;
-			float maxAngularError = 0.0f;
-			float maxAngularErrorRatio = 0.0f;
+			//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;
-
-			graph::computeMaxGraphErrors(
-					poses,
-					links,
-					maxLinearErrorRatio,
-					maxAngularErrorRatio,
-					maxLinearError,
-					maxAngularError,
-					&maxLinearLink,
-					&maxAngularLink);
-			if(maxLinearLink)
+			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())
 			{
-				UINFO("Max optimization linear error = %f m (link %d->%d)", maxLinearError, maxLinearLink->from(), maxLinearLink->to());
-				if(maxLinearErrorRatio > _optimizationMaxError)
+				graph::computeMaxGraphErrors(
+						optimizedPoses,
+						links,
+						maxLinearErrorRatio,
+						maxAngularErrorRatio,
+						maxLinearError,
+						maxAngularError,
+						&maxLinearLink,
+						&maxAngularLink);
+				if(maxLinearLink)
 				{
-					msg = uFormat("Rejecting edge %d->%d because "
-							  "graph error is too large after optimization (%f m for edge %d->%d with ratio %f > std=%f m). "
-							  "\"%s\" is %f.",
-							  link.from(),
-							  link.to(),
-							  maxLinearError,
-							  maxLinearLink->from(),
-							  maxLinearLink->to(),
-							  maxLinearErrorRatio,
-							  sqrt(maxLinearLink->transVariance()),
-							  Parameters::kRGBDOptimizeMaxError().c_str(),
-							  _optimizationMaxError);
+					UINFO("Max optimization linear error = %f m (link %d->%d)", maxLinearError, maxLinearLink->from(), maxLinearLink->to());
+					if(maxLinearErrorRatio > _optimizationMaxError)
+					{
+						msg = uFormat("Rejecting edge %d->%d because "
+								  "graph error is too large after optimization (%f m for edge %d->%d with ratio %f > std=%f m). "
+								  "\"%s\" is %f.",
+								  from,
+								  to,
+								  maxLinearError,
+								  maxLinearLink->from(),
+								  maxLinearLink->to(),
+								  maxLinearErrorRatio,
+								  sqrt(maxLinearLink->transVariance()),
+								  Parameters::kRGBDOptimizeMaxError().c_str(),
+								  _optimizationMaxError);
+					}
+				}
+				else if(maxAngularLink)
+				{
+					UINFO("Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0f/M_PI, maxAngularLink->from(), maxAngularLink->to());
+					if(maxAngularErrorRatio > _optimizationMaxError)
+					{
+						msg = uFormat("Rejecting edge %d->%d because "
+								  "graph error is too large after optimization (%f deg for edge %d->%d with ratio %f > std=%f deg). "
+								  "\"%s\" is %f m.",
+								  from,
+								  to,
+								  maxAngularError*180.0f/M_PI,
+								  maxAngularLink->from(),
+								  maxAngularLink->to(),
+								  maxAngularErrorRatio,
+								  sqrt(maxAngularLink->rotVariance()),
+								  Parameters::kRGBDOptimizeMaxError().c_str(),
+								  _optimizationMaxError);
+					}
 				}
 			}
-			else if(maxAngularLink)
+			else
 			{
-				UINFO("Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0f/M_PI, maxAngularLink->from(), maxAngularLink->to());
-				if(maxAngularErrorRatio > _optimizationMaxError)
+				msg = uFormat("Rejecting edge %d->%d because graph optimization has failed!",
+						  from,
+						  to);
+			}
+			if(!msg.empty())
+			{
+				UERROR("%s", msg.c_str());
+				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)
+			{
+				std::map<int, Transform>::iterator jter = poses.find(iter->first);
+				if(jter != poses.end())
 				{
-					msg = uFormat("Rejecting edge %d->%d because "
-							  "graph error is too large after optimization (%f deg for edge %d->%d with ratio %f > std=%f deg). "
-							  "\"%s\" is %f m.",
-							  link.from(),
-							  link.to(),
-							  maxAngularError*180.0f/M_PI,
-							  maxAngularLink->from(),
-							  maxAngularLink->to(),
-							  maxAngularErrorRatio,
-							  sqrt(maxAngularLink->rotVariance()),
-							  Parameters::kRGBDOptimizeMaxError().c_str(),
-							  _optimizationMaxError);
+					iter->second = jter->second;
 				}
 			}
-		}
-		if(!msg.empty())
-		{
-			UERROR("%s", msg.c_str());
-			_memory->removeLink(link.from(), link.to());
-			return false;
-		}
-
-		// Update optimized poses
-		for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
-		{
-			std::map<int, Transform>::iterator jter = poses.find(iter->first);
-			if(jter != poses.end())
+			if(!_optimizeFromGraphEnd)
 			{
-				iter->second = jter->second;
+				_mapCorrection = _optimizedPoses.rbegin()->second * _memory->getSignature(_optimizedPoses.rbegin()->first)->getPose().inverse();
 			}
-		}
-		if(!_optimizeFromGraphEnd)
-		{
-			_mapCorrection = _optimizedPoses.rbegin()->second * _memory->getSignature(_optimizedPoses.rbegin()->first)->getPose().inverse();
-		}
 
-		std::map<int, Transform> tmp;
-		// Update also the links if some have been added in WM
-		_memory->getMetricConstraints(uKeysSet(_optimizedPoses), tmp, _constraints, false);
-		// This will force rtabmap_ros to regenerate the global occupancy grid if there was one
-		_memory->save2DMap(cv::Mat(), 0, 0, 0);
+			std::map<int, Transform> tmp;
+			// Update also the links if some have been added in WM
+			_memory->getMetricConstraints(uKeysSet(_optimizedPoses), tmp, _constraints, false);
+			// This will force rtabmap_ros to regenerate the global occupancy grid if there was one
+			_memory->save2DMap(cv::Mat(), 0, 0, 0);
 
-		return true;
+			return true;
+		}
 	}
 	else // localization mode
 	{
@@ -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

@@ -4,12 +4,12 @@ All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
 modification, are permitted provided that the following conditions are met:
- * Redistributions of source code must retain the above copyright
+    * Redistributions of source code must retain the above copyright
       notice, this list of conditions and the following disclaimer.
- * Redistributions in binary form must reproduce the above copyright
+    * Redistributions in binary form must reproduce the above copyright
       notice, this list of conditions and the following disclaimer in the
       documentation and/or other materials provided with the distribution.
- * Neither the name of the Universite de Sherbrooke nor the
+    * Neither the name of the Universite de Sherbrooke nor the
       names of its contributors may be used to endorse or promote products
       derived from this software without specific prior written permission.
 
@@ -22,7 +22,7 @@ LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- */
+*/
 
 #include <rtabmap/core/camera/CameraK4A.h>
 #include <rtabmap/utilite/UTimer.h>
@@ -49,47 +49,34 @@ bool CameraK4A::available()
 }
 
 CameraK4A::CameraK4A(
-		int deviceId,
-		float imageRate,
-		const Transform & localTransform) :
-			Camera(imageRate, localTransform)
+	int deviceId,
+	float imageRate,
+	const Transform & localTransform) :
+	Camera(imageRate, localTransform)
 #ifdef RTABMAP_K4A
-			,
-			device_(NULL),
-			config_(K4A_DEVICE_CONFIG_INIT_DISABLE_ALL),
-			transformation_(NULL),
-			capture_(NULL),
-			playbackHandle_(NULL),
-			transformationHandle_(NULL),
-			deviceId_(deviceId),
-			rgb_resolution_(0),
-			framerate_(2),
-			depth_resolution_(2),
-			ir_(false),
-			previousStamp_(0.0)
+	,deviceId_(deviceId),
+	playbackHandle_(NULL),
+	transformationHandle_(NULL),
+	ir_(false),
+	previousStamp_(0.0)
 #endif
 {
 }
 
 CameraK4A::CameraK4A(
-		const std::string & fileName,
-		float imageRate,
-		const Transform & localTransform) :
-			Camera(imageRate, localTransform)
+	const std::string & fileName,
+	float imageRate,
+	const Transform & localTransform) :
+	Camera(imageRate, localTransform)
 #ifdef RTABMAP_K4A
-			,
-			device_(NULL),
-			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)
+	,
+	device_(NULL),
+	playbackHandle_(NULL),
+	transformationHandle_(NULL),
+	deviceId_(-1),
+	fileName_(fileName),
+	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
@@ -181,24 +157,24 @@ bool CameraK4A::init(const std::string & calibrationFolder, const std::string &
 		if (ir_)
 		{
 			model_ = CameraModel(
-					calibration.depth_camera_calibration.intrinsics.parameters.param.fx,
-					calibration.depth_camera_calibration.intrinsics.parameters.param.fy,
-					calibration.depth_camera_calibration.intrinsics.parameters.param.cx,
-					calibration.depth_camera_calibration.intrinsics.parameters.param.cy,
-					this->getLocalTransform(),
-					0,
-					cv::Size(calibration.depth_camera_calibration.resolution_width, calibration.depth_camera_calibration.resolution_height));
+				calibration.depth_camera_calibration.intrinsics.parameters.param.fx,
+				calibration.depth_camera_calibration.intrinsics.parameters.param.fy,
+				calibration.depth_camera_calibration.intrinsics.parameters.param.cx,
+				calibration.depth_camera_calibration.intrinsics.parameters.param.cy,
+				this->getLocalTransform(),
+				0,
+				cv::Size(calibration.depth_camera_calibration.resolution_width, calibration.depth_camera_calibration.resolution_height));
 		}
 		else
 		{
 			model_ = CameraModel(
-					calibration.color_camera_calibration.intrinsics.parameters.param.fx,
-					calibration.color_camera_calibration.intrinsics.parameters.param.fy,
-					calibration.color_camera_calibration.intrinsics.parameters.param.cx,
-					calibration.color_camera_calibration.intrinsics.parameters.param.cy,
-					this->getLocalTransform(),
-					0,
-					cv::Size(calibration.color_camera_calibration.resolution_width, calibration.color_camera_calibration.resolution_height));
+				calibration.color_camera_calibration.intrinsics.parameters.param.fx,
+				calibration.color_camera_calibration.intrinsics.parameters.param.fy,
+				calibration.color_camera_calibration.intrinsics.parameters.param.cx,
+				calibration.color_camera_calibration.intrinsics.parameters.param.cy,
+				this->getLocalTransform(),
+				0,
+				cv::Size(calibration.color_camera_calibration.resolution_width, calibration.color_camera_calibration.resolution_height));
 
 			transformationHandle_ = k4a_transformation_create(&calibration);
 		}
@@ -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,55 +240,35 @@ 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;
 		}
 
-		if (ir_)
-		{
-			model_ = CameraModel(
-					calibration_.depth_camera_calibration.intrinsics.parameters.param.fx,
-					calibration_.depth_camera_calibration.intrinsics.parameters.param.fy,
-					calibration_.depth_camera_calibration.intrinsics.parameters.param.cx,
-					calibration_.depth_camera_calibration.intrinsics.parameters.param.cy,
-					this->getLocalTransform(),
-					0,
-					cv::Size(calibration_.depth_camera_calibration.resolution_width, calibration_.depth_camera_calibration.resolution_height));
-		}
-		else
-		{
-			model_ = CameraModel(
-					calibration_.color_camera_calibration.intrinsics.parameters.param.fx,
-					calibration_.color_camera_calibration.intrinsics.parameters.param.fy,
-					calibration_.color_camera_calibration.intrinsics.parameters.param.cx,
-					calibration_.color_camera_calibration.intrinsics.parameters.param.cy,
-					this->getLocalTransform(),
-					0,
-					cv::Size(calibration_.color_camera_calibration.resolution_width, calibration_.color_camera_calibration.resolution_height));
-		}
+                if (ir_)
+                {
+                        model_ = CameraModel(
+                                calibration_.depth_camera_calibration.intrinsics.parameters.param.fx,
+                                calibration_.depth_camera_calibration.intrinsics.parameters.param.fy,
+                                calibration_.depth_camera_calibration.intrinsics.parameters.param.cx,
+                                calibration_.depth_camera_calibration.intrinsics.parameters.param.cy,
+                                this->getLocalTransform(),
+                                0,
+                                cv::Size(calibration_.depth_camera_calibration.resolution_width, calibration_.depth_camera_calibration.resolution_height));
+                }
+                else
+                {
+                        model_ = CameraModel(
+                                calibration_.color_camera_calibration.intrinsics.parameters.param.fx,
+                                calibration_.color_camera_calibration.intrinsics.parameters.param.fy,
+                                calibration_.color_camera_calibration.intrinsics.parameters.param.cx,
+                                calibration_.color_camera_calibration.intrinsics.parameters.param.cy,
+                                this->getLocalTransform(),
+                                0,
+                                cv::Size(calibration_.color_camera_calibration.resolution_width, calibration_.color_camera_calibration.resolution_height));
+                }
 
 		transformation_ = k4a_transformation_create(&calibration_);
 
-		// Get imu transform
-		k4a_calibration_extrinsics_t* imu_extrinsics;
-		if(ir_)
-		{
-			imu_extrinsics = &calibration_.extrinsics[K4A_CALIBRATION_TYPE_ACCEL][K4A_CALIBRATION_TYPE_DEPTH];
-		}
-		else
-		{
-			imu_extrinsics = &calibration_.extrinsics[K4A_CALIBRATION_TYPE_ACCEL][K4A_CALIBRATION_TYPE_COLOR];
-		}
-		imuLocalTransform_ = Transform(
-			imu_extrinsics->rotation[0], imu_extrinsics->rotation[1], imu_extrinsics->rotation[2], imu_extrinsics->translation[0] / 1000.0f,
-			imu_extrinsics->rotation[3], imu_extrinsics->rotation[4], imu_extrinsics->rotation[5], imu_extrinsics->translation[1] / 1000.0f,
-			imu_extrinsics->rotation[6], imu_extrinsics->rotation[7], imu_extrinsics->rotation[8], imu_extrinsics->translation[2] / 1000.0f);
-
-		UINFO("camera to imu=%s", imuLocalTransform_.prettyPrint().c_str());
-		UINFO("base to camera=%s", this->getLocalTransform().prettyPrint().c_str());
-		imuLocalTransform_ = this->getLocalTransform()*imuLocalTransform_;
-		UINFO("base to imu=%s", imuLocalTransform_.prettyPrint().c_str());
-
 		if (K4A_FAILED(k4a_device_start_imu(device_)))
 		{
 			UERROR("Failed to start K4A IMU");
@@ -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_;
-	}
-	return(serial_number_);
+	if(device_ != NULL)
+		return(serial_number_);
+	else
+		return fileName_.empty()?"":fileName_;
 #else
 	return "";
 #endif
@@ -560,7 +482,7 @@ SensorData CameraK4A::captureImage(CameraInfo * info)
 							if (sleepTime > 10000)
 							{
 								UWARN("Detected long delay (%d sec, stamps = %f vs %f). Waiting a maximum of 10 seconds.",
-										sleepTime / 1000, previousStamp_, stamp);
+									sleepTime / 1000, previousStamp_, stamp);
 								sleepTime = 10000;
 							}
 							if (sleepTime > 2)
@@ -598,115 +520,118 @@ 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_image_ != NULL)
+			if (ir_)
 			{
-				// Convert IR image
-				cv::Mat bgrCV16(k4a_image_get_height_pixels(ir_image_),
-						k4a_image_get_width_pixels(ir_image_),
-						CV_16UC1,
-						(void*)k4a_image_get_buffer(ir_image_));
+				// Retrieve IR image from capture
+				ir_image_ = k4a_capture_get_ir_image(capture_);
 
-				bgrCV16.convertTo(bgrCV, CV_8U);
+				if(ir_image_ != NULL)
+				{
+					// Convert IR image
+					cv::Mat bgrCV16(k4a_image_get_height_pixels(ir_image_),
+							k4a_image_get_width_pixels(ir_image_),
+							CV_16UC1,
+							(void*)k4a_image_get_buffer(ir_image_));
 
-				// Release the image
-				k4a_image_release(ir_image_);
+					bgrCV16.convertTo(bgrCV, CV_8U);
+
+					// Release the image
+					k4a_image_release(ir_image_);
+				}
 			}
 			else
 			{
-				// Convert RGB image
-				if (k4a_image_get_format(rgb_image_) == K4A_IMAGE_FORMAT_COLOR_MJPG)
+				// Retrieve RGB image from capture
+				rgb_image_ = k4a_capture_get_color_image(capture_);
+
+				if(rgb_image_ != NULL)
 				{
-					bgrCV = uncompressImage(cv::Mat(1, (int)k4a_image_get_size(rgb_image_),
-							CV_8UC1,
-							(void*)k4a_image_get_buffer(rgb_image_)));
-				}
-				else
-				{
-					cv::Mat bgra(k4a_image_get_height_pixels(rgb_image_),
-							k4a_image_get_width_pixels(rgb_image_),
-							CV_8UC4,
-							(void*)k4a_image_get_buffer(rgb_image_));
-
-					cv::cvtColor(bgra, bgrCV, CV_BGRA2BGR);
-				}
-
-				// Release the image
-				k4a_image_release(rgb_image_);
-			}
-
-			if(!bgrCV.empty())
-			{
-				// Retrieve depth image from capture
-				k4a_image_t depth_image_ = k4a_capture_get_depth_image(capture_);
-
-				if (depth_image_ != NULL)
-				{
-					if (ir_)
+					// Convert RGB image
+					if (k4a_image_get_format(rgb_image_) == K4A_IMAGE_FORMAT_COLOR_MJPG)
 					{
-						depthCV = cv::Mat(k4a_image_get_height_pixels(depth_image_),
-								k4a_image_get_width_pixels(depth_image_),
-								CV_16UC1,
-								(void*)k4a_image_get_buffer(depth_image_)).clone();
+						bgrCV = uncompressImage(cv::Mat(1, (int)k4a_image_get_size(rgb_image_),
+									CV_8UC1,
+									(void*)k4a_image_get_buffer(rgb_image_)));
 					}
 					else
 					{
-						k4a_image_t transformedDepth = NULL;
-						if (k4a_image_create(k4a_image_get_format(depth_image_),
-								bgrCV.cols, bgrCV.rows, bgrCV.cols * 2, &transformedDepth) == K4A_RESULT_SUCCEEDED)
-						{
-							if(k4a_transformation_depth_image_to_color_camera(transformation_, depth_image_, transformedDepth) == K4A_RESULT_SUCCEEDED)
-							{
-								depthCV = cv::Mat(k4a_image_get_height_pixels(transformedDepth),
-										k4a_image_get_width_pixels(transformedDepth),
-										CV_16UC1,
-										(void*)k4a_image_get_buffer(transformedDepth)).clone();
-							}
-							else
-							{
-								UERROR("K4A failed to register depth image");
-							}
+						cv::Mat bgra(k4a_image_get_height_pixels(rgb_image_),
+							     k4a_image_get_width_pixels(rgb_image_),
+							     CV_8UC4,
+							     (void*)k4a_image_get_buffer(rgb_image_));
 
-							k4a_image_release(transformedDepth);
-						}
-						else
-						{
-							UERROR("K4A failed to allocate registered depth image");
-						}
+						cv::cvtColor(bgra, bgrCV, CV_BGRA2BGR);
 					}
-					k4a_image_release(depth_image_);
+
+					// Release the image
+					k4a_image_release(rgb_image_);
 				}
 			}
 
+			// Retrieve depth image from capture
+			depth_image_ = k4a_capture_get_depth_image(capture_);
+
+			if (depth_image_ != NULL)
+			{
+				if (ir_)
+				{
+					depthCV = cv::Mat(k4a_image_get_height_pixels(depth_image_),
+							  k4a_image_get_width_pixels(depth_image_),
+							  CV_16UC1,
+							  (void*)k4a_image_get_buffer(depth_image_)).clone();
+				}
+				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)
+					{
+						if(k4a_transformation_depth_image_to_color_camera(transformation_, depth_image_, transformedDepth) == K4A_RESULT_SUCCEEDED)
+						{
+							depthCV = cv::Mat(k4a_image_get_height_pixels(transformedDepth),
+									  k4a_image_get_width_pixels(transformedDepth),
+									  CV_16UC1,
+									  (void*)k4a_image_get_buffer(transformedDepth)).clone();
+						}
+						else
+						{
+							UERROR("K4A failed to register depth image");
+						}
+
+						k4a_image_release(transformedDepth);
+					}
+					else
+					{
+						UERROR("K4A failed to allocate registered depth image");
+					}
+				}
+				k4a_image_release(depth_image_);
+		 	}
 			k4a_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),
-						cv::Mat::eye(3, 3, CV_64FC1),
-						cv::Vec3d(imu_sample_.acc_sample.xyz.x, imu_sample_.acc_sample.xyz.y, imu_sample_.acc_sample.xyz.z),
-						cv::Mat::eye(3, 3, CV_64FC1),
-						imuLocalTransform_);
+				imu = IMU(cv::Vec3d(-1 * imu_sample_.gyro_sample.xyz.x, imu_sample_.gyro_sample.xyz.y, -1 * imu_sample_.gyro_sample.xyz.z),
+					  cv::Mat::eye(3, 3, CV_64FC1),
+					  cv::Vec3d(-1 * imu_sample_.acc_sample.xyz.x, imu_sample_.acc_sample.xyz.y, -1 * imu_sample_.acc_sample.xyz.z),
+					  cv::Mat::eye(3, 3, CV_64FC1),
+					  Transform::getIdentity());
+
+				UINFO("IMU: %f %f %f %f %f %f", imu_sample_.gyro_sample.xyz.x, imu_sample_.gyro_sample.xyz.y, imu_sample_.gyro_sample.xyz.z,
+								imu_sample_.acc_sample.xyz.x, imu_sample_.acc_sample.xyz.y, imu_sample_.acc_sample.xyz.z);
 			}
 			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,7 +31,7 @@ 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 arcore/include/* $prefix/arm64-v8a/include/.

docker/bionic/android/rtabmap_api24/rtabmap.bash

@@ -31,7 +31,7 @@ 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 arcore/include/* $prefix/arm64-v8a/include/.

docker/bionic/android/rtabmap_api26/rtabmap.bash

@@ -31,7 +31,7 @@ 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 arcore/include/* $prefix/arm64-v8a/include/.

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)
+		if(samePose ||
+		   _visualizer->updatePointCloudPose(id, posef))
 		{
-			// PointCloud / Mesh
-			bool updated = _visualizer->updatePointCloudPose(id, posef);
-
-#if VTK_MAJOR_VERSION >= 7
-			if(!updated)
+			_addedClouds.find(id).value() = pose;
+			if(!samePose)
 			{
-				// TextureMesh, cannot use updateShapePose because it searches for vtkLODActor, not a vtkActor
-				pcl::visualization::ShapeActorMap::iterator am_it = _visualizer->getShapeActorMap()->find (id);
-				vtkActor* actor;
-				if (am_it != _visualizer->getShapeActorMap()->end ())
-				{
-					actor = vtkActor::SafeDownCast (am_it->second);
-					if (actor)
-					{
-						vtkSmartPointer<vtkMatrix4x4> matrix = vtkSmartPointer<vtkMatrix4x4>::New ();
-						pcl::visualization::PCLVisualizer::convertToVtkMatrix (pose.toEigen3f().matrix (), matrix);
-						actor->SetUserMatrix (matrix);
-						actor->Modified ();
-						updated = true;
-					}
-				}
-			}
-#endif
-
-			if(updated)
-			{
-				_addedClouds.find(id).value() = pose;
 				std::string idNormals = id+"-normals";
 				if(_addedClouds.find(idNormals)!=_addedClouds.end())
 				{
 					_visualizer->updatePointCloudPose(idNormals, posef);
 					_addedClouds.find(idNormals).value() = pose;
 				}
-				return true;
 			}
+			return true;
 		}
 	}
 	return false;
@@ -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
+
+  // 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,39 +2837,20 @@ 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)
 	{
-		if(lastOpacity != opacity)
-		{
-			_visualizer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, id);
-		}
+		_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)
 	{
-		if((int)lastSize != size)
-		{
-			_visualizer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, (double)size, id);
-		}
+		_visualizer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, (double)size, id);
 	}
 }
 
@@ -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,79 +118,76 @@ 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);
+
+			UDEBUG("Control move! Picked value: %f %f %f", picked[0], picked[1], picked[2]);
+
+			float textSize = 0.05;
+
+			viewer_->removeCloud("interactor_points_alt");
+			pointsHolder_->resize(2);
+			pcl::PointXYZRGB pt;
+			pt.r = 255;
+			pt.x = picked[0];
+			pt.y = picked[1];
+			pt.z = picked[2];
+			pointsHolder_->at(0) = pt;
+
+			viewer_->removeLine("interactor_ray_alt");
+			viewer_->removeText("interactor_ray_text_alt");
+
+			// Intersect the locator with the line
+			double length = 5.0;
+			double pickedNormal[3];
+			cellPicker->GetPickNormal(pickedNormal);
+			double lineP0[3] = {picked[0], picked[1], picked[2]};
+			double lineP1[3] = {picked[0]+pickedNormal[0]*length, picked[1]+pickedNormal[1]*length, picked[2]+pickedNormal[2]*length};
+			vtkSmartPointer<vtkPoints> intersectPoints = vtkSmartPointer<vtkPoints>::New();
+
+			viewer_->getLocators().begin()->second->IntersectWithLine(lineP0, lineP1, intersectPoints, NULL);
+
+			// Display list of intersections
+			double intersection[3];
+			double previous[3] = {picked[0], picked[1], picked[2]};
+			for(int i = 0; i < intersectPoints->GetNumberOfPoints(); i++ )
 			{
-				double picked[3];
-				this->Interactor->GetPicker()->GetPickPosition(picked);
+				intersectPoints->GetPoint(i, intersection);
 
-				UDEBUG("Control move! Picked value: %f %f %f", picked[0], picked[1], picked[2]);
-
-				float textSize = 0.05;
-
-				viewer_->removeCloud("interactor_points_alt");
-				pointsHolder_->resize(2);
-				pcl::PointXYZRGB pt;
-				pt.r = 255;
-				pt.x = picked[0];
-				pt.y = picked[1];
-				pt.z = picked[2];
-				pointsHolder_->at(0) = pt;
-
-				viewer_->removeLine("interactor_ray_alt");
-				viewer_->removeText("interactor_ray_text_alt");
-
-				// Intersect the locator with the line
-				double length = 5.0;
-				double pickedNormal[3];
-				cellPicker->GetPickNormal(pickedNormal);
-				double lineP0[3] = {picked[0], picked[1], picked[2]};
-				double lineP1[3] = {picked[0]+pickedNormal[0]*length, picked[1]+pickedNormal[1]*length, picked[2]+pickedNormal[2]*length};
-				vtkSmartPointer<vtkPoints> intersectPoints = vtkSmartPointer<vtkPoints>::New();
-
-				viewer_->getLocators().begin()->second->IntersectWithLine(lineP0, lineP1, intersectPoints, NULL);
-
-				// Display list of intersections
-				double intersection[3];
-				double previous[3] = {picked[0], picked[1], picked[2]};
-				for(int i = 0; i < intersectPoints->GetNumberOfPoints(); i++ )
+				Eigen::Vector3f v(intersection[0]-previous[0], intersection[1]-previous[1], intersection[2]-previous[2]);
+				float n = v.norm();
+				if(n  > 0.01f)
 				{
-					intersectPoints->GetPoint(i, intersection);
+					v/=n;
+					v *= n/2.0f;
+					pt.r = 125;
+					pt.g = 125;
+					pt.b = 125;
+					pt.x = intersection[0];
+					pt.y = intersection[1];
+					pt.z = intersection[2];
+					pointsHolder_->at(1) = pt;
+					viewer_->addOrUpdateText("interactor_ray_text_alt", uFormat("%.2f m", n),
+							Transform(previous[0]+v[0], previous[1]+v[1],previous[2]+v[2], 0, 0, 0),
+							textSize,
+							Qt::gray);
+					viewer_->addOrUpdateLine("interactor_ray_alt",
+							Transform(previous[0], previous[1], previous[2], 0, 0, 0),
+							Transform(intersection[0], intersection[1], intersection[2], 0, 0, 0),
+							Qt::gray);
 
-					Eigen::Vector3f v(intersection[0]-previous[0], intersection[1]-previous[1], intersection[2]-previous[2]);
-					float n = v.norm();
-					if(n  > 0.01f)
-					{
-						v/=n;
-						v *= n/2.0f;
-						pt.r = 125;
-						pt.g = 125;
-						pt.b = 125;
-						pt.x = intersection[0];
-						pt.y = intersection[1];
-						pt.z = intersection[2];
-						pointsHolder_->at(1) = pt;
-						viewer_->addOrUpdateText("interactor_ray_text_alt", uFormat("%.2f m", n),
-								Transform(previous[0]+v[0], previous[1]+v[1],previous[2]+v[2], 0, 0, 0),
-								textSize,
-								Qt::gray);
-						viewer_->addOrUpdateLine("interactor_ray_alt",
-								Transform(previous[0], previous[1], previous[2], 0, 0, 0),
-								Transform(intersection[0], intersection[1], intersection[2], 0, 0, 0),
-								Qt::gray);
-
-						previous[0] = intersection[0];
-						previous[1] = intersection[1];
-						previous[2] = intersection[2];
-						break;
-					}
+					previous[0] = intersection[0];
+					previous[1] = intersection[1];
+					previous[2] = intersection[2];
+					break;
 				}
-				viewer_->addCloud("interactor_points_alt", pointsHolder_);
-				viewer_->setCloudPointSize("interactor_points_alt", 15);
-				viewer_->setCloudOpacity("interactor_points_alt", 0.5);
 			}
+			viewer_->addCloud("interactor_points_alt", pointsHolder_);
+			viewer_->setCloudPointSize("interactor_points_alt", 15);
+			viewer_->setCloudOpacity("interactor_points_alt", 0.5);
 		}
 	}
 	// Forward events
@@ -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)
 			{
-				double picked[3];
-				this->Interactor->GetPicker()->GetPickPosition(picked);
-				UDEBUG("Double clicked! Picked value: %f %f %f", picked[0], picked[1], picked[2]);
 				vtkCamera *camera = this->CurrentRenderer->GetActiveCamera();
 				UASSERT(camera);
 				double position[3];
@@ -268,64 +265,61 @@ 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);
+
+			UDEBUG("Shift clicked! Picked value: %f %f %f", picked[0], picked[1], picked[2]);
+
+			float textSize = 0.05;
+
+			viewer_->removeCloud("interactor_points");
+			pointsHolder_->clear();
+			pcl::PointXYZRGB pt;
+			pt.r = 255;
+			pt.x = picked[0];
+			pt.y = picked[1];
+			pt.z = picked[2];
+			pointsHolder_->push_back(pt);
+
+			viewer_->removeLine("interactor_ray");
+			viewer_->removeText("interactor_ray_text");
+
+			if(	PreviousMeasure[0] != 0.0f && PreviousMeasure[1] != 0.0f && PreviousMeasure[2] != 0.0f &&
+				viewer_->getAddedLines().find("interactor_line") == viewer_->getAddedLines().end())
 			{
-				double picked[3];
-				this->Interactor->GetPicker()->GetPickPosition(picked);
-
-				UDEBUG("Shift clicked! Picked value: %f %f %f", picked[0], picked[1], picked[2]);
-
-				float textSize = 0.05;
-
-				viewer_->removeCloud("interactor_points");
-				pointsHolder_->clear();
-				pcl::PointXYZRGB pt;
-				pt.r = 255;
-				pt.x = picked[0];
-				pt.y = picked[1];
-				pt.z = picked[2];
+				viewer_->addOrUpdateLine("interactor_line",
+						Transform(PreviousMeasure[0], PreviousMeasure[1], PreviousMeasure[2], 0, 0, 0),
+						Transform(picked[0], picked[1], picked[2], 0, 0, 0),
+						Qt::red);
+				pt.x = PreviousMeasure[0];
+				pt.y = PreviousMeasure[1];
+				pt.z = PreviousMeasure[2];
 				pointsHolder_->push_back(pt);
 
-				viewer_->removeLine("interactor_ray");
-				viewer_->removeText("interactor_ray_text");
-
-				if(	PreviousMeasure[0] != 0.0f && PreviousMeasure[1] != 0.0f && PreviousMeasure[2] != 0.0f &&
-					viewer_->getAddedLines().find("interactor_line") == viewer_->getAddedLines().end())
-				{
-					viewer_->addOrUpdateLine("interactor_line",
-							Transform(PreviousMeasure[0], PreviousMeasure[1], PreviousMeasure[2], 0, 0, 0),
-							Transform(picked[0], picked[1], picked[2], 0, 0, 0),
-							Qt::red);
-					pt.x = PreviousMeasure[0];
-					pt.y = PreviousMeasure[1];
-					pt.z = PreviousMeasure[2];
-					pointsHolder_->push_back(pt);
-
-					Eigen::Vector3f v(picked[0]-PreviousMeasure[0], picked[1]-PreviousMeasure[1], picked[2]-PreviousMeasure[2]);
-					float n = v.norm();
-					v/=n;
-					v *= n/2.0f;
-					viewer_->addOrUpdateText("interactor_pose", uFormat("%.2f m", n),
-							Transform(PreviousMeasure[0]+v[0], PreviousMeasure[1]+v[1],PreviousMeasure[2]+v[2], 0, 0, 0),
-							textSize,
-							Qt::red);
-				}
-				else
-				{
-					viewer_->removeText("interactor_pose");
-					viewer_->removeLine("interactor_line");
-				}
-				PreviousMeasure[0] = picked[0];
-				PreviousMeasure[1] = picked[1];
-				PreviousMeasure[2] = picked[2];
-
-				viewer_->addCloud("interactor_points", pointsHolder_);
-				viewer_->setCloudPointSize("interactor_points", 15);
-				viewer_->setCloudOpacity("interactor_points", 0.5);
+				Eigen::Vector3f v(picked[0]-PreviousMeasure[0], picked[1]-PreviousMeasure[1], picked[2]-PreviousMeasure[2]);
+				float n = v.norm();
+				v/=n;
+				v *= n/2.0f;
+				viewer_->addOrUpdateText("interactor_pose", uFormat("%.2f m", n),
+						Transform(PreviousMeasure[0]+v[0], PreviousMeasure[1]+v[1],PreviousMeasure[2]+v[2], 0, 0, 0),
+						textSize,
+						Qt::red);
 			}
+			else
+			{
+				viewer_->removeText("interactor_pose");
+				viewer_->removeLine("interactor_line");
+			}
+			PreviousMeasure[0] = picked[0];
+			PreviousMeasure[1] = picked[1];
+			PreviousMeasure[2] = picked[2];
+
+			viewer_->addCloud("interactor_points", pointsHolder_);
+			viewer_->setCloudPointSize("interactor_points", 15);
+			viewer_->setCloudOpacity("interactor_points", 0.5);
 		}
 	}
 

guilib/src/DatabaseViewer.cpp

@@ -2090,7 +2090,7 @@ void DatabaseViewer::updateStatistics()
 		double firstStamp = 0.0;
 		std::map<int, std::pair<std::map<std::string, float>, double> > allStats = dbDriver_->getAllStatistics();
 
-		std::map<std::string, std::pair<std::vector<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());

tools/OdometryViewer/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/OdometryViewer/main.cpp

@@ -252,13 +252,15 @@ int main (int argc, char * argv[])
 	odomViewer.resize(1280, 480+QPushButton().minimumHeight());
 
 	rtabmap::Camera * camera = 0;
+	rtabmap::Transform t=rtabmap::Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0);
+
 	if(inputDatabase.size())
 	{
 		camera =  new rtabmap::DBReader(inputDatabase, rate, true);
 	}
 	else if(driver == 0)
 	{
-		camera = new rtabmap::CameraOpenni("", rate);
+		camera = new rtabmap::CameraOpenni("", rate, t);
 	}
 	else if(driver == 1)
 	{
@@ -267,7 +269,7 @@ int main (int argc, char * argv[])
 			UERROR("Not built with OpenNI2 support...");
 			exit(-1);
 		}
-		camera = new rtabmap::CameraOpenNI2("", rtabmap::CameraOpenNI2::kTypeColorDepth, rate);
+		camera = new rtabmap::CameraOpenNI2("", rtabmap::CameraOpenNI2::kTypeColorDepth, rate, t);
 	}
 	else if(driver == 2)
 	{
@@ -276,7 +278,7 @@ int main (int argc, char * argv[])
 			UERROR("Not built with Freenect support...");
 			exit(-1);
 		}
-		camera = new rtabmap::CameraFreenect(0, rtabmap::CameraFreenect::kTypeColorDepth, rate);
+		camera = new rtabmap::CameraFreenect(0, rtabmap::CameraFreenect::kTypeColorDepth, rate, t);
 	}
 	else if(driver == 3)
 	{
@@ -285,7 +287,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)
 	{
@@ -294,7 +296,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)
 	{
@@ -303,7 +305,7 @@ int main (int argc, char * argv[])
 			UERROR("Not built with Freenect2 support...");
 			exit(-1);
 		}
-		camera = new rtabmap::CameraFreenect2(0, rtabmap::CameraFreenect2::kTypeColor2DepthSD, rate);
+		camera = new rtabmap::CameraFreenect2(0, rtabmap::CameraFreenect2::kTypeColor2DepthSD, rate, t);
 	}
 	else if(driver == 6)
 	{
@@ -312,7 +314,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)
 	{
@@ -321,7 +323,7 @@ int main (int argc, char * argv[])
 			UERROR("Not built with FlyCapture2/Triclops support...");
 			exit(-1);
 		}
-		camera = new rtabmap::CameraStereoFlyCapture2(rate);
+		camera = new rtabmap::CameraStereoFlyCapture2(rate, t);
 	}
 	else if(driver == 8)
 	{
@@ -330,7 +332,7 @@ int main (int argc, char * argv[])
 			UERROR("Not built with ZED sdk support...");
 			exit(-1);
 		}
-		camera = new rtabmap::CameraStereoZed(0,3,1,0,100,false,rate);
+		camera = new rtabmap::CameraStereoZed(0,3,1,0,100,false,rate,t);
 	}
 	else if (driver == 9)
 	{
@@ -339,7 +341,7 @@ int main (int argc, char * argv[])
 			UERROR("Not built with RealSense support...");
 			exit(-1);
 		}
-		camera = new rtabmap::CameraRealSense(0, 0, 0, false, rate);
+		camera = new rtabmap::CameraRealSense(0, 0, 0, false, rate, t);
 	}
 	else if (driver == 10)
 	{
@@ -348,7 +350,7 @@ int main (int argc, char * argv[])
 			UERROR("Not built with Kinect for Windows 2 SDK support...");
 			exit(-1);
 		}
-		camera = new rtabmap::CameraK4W2(0, rtabmap::CameraK4W2::kTypeDepth2ColorSD, rate);
+		camera = new rtabmap::CameraK4W2(0, rtabmap::CameraK4W2::kTypeDepth2ColorSD, rate, t);
 	}
 	else if (driver == 11)
 	{
@@ -357,7 +359,7 @@ int main (int argc, char * argv[])
 			UERROR("Not built with RealSense2 SDK support...");
 			exit(-1);
 		}
-		camera = new rtabmap::CameraRealSense2("", rate);
+		camera = new rtabmap::CameraRealSense2("", rate, t);
 	}
 	else if (driver == 12)
 	{
@@ -375,7 +377,7 @@ int main (int argc, char * argv[])
 			UERROR("Not built with Mynt Eye S support...");
 			exit(-1);
 		}
-		camera = new rtabmap::CameraMyntEye("", false, false, rate);
+		camera = new rtabmap::CameraMyntEye("", false, false, rate, t);
 	}
 	else
 	{

tools/Report/main.cpp

@@ -47,8 +47,10 @@ using namespace rtabmap;
 void showUsage()
 {
 	printf("\nUsage:\n"
-			"rtabmap-report [\"Statistic/Id\"] [options] path\n"
 #ifdef WITH_QT
+			"rtabmap-report [\"Statistic/Id\"] [options] path\n"
+#else
+			"rtabmap-report [options] path\n"
 			"[Not built with Qt, statistics cannot be plotted]\n"
 #endif
 			"  path               Directory containing rtabmap databases or path of a database.\n"
@@ -61,8 +63,8 @@ void showUsage()
 			"                         and compute error based on the scaled path.\n"
 			"    --poses            Export poses to [path]_poses.txt, ground truth to [path]_gt.txt\n"
 			"                         and valid ground truth indices to [path]_indices.txt \n"
-			"    --stats            Show available statistics \"Statistic/Id\" to plot or get localization statistics (if path is a file). \n"
 #ifdef WITH_QT
+			"    --stats            Show available statistics to plot (if path is a file). \n"
 			"    --invert           When reading many databases, put all curves from a same \n"
 			"                       database in same figure, instead of all same curves from \n"
 			"                       different database in same figure. When reading a single \n"
@@ -70,45 +72,11 @@ void showUsage()
 			"    --ids              Use IDs for x axis instead of time in the figures. \n"
 			"    --start #          Start from this node ID for the figures.\n"
 #endif
-			"    --report           Export all evaluation statistics values in report.txt \n"
-			"    --loc #            Show localization statistics for each \"Statistic/Id\" per "
-			"                       session for 1=min,2=max,4=mean,8=stddev,16=total,32=nonnull%% "
-			"                       (it is a mask, we can combine those numbers, e.g., 63 for all) \n"
+			"    --report           Export all statistics values in report.txt \n"
 			"    --help             Show usage\n\n");
 	exit(1);
 }
 
-struct LocStats
-{
-	static LocStats from(const std::vector<float> & array)
-	{
-		LocStats values;
-		values.mean = uMean(array);
-		values.stddev = std::sqrt(uVariance(array, values.mean));
-		uMinMax(array, values.min, values.max);
-		values.total = array.size();
-		values.nonNull = 0.0f;
-		if(!array.empty())
-		{
-			for(size_t j=0; j<array.size(); ++j)
-			{
-				if(array[j] != 0)
-				{
-					values.nonNull+=1.0f;
-				}
-			}
-			values.nonNull = values.nonNull/float(array.size());
-		}
-		return values;
-	}
-	float mean;
-	float stddev;
-	float min;
-	float max;
-	int total;
-	float nonNull;
-};
-
 int main(int argc, char * argv[])
 {
 	if(argc < 2)
@@ -133,7 +101,6 @@ int main(int argc, char * argv[])
 	bool invertFigures = false;
 	bool useIds = false;
 	int startId = 0;
-	int showLoc = 0;
 	std::vector<std::string> statsToShow;
 #ifdef WITH_QT
 	std::map<std::string, UPlot*> figures;
@@ -184,20 +151,7 @@ int main(int argc, char * argv[])
 		{
 			useIds = true;
 		}
-		else if(strcmp(argv[i],"--loc") == 0)
-		{
-			++i;
-			if(i<argc-1)
-			{
-				showLoc = atoi(argv[i]);
-				printf("Localization statistics=%d (--loc)\n", showLoc);
-			}
-			else
-			{
-				printf("Missing type for \"--showLoc\" option.\n");
-				showUsage();
-			}
-		}
+#ifdef WITH_QT
 		else if(strcmp(argv[i],"--start") == 0)
 		{
 			++i;
@@ -217,22 +171,22 @@ int main(int argc, char * argv[])
 
 			statsToShow.push_back(argv[i]);
 		}
+#endif
 	}
 
 	std::string path = argv[argc-1];
 	path = uReplaceChar(path, '~', UDirectory::homeDir());
 
+#ifdef WITH_QT
 	if(!UDirectory::exists(path) && UFile::getExtension(path).compare("db") == 0)
 	{
 		invertFigures = !invertFigures;
 	}
-	std::map<std::string, std::map<std::string, std::vector<LocStats> > > localizationMultiStats; //<statsName, <Database<Session>> >
-	for(size_t i=0; i<statsToShow.size(); ++i)
+	if(!invertFigures)
 	{
-		std::string figureTitle = statsToShow[i];
-		if(!invertFigures)
+		for(size_t i=0; i<statsToShow.size(); ++i)
 		{
-#ifdef WITH_QT
+			std::string figureTitle = statsToShow[i];
 			printf("Plot %s\n", figureTitle.c_str());
 			UPlot * fig = new UPlot();
 			fig->resize(QSize(640,480));
@@ -246,17 +200,10 @@ int main(int argc, char * argv[])
 				fig->setXLabel("Time (s)");
 			}
 			figures.insert(std::make_pair(figureTitle, fig));
-#endif
 		}
-		if(showLoc & 0b111111)
-		{
-			localizationMultiStats.insert(std::make_pair(figureTitle, std::map<std::string, std::vector<LocStats> >()));
-		}
-	}
-	if(!invertFigures)
-	{
 		statsToShow.clear();
 	}
+#endif
 
 	std::string fileName;
 	std::list<std::string> paths;
@@ -276,7 +223,6 @@ int main(int argc, char * argv[])
 			if(UFile::getExtension(currentPath).compare("db") == 0)
 			{
 				currentPathIsDatabase=true;
-				localizationMultiStats.clear();
 				printf("Database: %s\n", currentPath.c_str());
 			}
 			else
@@ -309,7 +255,6 @@ int main(int argc, char * argv[])
 			}
 		}
 
-		// For all databases in currentDir
 		while(currentPathIsDatabase || !(fileName = currentDir.getNextFileName()).empty())
 		{
 			if(currentPathIsDatabase || UFile::getExtension(fileName).compare("db") == 0)
@@ -377,13 +322,12 @@ int main(int argc, char * argv[])
 					}
 
 					std::map<std::string, UPlotCurve*> curves;
+					std::map<std::string, double> firstStamps;
 					if(statsToShow.empty())
 					{
 						for(std::map<std::string, UPlot*>::iterator iter=figures.begin(); iter!=figures.end(); ++iter)
 						{
 							curves.insert(std::make_pair(iter->first, iter->second->addCurve(filePath.c_str())));
-							if(!localizationMultiStats.empty())
-								localizationMultiStats.at(iter->first).insert(std::make_pair(fileName, std::vector<LocStats>()));
 						}
 					}
 					else
@@ -408,31 +352,11 @@ int main(int argc, char * argv[])
 							for(size_t i=0; i<statsToShow.size(); ++i)
 							{
 								curves.insert(std::make_pair(statsToShow[i], fig->addCurve(statsToShow[i].c_str())));
-								if(!localizationMultiStats.empty())
-									localizationMultiStats.at(statsToShow[i]).insert(std::make_pair(fileName, std::vector<LocStats>()));
 							}
 						}
 					}
-#else
-					for(size_t i=0; i<statsToShow.size(); ++i)
-					{
-						if(!localizationMultiStats.empty())
-							localizationMultiStats.at(statsToShow[i]).insert(std::make_pair(fileName, std::vector<LocStats>()));
-					}
 #endif
 
-					// Find localization sessions and adjust startId
-					if(!localizationMultiStats.empty() && startId ==0)
-					{
-						std::map<int, Transform> poses = driver->loadOptimizedPoses();
-						if(!poses.empty())
-						{
-							startId = poses.rbegin()->first+1;
-						}
-					}
-
-					std::map<std::string, std::vector<float> > localizationSessionStats;
-					double previousStamp = 0.0;
 					for(std::set<int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
 					{
 						Transform p, gt;
@@ -509,63 +433,26 @@ int main(int argc, char * argv[])
 #ifdef WITH_QT
 								for(std::map<std::string, UPlotCurve*>::iterator jter=curves.begin(); jter!=curves.end(); ++jter)
 								{
-#else
-								for(std::map<std::string, std::map<std::string, std::vector<LocStats> > >::iterator jter=localizationMultiStats.begin();
-									jter!=localizationMultiStats.end();
-									++jter)
-								{
-#endif
-									if(*iter >= startId)
+									if(uContains(stat, jter->first) && *iter >= startId)
 									{
-										if(uContains(stat, jter->first))
+										if(!uContains(firstStamps, jter->first))
 										{
-											double y = stat.at(jter->first);
-#ifdef WITH_QT
-											double x = s;
-											if(useIds)
-											{
-												x = *iter;
-											}
-											jter->second->addValue(x,y);
-#endif
-
-											if(!localizationMultiStats.empty())
-											{
-												if(previousStamp > 0 && s - previousStamp > 10 && uContains(localizationSessionStats, jter->first))
-												{
-													// changed session
-													LocStats values = LocStats::from(localizationSessionStats.at(jter->first));
-													localizationMultiStats.at(jter->first).rbegin()->second.push_back(values);
-													localizationSessionStats.at(jter->first).clear();
-												}
-
-												if(!uContains(localizationSessionStats, jter->first))
-												{
-													localizationSessionStats.insert(std::make_pair(jter->first, std::vector<float>()));
-												}
-												localizationSessionStats.at(jter->first).push_back(y);
-											}
+											firstStamps.insert(std::make_pair(jter->first, s));
 										}
+										float x = s - firstStamps.at(jter->first);
+										if(useIds)
+										{
+											x = *iter;
+										}
+										float y = stat.at(jter->first);
+										jter->second->addValue(x,y);
 									}
 								}
-								previousStamp = s;
+#endif
 							}
 						}
 					}
 
-					for(std::map<std::string, std::map<std::string, std::vector<LocStats> > >::iterator jter=localizationMultiStats.begin();
-						jter!=localizationMultiStats.end();
-						++jter)
-					{
-						if(uContains(localizationSessionStats, jter->first) &&
-							!localizationSessionStats.at(jter->first).empty())
-						{
-							// changed session
-							LocStats values = LocStats::from(localizationSessionStats.at(jter->first));
-							localizationMultiStats.at(jter->first).rbegin()->second.push_back(values);
-						}
-					}
-
 					std::multimap<int, Link> links;
 					std::multimap<int, Link> allLinks;
 					driver->getAllLinks(allLinks, true, true);
@@ -941,51 +828,6 @@ int main(int argc, char * argv[])
 			currentPathIsDatabase = false;
 		}
 
-		for(std::map<std::string, std::map<std::string, std::vector<LocStats> > >::iterator iter=localizationMultiStats.begin();
-			iter!=localizationMultiStats.end();
-			++iter)
-		{
-			printf("%s\n", iter->first.c_str());
-			for(int k=0; k<6; ++k)
-			{
-				if(showLoc & (0x1 << k))
-				{
-					printf("  %s:\n",
-							k==0?"min":
-							k==1?"max":
-							k==2?"mean":
-							k==3?"stddev":
-							k==4?"total":
-							"nonnull%");
-					for(std::map<std::string, std::vector<LocStats> >::iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
-					{
-						printf("    %s ", jter->first.c_str());
-						for(size_t j=0; j<jter->second.size(); ++j)
-						{
-							if(k<4)
-							{
-								printf("%f ",
-										k==0?jter->second[j].min:
-										k==1?jter->second[j].max:
-										k==2?jter->second[j].mean:
-										jter->second[j].stddev);
-							}
-							else if(k==4)
-							{
-								printf("%d ",jter->second[j].total);
-							}
-							else if(k==5)
-							{
-								printf("%.2f ", (jter->second[j].nonNull*100));
-							}
-						}
-						printf("\n");
-					}
-				}
-			}
-			iter->second.clear();
-		}
-
 		for(std::list<std::string>::iterator iter=subDirs.begin(); iter!=subDirs.end(); ++iter)
 		{
 			paths.push_front(*iter);
@@ -1087,10 +929,6 @@ int main(int argc, char * argv[])
 	{
 		for(std::map<std::string, UPlot*>::iterator iter=figures.begin(); iter!=figures.end(); ++iter)
 		{
-			if(!useIds)
-			{
-				iter->second->frameData();
-			}
 			iter->second->show();
 		}
 		return app.exec();

tools/Reprocess/main.cpp

@@ -399,7 +399,10 @@ int main(int argc, char * argv[])
 					showLocalizationStats();
 					lastLocalizationOdomPose = info.odomPose;
 				}
-				rtabmap.triggerNewMap();
+				if(incrementalMemory)
+				{
+					rtabmap.triggerNewMap();
+				}
 			}
 			UTimer t;
 			if(!rtabmap.process(data, info.odomPose, info.odomCovariance, info.odomVelocity, globalMapStats))

tools/RgbdDataset/CMakeLists.txt

@@ -34,14 +34,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_dataset main.cpp)

utilite/src/UTimer.cpp

@@ -36,17 +36,10 @@ UTimer::~UTimer() {}
 #ifdef _WIN32
 double UTimer::now()
 {
-#if defined(_WIN32_WINNT) && (_WIN32_WINNT >= _WIN32_WINNT_WIN8)
-	FILETIME ft;
-	GetSystemTimePreciseAsFileTime(&ft);
-	__int64* val = (__int64*)&ft;
-	return static_cast<double>(*val) / 10000000.0 - 11644473600.0; // The Windows epoch is Jan 1 1601, the Unix epoch Jan 1 1970.
-#else
     LARGE_INTEGER count, freq;
     QueryPerformanceFrequency(&freq);
     QueryPerformanceCounter(&count);
     return double(count.QuadPart) / freq.QuadPart;
-#endif
 }
 
 void UTimer::start()