package.xml: 0.20.7

package.xml: removed libvtk-qt as there are conflicts on noetic with libpcl-dev (which is using vtk7)
RegIcp: don't assert if normals cannot be computed properly before doing complexity check, just reject the transform
2026-09-11 05:50:21 +08:00 · 2020-12-12 19:05:06 -05:00 · 2020-12-12 17:32:30 -05:00 · 2020-12-12 16:53:07 -05:00 · 2020-12-11 17:22:29 -05:00 · 2020-12-11 17:05:37 -05:00
239 changed files with 15624 additions and 5688 deletions
@@ -35,7 +35,7 @@ install:
  # OpenCV
  #- ps: wget 'http://kent.dl.sourceforge.net/project/opencvlibrary/opencv-win/3.3.1/opencv-3.3.1-vc14.exe' -outfile opencv-3.3.1-vc14.exe
  #- cmd: opencv-3.3.1-vc14.exe -o"C:\Program Files" -y
-  - ps: wget 'http://kent.dl.sourceforge.net/project/opencvlibrary/opencv-win/2.4.13/opencv-2.4.13.6-vc14.exe' -outfile opencv-2.4.13.6-vc14.exe
+  - ps: $url = "https://downloads.sourceforge.net/project/opencvlibrary/opencv-win/2.4.13/opencv-2.4.13.6-vc14.exe?r=&ts="+([int64](([datetime]::UtcNow)-(get-date "1/1/1970")).TotalSeconds) ; wget $url -outfile opencv-2.4.13.6-vc14.exe
  - cmd: opencv-2.4.13.6-vc14.exe -o"C:\Program Files" -y
  - ECHO "Installed OpenCV:"
  - ps: "ls \"C:/Program Files/opencv/build\""
@@ -115,6 +115,20 @@ install:
  - cmd: yaml-cpp.exe -o"C:\Program Files" -y
  - ECHO "Installed yaml-cpp:"
  - ps: "ls \"C:/Program Files/yaml-cpp\""
+  # RealSense2
+  - ps: wget 'https://github.com/IntelRealSense/librealsense/releases/download/v2.40.0/Intel.RealSense.SDK-WIN10-2.40.0.2482.exe' -outfile realsense2.exe
+  - cmd: realsense2.exe /VERYSILENT
+  - ECHO "Installed RealSense2:"
+  - ps: "ls \"C:/Program Files (x86)/Intel RealSense SDK 2.0\""
+  - set PATH=%PATH%;C:\Program Files (x86)\Intel RealSense SDK 2.0\bin\x64
+  - set RealSense2_ROOT_DIR=C:\Program Files (x86)\Intel RealSense SDK 2.0
+  # Kinect 4 Azure
+  - ps: wget 'https://download.microsoft.com/download/3/d/6/3d6d9e99-a251-4cf3-8c6a-8e108e960b4b/Azure%20Kinect%20SDK%201.4.1.exe' -outfile azure.exe
+  - cmd: azure.exe /quiet
+  - ECHO "Installed Kinect For Azure:"
+  - ps: "ls \"C:/Program Files/Azure Kinect SDK v1.4.1\""
+  - set PATH=%PATH%;C:\Program Files\Azure Kinect SDK v1.4.1\tools
+  - set K4A_ROOT_DIR=C:\Program Files\Azure Kinect SDK v1.4.1
  
 before_build:
  - cd c:\projects\rtabmap\build
@@ -8,3 +8,5 @@ app/android/.classpath
 app/android/.project
 app/android/AndroidManifest.xml
 app/android/res/raw/
+compile_flags.txt
+tags
@@ -52,6 +52,21 @@ matrix:
        - mkdir -p build && cd build
        - cmake .. 
        - make
+        
+    - dist: focal
+      install:
+        - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu focal main" > /etc/apt/sources.list.d/ros-latest.list'
+        - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
+        - sudo apt-get update
+        - sudo apt-get update && sudo apt-get install dpkg
+        - sudo apt-get -y install ros-noetic-rtabmap-ros
+        - sudo apt-get -y remove ros-noetic-rtabmap
+
+      script: 
+        - source /opt/ros/noetic/setup.bash
+        - mkdir -p build && cd build
+        - cmake .. 
+        - make

 notifications:
  email:
@@ -20,7 +20,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 # VERSION
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
-SET(RTABMAP_MINOR_VERSION 19)
+SET(RTABMAP_MINOR_VERSION 20)
 SET(RTABMAP_PATCH_VERSION 7)
 SET(RTABMAP_VERSION
  ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
@@ -61,7 +61,7 @@ ELSE ()
 ENDIF()

 if(POLICY CMP0020)
-    cmake_policy(SET CMP0020 OLD)
+    cmake_policy(SET CMP0020 NEW)
 endif()
 if(POLICY CMP0043)
    cmake_policy(SET CMP0043 OLD)
@@ -163,7 +163,8 @@ ELSE()
 option(WITH_QT            "Include Qt support"                   ON)
 ENDIF()
 option(WITH_ORB_OCTREE    "Include ORB Octree feature support"   ON)
-option(WITH_SP_TORCH      "Include SuperPoint Torch feature support"   ON)
+option(WITH_SUPERPOINT_TORCH  "Include SuperPoint Torch feature support"   ON)
+option(WITH_PYMATCHER     "Include Python3 matchers support"     OFF)
 option(WITH_FREENECT      "Include Freenect support"             ON)
 option(WITH_FREENECT2     "Include Freenect2 support"            ON)
 option(WITH_K4W2          "Include Kinect for Windows v2 support" ON)
@@ -321,12 +322,19 @@ IF(WITH_QT)
    ENDIF(QT4_FOUND OR Qt5_FOUND)
 ENDIF(WITH_QT)

-IF(WITH_SP_TORCH)
+IF(WITH_SUPERPOINT_TORCH)
    FIND_PACKAGE(Torch QUIET)
    IF(TORCH_FOUND)
        MESSAGE(STATUS "Found Torch: ${TORCH_INCLUDE_DIRS}")
    ENDIF(TORCH_FOUND)
-ENDIF(WITH_SP_TORCH)
+ENDIF(WITH_SUPERPOINT_TORCH)
+
+IF(WITH_PYMATCHER)
+    FIND_PACKAGE(Python3 COMPONENTS Interpreter Development)
+    IF(Python3_FOUND)
+        MESSAGE(STATUS "Found Python3")
+    ENDIF(Python3_FOUND)
+ENDIF(WITH_PYMATCHER)

 IF(WITH_FREENECT)
    FIND_PACKAGE(Freenect QUIET)
@@ -419,6 +427,10 @@ ENDIF(WITH_CVSBA)
 IF(WITH_POINTMATCHER)
    find_package(libpointmatcher QUIET)
    IF(libpointmatcher_FOUND)
+       find_package(Boost COMPONENTS thread filesystem system program_options date_time REQUIRED)
+       if (Boost_MINOR_VERSION GREATER 47)
+           find_package(Boost COMPONENTS thread filesystem system program_options date_time chrono timer REQUIRED)
+       endif (Boost_MINOR_VERSION GREATER 47)
       MESSAGE(STATUS "Found libpointmatcher: ${libpointmatcher_INCLUDE_DIRS}")
    ENDIF(libpointmatcher_FOUND)
 ENDIF(WITH_POINTMATCHER)
@@ -463,7 +475,7 @@ ENDIF(WITH_REALSENSE)
 IF(WITH_REALSENSE2)
    IF(WIN32)
        FIND_PACKAGE(RealSense2 QUIET)
-	ELSE()
+    ELSE()
        FIND_PACKAGE(realsense2 QUIET)
    ENDIF()
    IF(realsense2_FOUND)
@@ -507,9 +519,9 @@ IF(WITH_ALICE_VISION)
    IF(AliceVision_FOUND)
        SET(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH};/usr/local/lib/cmake/modules")
        find_package(Geogram REQUIRED QUIET)
-        # Make sure the two following lines are also commented in AliceVision to avoid Eigen memory alignment error
-        #add_definitions("-DEIGEN_DONT_ALIGN_STATICALLY=1")
-        #add_definitions("-DEIGEN_DONT_VECTORIZE=1")
+        add_definitions("-DRTABMAP_ALICE_VISION_MAJOR=${AliceVision_VERSION_MAJOR}")
+        add_definitions("-DRTABMAP_ALICE_VISION_MINOR=${AliceVision_VERSION_MINOR}")
+        add_definitions("-DRTABMAP_ALICE_VISION_PATCH=${AliceVision_VERSION_PATCH}")
    ENDIF(AliceVision_FOUND)
 ENDIF(WITH_ALICE_VISION)

@@ -592,7 +604,7 @@ IF(WITH_ORB_SLAM2 AND NOT G2O_FOUND)
    ENDIF(ORB_SLAM2_FOUND)
 ENDIF(WITH_ORB_SLAM2 AND NOT G2O_FOUND)

-IF(loam_velodyne_FOUND OR PCL_VERSION VERSION_GREATER "1.9.1")
+IF(loam_velodyne_FOUND OR PCL_VERSION VERSION_GREATER "1.9.1" OR TORCH_FOUND)
   #LOAM and PCL>=1.10 require c++14
   IF(NOT MSVC)
     include(CheckCXXCompilerFlag)
@@ -614,7 +626,8 @@ ELSEIF(G2O_FOUND OR
       okvis_FOUND OR 
       open_chisel_FOUND OR 
       msckf_vio_FOUND OR 
-       vins_FOUND)
+       vins_FOUND OR 
+       libpointmatcher_FOUND)
   #Newest versions require std11
   IF(NOT MSVC)
     include(CheckCXXCompilerFlag)
@@ -664,6 +677,10 @@ ENDIF(APPLE AND BUILD_AS_BUNDLE)
 SET(CONF_DEPENDENCIES 
    ${ZLIB_LIBRARIES}
 )
+
+# OpenCV2 has nonfree if OPENCV_NONFREE_FOUND
+# OpenCV<=3.4.2 has nonfree if OPENCV_XFEATURES2D_FOUND
+# OpenCV>3.4.2 has nonfree if OPENCV_XFEATURES2D_FOUND and OPENCV_ENABLE_NONFREE is defined
 IF(NOT (OPENCV_NONFREE_FOUND OR OPENCV_XFEATURES2D_FOUND))
   SET(NONFREE "//")
 ELSEIF(OpenCV_VERSION VERSION_GREATER "3.4.2")
@@ -826,7 +843,10 @@ IF(NOT WITH_ORB_OCTREE)
   SET(ORB_OCTREE "//")
 ENDIF()
 IF(NOT TORCH_FOUND)
-   SET(SP_TORCH "//")
+   SET(SUPERPOINT_TORCH "//")
+ENDIF()
+IF(NOT Python3_FOUND)
+   SET(PYMATCHER "//")
 ENDIF()
 IF(ADD_VTK_GUI_SUPPORT_QT_TO_CONF)
   SET(CONF_VTK_QT true)
@@ -841,9 +861,6 @@ IF(NOT WITH_MADGWICK)
   SET(MADGWICK "//")
 ENDIF()

-IF(NOT (OpenCV_FOUND AND NOT (OpenCV_VERSION_MAJOR LESS 3)))
-   SET(OPENCV3 "//")
-ENDIF(NOT (OpenCV_FOUND AND NOT (OpenCV_VERSION_MAJOR LESS 3)))
 CONFIGURE_FILE(Version.h.in ${PROJECT_SOURCE_DIR}/corelib/include/${PROJECT_PREFIX}/core/Version.h)

 ADD_SUBDIRECTORY( utilite )
@@ -1046,9 +1063,13 @@ IF(OpenCV_FOUND)
    ENDIF()
  ELSE()
    IF(OPENCV_XFEATURES2D_FOUND)
-      MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d module (SIFT/SURF/BRIEF/FREAK) = YES (License: Non commercial)")
+      IF(NONFREE STREQUAL "//")
+        MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = YES, nonfree = NO (License: BSD)")
+      ELSE()
+        MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = YES, nonfree = YES (License: Non commercial)")
+      ENDIF()
    ELSE()
-      MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d module (SIFT/SURF/BRIEF/FREAK) = NO (not found, License: BSD)")
+      MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = NO, nonfree = NO (License: BSD)")
    ENDIF()
  ENDIF()
 ENDIF(OpenCV_FOUND)
@@ -1057,7 +1078,7 @@ IF(QT4_FOUND)
 MESSAGE(STATUS "  With Qt4                  = YES (License: Open Source or Commercial)")
 MESSAGE(STATUS "  With VTK ${VTK_MAJOR_VERSION}.${VTK_MINOR_VERSION}              = YES (License: BSD)")
 ELSEIF(Qt5_FOUND)
-MESSAGE(STATUS "  With Qt5                  = YES (License: Open Source or Commercial)")
+MESSAGE(STATUS "  With Qt ${Qt5_VERSION}            = YES (License: Open Source or Commercial)")
 MESSAGE(STATUS "  With VTK ${VTK_MAJOR_VERSION}.${VTK_MINOR_VERSION}              = YES (License: BSD)")

 ELSEIF(NOT WITH_QT)
@@ -1079,11 +1100,19 @@ MESSAGE(STATUS "  With ORB OcTree           = NO (WITH_ORB_OCTREE=OFF)")
 ENDIF()

 IF(TORCH_FOUND)
-MESSAGE(STATUS "  With SupertPoint Torch    = YES (License: GPLv3) libtorch=${Torch_VERSION}")
-ELSEIF(NOT WITH_SP_TORCH)
-MESSAGE(STATUS "  With SupertPoint Torch    = NO (WITH_SP_TORCH=OFF)")
+MESSAGE(STATUS "  With SupertPoint          = YES (License: GPLv3) libtorch=${Torch_VERSION}")
+ELSEIF(NOT WITH_SUPERPOINT_TORCH)
+MESSAGE(STATUS "  With SupertPoint          = NO (WITH_SUPERPOINT_TORCH=OFF)")
 ELSE()
-MESSAGE(STATUS "  With SupertPoint Torch    = NO (libtorch not found)")
+MESSAGE(STATUS "  With SupertPoint          = NO (libtorch not found)")
+ENDIF()
+
+IF(Python3_FOUND)
+MESSAGE(STATUS "  With Python3              = YES (License: PSF)")
+ELSEIF(NOT WITH_PYMATCHER)
+MESSAGE(STATUS "  With Python3              = NO (WITH_PYMATCHER=OFF)")
+ELSE()
+MESSAGE(STATUS "  With Python3              = NO (python3 not found)")
 ENDIF()

 IF(WITH_MADGWICK)
@@ -1185,7 +1214,7 @@ MESSAGE(STATUS "  With OpenChisel           = NO (open_chisel not found)")
 ENDIF()

 IF(AliceVision_FOUND)
-MESSAGE(STATUS "  With AliceVision          = YES (License: MPLv2)")
+MESSAGE(STATUS "  With AliceVision ${AliceVision_VERSION}    = YES (License: MPLv2)")
 ELSEIF(NOT WITH_ALICE_VISION)
 MESSAGE(STATUS "  With AliceVision          = NO (WITH_ALICE_VISION=OFF)")
 ELSE()
@@ -1,4 +1,4 @@
-rtabmap ![Analytics](https://ga-beacon.appspot.com/UA-56986679-3/github-main?pixel) 
+rtabmap ![Analytics](https://ga-beacon-279122.nn.r.appspot.com/UA-56986679-3/github-main?pixel) 
 =======

 [![RTAB-Map Logo](https://raw.githubusercontent.com/introlab/rtabmap/master/guilib/src/images/RTAB-Map100.png)](http://introlab.github.io/rtabmap)
@@ -7,7 +7,7 @@ rtabmap ![Analytics](https://ga-beacon.appspot.com/UA-56986679-3/github-main?pix
 [![License][license-image]][license]
 Linux: [![Build Status](https://travis-ci.org/introlab/rtabmap.svg?branch=master)](https://travis-ci.org/introlab/rtabmap) Windows: [![Build status](https://ci.appveyor.com/api/projects/status/hr73xspix9oqa26h/branch/master?svg=true)](https://ci.appveyor.com/project/matlabbe/rtabmap/branch/master)

-[release-image]: https://img.shields.io/badge/release-0.18.0-green.svg?style=flat
+[release-image]: https://img.shields.io/badge/release-0.20.2-green.svg?style=flat
 [releases]: https://github.com/introlab/rtabmap/releases

 [license-image]: https://img.shields.io/badge/license-BSD-green.svg?style=flat
@@ -44,7 +44,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
@GTSAM@#define RTABMAP_GTSAM
@CERES@#define RTABMAP_CERES
@VERTIGO@#define RTABMAP_VERTIGO
-@OPENCV3@#define RTABMAP_OPENCV3
@OPENNI2@#define RTABMAP_OPENNI2
@FREENECT@#define RTABMAP_FREENECT
@FREENECT2@#define RTABMAP_FREENECT2
@@ -73,9 +72,21 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
@VINS@#define RTABMAP_VINS
@ORB_SLAM2@#define RTABMAP_ORB_SLAM2
@ORB_OCTREE@#define RTABMAP_ORB_OCTREE
-@SP_TORCH@#define RTABMAP_SP_TORCH
+@SUPERPOINT_TORCH@#define RTABMAP_SUPERPOINT_TORCH
+@PYMATCHER@#define RTABMAP_PYMATCHER
@MADGWICK@#define RTABMAP_MADGWICK

+#include <pcl/pcl_config.h>
+
+#if PCL_VERSION_COMPARE(>, 1, 11, 1)
+#include <pcl/types.h>
+#define RTABMAP_PCL_INDEX pcl::index_t
+#elif PCL_VERSION_COMPARE(>=, 1, 10, 0)
+#define RTABMAP_PCL_INDEX std::uint32_t
+#else
+#include <pcl/pcl_macros.h>
+#define RTABMAP_PCL_INDEX pcl::uint32_t
+#endif

 #endif /* VERSION_H_ */

@@ -22,6 +22,7 @@ set(sources
  scene.cpp
  point_cloud_drawable.cpp
  graph_drawable.cpp
+  background_renderer.cc
  tango-gl/axis.cpp 
  tango-gl/camera.cpp 
  tango-gl/conversions.cpp 
@@ -34,47 +34,31 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 namespace rtabmap {

-#ifdef DEPTH_TEST
-// Camera Callbacks
-static void CameraDeviceOnDisconnected(void* context, ACameraDevice* device) {
-  LOGE("Camera(id: %s) is disconnected.\n", ACameraDevice_getId(device));
-}
-static void CameraDeviceOnError(void* context, ACameraDevice* device,
-                                int error) {
-  LOGE("Error(code: %d) on Camera(id: %s).\n", error,
-       ACameraDevice_getId(device));
-}
-// Capture Callbacks
-bool g_captureSessionReady = false;
-static void CaptureSessionOnReady(void* context,
-                                  ACameraCaptureSession* session) {
-  LOGI("Session is ready.\n");
-  g_captureSessionReady = true;
-}
-static void CaptureSessionOnActive(void* context,
-                                   ACameraCaptureSession* session) {
-  LOGI("Session is activated.\n");
-}
-#endif // DEPTH_TEST
-
 //////////////////////////////
 // CameraARCore
 //////////////////////////////
-CameraARCore::CameraARCore(void* env, void* context, void* activity, bool smoothing):
+CameraARCore::CameraARCore(void* env, void* context, void* activity, bool depthFromMotion, bool smoothing):
 	CameraMobile(smoothing),
 	env_(env),
 	context_(context),
 	activity_(activity),
-	arInstallRequested_(false)
+	arInstallRequested_(false),
+	textureId_(9999),
+	uvs_initialized_(false),
+	updateOcclusionImage_(false),
+	depthFromMotion_(depthFromMotion)
 {
-	glGenTextures(1, &textureId_);
 }

 CameraARCore::~CameraARCore() {
 	// Disconnect ARCore service
 	close();

-	glDeleteTextures(1, &textureId_);
+	if(textureId_ != 9999)
+	{
+		glDeleteTextures(1, &textureId_);
+		textureId_ = 9999;
+	}
 }


@@ -146,132 +130,10 @@ std::string CameraARCore::getSerial() const
 	return "ARCore";
 }

-#ifdef DEPTH_TEST
-void OnImageCallback(void *ctx, AImageReader *reader) {
-  reinterpret_cast<CameraARCore *>(ctx)->imageCallback(reader);
-}
-void CameraARCore::imageCallback(AImageReader *reader) {
-	int32_t format;
-	media_status_t status = AImageReader_getFormat(reader, &format);
-	UWARN("format=%d", format);
-	UASSERT_MSG(status == AMEDIA_OK, "Failed to get the media format");
-
-	if (format == AIMAGE_FORMAT_DEPTH16) {
-		// Create a thread and write out the jpeg files
-		AImage *image = nullptr;
-		media_status_t status = AImageReader_acquireNextImage(reader, &image);
-		UASSERT_MSG(status == AMEDIA_OK && image, "Image is not available");
-
-		int planeCount;
-		status = AImage_getNumberOfPlanes(image, &planeCount);
-		UASSERT_MSG(status == AMEDIA_OK && planeCount == 1,
-				uFormat("Error: getNumberOfPlanes() planceCount = %d", planeCount).c_str());
-		uint8_t *data = nullptr;
-		int len = 0;
-		int stride;
-		int width;
-		int height;
-		AImage_getWidth(image, &width);
-		AImage_getHeight(image, &height);
-		AImage_getPlaneRowStride(image, 0, &stride);
-		AImage_getPlaneData(image, 0, &data, &len);
-
-		cv::Mat output(height, width, CV_16UC1);
-		uint16_t *dataShort = (uint16_t *)data;
-		uint16_t max=0x0;
-		for (int y = 0; y < output.rows; ++y)
-		{
-			for (int x = 0; x < output.cols; ++x)
-			{
-				uint16_t depthSample = dataShort[y*output.cols + x];
-				uint16_t depthRange = (depthSample & 0x1FFF); // first 3 bits are confidence
-				output.at<uint16_t>(y,x) = depthRange;
-				if(depthRange > max)
-				{
-					max = depthRange;
-				}
-			}
-		}
-		UWARN("width=%d, height=%d, bytes=%d stride=%d max=%dmm",
-				width, height, len, stride, (int)max);
-
-		std::string path = "/storage/emulated/0/RTAB-Map/depth.png";
-		cv::imwrite(path, output);
-		UWARN("depth image saved to %s", path.c_str());
-
-		AImage_delete(image);
-	}
-}
-#endif // DEPTH_TEST
-
 bool CameraARCore::init(const std::string & calibrationFolder, const std::string & cameraName)
 {
 	close();

-#ifdef DEPTH_TEST
-	///////////////////////////
-	// Depth image using camera2 API
-	/////////////////////////////
-	camera_status_t cameraStatus = ACAMERA_OK;
-
-	cameraManager_ = ACameraManager_create();
-
-	deviceStateCallbacks_.onDisconnected = CameraDeviceOnDisconnected;
-	deviceStateCallbacks_.onError = CameraDeviceOnError;
-
-	const char * cameraId = "0";
-	cameraStatus = ACameraManager_openCamera(cameraManager_, cameraId, &deviceStateCallbacks_, &cameraDevice_);
-	UASSERT_MSG(cameraStatus == ACAMERA_OK, uFormat("Failed to open camera device (id: %s)",
-			cameraId).c_str());
-
-	// Currently only working resolution on Huawei P30 Pro
-	cv::Size size(240, 180);
-	int format = AIMAGE_FORMAT_DEPTH16;
-
-	media_status_t mediaStatus = AImageReader_new(size.width, size.height, format, 2, &imageReader_);
-	UASSERT_MSG(imageReader_ && mediaStatus == AMEDIA_OK, uFormat("Failed to create AImageReader %dx%d format=%d",
-			size.width, size.height, format).c_str());
-
-	AImageReader_ImageListener listener{
-	  .context = this,
-	  .onImageAvailable = OnImageCallback,
-	};
-	AImageReader_setImageListener(imageReader_, &listener);
-
-	//
-	ANativeWindow *nativeWindow;
-	mediaStatus = AImageReader_getWindow(imageReader_, &nativeWindow);
-	UASSERT_MSG(mediaStatus == AMEDIA_OK, "Could not get ANativeWindow");
-
-	outputNativeWindow_ = nativeWindow;
-	ACaptureSessionOutputContainer_create(&captureSessionOutputContainer_);
-	ANativeWindow_acquire(outputNativeWindow_);
-	ACaptureSessionOutput_create(outputNativeWindow_, &sessionOutput_);
-	ACaptureSessionOutputContainer_add(captureSessionOutputContainer_, sessionOutput_);
-	ACameraOutputTarget_create(outputNativeWindow_, &cameraOutputTarget_);
-
-	cameraStatus = ACameraDevice_createCaptureRequest(cameraDevice_, TEMPLATE_RECORD, &captureRequest_);
-	UASSERT_MSG(cameraStatus == ACAMERA_OK,
-			uFormat("Failed to create preview capture request (id: %s, status=%d)",
-					cameraId, cameraStatus).c_str());
-
-	ACaptureRequest_addTarget(captureRequest_, cameraOutputTarget_);
-
-	captureSessionStateCallbacks_.onReady = CaptureSessionOnReady;
-	captureSessionStateCallbacks_.onActive = CaptureSessionOnActive;
-	ACameraDevice_createCaptureSession(
-			cameraDevice_,
-			captureSessionOutputContainer_, // outputs
-			&captureSessionStateCallbacks_, // callbacks
-			&captureSession_);
-
-	ACameraCaptureSession_setRepeatingRequest(captureSession_, nullptr, 1,
-			&captureRequest_, nullptr);
-
-	// Don't start ARCore as we cannot use both at the same time
-	return true;
-#endif // DEPTH_TEST
-
 	UScopeMutex lock(arSessionMutex_);

 	ArInstallStatus install_status;
@@ -302,10 +164,19 @@ bool CameraARCore::init(const std::string & calibrationFolder, const std::string
 	UASSERT(ArSession_create(env_, context_, &arSession_) == AR_SUCCESS);
 	UASSERT(arSession_);

+	int32_t is_depth_supported = 0;
+	ArSession_isDepthModeSupported(arSession_, AR_DEPTH_MODE_AUTOMATIC, &is_depth_supported);
+
 	ArConfig_create(arSession_, &arConfig_);
 	UASSERT(arConfig_);

-	ArConfig_setFocusMode(arSession_, arConfig_, AR_FOCUS_MODE_FIXED);
+	if (is_depth_supported!=0) {
+	  ArConfig_setDepthMode(arSession_, arConfig_, AR_DEPTH_MODE_AUTOMATIC);
+	} else {
+	  ArConfig_setDepthMode(arSession_, arConfig_, AR_DEPTH_MODE_DISABLED);
+	}
+
+	ArConfig_setFocusMode(arSession_, arConfig_, AR_FOCUS_MODE_AUTO);
 	UASSERT(ArSession_configure(arSession_, arConfig_) == AR_SUCCESS);

 	ArFrame_create(arSession_, &arFrame_);
@@ -361,9 +232,6 @@ bool CameraARCore::init(const std::string & calibrationFolder, const std::string

 	deviceTColorCamera_ = opticalRotation;

-	// Required as ArSession_update does some off-screen OpenGL stuff...
-	ArSession_setCameraTextureName(arSession_, textureId_);
-
 	if (ArSession_resume(arSession_) != ArStatus::AR_SUCCESS)
 	{
 		UERROR("Cannot resume camera!");
@@ -410,45 +278,8 @@ void CameraARCore::close()
 	}
 	arPose_ = nullptr;

-#ifdef DEPTH_TEST
-
-	if(captureSession_!=nullptr)
-	{
-		g_captureSessionReady = false;
-		ACameraCaptureSession_stopRepeating(captureSession_);
-		double start = UTimer::now();
-		while(g_captureSessionReady != true && UTimer::now()-start < 2.0){
-			uSleep(100);
-			UWARN("Waiting session to close.... max 2 seconds");
-		}
-		//ACameraCaptureSession_close(captureSession_); // FIXME: this crashes?!
-		captureSession_ = nullptr;
-
-		ACaptureRequest_removeTarget(captureRequest_, cameraOutputTarget_);
-		ACaptureRequest_free(captureRequest_);
-		ACameraOutputTarget_free(cameraOutputTarget_);
-		captureRequest_ = nullptr;
-		cameraOutputTarget_ = nullptr;
-
-		ACaptureSessionOutputContainer_remove(captureSessionOutputContainer_, sessionOutput_);
-		ANativeWindow_release(outputNativeWindow_);
-		ACaptureSessionOutputContainer_free(captureSessionOutputContainer_);
-		ACaptureSessionOutput_free(sessionOutput_);
-		captureSessionOutputContainer_ = nullptr;
-		sessionOutput_ = nullptr;
-
-		ACameraDevice_close(cameraDevice_);
-		cameraDevice_ = nullptr;
-
-		ACameraManager_delete(cameraManager_);
-		cameraManager_ = nullptr;
-
-		AImageReader_delete(imageReader_);
-		imageReader_ = nullptr;
-	}
-#endif
-
 	CameraMobile::close();
+	occlusionImage_ = cv::Mat();
 }

 LaserScan CameraARCore::scanFromPointCloudData(
@@ -499,6 +330,20 @@ LaserScan CameraARCore::scanFromPointCloudData(
 	return LaserScan();
 }

+void CameraARCore::setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height)
+{
+	CameraMobile::setScreenRotationAndSize(colorCameraToDisplayRotation, width, height);
+	if(arSession_)
+	{
+		int ret = static_cast<int>(colorCameraToDisplayRotation) + 1; // remove 90deg camera rotation
+		  if (ret > 3) {
+		    ret -= 4;
+		  }
+
+		ArSession_setDisplayGeometry(arSession_, ret, width, height);
+	}
+}
+
 SensorData CameraARCore::captureImage(CameraInfo * info)
 {
 	UScopeMutex lock(arSessionMutex_);
@@ -510,15 +355,44 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 		return data;
 	}

+	if(textureId_ == 9999)
+	{
+		glGenTextures(1, &textureId_);
+		glBindTexture(GL_TEXTURE_EXTERNAL_OES, textureId_);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+	}
+	ArSession_setCameraTextureName(arSession_, textureId_);
+
 	// Update session to get current frame and render camera background.
 	if (ArSession_update(arSession_, arFrame_) != AR_SUCCESS) {
 		LOGE("CameraARCore::captureImage() ArSession_update error");
 		return data;
 	}

+	// If display rotation changed (also includes view size change), we need to
+	// re-query the uv coordinates for the on-screen portion of the camera image.
+	int32_t geometry_changed = 0;
+	ArFrame_getDisplayGeometryChanged(arSession_, arFrame_, &geometry_changed);
+	if (geometry_changed != 0 || !uvs_initialized_) {
+		ArFrame_transformCoordinates2d(
+				arSession_, arFrame_, AR_COORDINATES_2D_OPENGL_NORMALIZED_DEVICE_COORDINATES,
+				BackgroundRenderer::kNumVertices, BackgroundRenderer_kVertices, AR_COORDINATES_2D_TEXTURE_NORMALIZED,
+				transformed_uvs_);
+		UASSERT(transformed_uvs_);
+		uvs_initialized_ = true;
+	}
+
 	ArCamera* ar_camera;
 	ArFrame_acquireCamera(arSession_, arFrame_, &ar_camera);

+	ArCamera_getViewMatrix(arSession_, ar_camera, glm::value_ptr(viewMatrix_));
+	ArCamera_getProjectionMatrix(arSession_, ar_camera,
+							   /*near=*/0.1f, /*far=*/100.f,
+							   glm::value_ptr(projectionMatrix_));
+
 	ArTrackingState camera_tracking_state;
 	ArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state);

@@ -551,17 +425,55 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 			ArPointCloud * pointCloud = nullptr;
 			ArFrame_acquirePointCloud(arSession_, arFrame_, &pointCloud);

+			int32_t is_depth_supported = 0;
+			ArSession_isDepthModeSupported(arSession_, AR_DEPTH_MODE_AUTOMATIC, &is_depth_supported);
+
 			ArImage * image = nullptr;
 			ArStatus status = ArFrame_acquireCameraImage(arSession_, arFrame_, &image);
 			if(status == AR_SUCCESS)
 			{
+				if(is_depth_supported && (updateOcclusionImage_||depthFromMotion_))
+				{
+					LOGD("Acquire depth image!");
+					ArImage * depthImage = nullptr;
+					ArFrame_acquireDepthImage(arSession_, arFrame_, &depthImage);
+
+					ArImageFormat format;
+					ArImage_getFormat(arSession_, depthImage, &format);
+					if(format == AR_IMAGE_FORMAT_DEPTH16)
+					{
+						LOGD("Depth format detected!");
+						int planeCount;
+						ArImage_getNumberOfPlanes(arSession_, depthImage, &planeCount);
+						LOGD("planeCount=%d", planeCount);
+						UASSERT_MSG(planeCount == 1, uFormat("Error: getNumberOfPlanes() planceCount = %d", planeCount).c_str());
+						const uint8_t *data = nullptr;
+						int len = 0;
+						int stride;
+						int depth_width;
+						int depth_height;
+						ArImage_getWidth(arSession_, depthImage, &depth_width);
+						ArImage_getHeight(arSession_, depthImage, &depth_height);
+						ArImage_getPlaneRowStride(arSession_, depthImage, 0, &stride);
+						ArImage_getPlaneData(arSession_, depthImage, 0, &data, &len);
+
+						LOGD("width=%d, height=%d, bytes=%d stride=%d", depth_width, depth_height, len, stride);
+
+						occlusionImage_ = cv::Mat(depth_height, depth_width, CV_16UC1, (void*)data).clone();
+
+						float scaleX = (float)depth_width / (float)width;
+						float scaleY = (float)depth_height / (float)height;
+						occlusionModel_ = CameraModel(fx*scaleX, fy*scaleY, cx*scaleX, cy*scaleY, pose*deviceTColorCamera_, 0, cv::Size(depth_width, depth_height));
+					}
+					ArImage_release(depthImage);
+				}
+
 				int64_t timestamp_ns;
 				ArImageFormat format;
 				ArImage_getTimestamp(arSession_, image, &timestamp_ns);
 				ArImage_getFormat(arSession_, image, &format);
 				if(format == AR_IMAGE_FORMAT_YUV_420_888)
 				{
-
 #ifndef DISABLE_LOG
 					int32_t num_planes;
 					ArImage_getNumberOfPlanes(arSession_, image, &num_planes);
@@ -623,7 +535,7 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 							LOGI("pointCloud empty");
 						}

-						data = SensorData(scan, rgb, cv::Mat(), model, 0, stamp);
+						data = SensorData(scan, rgb, depthFromMotion_?occlusionImage_:cv::Mat(), model, 0, stamp);
 						data.setFeatures(kpts,  kpts3, cv::Mat());
 					}
 				}
@@ -662,21 +574,49 @@ void CameraARCore::capturePoseOnly()
 	UScopeMutex lock(arSessionMutex_);
 	//LOGI("Capturing image...");

-	SensorData data;
 	if(!arSession_)
 	{
 		return;
 	}

+	if(textureId_ == 9999)
+	{
+		glGenTextures(1, &textureId_);
+		glBindTexture(GL_TEXTURE_EXTERNAL_OES, textureId_);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+	}
+	ArSession_setCameraTextureName(arSession_, textureId_);
+
 	// Update session to get current frame and render camera background.
 	if (ArSession_update(arSession_, arFrame_) != AR_SUCCESS) {
-		LOGE("CameraARCore::captureImage() ArSession_update error");
+		LOGE("CameraARCore::capturePoseOnly() ArSession_update error");
 		return;
 	}

+	// If display rotation changed (also includes view size change), we need to
+	// re-query the uv coordinates for the on-screen portion of the camera image.
+	int32_t geometry_changed = 0;
+	ArFrame_getDisplayGeometryChanged(arSession_, arFrame_, &geometry_changed);
+	if (geometry_changed != 0 || !uvs_initialized_) {
+		ArFrame_transformCoordinates2d(
+				arSession_, arFrame_, AR_COORDINATES_2D_OPENGL_NORMALIZED_DEVICE_COORDINATES,
+				BackgroundRenderer::kNumVertices, BackgroundRenderer_kVertices, AR_COORDINATES_2D_TEXTURE_NORMALIZED,
+				transformed_uvs_);
+		UASSERT(transformed_uvs_);
+		uvs_initialized_ = true;
+	}
+
 	ArCamera* ar_camera;
 	ArFrame_acquireCamera(arSession_, arFrame_, &ar_camera);

+	ArCamera_getViewMatrix(arSession_, ar_camera, glm::value_ptr(viewMatrix_));
+	ArCamera_getProjectionMatrix(arSession_, ar_camera,
+							   /*near=*/0.1f, /*far=*/100.f,
+							   glm::value_ptr(projectionMatrix_));
+
 	ArTrackingState camera_tracking_state;
 	ArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state);

@@ -694,6 +634,52 @@ void CameraARCore::capturePoseOnly()
 			pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
 			this->poseReceived(pose);
 		}
+
+		int32_t is_depth_supported = 0;
+		ArSession_isDepthModeSupported(arSession_, AR_DEPTH_MODE_AUTOMATIC, &is_depth_supported);
+
+		if(is_depth_supported && updateOcclusionImage_)
+		{
+			LOGD("Acquire depth image!");
+			ArImage * depthImage = nullptr;
+			ArFrame_acquireDepthImage(arSession_, arFrame_, &depthImage);
+
+			ArImageFormat format;
+			ArImage_getFormat(arSession_, depthImage, &format);
+			if(format == AR_IMAGE_FORMAT_DEPTH16)
+			{
+				LOGD("Depth format detected!");
+				int planeCount;
+				ArImage_getNumberOfPlanes(arSession_, depthImage, &planeCount);
+				LOGD("planeCount=%d", planeCount);
+				UASSERT_MSG(planeCount == 1, uFormat("Error: getNumberOfPlanes() planceCount = %d", planeCount).c_str());
+				const uint8_t *data = nullptr;
+				int len = 0;
+				int stride;
+				int width;
+				int height;
+				ArImage_getWidth(arSession_, depthImage, &width);
+				ArImage_getHeight(arSession_, depthImage, &height);
+				ArImage_getPlaneRowStride(arSession_, depthImage, 0, &stride);
+				ArImage_getPlaneData(arSession_, depthImage, 0, &data, &len);
+
+				LOGD("width=%d, height=%d, bytes=%d stride=%d", width, height, len, stride);
+
+				occlusionImage_ = cv::Mat(height, width, CV_16UC1, (void*)data).clone();
+
+				float fx,fy, cx, cy;
+				int32_t rgb_width, rgb_height;
+				ArCamera_getImageIntrinsics(arSession_, ar_camera, arCameraIntrinsics_);
+				ArCameraIntrinsics_getFocalLength(arSession_, arCameraIntrinsics_, &fx, &fy);
+				ArCameraIntrinsics_getPrincipalPoint(arSession_, arCameraIntrinsics_, &cx, &cy);
+				ArCameraIntrinsics_getImageDimensions(arSession_, arCameraIntrinsics_, &rgb_width, &rgb_height);
+
+				float scaleX = (float)width / (float)rgb_width;
+				float scaleY = (float)height / (float)rgb_height;
+				occlusionModel_ = CameraModel(fx*scaleX, fy*scaleY, cx*scaleX, cy*scaleY, pose*deviceTColorCamera_, 0, cv::Size(width, height));
+			}
+			ArImage_release(depthImage);
+		}
 	}

 	ArCamera_release(ar_camera);
@@ -38,14 +38,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UEvent.h>
 #include <rtabmap/utilite/UTimer.h>
 #include <boost/thread/mutex.hpp>
+#include <background_renderer.h>

 #include <arcore_c_api.h>
-#ifdef DEPTH_TEST
 #include <camera/NdkCameraDevice.h>
 #include <camera/NdkCameraManager.h>
 #include <media/NdkImageReader.h>
 #include <android/native_window.h>
-#endif

 namespace rtabmap {

@@ -61,19 +60,28 @@ public:
 			std::vector<cv::Point3f> * kpts3D = 0);

 public:
-	CameraARCore(void* env, void* context, void* activity, bool smoothing = false);
+	CameraARCore(void* env, void* context, void* activity, bool depthFromMotion = false, bool smoothing = false);
 	virtual ~CameraARCore();

+	bool uvsInitialized() const {return uvs_initialized_;}
+	const float* uvsTransformed() const {return transformed_uvs_;}
+	void getVPMatrices(glm::mat4 & view, glm::mat4 & projection) const {view=viewMatrix_; projection=projectionMatrix_;}
+
+	void updateOcclusionImage(bool enabled) {updateOcclusionImage_ = enabled;}
+	const cv::Mat & getOcclusionImage(CameraModel * model=0) const {if(model)*model=occlusionModel_; return occlusionImage_; }
+
+	virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height);
+
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	void setupGL();
 	virtual void close(); // close Tango connection
 	virtual std::string getSerial() const;
+	GLuint getTextureId() const {return textureId_;}

-#ifdef DEPTH_TEST
 	void imageCallback(AImageReader *reader);
-#endif // DEPTH_TEST

 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(CameraInfo * info = 0); // should be called in opengl thread
 	virtual void capturePoseOnly();

 private:
@@ -92,22 +100,15 @@ private:
 	GLuint textureId_;
 	UMutex arSessionMutex_;

-#ifdef DEPTH_TEST
-	// Camera variables
-	ACameraDevice* cameraDevice_ = nullptr;
-	ACaptureRequest* captureRequest_ = nullptr;
-	ACameraOutputTarget* cameraOutputTarget_ = nullptr;
-	ACaptureSessionOutput* sessionOutput_ = nullptr;
-	ACaptureSessionOutputContainer* captureSessionOutputContainer_ = nullptr;
-	ACameraCaptureSession* captureSession_ = nullptr;
-	ANativeWindow *outputNativeWindow_ = nullptr;
+	float transformed_uvs_[BackgroundRenderer::kNumVertices*2];
+	bool uvs_initialized_ = false;
+	glm::mat4 viewMatrix_;
+	glm::mat4 projectionMatrix_;

-	ACameraDevice_StateCallbacks deviceStateCallbacks_;
-	ACameraCaptureSession_stateCallbacks captureSessionStateCallbacks_;
-
-	ACameraManager* cameraManager_ = nullptr;
-	AImageReader* imageReader_ = nullptr;
-#endif // DEPTH_TEST
+	bool updateOcclusionImage_;
+	cv::Mat occlusionImage_;
+	CameraModel occlusionModel_;
+	bool depthFromMotion_;
 };

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

 		rtabmap::Transform pose = info.odomPose;
@@ -94,7 +94,7 @@ public:
 	const CameraModel & getCameraModel() const {return model_;}
 	const Transform & getDeviceTColorCamera() const {return deviceTColorCamera_;}
 	void setSmoothing(bool enabled) {smoothing_ = enabled;}
-	void setScreenRotation(ScreenRotation colorCameraToDisplayRotation) {colorCameraToDisplayRotation_ = colorCameraToDisplayRotation;}
+	virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height) {colorCameraToDisplayRotation_ = colorCameraToDisplayRotation;}
 	void setGPS(const GPS & gps);
 	void addEnvSensor(int type, float value);
 	void setData(const SensorData & data, const Transform & pose);
@@ -69,6 +69,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/surface/poisson.h>
 #include <pcl/surface/vtk_smoothing/vtk_mesh_quadric_decimation.h>

+
 #define LOW_RES_PIX 2
 //#define DEBUG_RENDERING_PERFORMANCE

@@ -263,7 +264,7 @@ void RTABMapApp::setScreenRotation(int displayRotation, int cameraRotation)
 	boost::mutex::scoped_lock  lock(cameraMutex_);
 	if(camera_)
 	{
-		camera_->setScreenRotation(rotation);
+		camera_->setScreenRotationAndSize(main_scene_.getScreenRotation(), main_scene_.getViewPortWidth(), main_scene_.getViewPortHeight());
 	}
 }

@@ -307,7 +308,7 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 		optRefPose_ = 0;
 	}
 	cv::Mat cloudMat;
-	std::vector<std::vector<std::vector<unsigned int> > > polygons;
+	std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons;
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 	std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > texCoords;
 #else
@@ -398,11 +399,15 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 	std::multimap<int, rtabmap::Link> links;
 	LOGI("Loading full map from database...");
 	UEventsManager::post(new rtabmap::RtabmapEventInit(rtabmap::RtabmapEventInit::kInfo, "Loading data from database..."));
-	rtabmap_->get3DMap(
-			signatures,
+	rtabmap_->getGraph(
 			poses,
 			links,
 			true,
+			true,
+			&signatures,
+			true,
+			true,
+			true,
 			true);

 	if(signatures.size() && poses.empty())
@@ -653,6 +658,10 @@ bool RTABMapApp::isBuiltWith(int cameraDriver) const

 bool RTABMapApp::startCamera(JNIEnv* env, jobject iBinder, jobject context, jobject activity, int driver)
 {
+	//ccapp = new computer_vision::ComputerVisionApplication();
+	//ccapp->OnResume(env, context, activity);
+	//return true;
+
 	cameraDriver_ = driver;
 	LOGW("startCamera() camera driver=%d", cameraDriver_);
 	boost::mutex::scoped_lock  lock(cameraMutex_);
@@ -681,8 +690,7 @@ bool RTABMapApp::startCamera(JNIEnv* env, jobject iBinder, jobject context, jobj
 	else if(cameraDriver_ == 1)
 	{
 #ifdef RTABMAP_ARCORE
-		camera_ = new rtabmap::CameraARCore(env, context, activity, smoothing_);
-
+		camera_ = new rtabmap::CameraARCore(env, context, activity, depthFromMotion_, smoothing_);
 #else
 		UERROR("RTAB-Map is not built with ARCore support!");
 #endif
@@ -708,7 +716,7 @@ bool RTABMapApp::startCamera(JNIEnv* env, jobject iBinder, jobject context, jobj

 	if(camera_->init())
 	{
-		camera_->setScreenRotation(main_scene_.getScreenRotation());
+		camera_->setScreenRotationAndSize(main_scene_.getScreenRotation(), main_scene_.getViewPortWidth(), main_scene_.getViewPortHeight());

 		//update mesh decimation based on camera calibration
 		LOGI("Cloud density level %d", cloudDensityLevel_);
@@ -933,6 +941,11 @@ void RTABMapApp::SetViewPort(int width, int height)
 {
 	UINFO("");
 	main_scene_.SetupViewPort(width, height);
+	boost::mutex::scoped_lock  lock(cameraMutex_);
+	if(camera_)
+	{
+		camera_->setScreenRotationAndSize(main_scene_.getScreenRotation(), main_scene_.getViewPortWidth(), main_scene_.getViewPortHeight());
+	}
 }

 class PostRenderEvent : public UEvent
@@ -1097,12 +1110,59 @@ int RTABMapApp::Render()
 		}

 		// ARCore and AREngine capture should be done in opengl thread!
+		const float* uvsTransformed = 0;
+		glm::mat4 arProjectionMatrix(0);
+		glm::mat4 arViewMatrix(0);
+		rtabmap::Mesh occlusionMesh;
 		if((cameraDriver_ == 1 || cameraDriver_ == 2) && camera_!=0)
 		{
 			boost::mutex::scoped_lock  lock(cameraMutex_);
 			if(camera_!=0)
 			{
+#ifdef RTABMAP_ARCORE
+				if(cameraDriver_ == 1)
+				{
+					((rtabmap::CameraARCore*)camera_)->updateOcclusionImage(!visualizingMesh_ && main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson);
+				}
+#endif
+
 				camera_->spinOnce();
+
+#ifdef RTABMAP_ARCORE
+				if(cameraDriver_ == 1)
+				{
+					if(main_scene_.background_renderer_ == 0)
+					{
+						main_scene_.background_renderer_ = new BackgroundRenderer();
+						main_scene_.background_renderer_->InitializeGlContent(((rtabmap::CameraARCore*)camera_)->getTextureId());
+					}
+					if(((rtabmap::CameraARCore*)camera_)->uvsInitialized())
+					{
+						uvsTransformed = ((rtabmap::CameraARCore*)camera_)->uvsTransformed();
+						((rtabmap::CameraARCore*)camera_)->getVPMatrices(arViewMatrix, arProjectionMatrix);
+					}
+					if(!visualizingMesh_ && main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson)
+					{
+						rtabmap::CameraModel occlusionModel;
+						cv::Mat occlusionImage = ((rtabmap::CameraARCore*)camera_)->getOcclusionImage(&occlusionModel);
+
+						if(occlusionModel.isValidForProjection())
+						{
+							pcl::IndicesPtr indices(new std::vector<int>);
+							pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::cloudFromDepth(occlusionImage, occlusionModel, 1, 0, 0, indices.get());
+							cloud = rtabmap::util3d::transformPointCloud(cloud, rtabmap::opengl_world_T_rtabmap_world*occlusionModel.localTransform());
+							occlusionMesh.cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>());
+							pcl::copyPointCloud(*cloud, *occlusionMesh.cloud);
+							occlusionMesh.indices = indices;
+							occlusionMesh.polygons = rtabmap::util3d::organizedFastMesh(cloud, 1.0*M_PI/180.0, false, meshTrianglePix_);
+						}
+						else
+						{
+							UERROR("invalid occlusionModel: %f %f %f %f %dx%d", occlusionModel.fx(), occlusionModel.fy(), occlusionModel.cx(), occlusionModel.cy(), occlusionModel.imageWidth(), occlusionModel.imageHeight());
+						}
+					}
+				}
+#endif
 			}
 		}

@@ -1788,7 +1848,7 @@ int RTABMapApp::Render()

 			fpsTime.restart();
 			main_scene_.setFrustumVisible(camera_!=0);
-			lastDrawnCloudsCount_ = main_scene_.Render();
+			lastDrawnCloudsCount_ = main_scene_.Render(uvsTransformed, arViewMatrix, arProjectionMatrix, occlusionMesh);
 			if(renderingTime_ < fpsTime.elapsed())
 			{
 				renderingTime_ = fpsTime.elapsed();
@@ -2074,6 +2134,14 @@ void RTABMapApp::setSmoothing(bool enabled)
 	}
 }

+void RTABMapApp::setDepthFromMotion(bool enabled)
+{
+	if(depthFromMotion_ != enabled)
+	{
+		depthFromMotion_ = enabled;
+	}
+}
+
 void RTABMapApp::setAppendMode(bool enabled)
 {
 	if(appendMode_ != enabled)
@@ -2391,12 +2459,13 @@ bool RTABMapApp::exportMesh(
 							gains[1] = jter->second.gains[1];
 							gains[2] = jter->second.gains[2];

-							rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, false);
+							rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, true, false, false, false);
 							data.uncompressData(0, &depth);
 						}
 						else
 						{
-							rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, true);
+							rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, true, false, false, false);
+							data.uncompressData();
 							if(!data.imageRaw().empty() && !data.depthRaw().empty() && data.cameraModels().size() == 1)
 							{
 								cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation_, maxCloudDepth_, minCloudDepth_, indices.get());
@@ -2661,7 +2730,8 @@ bool RTABMapApp::exportMesh(
 						}
 						else
 						{
-							rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, true);
+							rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, true, false, false, false);
+							data.uncompressData();
 							if(!data.imageRaw().empty() && !data.depthRaw().empty() && data.cameraModels().size() == 1)
 							{
 								cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation_, maxCloudDepth_, minCloudDepth_);
@@ -2832,7 +2902,7 @@ bool RTABMapApp::exportMesh(
 						pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 						pcl::fromPCLPointCloud2(polygonMesh->cloud, *cloud);
 						cv::Mat cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*cloud, rtabmap::Transform(), false)); // for database
-						std::vector<std::vector<std::vector<unsigned int> > > polygons(1);
+						std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons(1);
 						polygons[0].resize(polygonMesh->polygons.size());
 						for(unsigned int p=0; p<polygonMesh->polygons.size(); ++p)
 						{
@@ -2851,7 +2921,7 @@ bool RTABMapApp::exportMesh(
 					cv::Mat cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*cloud, rtabmap::Transform(), false)); // for database

 					// save in database
-					std::vector<std::vector<std::vector<unsigned int> > > polygons(textureMesh->tex_polygons.size());
+					std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons(textureMesh->tex_polygons.size());
 					for(unsigned int t=0; t<textureMesh->tex_polygons.size(); ++t)
 					{
 						polygons[t].resize(textureMesh->tex_polygons[t].size());
@@ -2894,7 +2964,8 @@ bool RTABMapApp::exportMesh(
 						gains[1] = jter->second.gains[1];
 						gains[2] = jter->second.gains[2];
 					}
-					rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, true);
+					rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, true, false, false, false);
+					data.uncompressData();
 					if(!data.imageRaw().empty() && !data.depthRaw().empty())
 					{
 						// full resolution
@@ -2913,7 +2984,8 @@ bool RTABMapApp::exportMesh(
 					}
 					else
 					{
-						rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, true);
+						rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, true, false, false, false);
+						data.uncompressData();
 						if(!data.imageRaw().empty() && !data.depthRaw().empty())
 						{
 							cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation_, maxCloudDepth_, minCloudDepth_, indices.get());
@@ -3042,7 +3114,7 @@ bool RTABMapApp::postExportation(bool visualize)
 	{
 		visualizingMesh_ = false;
 		cv::Mat cloudMat;
-		std::vector<std::vector<std::vector<unsigned int> > > polygons;
+		std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons;
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 		std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > texCoords;
 #else
@@ -3099,7 +3171,7 @@ bool RTABMapApp::writeExportedMesh(const std::string & directory, const std::str
 	pcl::PolygonMesh::Ptr polygonMesh(new pcl::PolygonMesh);
 	pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh);
 	cv::Mat cloudMat;
-	std::vector<std::vector<std::vector<unsigned int> > > polygons;
+	std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons;
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 	std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > texCoords;
 #else
@@ -3114,7 +3186,7 @@ bool RTABMapApp::writeExportedMesh(const std::string & directory, const std::str
 			LOGI("writeExportedMesh: Found optimized mesh!");
 			if(textures.empty())
 			{
-				polygonMesh = rtabmap::util3d::assemblePolygonMesh(cloudMat, polygons.size() == 1?polygons[0]:std::vector<std::vector<unsigned int> >());
+				polygonMesh = rtabmap::util3d::assemblePolygonMesh(cloudMat, polygons.size() == 1?polygons[0]:std::vector<std::vector<RTABMAP_PCL_INDEX> >());
 			}
 			else
 			{
@@ -44,6 +44,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/pcl_base.h>
 #include <pcl/TextureMesh.h>

+
 // RTABMapApp handles the application lifecycle and resources.
 class RTABMapApp : public UEventsHandler {
 public:
@@ -109,6 +110,7 @@ class RTABMapApp : public UEventsHandler {
  void setCameraColor(bool enabled);
  void setFullResolution(bool enabled);
  void setSmoothing(bool enabled);
+  void setDepthFromMotion(bool enabled);
  void setAppendMode(bool enabled);
  void setDataRecorderMode(bool enabled);
  void setMaxCloudDepth(float value);
@@ -182,6 +184,7 @@ class RTABMapApp : public UEventsHandler {
  bool trajectoryMode_;
  bool rawScanSaved_;
  bool smoothing_;
+  bool depthFromMotion_;
  bool cameraColor_;
  bool fullResolution_;
  bool appendMode_;
@@ -0,0 +1,92 @@
+/*
+ * Copyright 2018 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+// This modules handles drawing the passthrough camera image into the OpenGL
+// scene.
+
+#include "background_renderer.h"
+
+#include <type_traits>
+
+namespace {
+
+const std::string kVertexShader =
+		"attribute vec4 a_Position;\n"
+		"attribute vec2 a_TexCoord;\n"
+
+		"varying vec2 v_TexCoord;\n"
+
+		"void main() {\n"
+		"   gl_Position = a_Position;\n"
+		"   v_TexCoord = a_TexCoord;\n"
+		"}\n";
+
+const std::string kFragmentShader =
+	"#extension GL_OES_EGL_image_external : require\n"
+
+	"precision mediump float;\n"
+	"varying vec2 v_TexCoord;\n"
+	"uniform samplerExternalOES sTexture;\n"
+
+	"void main() {\n"
+	"    vec4 sample = texture2D(sTexture, v_TexCoord);\n"
+	"    float grey = 0.21 * sample.r + 0.71 * sample.g + 0.07 * sample.b;\n"
+	"    gl_FragColor = vec4(grey, grey, grey, 0.5);\n"
+	"}\n";
+
+}  // namespace
+
+void BackgroundRenderer::InitializeGlContent(GLuint textureId)
+{
+  texture_id_ = textureId;
+
+  shader_program_ = tango_gl::util::CreateProgram(kVertexShader.c_str(), kFragmentShader.c_str());
+  if (!shader_program_) {
+    LOGE("Could not create program.");
+  }
+  glUseProgram(shader_program_);
+  attribute_vertices_ = glGetAttribLocation(shader_program_, "a_Position");
+  attribute_uvs_ = glGetAttribLocation(shader_program_, "a_TexCoord");
+  glUseProgram(0);
+}
+
+void BackgroundRenderer::Draw(const float * transformed_uvs) {
+  static_assert(std::extent<decltype(BackgroundRenderer_kVertices)>::value == kNumVertices * 2, "Incorrect kVertices length");
+
+  glUseProgram(shader_program_);
+  glDepthMask(GL_FALSE);
+  glEnable (GL_BLEND);
+
+  glActiveTexture(GL_TEXTURE0);
+  glBindTexture(GL_TEXTURE_EXTERNAL_OES, texture_id_);
+
+  glVertexAttribPointer(attribute_vertices_, 2, GL_FLOAT, GL_FALSE, 0, BackgroundRenderer_kVertices);
+  glVertexAttribPointer(attribute_uvs_, 2, GL_FLOAT, GL_FALSE, 0, transformed_uvs);
+
+  glEnableVertexAttribArray(attribute_vertices_);
+  glEnableVertexAttribArray(attribute_uvs_);
+
+  glDrawArrays(GL_TRIANGLE_STRIP, 0, 4);
+
+  glDisableVertexAttribArray(attribute_vertices_);
+  glDisableVertexAttribArray(attribute_uvs_);
+
+  glUseProgram(0);
+  glDepthMask(GL_TRUE);
+  glDisable (GL_BLEND);
+  tango_gl::util::CheckGlError("BackgroundRenderer::Draw() error");
+}
+
@@ -0,0 +1,58 @@
+/*
+ * Copyright 2018 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef C_ARCORE_AUGMENTED_IMAGE_BACKGROUND_RENDERER_H_
+#define C_ARCORE_AUGMENTED_IMAGE_BACKGROUND_RENDERER_H_
+
+#include <GLES2/gl2.h>
+#include <GLES2/gl2ext.h>
+#include <cstdlib>
+
+#include "util.h"
+
+static const GLfloat BackgroundRenderer_kVertices[] = {
+	-1.0f, -1.0f, +1.0f, -1.0f, -1.0f, +1.0f, +1.0f, +1.0f,
+};
+
+// This class renders the passthrough camera image into the OpenGL frame.
+class BackgroundRenderer {
+public:
+	// Positions of the quad vertices in clip space (X, Y).
+
+	static constexpr int kNumVertices = 4;
+
+ public:
+  BackgroundRenderer() = default;
+  ~BackgroundRenderer() = default;
+
+  // Sets up OpenGL state.  Must be called on the OpenGL thread and before any
+  // other methods below.
+  void InitializeGlContent(GLuint textureId);
+
+  // Draws the background image.  This methods must be called for every ArFrame
+  // returned by ArSession_update() to catch display geometry change events.
+  void Draw(const float * transformed_uvs);
+
+ private:
+
+  GLuint shader_program_;
+  GLuint texture_id_;
+
+  GLuint attribute_vertices_;
+  GLuint attribute_uvs_;
+};
+
+#endif  // C_ARCORE_AUGMENTED_IMAGE_BACKGROUND_RENDERER_H_
@@ -512,6 +512,19 @@ Java_com_introlab_rtabmap_RTABMapLib_setSmoothing(
 	}
 }
 JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setDepthFromMotion(
+		JNIEnv*, jclass, jlong native_application, bool enabled)
+{
+	if(native_application)
+	{
+		return native(native_application)->setDepthFromMotion(enabled);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setCameraColor(
 		JNIEnv*, jclass, jlong native_application, bool enabled)
 {
@@ -69,6 +69,7 @@ const std::string kGraphFragmentShader =


 Scene::Scene() :
+		background_renderer_(0),
 		gesture_camera_(0),
 		axis_(0),
 		frustum_(0),
@@ -160,6 +161,8 @@ void Scene::DeleteResources() {
 		delete trace_;
 		delete grid_;
 		delete box_;
+		delete background_renderer_;
+		background_renderer_ = 0;
 	}

 	PointCloudDrawable::releaseShaderPrograms();
@@ -364,7 +367,7 @@ bool intersectFrustumAABB(
 }

 //Should only be called in OpenGL thread!
-int Scene::Render() {
+int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat4 arProjectionMatrix, const rtabmap::Mesh & occlusionMesh) {
 	UASSERT(gesture_camera_ != 0);

 	if(currentPose_ == 0)
@@ -395,6 +398,17 @@ int Scene::Render() {
 	glm::mat4 projectionMatrix = gesture_camera_->GetProjectionMatrix();
 	glm::mat4 viewMatrix = gesture_camera_->GetViewMatrix();

+	bool renderBackgroundCamera =
+			background_renderer_ &&
+			gesture_camera_->GetCameraType() == tango_gl::GestureCamera::kFirstPerson &&
+			!rtabmap::glmToTransform(arProjectionMatrix).isNull() &&
+			uvsTransformed;
+	if(renderBackgroundCamera)
+	{
+		projectionMatrix = arProjectionMatrix;
+		viewMatrix = arViewMatrix;
+	}
+
 	rtabmap::Transform openglCamera = GetOpenGLCameraPose();//*rtabmap::Transform(0.0f, 0.0f, 3.0f, 0.0f, 0.0f, 0.0f);
 	// transform in same coordinate as frustum filtering
 	openglCamera *= rtabmap::Transform(
@@ -444,7 +458,7 @@ int Scene::Render() {

 	UTimer timer;

-	bool onlineBlending = blending_ && gesture_camera_->GetCameraType()!=tango_gl::GestureCamera::kTopOrtho && mapRendering_ && meshRendering_ && cloudsToDraw.size()>1;
+	bool onlineBlending = (renderBackgroundCamera && occlusionMesh.cloud.get() && occlusionMesh.cloud->size()) || (blending_ && gesture_camera_->GetCameraType()!=tango_gl::GestureCamera::kTopOrtho && mapRendering_ && meshRendering_ && cloudsToDraw.size()>1);
 	if(onlineBlending && fboId_)
 	{
 		// set the rendering destination to FBO
@@ -454,11 +468,19 @@ int Scene::Render() {
 		glClearColor(1, 1, 1, 1);
 		glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);

-		// Draw scene
-		for(std::vector<PointCloudDrawable*>::const_iterator iter=cloudsToDraw.begin(); iter!=cloudsToDraw.end(); ++iter)
+		if(renderBackgroundCamera)
 		{
-			// set large distance to cam to use low res polygons for fast processing
-			(*iter)->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, false, false, 999.0f);
+			PointCloudDrawable drawable(occlusionMesh);
+			drawable.Render(projectionMatrix, viewMatrix, true, pointSize_, false, false, 999.0f);
+		}
+		else
+		{
+			// Draw scene
+			for(std::vector<PointCloudDrawable*>::const_iterator iter=cloudsToDraw.begin(); iter!=cloudsToDraw.end(); ++iter)
+			{
+				// set large distance to cam to use low res polygons for fast processing
+				(*iter)->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, false, false, 999.0f);
+			}
 		}

 		// back to normal window-system-provided framebuffer
@@ -495,6 +517,15 @@ int Scene::Render() {
 	glClearColor(r_, g_, b_, 1.0f);
 	glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);

+	if(renderBackgroundCamera)
+	{
+		background_renderer_->Draw(uvsTransformed);
+
+		//To debug occlusion image:
+		//PointCloudDrawable drawable(occlusionMesh);
+		//drawable.Render(projectionMatrix, viewMatrix, true, pointSize_, false, false, 999.0f);
+	}
+
 	if(!currentPose_->isNull())
 	{
 		if (frustumVisible_ && gesture_camera_->GetCameraType() != tango_gl::GestureCamera::kFirstPerson)
@@ -523,7 +554,7 @@ int Scene::Render() {
 		}
 	}

-	if(gridVisible_)
+	if(gridVisible_ && !renderBackgroundCamera)
 	{
 		grid_->Render(projectionMatrix, viewMatrix);
 	}
@@ -38,6 +38,7 @@
 #include <point_cloud_drawable.h>
 #include <graph_drawable.h>
 #include <bounding_box_drawable.h>
+#include <background_renderer.h>

 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
@@ -71,13 +72,14 @@ class Scene {
  //         frame's timestamp.
  // @param: point_cloud_vertices, point cloud's vertices of the current point
  //         frame.
-  int Render();
+  int Render(const float * uvsTransformed = 0, glm::mat4 arViewMatrix = glm::mat4(0), glm::mat4 arProjectionMatrix=glm::mat4(0), const rtabmap::Mesh & occlusionMesh=rtabmap::Mesh());

  // Set render camera's viewing angle, first person, third person or top down.
  //
  // @param: camera_type, camera type includes first person, third person and
  //         top down
  void SetCameraType(tango_gl::GestureCamera::CameraType camera_type);
+  tango_gl::GestureCamera::CameraType GetCameraType() const {return gesture_camera_->GetCameraType();}

  void SetCameraPose(const rtabmap::Transform & pose); // opengl camera
  rtabmap::Transform GetCameraPose() const {return currentPose_!=0?*currentPose_:rtabmap::Transform();}
@@ -152,6 +154,8 @@ class Scene {
  bool isLighting() const {return lighting_;}
  bool isBackfaceCulling() const {return backfaceCulling_;}

+  BackgroundRenderer * background_renderer_;
+
 private:
  // Camera object that allows user to use touch input to interact with.
  tango_gl::GestureCamera* gesture_camera_;
@@ -199,8 +199,8 @@ void GestureCamera::SetCameraType(CameraType camera_index) {
      SetRotation(glm::quat(1.0f, 0.0f, 0.0f, 0.0f));
      cam_cur_dist_ = kThirdPersonFollow?kThirdPersonFollowCameraDist:kThirdPersonCameraDist;
      anchor_offset_ = glm::vec3(0.0f,0.0f,0.0f);
-      cam_cur_angle_.x = -M_PI / 6.0f;
-      cam_cur_angle_.y = kThirdPersonFollow?0:M_PI / 4.0f;
+      cam_cur_angle_.x = -M_PI / 12.0f;
+      cam_cur_angle_.y = kThirdPersonFollow?0:M_PI / 2.0f;
      cam_cur_target_rot_ = glm::quat(1,0,0,0);
      StartCameraToCurrentTransform();
      break;
@@ -252,7 +252,7 @@ inline ScreenRotation GetAndroidRotationFromColorCameraToDisplay(
 // @param display: integer value of display orientation, values available
 // are 0, 1, 2 ,3. Followed by Android display orientation standard:
 // https://developer.android.com/reference/android/view/Display.html#getRotation()
-// @param color_camera: integer value of color camera oreintation, values
+// @param color_camera: integer value of color camera orientation, values
 // available are 0, 90, 180, 270. Followed by Android camera orientation
 // standard:
 // https://developer.android.com/reference/android/hardware/Camera.CameraInfo.html#orientation
@@ -163,6 +163,7 @@
        android:layout_height="100dp"
        android:layout_alignLeft="@+id/button_library"
        android:layout_below="@+id/button_library"
+        android:layout_marginTop="20dp"
        android:text="@string/new_scan" />

 </RelativeLayout>        
@@ -7,7 +7,7 @@
   			android:entries="@array/pref_camera_driver_keys"
   			android:entryValues="@array/pref_camera_driver_values"
   			android:defaultValue="@string/pref_default_camera_driver"/>
-    
+        
    <PreferenceCategory
        android:title="@string/pref_title_rendering">
        <ListPreference 
@@ -97,6 +97,11 @@
 	            android:title="@string/pref_title_resolution"
 	            android:summary="@string/pref_summary_resolution"
 	            android:defaultValue="@string/pref_default_resolution"/>
+		     <com.introlab.rtabmap.CustomSwitchPreference
+                android:key="@string/pref_key_depth_from_motion"
+                android:title="@string/pref_title_depth_from_motion"
+                android:summary="@string/pref_summary_depth_from_motion"
+                android:defaultValue="@string/pref_default_depth_from_motion"/>
 		    <com.introlab.rtabmap.CustomSwitchPreference
 		        android:key="@string/pref_key_smoothing"
 		        android:title="@string/pref_title_smoothing"
@@ -81,6 +81,8 @@
    
    <string name="pref_key_camera_driver">pref_key_camera_driver</string>
    <string name="pref_default_camera_driver">0</string>
+    <string name="pref_key_depth_from_motion">pref_key_depth_from_motion</string>
+    <string name="pref_default_depth_from_motion">false</string>
    <string name="pref_key_update_rate">pref_key_update_rate</string>
    <string name="pref_default_update_rate">1</string>
    <string name="pref_key_max_speed">pref_key_max_speed</string>
@@ -323,6 +325,8 @@
    <string name="pref_title_mapping_database">Database</string>
    <string name="pref_title_camera_driver">Camera Driver</string>
    <string name="pref_summary_camera_driver">AR sdk use for capturing 6DoF poses and images. A TOF camera is required to record a 3D model.</string>
+    <string name="pref_title_depth_from_motion">Depth From Motion</string>
+    <string name="pref_summary_depth_from_motion">Use ARCore\'s depth API to compute depth image from motion. If the phone has a TOF camera and is supported by ARCore, results should be better. Currently supported only with ARCore NDK driver.</string>
    <string name="pref_title_append">Append Mode</string>
    <string name="pref_summary_append">When resuming mapping, wait for a relocalization on the current map before starting a new map.</string>
    <string name="pref_title_resolution">HD Mode</string>
@@ -992,6 +992,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 			RTABMapLib.setRawScanSaved(nativeApplication, sharedPref.getBoolean(getString(R.string.pref_key_raw_scan_saved), Boolean.parseBoolean(getString(R.string.pref_default_raw_scan_saved))));
 			RTABMapLib.setFullResolution(nativeApplication, sharedPref.getBoolean(getString(R.string.pref_key_resolution), Boolean.parseBoolean(getString(R.string.pref_default_resolution))));
 			RTABMapLib.setSmoothing(nativeApplication, sharedPref.getBoolean(getString(R.string.pref_key_smoothing), Boolean.parseBoolean(getString(R.string.pref_default_smoothing))));
+			RTABMapLib.setDepthFromMotion(nativeApplication, sharedPref.getBoolean(getString(R.string.pref_key_depth_from_motion), Boolean.parseBoolean(getString(R.string.pref_default_depth_from_motion))));
 			RTABMapLib.setCameraColor(nativeApplication, !sharedPref.getBoolean(getString(R.string.pref_key_fisheye), Boolean.parseBoolean(getString(R.string.pref_default_fisheye))));
 			RTABMapLib.setAppendMode(nativeApplication, sharedPref.getBoolean(getString(R.string.pref_key_append), Boolean.parseBoolean(getString(R.string.pref_default_append))));
 			RTABMapLib.setMappingParameter(nativeApplication, "Rtabmap/DetectionRate", mUpdateRate);
@@ -1162,6 +1163,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener

 		SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(this);
 		String cameraDriverStr = sharedPref.getString(getString(R.string.pref_key_camera_driver), getString(R.string.pref_default_camera_driver));
+		final boolean depthFromMotion = sharedPref.getBoolean(getString(R.string.pref_key_depth_from_motion), Boolean.parseBoolean(getString(R.string.pref_default_depth_from_motion)));
 		mCameraDriver = Integer.parseInt(cameraDriverStr);

 		if(!DISABLE_LOG) Log.i(TAG, String.format("startCamera() driver=%d", mCameraDriver));
@@ -1218,7 +1220,8 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 			}
 			Thread bindThread = new Thread(new Runnable() {
 				public void run() {
-					if(mCameraDriver==1)
+					
+					if(mCameraDriver==1 && !depthFromMotion)
 					{
 						RTABMapLib.setMeshRendering(
 								nativeApplication, 
@@ -1263,9 +1266,9 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 							}
 							else
 							{
-								if((mState==State.STATE_IDLE || mState==State.STATE_WELCOME) && mCameraDriver == 1)
+								if((mState==State.STATE_IDLE || mState==State.STATE_WELCOME) && mCameraDriver == 1 && !depthFromMotion)
 								{
-									mToast.makeText(getApplicationContext(), "Currently ARCore NDK driver doesn't support depth, only poses and RGB images can be recorded.", mToast.LENGTH_LONG).show();
+									mToast.makeText(getApplicationContext(), "Currently ARCore NDK driver doesn't support depth, only poses, RGB images and 3d features can be recorded.", mToast.LENGTH_LONG).show();
 								}
 								updateState(mState==State.STATE_VISUALIZING?State.STATE_VISUALIZING_CAMERA:State.STATE_CAMERA);	
 								if(mState==State.STATE_VISUALIZING_CAMERA && mItemLocalizationMode.isChecked())
@@ -2311,6 +2314,15 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener

 		updateState(State.STATE_IDLE);

+		if(mArCoreCamera != null)
+		{
+			synchronized (this) {
+				mRenderer.setCamera(null);
+				mArCoreCamera.close();
+				mArCoreCamera = null;
+			}
+		}
+		
 		Thread stopThread = new Thread(new Runnable() {
 			public void run() {
 				if(!DISABLE_LOG) Log.i(TAG, String.format("setPausedMapping()"));
@@ -72,6 +72,7 @@ public class RTABMapLib
    public static native void setRawScanSaved(long nativeApplication, boolean enabled);
    public static native void setFullResolution(long nativeApplication, boolean enabled);
    public static native void setSmoothing(long nativeApplication, boolean enabled);
+    public static native void setDepthFromMotion(long nativeApplication, boolean enabled);
    public static native void setCameraColor(long nativeApplication, boolean enabled);
    public static native void setAppendMode(long nativeApplication, boolean enabled);
    public static native void setDataRecorderMode(long nativeApplication, boolean enabled);
@@ -137,6 +137,16 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
          DESTINATION ${openni2_dest_dir} 
          COMPONENT runtime)
  ENDIF(OpenNI2_FOUND)
+  
+  IF(k4a_FOUND)
+	# Install needed depthengine_2_0.dll
+    IF(WIN32)
+		file(TO_CMAKE_PATH "$ENV{K4A_ROOT_DIR}" ENV_K4A_ROOT_DIR)
+		INSTALL(FILES "${ENV_K4A_ROOT_DIR}/tools/depthengine_2_0.dll" 
+          DESTINATION ${plugin_dest_dir} 
+          COMPONENT runtime)
+	ENDIF(WIN32)
+  ENDIF(k4a_FOUND)

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

 	cv::Mat generatePrediction(const Memory * memory, const std::vector<int> & ids);

+	unsigned long getMemoryUsed() const;
+
 private:
 	cv::Mat updatePrediction(const cv::Mat & oldPrediction,
 			const Memory * memory,
@@ -53,6 +53,7 @@ public:
 	virtual ~Camera();
 	SensorData takeImage(CameraInfo * info = 0);

+	bool initFromFile(const std::string & calibrationPath);
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "") = 0;
 	virtual bool isCalibrated() const = 0;
 	virtual std::string getSerial() const = 0;
@@ -73,7 +74,7 @@ protected:
 	 *
 	 * @param imageRate : image/second , 0 for fast as the camera can
 	 */
-	Camera(float imageRate = 0, const Transform & localTransform = Transform::getIdentity());
+	Camera(float imageRate = 0, const Transform & localTransform = CameraModel::opticalRotation());

 	/**
 	 * returned rgb and depth images should be already rectified if calibration was loaded
@@ -37,6 +37,13 @@ namespace rtabmap {

 class RTABMAP_EXP CameraModel
 {
+public:
+	/**
+	 * Optical rotation used to transform image coordinate frame (x->right, y->down, z->forward)
+	 * to robot coordinate frame (x->forward, y->left, z->up).
+	 */
+	static Transform opticalRotation() {return Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0);}
+
 public:
 	CameraModel();
 	// K is the camera intrinsic 3x3 CV_64FC1
@@ -50,7 +57,7 @@ public:
 			const cv::Mat & D,
 			const cv::Mat & R,
 			const cv::Mat & P,
-			const Transform & localTransform = Transform::getIdentity());
+			const Transform & localTransform = opticalRotation());

 	// minimal
 	CameraModel(
@@ -58,7 +65,7 @@ public:
 			double fy,
 			double cx,
 			double cy,
-			const Transform & localTransform = Transform::getIdentity(),
+			const Transform & localTransform = opticalRotation(),
 			double Tx = 0.0f,
 			const cv::Size & imageSize = cv::Size(0,0));
 	// minimal to be saved
@@ -68,7 +75,7 @@ public:
 			double fy,
 			double cx,
 			double cy,
-			const Transform & localTransform = Transform::getIdentity(),
+			const Transform & localTransform = opticalRotation(),
 			double Tx = 0.0f,
 			const cv::Size & imageSize = cv::Size(0,0));

@@ -113,6 +120,12 @@ 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
+
+	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;
@@ -122,9 +135,6 @@ public:
 	CameraModel scaled(double scale) const;
 	CameraModel roi(const cv::Rect & roi) const;

-	double horizontalFOV() const; // in degrees
-	double verticalFOV() const;   // in degrees
-
 	// For depth images, your should use cv::INTER_NEAREST
 	cv::Mat rectifyImage(const cv::Mat & raw, int interpolation = cv::INTER_LINEAR) const;
 	cv::Mat rectifyDepth(const cv::Mat & raw) const;
@@ -148,5 +158,7 @@ private:
 	Transform localTransform_;
 };

+RTABMAP_EXP std::ostream& operator<<(std::ostream& os, const CameraModel& model);
+
 } /* namespace rtabmap */
 #endif /* CAMERAMODEL_H_ */
@@ -72,6 +72,15 @@ public:
 	void enableIMUFiltering(int filteringStrategy=1, const ParametersMap & parameters = ParametersMap());
 	void disableIMUFiltering();

+	RTABMAP_DEPRECATED(void setScanParameters(
+			bool fromDepth,
+			int downsampleStep, // decimation of the depth image in case the scan is from depth image
+			float rangeMin,
+			float rangeMax,
+			float voxelSize,
+			int normalsK,
+			int normalsRadius,
+			bool forceGroundNormalsUp) , "Use new version of this function with groundNormalsUp=0.8 for forceGroundNormalsUp=True and groundNormalsUp=0.0 for forceGroundNormalsUp=False.");
 	void setScanParameters(
 			bool fromDepth,
 			int downsampleStep=1, // decimation of the depth image in case the scan is from depth image
@@ -80,17 +89,7 @@ public:
 			float voxelSize = 0.0f,
 			int normalsK = 0,
 			int normalsRadius = 0.0f,
-			bool forceGroundNormalsUp = false)
-	{
-		_scanFromDepth = fromDepth;
-		_scanDownsampleStep=downsampleStep;
-		_scanRangeMin = rangeMin;
-		_scanRangeMax = rangeMax;
-		_scanVoxelSize = voxelSize;
-		_scanNormalsK = normalsK;
-		_scanNormalsRadius = normalsRadius;
-		_scanForceGroundNormalsUp = forceGroundNormalsUp;
-	}
+			float groundNormalsUp = 0.0f);

 	void postUpdate(SensorData * data, CameraInfo * info = 0) const;

@@ -119,7 +118,7 @@ private:
 	float _scanVoxelSize;
 	int _scanNormalsK;
 	float _scanNormalsRadius;
-	bool _scanForceGroundNormalsUp;
+	float _scanForceGroundNormalsUp;
 	StereoDense * _stereoDense;
 	clams::DiscreteDepthDistortionModel * _distortionModel;
 	bool _bilateralFiltering;
@@ -70,6 +70,7 @@ public:
 	virtual void parseParameters(const ParametersMap & parameters);
 	virtual bool isInMemory() const {return _url.empty();}
 	const std::string & getUrl() const {return _url;}
+	const std::string & getTargetVersion() const {return _targetVersion;}

 	void beginTransaction() const;
 	void commit() const;
@@ -100,7 +101,7 @@ public:

 public:
 	void addInfoAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed, int dictionarySize, const ParametersMap & parameters) const;
-	void addStatistics(const Statistics & statistics) const;
+	void addStatistics(const Statistics & statistics, bool saveWmState) const;
 	void savePreviewImage(const cv::Mat & image) const;
 	cv::Mat loadPreviewImage() const;
 	void saveOptimizedPoses(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const;
@@ -109,7 +110,7 @@ public:
 	cv::Mat load2DMap(float & xMin, float & yMin, float & cellSize) const;
 	void saveOptimizedMesh(
 			const cv::Mat & cloud,
-			const std::vector<std::vector<std::vector<unsigned int> > > & polygons = std::vector<std::vector<std::vector<unsigned int> > >(),      // Textures -> polygons -> vertices
+			const std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > & polygons = std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > >(),      // Textures -> polygons -> vertices
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords = std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > >(), // Textures -> uv coords for each vertex of the polygons
 #else
@@ -117,7 +118,7 @@ public:
 #endif
 			const cv::Mat & textures = cv::Mat()) const; // concatenated textures (assuming square textures with all same size);
 	cv::Mat loadOptimizedMesh(
-			std::vector<std::vector<std::vector<unsigned int> > > * polygons = 0,
+			std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > * polygons = 0,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords = 0,
 #else
@@ -131,7 +132,7 @@ public:
 	bool openConnection(const std::string & url, bool overwritten = false);
 	void closeConnection(bool save = true, const std::string & outputUrl = "");
 	bool isConnected() const;
-	long getMemoryUsed() const; // In bytes
+	unsigned long getMemoryUsed() const; // In bytes
 	std::string getDatabaseVersion() const;
 	long getNodesMemoryUsed() const;
 	long getLinksMemoryUsed() const;
@@ -188,7 +189,7 @@ protected:
 	virtual bool connectDatabaseQuery(const std::string & url, bool overwritten = false) = 0;
 	virtual void disconnectDatabaseQuery(bool save = true, const std::string & outputUrl = "") = 0;
 	virtual bool isConnectedQuery() const = 0;
-	virtual long getMemoryUsedQuery() const = 0; // In bytes
+	virtual unsigned long getMemoryUsedQuery() const = 0; // In bytes
 	virtual bool getDatabaseVersionQuery(std::string & version) const = 0;
 	virtual long getNodesMemoryUsedQuery() const = 0;
 	virtual long getLinksMemoryUsedQuery() const = 0;
@@ -238,7 +239,7 @@ protected:
 			int nodeId,
 			const LaserScan & scan) const = 0;

-	virtual void addStatisticsQuery(const Statistics & statistics) const = 0;
+	virtual void addStatisticsQuery(const Statistics & statistics, bool saveWmState) const = 0;
 	virtual void savePreviewImageQuery(const cv::Mat & image) const = 0;
 	virtual cv::Mat loadPreviewImageQuery() const = 0;
 	virtual void saveOptimizedPosesQuery(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const = 0;
@@ -247,7 +248,7 @@ protected:
 	virtual cv::Mat load2DMapQuery(float & xMin, float & yMin, float & cellSize) const = 0;
 	virtual void saveOptimizedMeshQuery(
 				const cv::Mat & cloud,
-				const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
+				const std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > & polygons,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 				const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
 #else
@@ -255,7 +256,7 @@ protected:
 #endif
 				const cv::Mat & textures) const = 0;
 	virtual cv::Mat loadOptimizedMeshQuery(
-				std::vector<std::vector<std::vector<unsigned int> > > * polygons,
+				std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > * polygons,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 				std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords,
 #else
@@ -300,6 +301,7 @@ private:
 	USemaphore _addSem;
 	double _emptyTrashesTime;
 	std::string _url;
+	std::string _targetVersion;
 	bool _timestampUpdate;
 };

@@ -54,7 +54,7 @@ protected:
 	virtual bool connectDatabaseQuery(const std::string & url, bool overwritten = false);
 	virtual void disconnectDatabaseQuery(bool save = true, const std::string & outputUrl = "");
 	virtual bool isConnectedQuery() const;
-	virtual long getMemoryUsedQuery() const; // In bytes
+	virtual unsigned long getMemoryUsedQuery() const; // In bytes
 	virtual bool getDatabaseVersionQuery(std::string & version) const;
 	virtual long getNodesMemoryUsedQuery() const;
 	virtual long getLinksMemoryUsedQuery() const;
@@ -104,7 +104,7 @@ protected:
 			int nodeId,
 			const LaserScan & scan) const;

-	virtual void addStatisticsQuery(const Statistics & statistics) const;
+	virtual void addStatisticsQuery(const Statistics & statistics, bool saveWmState) const;
 	virtual void savePreviewImageQuery(const cv::Mat & image) const;
 	virtual cv::Mat loadPreviewImageQuery() const;
 	virtual void saveOptimizedPosesQuery(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const;
@@ -113,7 +113,7 @@ protected:
 	virtual cv::Mat load2DMapQuery(float & xMin, float & yMin, float & cellSize) const;
 	virtual void saveOptimizedMeshQuery(
 			const cv::Mat & cloud,
-			const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
+			const std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > & polygons,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
 #else
@@ -121,7 +121,7 @@ protected:
 #endif
 			const cv::Mat & textures) const;
 	virtual cv::Mat loadOptimizedMeshQuery(
-			std::vector<std::vector<std::vector<unsigned int> > > * polygons,
+			std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > * polygons,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords,
 #else
@@ -160,6 +160,7 @@ private:
 	std::string queryStepLink() const;
 	std::string queryStepWordsChanged() const;
 	std::string queryStepKeypoint() const;
+	std::string queryStepGlobalDescriptor() const;
 	std::string queryStepOccupancyGridUpdate() const;
 	void stepNode(sqlite3_stmt * ppStmt, const Signature * s) const;
 	void stepImage(sqlite3_stmt * ppStmt, int id, const cv::Mat & imageBytes) const;
@@ -169,7 +170,8 @@ private:
 	void stepSensorData(sqlite3_stmt * ppStmt, const SensorData & sensorData) const;
 	void stepLink(sqlite3_stmt * ppStmt, const Link & link) const;
 	void stepWordsChanged(sqlite3_stmt * ppStmt, int signatureId, int oldWordId, int newWordId) const;
-	void stepKeypoint(sqlite3_stmt * ppStmt, int signatureId, int wordId, const cv::KeyPoint & kp, const cv::Point3f & pt, const cv::Mat & descriptor) const;
+	void stepKeypoint(sqlite3_stmt * ppStmt, int nodeID, int wordId, const cv::KeyPoint & kp, const cv::Point3f & pt, const cv::Mat & descriptor) const;
+	void stepGlobalDescriptor(sqlite3_stmt * ppStmt, int nodeId, const GlobalDescriptor & descriptor) const;
 	void stepOccupancyGridUpdate(sqlite3_stmt * ppStmt,
 			int nodeId,
 			const cv::Mat & ground,
@@ -187,7 +189,7 @@ protected:
 	std::string _version;

 private:
-	long _memoryUsedEstimate;
+	unsigned long _memoryUsedEstimate;
 	bool _dbInMemory;
 	unsigned int _cacheSize;
 	int _journalMode;
@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
 #include "rtabmap/core/Parameters.h"
+#include "rtabmap/utilite/UStl.h"
 #include <opencv2/core/core.hpp>
 #include <opencv2/features2d/features2d.hpp>
 #include <pcl/point_cloud.h>
@@ -75,8 +76,8 @@ public:
 	static cv::Mat findFFromWords(
 			const std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs, // id, kpt1, kpt2
 			std::vector<uchar> & status,
-			double ransacParam1 = 3.0,
-			double ransacParam2 = 0.99);
+			double ransacReprojThreshold = 3.0,
+			double ransacConfidence = 0.99);

 	// assume a canonical camera (without K)
 	static void findRTFromP(
@@ -91,41 +92,133 @@ public:
 	 * if a=[1 2 3 4 6], b=[1 2 4 5 6], results= [(1,1) (2,2) (4,4) (6,6)]
 	 * realPairsCount = 4
 	 */
+	template<typename T>
 	static int findPairs(
-			const std::map<int, cv::KeyPoint> & wordsA,
-			const std::map<int, cv::KeyPoint> & wordsB,
-			std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
-			bool ignoreNegativeIds = true);
+			const std::map<int, T> & wordsA,
+			const std::map<int, T> & wordsB,
+			std::list<std::pair<int, std::pair<T, T> > > & pairs,
+			bool ignoreNegativeIds = true)
+	{
+		int realPairsCount = 0;
+		pairs.clear();
+		for(typename std::map<int, T>::const_iterator i=wordsA.begin(); i!=wordsA.end(); ++i)
+		{
+			if(!ignoreNegativeIds || (ignoreNegativeIds && i->first>=0))
+			{
+				std::map<int, cv::KeyPoint>::const_iterator ptB = wordsB.find(i->first);
+				if(ptB != wordsB.end())
+				{
+					pairs.push_back(std::pair<int, std::pair<T, T> >(i->first, std::make_pair(i->second, ptB->second)));
+					++realPairsCount;
+				}
+			}
+		}
+		return realPairsCount;
+	}

 	/**
 	 * if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(1,1a) (2,2) (4,4) (6a,6a) (6b,6b)]
 	 * realPairsCount = 5
 	 */
+	template<typename T>
 	static int findPairs(
-			const std::multimap<int, cv::KeyPoint> & wordsA,
-			const std::multimap<int, cv::KeyPoint> & wordsB,
-			std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
-			bool ignoreNegativeIds = true);
+			const std::multimap<int, T> & wordsA,
+			const std::multimap<int, T> & wordsB,
+			std::list<std::pair<int, std::pair<T, T> > > & pairs,
+			bool ignoreNegativeIds = true)
+	{
+		const std::list<int> & ids = uUniqueKeys(wordsA);
+		typename std::multimap<int, T>::const_iterator iterA;
+		typename std::multimap<int, T>::const_iterator iterB;
+		pairs.clear();
+		int realPairsCount = 0;
+		for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
+		{
+			if(!ignoreNegativeIds || (ignoreNegativeIds && *i >= 0))
+			{
+				iterA = wordsA.find(*i);
+				iterB = wordsB.find(*i);
+				while(iterA != wordsA.end() && iterB != wordsB.end() && (*iterA).first == (*iterB).first && (*iterA).first == *i)
+				{
+					pairs.push_back(std::pair<int, std::pair<T, T> >(*i, std::make_pair((*iterA).second, (*iterB).second)));
+					++iterA;
+					++iterB;
+					++realPairsCount;
+				}
+			}
+		}
+		return realPairsCount;
+	}

 	/**
 	 * if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(2,2) (4,4)]
 	 * realPairsCount = 5
 	 */
+	template<typename T>
 	static int findPairsUnique(
-			const std::multimap<int, cv::KeyPoint> & wordsA,
-			const std::multimap<int, cv::KeyPoint> & wordsB,
-			std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
-			bool ignoreNegativeIds = true);
+			const std::multimap<int, T> & wordsA,
+			const std::multimap<int, T> & wordsB,
+			std::list<std::pair<int, std::pair<T, T> > > & pairs,
+			bool ignoreNegativeIds = true)
+	{
+		const std::list<int> & ids = uUniqueKeys(wordsA);
+		int realPairsCount = 0;
+		pairs.clear();
+		for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
+		{
+			if(!ignoreNegativeIds || (ignoreNegativeIds && *i>=0))
+			{
+				std::list<T> ptsA = uValues(wordsA, *i);
+				std::list<T> ptsB = uValues(wordsB, *i);
+				if(ptsA.size() == 1 && ptsB.size() == 1)
+				{
+					pairs.push_back(std::pair<int, std::pair<T, T> >(*i, std::pair<T, T>(ptsA.front(), ptsB.front())));
+					++realPairsCount;
+				}
+				else if(ptsA.size()>1 && ptsB.size()>1)
+				{
+					// just update the count
+					realPairsCount += ptsA.size() > ptsB.size() ? ptsB.size() : ptsA.size();
+				}
+			}
+		}
+		return realPairsCount;
+	}

 	/**
 	 * if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(1,1a) (1,1b) (2,2) (4,4) (6a,6a) (6a,6b) (6b,6a) (6b,6b)]
 	 * realPairsCount = 5
 	 */
+	template<typename T>
 	static int findPairsAll(
-			const std::multimap<int, cv::KeyPoint> & wordsA,
-			const std::multimap<int, cv::KeyPoint> & wordsB,
-			std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
-			bool ignoreNegativeIds = true);
+			const std::multimap<int, T> & wordsA,
+			const std::multimap<int, T> & wordsB,
+			std::list<std::pair<int, std::pair<T, T> > > & pairs,
+			bool ignoreNegativeIds = true)
+	{
+		const std::list<int> & ids = uUniqueKeys(wordsA);
+		pairs.clear();
+		int realPairsCount = 0;;
+		for(std::list<int>::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter)
+		{
+			if(!ignoreNegativeIds || (ignoreNegativeIds && *iter>=0))
+			{
+				std::list<T> ptsA = uValues(wordsA, *iter);
+				std::list<T> ptsB = uValues(wordsB, *iter);
+
+				realPairsCount += ptsA.size() > ptsB.size() ? ptsB.size() : ptsA.size();
+
+				for(typename std::list<T>::iterator jter=ptsA.begin(); jter!=ptsA.end(); ++jter)
+				{
+					for(typename std::list<T>::iterator kter=ptsB.begin(); kter!=ptsB.end(); ++kter)
+					{
+						pairs.push_back(std::pair<int, std::pair<T, T> >(*iter, std::pair<T, T>(*jter, *kter)));
+					}
+				}
+			}
+		}
+		return realPairsCount;
+	}

 	static cv::Mat linearLSTriangulation(
 			cv::Point3d u,    //homogenous image point (u,v,1)
@@ -61,8 +61,11 @@ typedef cv::gpu::FAST_GPU CV_FAST_GPU;
 namespace cv{
 namespace xfeatures2d {
 class FREAK;
+class DAISY;
 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)))
 class SIFT;
+#endif
 class SURF;
 }
 namespace cuda {
@@ -71,10 +74,15 @@ 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)))
 typedef cv::xfeatures2d::SIFT CV_SIFT;
+#else
+typedef cv::SIFT CV_SIFT; // SIFT is back in features2d since 4.4.0 / 3.4.11
+#endif
 typedef cv::xfeatures2d::SURF CV_SURF;
 typedef cv::FastFeatureDetector CV_FAST;
 typedef cv::xfeatures2d::FREAK CV_FREAK;
+typedef cv::xfeatures2d::DAISY CV_DAISY;
 typedef cv::GFTTDetector CV_GFTT;
 typedef cv::xfeatures2d::BriefDescriptorExtractor CV_BRIEF;
 typedef cv::BRISK CV_BRISK;
@@ -109,7 +117,48 @@ public:
 		kFeatureGfttOrb=8,  //new 0.10.11
 		kFeatureKaze=9,     //new 0.13.2
 		kFeatureOrbOctree=10, //new 0.19.2
-		kFeatureSuperPointTorch=11}; //new 0.19.7
+		kFeatureSuperPointTorch=11, //new 0.19.7
+		kFeatureSurfFreak=12, //new 0.20.4
+		kFeatureGfttDaisy=13, //new 0.20.6
+		kFeatureSurfDaisy=14};  //new 0.20.6
+
+	static std::string typeName(Type type)
+	{
+		switch(type){
+		case kFeatureSurf:
+			return "SURF";
+		case kFeatureSift:
+			return "SIFT";
+		case kFeatureOrb:
+			return "ORB";
+		case kFeatureFastFreak:
+			return "FAST+FREAK";
+		case kFeatureFastBrief:
+			return "FAST+BRIEF";
+		case kFeatureGfttFreak:
+			return "GFTT+Freak";
+		case kFeatureGfttBrief:
+			return "GFTT+Brief";
+		case kFeatureBrisk:
+			return "BRISK";
+		case kFeatureGfttOrb:
+			return "GFTT+ORB";
+		case kFeatureKaze:
+			return "KAZE";
+		case kFeatureOrbOctree:
+			return "ORB-OCTREE";
+		case kFeatureSuperPointTorch:
+			return "SUPERPOINT";
+		case kFeatureSurfFreak:
+			return "SURF+Freak";
+		case kFeatureGfttDaisy:
+			return "GFTT+Daisy";
+		case kFeatureSurfDaisy:
+			return "SURF+Daisy";
+		default:
+			return "Unknown";
+		}
+	}

 	static Feature2D * create(const ParametersMap & parameters = ParametersMap());
 	static Feature2D * create(Feature2D::Type type, const ParametersMap & parameters = ParametersMap()); // for convenience
@@ -146,6 +195,7 @@ public:
 	static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors, int maxKeypoints);
 	static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> & keypoints3D, cv::Mat & descriptors, int maxKeypoints);
 	static void limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints);
+	static void limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints, const cv::Size & imageSize, int gridRows, int gridCols);

 	static cv::Rect computeRoi(const cv::Mat & image, const std::string & roiRatios);
 	static cv::Rect computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios);
@@ -153,6 +203,8 @@ public:
 	int getMaxFeatures() const {return maxFeatures_;}
 	float getMinDepth() const {return _minDepth;}
 	float getMaxDepth() const {return _maxDepth;}
+	int getGridRows() const {return gridRows_;}
+	int getGridCols() const {return gridCols_;}

 public:
 	virtual ~Feature2D();
@@ -239,6 +291,7 @@ private:
 	double contrastThreshold_;
 	double edgeThreshold_;
 	double sigma_;
+	bool rootSIFT_;

 	cv::Ptr<CV_SIFT> _sift;
 };
@@ -414,6 +467,28 @@ private:
 	cv::Ptr<CV_FREAK> _freak;
 };

+//SURF_FREAK
+class RTABMAP_EXP SURF_FREAK : public SURF
+{
+public:
+	SURF_FREAK(const ParametersMap & parameters = ParametersMap());
+	virtual ~SURF_FREAK();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual Feature2D::Type getType() const {return kFeatureSurfFreak;}
+
+private:
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+
+private:
+	bool orientationNormalized_;
+	bool scaleNormalized_;
+	float patternScale_;
+	int nOctaves_;
+
+	cv::Ptr<CV_FREAK> _freak;
+};
+
 //GFTT_ORB
 class RTABMAP_EXP GFTT_ORB : public GFTT
 {
@@ -497,6 +572,8 @@ private:
 private:
 	float scaleFactor_;
 	int nLevels_;
+	int patchSize_;
+	int edgeThreshold_;
 	int fastThreshold_;
 	int fastMinThreshold_;

@@ -527,6 +604,54 @@ private:
 	bool cuda_;
 };

+//GFTT_DAISY
+class RTABMAP_EXP GFTT_DAISY : public GFTT
+{
+public:
+	GFTT_DAISY(const ParametersMap & parameters = ParametersMap());
+	virtual ~GFTT_DAISY();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual Feature2D::Type getType() const {return kFeatureGfttDaisy;}
+
+private:
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+
+private:
+	bool orientationNormalized_;
+	bool scaleNormalized_;
+	float patternScale_;
+	int nOctaves_;
+
+#if CV_MAJOR_VERSION > 2
+	cv::Ptr<CV_DAISY> _daisy;
+#endif
+};
+
+//SURF_DAISY
+class RTABMAP_EXP SURF_DAISY : public SURF
+{
+public:
+	SURF_DAISY(const ParametersMap & parameters = ParametersMap());
+	virtual ~SURF_DAISY();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual Feature2D::Type getType() const {return kFeatureSurfDaisy;}
+
+private:
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+
+private:
+	bool orientationNormalized_;
+	bool scaleNormalized_;
+	float patternScale_;
+	int nOctaves_;
+
+#if CV_MAJOR_VERSION > 2
+	cv::Ptr<CV_DAISY> _daisy;
+#endif
+};
+
 }

 #endif /* FEATURES2D_H_ */
@@ -43,8 +43,8 @@ public:
 	void release();
 	unsigned int indexedFeatures() const;

-	// return KB
-	unsigned int memoryUsed() const;
+	// return Bytes
+	unsigned long memoryUsed() const;

 	// Note that useDistanceL1 doesn't have any effect if LSH is used
 	void buildLinearIndex(
@@ -74,7 +74,7 @@ public:
 	int featuresType() const {return featuresType_;}
 	int featuresDim() const {return featuresDim_;}

-	unsigned int addPoints(const cv::Mat & features);
+	std::vector<unsigned int> addPoints(const cv::Mat & features);

 	void removePoint(unsigned int index);

@@ -0,0 +1,59 @@
+/*
+Copyright (c) 2010-2020, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#pragma once
+
+#include <opencv2/core/core.hpp>
+
+namespace rtabmap
+{
+
+class GlobalDescriptor
+{
+
+public:
+	GlobalDescriptor(int type, const cv::Mat & data, const cv::Mat & info = cv::Mat()) :
+		type_(type),
+		info_(info),
+		data_(data)
+	{}
+	GlobalDescriptor() :
+		type_(-1) // Not set
+	{}
+	virtual ~GlobalDescriptor() {}
+
+	int type() const {return type_;}
+	const cv::Mat info() const {return info_;}
+	const cv::Mat data() const {return data_;}
+
+private:
+	int type_;
+	cv::Mat info_;
+	cv::Mat data_;
+};
+
+} // namespace rtabmap
@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Parameters.h>
 #include <rtabmap/core/Link.h>
 #include <rtabmap/core/GPS.h>
+#include <rtabmap/core/CameraModel.h>

 namespace rtabmap {
 class Memory;
@@ -55,10 +56,10 @@ namespace graph {

 bool RTABMAP_EXP importPoses(
 		const std::string & filePath,
-		int format, // 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame), 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe
+		int format, // 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame), 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe, 9=EuRoC MAV
 		std::map<int, Transform> & poses,
 		std::multimap<int, Link> * constraints = 0, // optional for formats 3 and 4
-		std::map<int, double> * stamps = 0); // optional for format 1
+		std::map<int, double> * stamps = 0); // optional for format 1 and 9

 bool RTABMAP_EXP exportGPS(
 		const std::string & filePath,
@@ -155,12 +156,20 @@ std::list<Link> RTABMAP_EXP findLinks(

 std::multimap<int, Link> RTABMAP_EXP filterDuplicateLinks(
 		const std::multimap<int, Link> & links);
+/**
+ * Return links not of type "filteredType". If inverted=true, return links of of type "filteredType".
+ */
 std::multimap<int, Link> RTABMAP_EXP filterLinks(
 		const std::multimap<int, Link> & links,
-		Link::Type filteredType);
+		Link::Type filteredType,
+		bool inverted = false);
+/**
+ * Return links not of type "filteredType". If inverted=true, return links of of type "filteredType".
+ */
 std::map<int, Link> RTABMAP_EXP filterLinks(
 		const std::map<int, Link> & links,
-		Link::Type filteredType);
+		Link::Type filteredType,
+		bool inverted = false);

 //Note: This assumes a coordinate system where X is forward, * Y is up, and Z is right.
 std::map<int, Transform> RTABMAP_EXP frustumPosesFiltering(
@@ -255,11 +264,26 @@ std::list<std::pair<int, Transform> > RTABMAP_EXP computePath(
 		float linearVelocity = 0.0f,   // m/sec
 		float angularVelocity = 0.0f); // rad/sec

+/**
+ * Get the nearest node of the target pose
+ * @param nodes the nodes to search for
+ * @param targetPose the target pose to search around
+ * @param distance squared distance of the nearest node found (optional)
+ * @return the node id.
+ */
 int RTABMAP_EXP findNearestNode(
 		const std::map<int, rtabmap::Transform> & nodes,
-		const rtabmap::Transform & targetPose);
+		const rtabmap::Transform & targetPose,
+		float * distance = 0);

-std::vector<int> RTABMAP_EXP findNearestNodes(
+/**
+ * Get K nearest nodes of the target pose
+ * @param nodes the nodes to search for
+ * @param targetPose the target pose to search around
+ * @param k number of nearest neighbors to search for
+ * @return the node ids with squared distance to target pose.
+ */
+std::map<int, float> RTABMAP_EXP findNearestNodes(
 		const std::map<int, rtabmap::Transform> & nodes,
 		const rtabmap::Transform & targetPose,
 		int k);
@@ -275,11 +299,20 @@ std::map<int, float> RTABMAP_EXP getNodesInRadius(
 		int nodeId,
 		const std::map<int, Transform> & nodes,
 		float radius);
+std::map<int, float> RTABMAP_EXP getNodesInRadius(
+		const Transform & targetPose,
+		const std::map<int, Transform> & nodes,
+		float radius);
 std::map<int, Transform> RTABMAP_EXP getPosesInRadius(
 		int nodeId,
 		const std::map<int, Transform> & nodes,
 		float radius,
 		float angle = 0.0f);
+std::map<int, Transform> RTABMAP_EXP getPosesInRadius(
+		const Transform & targetPose,
+		const std::map<int, Transform> & nodes,
+		float radius,
+		float angle = 0.0f);

 float RTABMAP_EXP computePathLength(
 		const std::vector<std::pair<int, Transform> > & path,
@@ -97,6 +97,7 @@ public:
 	float angleIncrement() const {return angleIncrement_;}
 	Transform localTransform() const {return localTransform_;}

+	bool empty() const {return data_.empty();}
 	bool isEmpty() const {return data_.empty();}
 	int size() const {return data_.cols;}
 	int dataType() const {return data_.type();}
@@ -50,6 +50,8 @@ private:
 	cv::Ptr<cv::aruco::DetectorParameters> detectorParams_;
 	float markerLength_;
 	float maxDepthError_;
+	float maxRange_;
+	float minRange_;
 	int dictionaryId_;
 	cv::Ptr<cv::aruco::Dictionary> dictionary_;
 #endif
@@ -93,7 +93,7 @@ public:
 	std::set<int> reactivateSignatures(const std::list<int> & ids, unsigned int maxLoaded, double & timeDbAccess);

 	int cleanup();
-	void saveStatistics(const Statistics & statistics);
+	void saveStatistics(const Statistics & statistics, bool saveWMState);
 	void savePreviewImage(const cv::Mat & image) const;
 	cv::Mat loadPreviewImage() const;
 	void saveOptimizedPoses(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const;
@@ -102,7 +102,7 @@ public:
 	cv::Mat load2DMap(float & xMin, float & yMin, float & cellSize) const;
 	void saveOptimizedMesh(
 			const cv::Mat & cloud,
-			const std::vector<std::vector<std::vector<unsigned int> > > & polygons = std::vector<std::vector<std::vector<unsigned int> > >(),      // Textures -> polygons -> vertices
+			const std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > & polygons = std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > >(),      // Textures -> polygons -> vertices
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords = std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > >(), // Textures -> uv coords for each vertex of the polygons
 #else
@@ -110,7 +110,7 @@ public:
 #endif
 			const cv::Mat & textures = cv::Mat()) const; // concatenated textures (assuming square textures with all same size)
 	cv::Mat loadOptimizedMesh(
-			std::vector<std::vector<std::vector<unsigned int> > > * polygons = 0,
+			std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > * polygons = 0,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords = 0,
 #else
@@ -197,16 +197,17 @@ public:
 			EnvSensors & sensors,
 			bool lookInDatabase = false) const;
 	cv::Mat getImageCompressed(int signatureId) const;
-	SensorData getNodeData(int nodeId, bool uncompressedData = false) const;
-	void getNodeWords(int nodeId,
-			std::multimap<int, cv::KeyPoint> & words,
-			std::multimap<int, cv::Point3f> & words3,
-			std::multimap<int, cv::Mat> & wordsDescriptors);
+	SensorData getNodeData(int locationId, bool images, bool scan, bool userData, bool occupancyGrid) const;
+	void getNodeWordsAndGlobalDescriptors(int nodeId,
+			std::multimap<int, int> & words,
+			std::vector<cv::KeyPoint> & wordsKpts,
+			std::vector<cv::Point3f> & words3,
+			cv::Mat & wordsDescriptors,
+			std::vector<GlobalDescriptor> & globalDescriptors) const;
 	void getNodeCalibration(int nodeId,
 			std::vector<CameraModel> & models,
-			StereoCameraModel & stereoModel);
-	SensorData getSignatureDataConst(int locationId, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
-	std::set<int> getAllSignatureIds() const;
+			StereoCameraModel & stereoModel) const;
+	std::set<int> getAllSignatureIds(bool ignoreChildren = true) const;
 	bool memoryChanged() const {return _memoryChanged;}
 	bool isIncremental() const {return _incrementalMemory;}
 	bool isLocalizationDataSaved() const {return _localizationDataSaved;}
@@ -225,6 +226,7 @@ public:
 	virtual void dumpMemory(std::string directory) const;
 	virtual void dumpSignatures(const char * fileNameSign, bool words3D) const;
 	void dumpDictionary(const char * fileNameRef, const char * fileNameDesc) const;
+	unsigned long getMemoryUsed() const; //Bytes

 	void generateGraph(const std::string & fileName, const std::set<int> & ids = std::set<int>());

@@ -302,6 +304,7 @@ private:
 	bool _badSignaturesIgnored;
 	bool _mapLabelsAdded;
 	bool _depthAsMask;
+	bool _stereoFromMotion;
 	int _imagePreDecimation;
 	int _imagePostDecimation;
 	bool _compressionParallelized;
@@ -104,6 +104,8 @@ public:
 	const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapObstacles() const {return assembledObstacles_;}
 	const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapEmptyCells() const {return assembledEmptyCells_;}

+	unsigned long getMemoryUsed() const;
+
 private:
 	ParametersMap parameters_;
 	int cloudDecimation_;
@@ -168,9 +168,6 @@ class RtabmapColorOcTree : public octomap::OccupancyOcTreeBase <RtabmapColorOcTr
  };

 class RTABMAP_EXP OctoMap {
-public:
-	static void HSVtoRGB(float *r, float *g, float *b, float h, float s, float v);
-
 public:
 	OctoMap(const ParametersMap & parameters);
 	OctoMap(float cellSize = 0.1f, float occupancyThr = 0.5f, bool fullUpdate = false, float updateError=0.01f);
@@ -78,6 +78,9 @@ public:
 	unsigned int framesProcessed() const {return framesProcessed_;}
 	bool imagesAlreadyRectified() const {return _imagesAlreadyRectified;}

+protected:
+	const std::map<double, Transform> & imus() const {return imus_;}
+
 private:
 	virtual Transform computeTransform(SensorData & data, const Transform & guess = Transform(), OdometryInfo * info = 0) = 0;

@@ -90,7 +93,7 @@ private:
 	bool _force3DoF;
 	bool _holonomic;
 	bool guessFromMotion_;
-	bool guessSmoothingDelay_;
+	float guessSmoothingDelay_;
 	int _filteringStrategy;
 	int _particleSize;
 	float _particleNoiseT;
@@ -109,6 +112,7 @@ private:
 	double previousStamp_;
 	std::list<std::pair<std::vector<float>, double> > previousVelocities_;
 	Transform velocityGuess_;
+	Transform imuLastTransform_;
 	Transform previousGroundTruthPose_;
 	float distanceTravelled_;
 	unsigned int framesProcessed_;
@@ -116,6 +120,7 @@ private:
 	std::vector<ParticleFilter *> particleFilters_;
 	cv::KalmanFilter kalmanFilter_;
 	StereoCameraModel stereoModel_;
+	std::map<double, Transform> imus_;

 protected:
 	Odometry(const rtabmap::ParametersMap & parameters);
@@ -56,6 +56,8 @@ public:
 		interval(0),
 		distanceTravelled(0.0f),
 		memoryUsage(0),
+		gravityRollError(0.0),
+		gravityPitchError(0.0),
 		type(0)
 	{}

@@ -84,6 +86,8 @@ public:
 		output.guessVelocity = guessVelocity;
 		output.distanceTravelled = distanceTravelled;
 		output.memoryUsage = memoryUsage;
+		output.gravityRollError = gravityRollError;
+		output.gravityPitchError = gravityPitchError;
 		output.type = type;
 		return output;
 	}
@@ -110,6 +114,8 @@ public:
 	Transform guessVelocity;
 	float distanceTravelled;
 	int memoryUsage; //MB
+	double gravityRollError;
+	double gravityPitchError;

 	int type;

@@ -32,6 +32,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
 #include "rtabmap/core/Version.h" // DLL export/import defines
 #include <rtabmap/utilite/UConversion.h>
+#include <opencv2/core/version.hpp>
+#include <opencv2/opencv_modules.hpp>
 #include <string>
 #include <map>

@@ -217,6 +219,7 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(Mem, BadSignaturesIgnored,        bool, false,    "Bad signatures are ignored.");
    RTABMAP_PARAM(Mem, InitWMWithAllNodes,          bool, false,    "Initialize the Working Memory with all nodes in Long-Term Memory. When false, it is initialized with nodes of the previous session.");
    RTABMAP_PARAM(Mem, DepthAsMask,                 bool, true,     "Use depth image as mask when extracting features for vocabulary.");
+    RTABMAP_PARAM(Mem, StereoFromMotion,            bool, false,    uFormat("Triangulate features without depth using stereo from motion (odometry). It would be ignored if %s is true and the feature detector used supports masking.", kMemDepthAsMask().c_str()));
    RTABMAP_PARAM(Mem, ImagePreDecimation,          int, 1,         "Image decimation (>=1) before features extraction.");
    RTABMAP_PARAM(Mem, ImagePostDecimation,         int, 1,         "Image decimation (>=1) of saved data in created signatures (after features extraction). Decimation is done from the original image.");
    RTABMAP_PARAM(Mem, CompressionParallelized,     bool, true,     "Compression of sensor data is multi-threaded.");
@@ -233,20 +236,17 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(Kp, IncrementalDictionary,    bool, true,   "");
    RTABMAP_PARAM(Kp, IncrementalFlann,         bool, true,   uFormat("When using FLANN based strategy, add/remove points to its index without always rebuilding the index (the index is built only when the dictionary increases of the factor \"%s\" in size).", kKpFlannRebalancingFactor().c_str()));
    RTABMAP_PARAM(Kp, FlannRebalancingFactor,   float, 2.0,   uFormat("Factor used when rebuilding the incremental FLANN index (see \"%s\"). Set <=1 to disable.", kKpIncrementalFlann().c_str()));
+    RTABMAP_PARAM(Kp, ByteToFloat,              bool, false,  uFormat("For %s=1, binary descriptors are converted to float by converting each byte to float instead of converting each bit to float. When converting bytes instead of bits, less memory is used and search is faster at the cost of slightly less accurate matching.", kKpNNStrategy().c_str()));
    RTABMAP_PARAM(Kp, MaxDepth,                 float, 0,     "Filter extracted keypoints by depth (0=inf).");
    RTABMAP_PARAM(Kp, MinDepth,                 float, 0,     "Filter extracted keypoints by depth.");
    RTABMAP_PARAM(Kp, MaxFeatures,              int, 500,     "Maximum features extracted from the images (0 means not bounded, <0 means no extraction).");
    RTABMAP_PARAM(Kp, BadSignRatio,             float, 0.5,   "Bad signature ratio (less than Ratio x AverageWordsPerImage = bad).");
    RTABMAP_PARAM(Kp, NndrRatio,                float, 0.8,   "NNDR ratio (A matching pair is detected, if its distance is closer than X times the distance of the second nearest neighbor.)");
-#ifndef RTABMAP_NONFREE
-#ifdef RTABMAP_OPENCV3
-    // OpenCV 3 without xFeatures2D module doesn't have BRIEF
-    RTABMAP_PARAM(Kp, DetectorStrategy,         int, 8,       "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint Torch.");
+#if CV_MAJOR_VERSION > 2 && !defined(HAVE_OPENCV_XFEATURES2D)
+    // OpenCV>2 without xFeatures2D module doesn't have BRIEF
+    RTABMAP_PARAM(Kp, DetectorStrategy,         int, 8,       "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY");
 #else
-    RTABMAP_PARAM(Kp, DetectorStrategy,         int, 6,       "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint Torch.");
-#endif
-#else
-    RTABMAP_PARAM(Kp, DetectorStrategy,         int, 6,       "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint Torch.");
+    RTABMAP_PARAM(Kp, DetectorStrategy,         int, 6,       "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY");
 #endif
    RTABMAP_PARAM(Kp, TfIdfLikelihoodUsed,      bool, true,   "Use of the td-idf strategy to compute the likelihood.");
    RTABMAP_PARAM(Kp, Parallelized,             bool, true,   "If the dictionary update and signature creation were parallelized.");
@@ -265,6 +265,7 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(DbSqlite3, JournalMode,  int, 3,           "0=DELETE, 1=TRUNCATE, 2=PERSIST, 3=MEMORY, 4=OFF (see sqlite3 doc : \"PRAGMA journal_mode\")");
    RTABMAP_PARAM(DbSqlite3, Synchronous,  int, 0,           "0=OFF, 1=NORMAL, 2=FULL (see sqlite3 doc : \"PRAGMA synchronous\")");
    RTABMAP_PARAM(DbSqlite3, TempStore,    int, 2,           "0=DEFAULT, 1=FILE, 2=MEMORY (see sqlite3 doc : \"PRAGMA temp_store\")");
+    RTABMAP_PARAM_STR(Db, TargetVersion,   "",               "Target database version for backward compatibility purpose. Only Major and minor versions are used and should be set (e.g., 0.19 vs 0.20 or 1.0 vs 2.0). Patch version is ignored (e.g., 0.20.1 and 0.20.3 will generate a 0.20 database).");

    // Keypoints descriptors/detectors
    RTABMAP_PARAM(SURF, Extended,          bool, false,  "Extended descriptor flag (true - use extended 128-element descriptors; false - use 64-element descriptors).");
@@ -280,6 +281,7 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(SIFT, ContrastThreshold, double, 0.04, "The contrast threshold used to filter out weak features in semi-uniform (low-contrast) regions. The larger the threshold, the less features are produced by the detector.");
    RTABMAP_PARAM(SIFT, EdgeThreshold,     double, 10,   "The threshold used to filter out edge-like features. Note that the its meaning is different from the contrastThreshold, i.e. the larger the edgeThreshold, the less features are filtered out (more features are retained).");
    RTABMAP_PARAM(SIFT, Sigma,             double, 1.6,  "The sigma of the Gaussian applied to the input image at the octave #0. If your image is captured with a weak camera with soft lenses, you might want to reduce the number.");
+    RTABMAP_PARAM(SIFT, RootSIFT,          bool,  false, "Apply RootSIFT normalization of the descriptors.");

    RTABMAP_PARAM(BRIEF, Bytes,            int, 32,      "Bytes is a length of descriptor in bytes. It can be equal 16, 32 or 64 bytes.");

@@ -299,9 +301,9 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(GFTT, UseHarrisDetector, bool, false,  "");
    RTABMAP_PARAM(GFTT, K,                 double, 0.04, "");

-    RTABMAP_PARAM(ORB, ScaleFactor,   float, 1.2,  "Pyramid decimation ratio, greater than 1. scaleFactor==2 means the classical pyramid, where each next level has 4x less pixels than the previous, but such a big scale factor will degrade feature matching scores dramatically. On the other hand, too close to 1 scale factor will mean that to cover certain scale range you will need more pyramid levels and so the speed will suffer.");
-    RTABMAP_PARAM(ORB, NLevels,       int, 8,      "The number of pyramid levels. The smallest level will have linear size equal to input_image_linear_size/pow(scaleFactor, nlevels).");
-    RTABMAP_PARAM(ORB, EdgeThreshold, int, 31,     "This is size of the border where the features are not detected. It should roughly match the patchSize parameter.");
+    RTABMAP_PARAM(ORB, ScaleFactor,   float, 2,  "Pyramid decimation ratio, greater than 1. scaleFactor==2 means the classical pyramid, where each next level has 4x less pixels than the previous, but such a big scale factor will degrade feature matching scores dramatically. On the other hand, too close to 1 scale factor will mean that to cover certain scale range you will need more pyramid levels and so the speed will suffer.");
+    RTABMAP_PARAM(ORB, NLevels,       int, 3,      "The number of pyramid levels. The smallest level will have linear size equal to input_image_linear_size/pow(scaleFactor, nlevels).");
+    RTABMAP_PARAM(ORB, EdgeThreshold, int, 19,     "This is size of the border where the features are not detected. It should roughly match the patchSize parameter.");
    RTABMAP_PARAM(ORB, FirstLevel,    int, 0,      "It should be 0 in the current implementation.");
    RTABMAP_PARAM(ORB, WTA_K,         int, 2,      "The number of points that produce each element of the oriented BRIEF descriptor. The default value 2 means the BRIEF where we take a random point pair and compare their brightnesses, so we get 0/1 response. Other possible values are 3 and 4. For example, 3 means that we take 3 random points (of course, those point coordinates are random, but they are generated from the pre-defined seed, so each element of BRIEF descriptor is computed deterministically from the pixel rectangle), find point of maximum brightness and output index of the winner (0, 1 or 2). Such output will occupy 2 bits, and therefore it will need a special variant of Hamming distance, denoted as NORM_HAMMING2 (2 bits per bin). When WTA_K=4, we take 4 random points to compute each bin (that will also occupy 2 bits with possible values 0, 1, 2 or 3).");
    RTABMAP_PARAM(ORB, ScoreType,     int, 0,      "The default HARRIS_SCORE=0 means that Harris algorithm is used to rank features (the score is written to KeyPoint::score and is used to retain best nfeatures features); FAST_SCORE=1 is alternative value of the parameter that produces slightly less stable keypoints, but it is a little faster to compute.");
@@ -324,11 +326,11 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(KAZE, NOctaveLayers,  int, 4,        "Default number of sublevels per scale level.");
    RTABMAP_PARAM(KAZE, Diffusivity,    int, 1,        "Diffusivity type: 0=DIFF_PM_G1, 1=DIFF_PM_G2, 2=DIFF_WEICKERT or 3=DIFF_CHARBONNIER.");

-    RTABMAP_PARAM_STR(SPTorch, ModelPath,              "", "[Required] Path to pre-trained weights Torch file of SuperPoint (*.pt).");
-    RTABMAP_PARAM(SPTorch, Threshold,         float,  0.200, "Detector response threshold to accept keypoint.");
-    RTABMAP_PARAM(SPTorch, NMS,               bool,  true, "If true, non-maximum suppression is applied to detected keypoints.");
-    RTABMAP_PARAM(SPTorch, MinDistance,       int,      4, uFormat("[%s=true] Minimum distance (pixels) between keypoints.", kSPTorchNMS().c_str()));
-    RTABMAP_PARAM(SPTorch, Cuda,              bool, false, "Use Cuda device for Torch, otherwise CPU device is used by default.");
+    RTABMAP_PARAM_STR(SuperPoint, ModelPath, "",           "[Required] Path to pre-trained weights Torch file of SuperPoint (*.pt).");
+    RTABMAP_PARAM(SuperPoint, Threshold,     float, 0.010, "Detector response threshold to accept keypoint.");
+    RTABMAP_PARAM(SuperPoint, NMS,           bool,  true,  "If true, non-maximum suppression is applied to detected keypoints.");
+    RTABMAP_PARAM(SuperPoint, NMSRadius,     int,  4,      uFormat("[%s=true] Minimum distance (pixels) between keypoints.", kSuperPointNMS().c_str()));
+    RTABMAP_PARAM(SuperPoint, Cuda,          bool, true,   "Use Cuda device for Torch, otherwise CPU device is used by default.");

    // BayesFilter
    RTABMAP_PARAM(Bayes, VirtualPlacePriorThr, float, 0.9,  "Virtual place prior");
@@ -378,6 +380,7 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(RGBD, ProximityPathMaxNeighbors,    int, 0,      "Maximum neighbor nodes compared on each path. Set to 0 to disable merging the laser scans.");
    RTABMAP_PARAM(RGBD, ProximityPathRawPosesUsed,    bool, true,  "When comparing to a local path, merge the scan using the odometry poses (with neighbor link optimizations) instead of the ones in the optimized local graph.");
    RTABMAP_PARAM(RGBD, ProximityAngle,               float, 45,   "Maximum angle (degrees) for visual proximity detection.");
+    RTABMAP_PARAM(RGBD, ProximityOdomGuess,           bool, false, "Use odometry as motion guess for visual proximity detection.");

    // Graph optimization
 #ifdef RTABMAP_GTSAM
@@ -577,21 +580,17 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(Vis, PnPRefineIterations,      int, 1,        uFormat("[%s = 1] Refine iterations. Set to 0 if \"%s\" is also used.", kVisEstimationType().c_str(), kVisBundleAdjustment().c_str()));
 #endif

-    RTABMAP_PARAM(Vis, EpipolarGeometryVar,      float, 0.02,   uFormat("[%s = 2] Epipolar geometry maximum variance to accept the transformation.", kVisEstimationType().c_str()));
+    RTABMAP_PARAM(Vis, EpipolarGeometryVar,      float, 0.1,    uFormat("[%s = 2] Epipolar geometry maximum variance to accept the transformation.", kVisEstimationType().c_str()));
    RTABMAP_PARAM(Vis, MinInliers,               int, 20,       "Minimum feature correspondences to compute/accept the transformation.");
    RTABMAP_PARAM(Vis, MeanInliersDistance,      float, 0.0,    "Maximum distance (m) of the mean distance of inliers from the camera to accept the transformation. 0 means disabled.");
    RTABMAP_PARAM(Vis, MinInliersDistribution,   float, 0.0,    "Minimum distribution value of the inliers in the image to accept the transformation. The distribution is the second eigen value of the PCA (Principal Component Analysis) on the keypoints of the normalized image [-0.5, 0.5]. The value would be between 0 and 0.5. 0 means disabled.");

    RTABMAP_PARAM(Vis, Iterations,               int, 300,      "Maximum iterations to compute the transform.");
-#ifndef RTABMAP_NONFREE
-#ifdef RTABMAP_OPENCV3
-    // OpenCV 3 without xFeatures2D module doesn't have BRIEF
-    RTABMAP_PARAM(Vis, FeatureType, int, 8, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint Torch.");
+#if CV_MAJOR_VERSION > 2 && !defined(HAVE_OPENCV_XFEATURES2D)
+    // OpenCV>2 without xFeatures2D module doesn't have BRIEF
+    RTABMAP_PARAM(Vis, FeatureType, int, 8, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY");
 #else
-    RTABMAP_PARAM(Vis, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint Torch.");
-#endif
-#else
-    RTABMAP_PARAM(Vis, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint Torch.");
+    RTABMAP_PARAM(Vis, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY");
 #endif
    RTABMAP_PARAM(Vis, MaxFeatures,              int, 1000,   "0 no limits.");
    RTABMAP_PARAM(Vis, MaxDepth,                  float, 0,   "Max depth of the features (0 means no limit).");
@@ -604,9 +603,8 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(Vis, GridRows,                 int, 1,      uFormat("Number of rows of the grid used to extract uniformly \"%s / grid cells\" features from each cell.", kVisMaxFeatures().c_str()));
    RTABMAP_PARAM(Vis, GridCols,                 int, 1,      uFormat("Number of columns of the grid used to extract uniformly \"%s / grid cells\" features from each cell.", kVisMaxFeatures().c_str()));
    RTABMAP_PARAM(Vis, CorType,                  int, 0,      "Correspondences computation approach: 0=Features Matching, 1=Optical Flow");
-    RTABMAP_PARAM(Vis, CorNNType,                int, 1,    uFormat("[%s=0] kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4. Used for features matching approach.", kVisCorType().c_str()));
-    RTABMAP_PARAM(Vis, CorNNDR,                  float, 0.6,  uFormat("[%s=0] NNDR: nearest neighbor distance ratio. Used for knn features matching approach.", kVisCorType().c_str()));
-    RTABMAP_PARAM(Vis, CorCrossCheck,            bool, false,  uFormat("[%s=0] If true, brute force crosscheck matching is done instead of knn matching approach (%s).", kVisCorType().c_str(), kVisCorNNDR().c_str()));
+    RTABMAP_PARAM(Vis, CorNNType,                int, 1,    uFormat("[%s=0] kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4, BruteForceCrossCheck=5, SuperGlue=6, GMS=7. Used for features matching approach.", kVisCorType().c_str()));
+    RTABMAP_PARAM(Vis, CorNNDR,                  float, 0.8,  uFormat("[%s=0] NNDR: nearest neighbor distance ratio. Used for knn features matching approach.", kVisCorType().c_str()));
    RTABMAP_PARAM(Vis, CorGuessWinSize,          int, 20,     uFormat("[%s=0] Matching window size (pixels) around projected points when a guess transform is provided to find correspondences. 0 means disabled.", kVisCorType().c_str()));
    RTABMAP_PARAM(Vis, CorGuessMatchToProjection, bool, false, uFormat("[%s=0] Match frame's corners to source's projected points (when guess transform is provided) instead of projected points to frame's corners.", kVisCorType().c_str()));
    RTABMAP_PARAM(Vis, CorFlowWinSize,           int, 16,     uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
@@ -619,6 +617,17 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(Vis, BundleAdjustment,         int, 0,      "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres.");
 #endif

+    // Features matching approaches
+    RTABMAP_PARAM_STR(PyMatcher, Path,       "",           "Path to python script file (see available ones in rtabmap/corelib/src/pymatcher/*). See the header to see where the script should be copied.");
+	RTABMAP_PARAM(PyMatcher, Iterations,     int, 20,      "Sinkhorn iterations. Used by SuperGlue.");
+	RTABMAP_PARAM(PyMatcher, Threshold,      float, 0.2,   "Used by SuperGlue.");
+	RTABMAP_PARAM(PyMatcher, Cuda,           bool, true,   "Used by SuperGlue.");
+	RTABMAP_PARAM_STR(PyMatcher, Model,        "indoor",   "For SuperGlue, set only \"indoor\" or \"outdoor\". For OANet, set path to one of the pth file (e.g., \"OANet/model/gl3d/sift-4000/model_best.pth\").");
+
+	RTABMAP_PARAM(GMS, WithRotation,         bool, false,   "Take rotation transformation into account.");
+	RTABMAP_PARAM(GMS, WithScale,            bool, false,   "Take scale transformation into account.");
+	RTABMAP_PARAM(GMS, ThresholdFactor,      double, 6.0,   "The higher, the less matches.");
+
    // ICP registration parameters
    RTABMAP_PARAM(Icp, MaxTranslation,            float, 0.2,   "Maximum ICP translation correction accepted (m).");
    RTABMAP_PARAM(Icp, MaxRotation,               float, 0.78,  "Maximum ICP rotation correction accepted (rad).");
@@ -635,13 +644,15 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(Icp, Epsilon,                   float, 0,     "Set the transformation epsilon (maximum allowable difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution.");
    RTABMAP_PARAM(Icp, CorrespondenceRatio,       float, 0.1,   "Ratio of matching correspondences to accept the transform.");
 #ifdef RTABMAP_POINTMATCHER
-    RTABMAP_PARAM(Icp, PointToPlane,              bool, true,   "Use point to plane ICP.");
+    RTABMAP_PARAM(Icp, PointToPlane,                bool, true,   "Use point to plane ICP.");
 #else
-    RTABMAP_PARAM(Icp, PointToPlane,              bool, false,  "Use point to plane ICP.");
+    RTABMAP_PARAM(Icp, PointToPlane,                bool, false,  "Use point to plane ICP.");
 #endif
-    RTABMAP_PARAM(Icp, PointToPlaneK,             int, 5,       "Number of neighbors to compute normals for point to plane if the cloud doesn't have already normals.");
-    RTABMAP_PARAM(Icp, PointToPlaneRadius,        float, 1.0,   "Search radius to compute normals for point to plane if the cloud doesn't have already normals.");
-    RTABMAP_PARAM(Icp, PointToPlaneMinComplexity, float, 0.02,  "Minimum structural complexity (0.0=low, 1.0=high) of the scan to do point to plane registration, otherwise point to point registration is done instead.");
+    RTABMAP_PARAM(Icp, PointToPlaneK,               int, 5,       "Number of neighbors to compute normals for point to plane if the cloud doesn't have already normals.");
+    RTABMAP_PARAM(Icp, PointToPlaneRadius,          float, 1.0,   "Search radius to compute normals for point to plane if the cloud doesn't have already normals.");
+    RTABMAP_PARAM(Icp, PointToPlaneGroundNormalsUp, float, 0.0,   "Invert normals on ground if they are pointing down (useful for ring-like 3D LiDARs). 0 means disabled, 1 means only normals perfectly aligned with -z axis. This is only done with 3D scans.");
+    RTABMAP_PARAM(Icp, PointToPlaneMinComplexity,   float, 0.02,  uFormat("Minimum structural complexity (0.0=low, 1.0=high) of the scan to do PointToPlane registration, otherwise PointToPoint registration is done instead and strategy from %s is used. This check is done only when %s=true.", kIcpPointToPlaneLowComplexityStrategy().c_str(), kIcpPointToPlane().c_str()));
+    RTABMAP_PARAM(Icp, PointToPlaneLowComplexityStrategy, int, 1, uFormat("If structural complexity is below %s: set to 0 to so that the transform is automatically rejected, set to 1 to limit ICP correction in axes with most constraints (e.g., for a corridor-like environment, the resulting transform will be limited in y and yaw, x will taken from the guess), set to 2 to accept \"as is\" the transform computed by PointToPoint.", kIcpPointToPlaneMinComplexity().c_str()));

    // libpointmatcher
 #ifdef RTABMAP_POINTMATCHER
@@ -652,6 +663,7 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM_STR(Icp, PMConfig,             "",            uFormat("Configuration file (*.yaml) used by libpointmatcher. Note that data filters set for libpointmatcher are done after filtering done by rtabmap (i.e., %s, %s), so make sure to disable those in rtabmap if you want to use only those from libpointmatcher. Parameters %s, %s and %s are also ignored if configuration file is set.", kIcpVoxelSize().c_str(), kIcpDownsamplingStep().c_str(), kIcpIterations().c_str(), kIcpEpsilon().c_str(), kIcpMaxCorrespondenceDistance().c_str()).c_str());
    RTABMAP_PARAM(Icp, PMMatcherKnn,             int, 1,        "KDTreeMatcher/knn: number of nearest neighbors to consider it the reference. For convenience when configuration file is not set.");
    RTABMAP_PARAM(Icp, PMMatcherEpsilon,         float, 0.0,    "KDTreeMatcher/epsilon: approximation to use for the nearest-neighbor search. For convenience when configuration file is not set.");
+    RTABMAP_PARAM(Icp, PMMatcherIntensity,       bool, false,   uFormat("KDTreeMatcher:  among nearest neighbors, keep only the one with the most similar intensity. This only work with %s>1.", kIcpPMMatcherKnn().c_str()));
    RTABMAP_PARAM(Icp, PMOutlierRatio,           float, 0.95,   "TrimmedDistOutlierFilter/ratio: For convenience when configuration file is not set. For kinect-like point cloud, use 0.65.");

    // Stereo disparity
@@ -745,6 +757,8 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(Marker, VarianceLinear,         float, 0.001, "Linear variance to set on marker detections.");
    RTABMAP_PARAM(Marker, VarianceAngular,        float, 0.01,  "Angular variance to set on marker detections. Set to >=9999 to use only position (xyz) constraint in graph optimization.");
    RTABMAP_PARAM(Marker, CornerRefinementMethod, int,   0,     "Corner refinement method (0: None, 1: Subpixel, 2:contour, 3: AprilTag2). For OpenCV <3.3.0, this is \"doCornerRefinement\" parameter: set 0 for false and 1 for true.");
+    RTABMAP_PARAM(Marker, MaxRange,               float, 0.0,    "Maximum range in which markers will be detected. <=0 for unlimited range.");
+    RTABMAP_PARAM(Marker, MinRange,               float, 0.0,    "Miniminum range in which markers will be detected. <=0 for unlimited range.");

    RTABMAP_PARAM(ImuFilter, MadgwickGain,                  double, 0.1,  "Gain of the filter. Higher values lead to faster convergence but more noise. Lower values lead to slower convergence but smoother signal, belongs in [0, 1].");
    RTABMAP_PARAM(ImuFilter, MadgwickZeta,                  double, 0.0,  "Gyro drift gain (approx. rad/s), belongs in [-1, 1].");
@@ -832,4 +846,3 @@ private:
 }

 #endif /* PARAMETERS_H_ */
-
@@ -69,11 +69,14 @@ private:
 	bool _pointToPlane;
 	int _pointToPlaneK;
 	float _pointToPlaneRadius;
+	float _pointToPlaneGroundNormalsUp;
 	float _pointToPlaneMinComplexity;
+	int _pointToPlaneLowComplexityStrategy;
 	bool _libpointmatcher;
 	std::string _libpointmatcherConfig;
 	int _libpointmatcherKnn;
 	float _libpointmatcherEpsilon;
+	bool _libpointmatcherIntensity;
 	float _libpointmatcherOutlierRatio;
 	void * _libpointmatcherICP;
 };
@@ -75,6 +75,7 @@ public:

 	// RegistrationVis
 	int inliers;
+	float inliersRatio;
 	float inliersMeanDistance;
 	float inliersDistribution;
 	std::vector<int> inliersIDs;
@@ -37,6 +37,10 @@ namespace rtabmap {

 class Feature2D;

+#ifdef RTABMAP_PYMATCHER
+class PyMatcher;
+#endif
+
 // Visual registration
 class RTABMAP_EXP RegistrationVis : public Registration
 {
@@ -50,6 +54,11 @@ public:
 	float getInlierDistance() const {return _inlierDistance;}
 	int getIterations() const {return _iterations;}
 	int getMinInliers() const {return _minInliers;}
+	int getNNType() const {return _nnType;}
+	float getNNDR() const {return _nndr;}
+	int getEstimationType() const {return _estimationType;}
+
+	const Feature2D * getDetector() const {return _detectorFrom;}

 protected:
 	virtual Transform computeTransformationImpl(
@@ -78,8 +87,11 @@ private:
 	int _flowIterations;
 	float _flowEps;
 	int _flowMaxLevel;
-	bool _bfCrossCheck;
 	float _nndr;
+	int _nnType;
+	bool _gmsWithRotation;
+	bool _gmsWithScale;
+	double _gmsThresholdFactor;
 	int _guessWinSize;
 	bool _guessMatchToProjection;
 	int _bundleAdjustment;
@@ -92,6 +104,10 @@ private:

 	Feature2D * _detectorFrom;
 	Feature2D * _detectorTo;
+
+#ifdef RTABMAP_PYMATCHER
+	PyMatcher * _pyMatcher;
+#endif
 };

 }
@@ -86,8 +86,32 @@ public:
 			const cv::Mat & image,
 			int id=0, const std::map<std::string, float> & externalStats = std::map<std::string, float>());

-	void init(const ParametersMap & parameters, const std::string & databasePath = "");
-	void init(const std::string & configFile = "", const std::string & databasePath = "");
+	/**
+	 * Initialize Rtabmap with parameters and a database
+	 * @param parameters Parameters overriding default parameters and database parameters
+	 *                   (@see loadDatabaseParameters)
+	 * @param databasePath The database input/output path. If not set, an
+	 *                     empty database is used in RAM. If set and the file doesn't exist,
+	 *                     it will be created empty. If the database exists, nodes and
+	 *                     vocabulary will be loaded in working memory.
+	 * @param loadDatabaseParameters If an existing database is used (@see databasePath),
+	 *                               the parameters inside are loaded and set to current
+	 *                               Rtabmap instance.
+	 */
+	void init(const ParametersMap & parameters, const std::string & databasePath = "", bool loadDatabaseParameters = false);
+	/**
+	 * Initialize Rtabmap with parameters from a configuration file and a database
+	 * @param configFile Configuration file (*.ini) overriding default parameters and database parameters
+	 *                   (@see loadDatabaseParameters)
+	 * @param databasePath The database input/output path. If not set, an
+	 *                     empty database is used in RAM. If set and the file doesn't exist,
+	 *                     it will be created empty. If the database exists, nodes and
+	 *                     vocabulary will be loaded in working memory.
+	 * @param loadDatabaseParameters If an existing database is used (@see databasePath),
+	 *                               the parameters inside are loaded and set to current
+	 *                               Rtabmap instance.
+	 */
+	void init(const std::string & configFile = "", const std::string & databasePath = "", bool loadDatabaseParameters = false);

 	/**
 	 * Close rtabmap. This will delete rtabmap object if set.
@@ -112,11 +136,9 @@ public:
 	std::map<int, int> getWeights() const;
 	int getTotalMemSize() const;
 	double getLastProcessTime() const {return _lastProcessTime;};
-	std::multimap<int, cv::KeyPoint> getWords(int locationId) const;
 	bool isInSTM(int locationId) const;
 	bool isIDsGenerated() const;
 	const Statistics & getStatistics() const;
-	//bool getMetricData(int locationId, cv::Mat & rgb, cv::Mat & depth, float & depthConstant, Transform & pose, Transform & localTransform) const;
 	const std::map<int, Transform> & getLocalOptimizedPoses() const {return _optimizedPoses;}
 	const std::multimap<int, Link> & getLocalConstraints() const {return _constraints;}
 	Transform getPose(int locationId) const;
@@ -156,16 +178,26 @@ public:
 	void rejectLastLoopClosure();
 	void deleteLastLocation();
 	void setOptimizedPoses(const std::map<int, Transform> & poses);
-	void get3DMap(std::map<int, Signature> & signatures,
-			std::map<int, Transform> & poses,
-			std::multimap<int, Link> & constraints,
-			bool optimized,
-			bool global) const;
+	Signature getSignatureCopy(int id, bool images, bool scan, bool userData, bool occupancyGrid, bool withWords, bool withGlobalDescriptors) const;
+	RTABMAP_DEPRECATED(
+		void get3DMap(std::map<int, Signature> & signatures,
+				std::map<int, Transform> & poses,
+				std::multimap<int, Link> & constraints,
+				bool optimized,
+				bool global) const, "Use getGraph() instead with withImages=true, withScan=true, withUserData=true and withGrid=true.");
 	void getGraph(std::map<int, Transform> & poses,
 			std::multimap<int, Link> & constraints,
 			bool optimized,
 			bool global,
-			std::map<int, Signature> * signatures = 0);
+			std::map<int, Signature> * signatures = 0,
+			bool withImages = false,
+			bool withScan = false,
+			bool withUserData = false,
+			bool withGrid = false,
+			bool withWords = true,
+			bool withGlobalDescriptors = true) const;
+	std::map<int, Transform> getNodesInRadius(const Transform & pose, float radius); // If radius=0, RGBD/LocalRadius is used. Can return landmarks.
+	std::map<int, Transform> getNodesInRadius(int nodeId, float radius); // If nodeId==0, return poses around latest node. If radius=0, RGBD/LocalRadius is used. Can return landmarks and use landmark id (negative) as request.
 	int detectMoreLoopClosures(
 			float clusterRadius = 0.5f,
 			float clusterAngle = M_PI/6.0f,
@@ -257,6 +289,7 @@ private:
 	float _proximityFilteringRadius;
 	bool _proximityRawPosesUsed;
 	float _proximityAngle;
+	bool _proximityOdomGuess;
 	std::string _databasePath;
 	bool _optimizeFromGraphEnd;
 	float _optimizationMaxError;
@@ -277,6 +310,8 @@ private:
 	double _lastProcessTime;
 	bool _someNodesHaveBeenTransferred;
 	float _distanceTravelled;
+	float _distanceTravelledSinceLastLocalization;
+	bool _optimizeFromGraphEndChanged;

 	// Abstract classes containing all loop closure
 	// strategies for a type of signature or configuration.
@@ -306,6 +341,8 @@ private:
 	bool _currentSessionHasGPS;
 	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;
@@ -40,6 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/GPS.h>
 #include <rtabmap/core/EnvSensor.h>
 #include <rtabmap/core/Landmark.h>
+#include <rtabmap/core/GlobalDescriptor.h>

 namespace rtabmap
 {
@@ -249,6 +250,11 @@ public:
 	const std::vector<cv::Point3f> & keypoints3D() const {return _keypoints3D;}
 	const cv::Mat & descriptors() const {return _descriptors;}

+	void addGlobalDescriptor(const GlobalDescriptor & descriptor) {_globalDescriptors.push_back(descriptor);}
+	void setGlobalDescriptors(const std::vector<GlobalDescriptor> & descriptors) {_globalDescriptors = descriptors;}
+	void clearGlobalDescriptors() {_globalDescriptors.clear();}
+	const std::vector<GlobalDescriptor> & globalDescriptors() const {return _globalDescriptors;}
+
 	void setGroundTruth(const Transform & pose) {groundTruth_ = pose;}
 	const Transform & groundTruth() const {return groundTruth_;}

@@ -269,7 +275,7 @@ public:
 	void setLandmarks(const Landmarks & landmarks) {_landmarks = landmarks;}
 	const Landmarks & landmarks() const {return _landmarks;}

-	long getMemoryUsed() const; // Return memory usage in Bytes
+	unsigned long getMemoryUsed() const; // Return memory usage in Bytes
 	/**
 	 * Clear compressed rgb/depth (left/right) images, compressed laser scan and compressed user data.
 	 * Raw data are kept is set.
@@ -323,6 +329,9 @@ private:
 	std::vector<cv::Point3f> _keypoints3D;
 	cv::Mat _descriptors;

+	// global descriptors
+	std::vector<GlobalDescriptor> _globalDescriptors;
+
 	Transform groundTruth_;

 	Transform globalPose_;
@@ -104,19 +104,18 @@ public:

 	//visual words stuff
 	void removeAllWords();
-	void removeWord(int wordId);
 	void changeWordsRef(int oldWordId, int activeWordId);
-	void setWords(const std::multimap<int, cv::KeyPoint> & words);
+	void setWords(const std::multimap<int, int> & words, const std::vector<cv::KeyPoint> & keypoints, const std::vector<cv::Point3f> & words3, const cv::Mat & descriptors);
 	bool isEnabled() const {return _enabled;}
 	void setEnabled(bool enabled) {_enabled = enabled;}
-	const std::multimap<int, cv::KeyPoint> & getWords() const {return _words;}
+	const std::multimap<int, int> & getWords() const {return _words;}
+	const std::vector<cv::KeyPoint> & getWordsKpts() const {return _wordsKpts;}
 	int getInvalidWordsCount() const {return _invalidWordsCount;}
 	const std::map<int, int> & getWordsChanged() const {return _wordsChanged;}
-	const std::multimap<int, cv::Mat> & getWordsDescriptors() const {return _wordsDescriptors;}
-	void setWordsDescriptors(const std::multimap<int, cv::Mat> & descriptors) {_wordsDescriptors = descriptors;}
+	const cv::Mat & getWordsDescriptors() const {return _wordsDescriptors;}
+	void setWordsDescriptors(const cv::Mat & descriptors);

 	//metric stuff
-	void setWords3(const std::multimap<int, cv::Point3f> & words3) {_words3 = words3;}
 	void setPose(const Transform & pose) {_pose = pose;}
 	void setGroundTruthPose(const Transform & pose) {_groundTruthPose = pose;}
 	void setVelocity(float vx, float vy, float vz, float vroll, float vpitch, float vyaw) {
@@ -129,7 +128,7 @@ public:
 		_velocity[5]=vyaw;
 	}

-	const std::multimap<int, cv::Point3f> & getWords3() const {return _words3;}
+	const std::vector<cv::Point3f> & getWords3() const {return _words3;}
 	const Transform & getPose() const {return _pose;}
 	cv::Mat getPoseCovariance() const;
 	const Transform & getGroundTruthPose() const {return _groundTruthPose;}
@@ -138,7 +137,7 @@ public:
 	SensorData & sensorData() {return _sensorData;}
 	const SensorData & sensorData() const {return _sensorData;}

-	long getMemoryUsed(bool withSensorData=true) const; // Return memory usage in Bytes
+	unsigned long getMemoryUsed(bool withSensorData=true) const; // Return memory usage in Bytes

 private:
 	int _id;
@@ -155,9 +154,10 @@ private:
 	// Contains all words (Some can be duplicates -> if a word appears 2
 	// times in the signature, it will be 2 times in this list)
 	// Words match with the CvSeq keypoints and descriptors
-	std::multimap<int, cv::KeyPoint> _words; // word <id, keypoint>
-	std::multimap<int, cv::Point3f> _words3; // word <id, point> // in base_link frame (localTransform applied))
-	std::multimap<int, cv::Mat> _wordsDescriptors;
+	std::multimap<int, int> _words; // word <id, keypoint index>
+	std::vector<cv::KeyPoint> _wordsKpts;
+	std::vector<cv::Point3f> _words3; // in base_link frame (localTransform applied))
+	cv::Mat _wordsDescriptors;
 	std::map<int, int> _wordsChanged; // <oldId, newId>
 	bool _enabled;
 	int _invalidWordsCount;
@@ -52,16 +52,22 @@ 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,);
 	RTABMAP_STATS(Loop, Highest_hypothesis_id,);
 	RTABMAP_STATS(Loop, Highest_hypothesis_value,);
 	RTABMAP_STATS(Loop, Vp_hypothesis,);
 	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_inliers_ratio,);
 	RTABMAP_STATS(Loop, Visual_matches,);
+	RTABMAP_STATS(Loop, Distance_since_last_loc,);
 	RTABMAP_STATS(Loop, Last_id,);
 	RTABMAP_STATS(Loop, Optimization_max_error, m);
 	RTABMAP_STATS(Loop, Optimization_max_error_ratio, );
@@ -73,13 +79,40 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Loop, Landmark_detected_node_ref,);
 	RTABMAP_STATS(Loop, Visual_inliers_mean_dist,m);
 	RTABMAP_STATS(Loop, Visual_inliers_distribution,);
-	RTABMAP_STATS(Loop, Map_correction_norm, m);
-	RTABMAP_STATS(Loop, Map_correction_x, m);
-	RTABMAP_STATS(Loop, Map_correction_y, m);
-	RTABMAP_STATS(Loop, Map_correction_z, m);
-	RTABMAP_STATS(Loop, Map_correction_roll, deg);
-	RTABMAP_STATS(Loop, Map_correction_pitch, deg);
-	RTABMAP_STATS(Loop, Map_correction_yaw, deg);
+	//Odom correction
+	RTABMAP_STATS(Loop, Odom_correction_norm, m);
+	RTABMAP_STATS(Loop, Odom_correction_angle, deg);
+	RTABMAP_STATS(Loop, Odom_correction_x, m);
+	RTABMAP_STATS(Loop, Odom_correction_y, m);
+	RTABMAP_STATS(Loop, Odom_correction_z, m);
+	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);
+	RTABMAP_STATS(Loop, MapToOdom_x, m);
+	RTABMAP_STATS(Loop, MapToOdom_y, m);
+	RTABMAP_STATS(Loop, MapToOdom_z, m);
+	RTABMAP_STATS(Loop, MapToOdom_roll, deg);
+	RTABMAP_STATS(Loop, MapToOdom_pitch, deg);
+	RTABMAP_STATS(Loop, MapToOdom_yaw, deg);
+	// Map to Base
+	RTABMAP_STATS(Loop, MapToBase_x, m);
+	RTABMAP_STATS(Loop, MapToBase_y, m);
+	RTABMAP_STATS(Loop, MapToBase_z, m);
+	RTABMAP_STATS(Loop, MapToBase_roll, deg);
+	RTABMAP_STATS(Loop, MapToBase_pitch, deg);
+	RTABMAP_STATS(Loop, MapToBase_yaw, deg);

 	RTABMAP_STATS(Proximity, Time_detections,);
 	RTABMAP_STATS(Proximity, Space_last_detection_id,);
@@ -117,6 +150,7 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Memory, Odometry_variance_lin,);
 	RTABMAP_STATS(Memory, Distance_travelled, m);
 	RTABMAP_STATS(Memory, RAM_usage, MB);
+	RTABMAP_STATS(Memory, RAM_estimated, MB);
 	RTABMAP_STATS(Memory, Triangulated_points, );

 	RTABMAP_STATS(Timing, Memory_update, ms);
@@ -139,6 +173,7 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Timing, Joining_trash, ms);
 	RTABMAP_STATS(Timing, Emptying_trash, ms);
 	RTABMAP_STATS(Timing, Finalizing_statistics, ms);
+	RTABMAP_STATS(Timing, RAM_estimation, ms);

 	RTABMAP_STATS(TimingMem, Pre_update, ms);
 	RTABMAP_STATS(TimingMem, Signature_creation, ms);
@@ -159,6 +194,7 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(TimingMem, Markers_detection, ms);

 	RTABMAP_STATS(Keypoint, Dictionary_size, words);
+	RTABMAP_STATS(Keypoint, Current_frame, words);
 	RTABMAP_STATS(Keypoint, Indexed_words, words);
 	RTABMAP_STATS(Keypoint, Index_memory_usage, KB);

@@ -43,7 +43,7 @@ public:
 			const cv::Size & imageSize2,
 			const cv::Mat & K2, const cv::Mat & D2, const cv::Mat & R2, const cv::Mat & P2,
 			const cv::Mat & R, const cv::Mat & T, const cv::Mat & E, const cv::Mat & F,
-			const Transform & localTransform = Transform::getIdentity());
+			const Transform & localTransform = Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0));

 	// if R and T are not null, left and right camera models should be valid to be rectified.
 	StereoCameraModel(
@@ -68,7 +68,7 @@ public:
 			double cx,
 			double cy,
 			double baseline,
-			const Transform & localTransform = Transform::getIdentity(),
+			const Transform & localTransform = Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0),
 			const cv::Size & imageSize = cv::Size(0,0));
 	//minimal to be saved
 	StereoCameraModel(
@@ -78,7 +78,7 @@ public:
 			double cx,
 			double cy,
 			double baseline,
-			const Transform & localTransform = Transform::getIdentity(),
+			const Transform & localTransform = Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0),
 			const cv::Size & imageSize = cv::Size(0,0));
 	virtual ~StereoCameraModel() {}

@@ -98,6 +98,7 @@ public:

 	float theta() const;

+	bool isInvertible() const;
 	Transform inverse() const;
 	Transform rotation() const;
 	Transform translation() const;
@@ -140,6 +141,15 @@ public:
 	static Transform fromEigen3f(const Eigen::Isometry3f & matrix);
 	static Transform fromEigen3d(const Eigen::Isometry3d & matrix);

+	static Transform opengl_T_rtabmap() {return Transform(
+			 0.0f, -1.0f, 0.0f, 0.0f,
+			 0.0f,  0.0f, 1.0f, 0.0f,
+			-1.0f,  0.0f, 0.0f, 0.0f);}
+	static Transform rtabmap_T_opengl() {return Transform(
+			 0.0f,  0.0f,-1.0f, 0.0f,
+			-1.0f,  0.0f, 0.0f, 0.0f,
+			 0.0f,  1.0f, 0.0f, 0.0f);}
+
 	/**
 	 * Format (3 values): x y z
 	 * Format (6 values): x y z roll pitch yaw
@@ -55,6 +55,23 @@ public:
 		kNNUndef};
 	static const int ID_START;
 	static const int ID_INVALID;
+	static std::string nnStrategyName(NNStrategy strategy)
+	{
+		switch(strategy) {
+		case kNNFlannNaive:
+			return "FLANN NAIVE";
+		case kNNFlannKdTree:
+			return "FLANN KD-TREE";
+		case kNNFlannLSH:
+			return "FLANN LSH";
+		case kNNBruteForce:
+			return "BRUTE FORCE";
+		case kNNBruteForceGPU:
+			return "BRUTE FORCE GPU";
+		default:
+			return "Unknown";
+		}
+	}

 public:
 	VWDictionary(const ParametersMap & parameters = ParametersMap());
@@ -83,8 +100,9 @@ public:
 	int getLastIndexedWordId() const;
 	int getTotalActiveReferences() const {return _totalActiveReferences;}
 	unsigned int getIndexedWordsCount() const;
-	unsigned int getIndexMemoryUsed() const;
-	void setNNStrategy(NNStrategy strategy);
+	unsigned int getIndexMemoryUsed() const; // KB
+	unsigned long getMemoryUsed() const; //Bytes
+	bool setNNStrategy(NNStrategy strategy); // Return true if the search tree has been re-initialized
 	bool isIncremental() const {return _incrementalDictionary;}
 	bool isIncrementalFlann() const {return _incrementalFlann;}
 	void setIncrementalDictionary();
@@ -100,8 +118,8 @@ public:
 	void deleteUnusedWords();

 public:
-	static cv::Mat convertBinTo32F(const cv::Mat & descriptorsIn);
-	static cv::Mat convert32FToBin(const cv::Mat & descriptorsIn);
+	static cv::Mat convertBinTo32F(const cv::Mat & descriptorsIn, bool byteToFloat = true);
+	static cv::Mat convert32FToBin(const cv::Mat & descriptorsIn, bool byteToFloat = true);

 protected:
 	int getNextId();
@@ -114,6 +132,7 @@ private:
 	bool _incrementalDictionary;
 	bool _incrementalFlann;
 	float _rebalancingFactor;
+	bool _byteToFloat;
 	float _nndrRatio;
 	std::string _dictionaryPath; // a pre-computed dictionary (.txt or .db)
 	std::string _newDictionaryPath; // a pre-computed dictionary (.txt or .db)
@@ -43,6 +43,7 @@ public:

 	void addRef(int signatureId);
 	int removeAllRef(int signatureId);
+	unsigned long getMemoryUsed() const;

 	int getTotalReferences() const {return _totalReferences;}
 	int id() const {return _id;}
@@ -53,7 +53,7 @@ public:
 	CameraFreenect(int deviceId= 0,
 					Type type = kTypeColorDepth,
 					float imageRate=0.0f,
-					const Transform & localTransform = Transform::getIdentity());
+					const Transform & localTransform = CameraModel::opticalRotation());
 	virtual ~CameraFreenect();

 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
@@ -65,7 +65,7 @@ public:
 	CameraFreenect2(int deviceId= 0,
 					Type type = kTypeDepth2ColorSD,
 					float imageRate=0.0f,
-					const Transform & localTransform = Transform::getIdentity(),
+					const Transform & localTransform = CameraModel::opticalRotation(),
 					float minDepth = 0.3f,
 					float maxDepth = 12.0f,
 					bool bilateralFiltering = true,
@@ -46,7 +46,7 @@ public:
 	CameraImages(
 			const std::string & path,
 			float imageRate = 0,
-			const Transform & localTransform = Transform::getIdentity());
+			const Transform & localTransform = CameraModel::opticalRotation());
 	virtual ~CameraImages();

 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
@@ -74,6 +74,11 @@ public:
 		_syncImageRateWithStamps = syncImageRateWithStamps;
 	}

+	void setConfigForEachFrame(bool value)
+	{
+		_hasConfigForEachFrame = value;
+	}
+
 	void setScanPath(
 			const std::string & dir,
 			int maxScanPts = 0,
@@ -116,12 +121,14 @@ public:

 protected:
 	virtual SensorData captureImage(CameraInfo * info = 0);
+
+private:
 	bool readPoses(
-			std::list<Transform> & outputPoses,
-			std::list<double> & stamps,
-			const std::string & filePath,
-			int format,
-			double maxTimeDiff) const;
+		std::list<Transform> & outputPoses,
+		std::list<double> & stamps,
+		const std::string & filePath,
+		int format,
+		double maxTimeDiff) const;

 private:
 	std::string _path;
@@ -151,6 +158,7 @@ private:
 	bool _depthFromScanFillHolesFromBorder;

 	bool _filenamesAreTimestamps;
+	bool _hasConfigForEachFrame;
 	std::string _timestampsPath;
 	bool _syncImageRateWithStamps;

@@ -162,8 +170,10 @@ private:

 	std::list<double> _stamps;
 	std::list<Transform> odometry_;
+	std::list<cv::Mat> covariances_;
 	std::list<Transform> groundTruth_;
 	CameraModel _model;
+	std::list<CameraModel> _models;

 	UTimer _captureTimer;
 	double _captureDelay;
@@ -34,6 +34,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Version.h"
 #include "rtabmap/utilite/UTimer.h"

+#ifdef RTABMAP_K4A
+#include <k4a/k4atypes.h>
+#include <k4arecord/playback.h>
+#endif
+
 namespace rtabmap
 {

@@ -46,10 +51,10 @@ public:
 public:
 	CameraK4A(int deviceId = 0,
 		float imageRate = 0.0f,
-		const Transform & localTransform = Transform::getIdentity());
+		const Transform & localTransform = CameraModel::opticalRotation());
 	CameraK4A(const std::string & fileName,
 		float imageRate = 0.0f,
-		const Transform & localTransform = Transform::getIdentity());
+		const Transform & localTransform = CameraModel::opticalRotation());
 	virtual ~CameraK4A();

 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
@@ -57,6 +62,7 @@ public:
 	virtual std::string getSerial() const;

 	void setIRDepthFormat(bool enabled);
+	void setPreferences(int rgb_resolution, int framerate, int depth_resolution);

 protected:
 	virtual SensorData captureImage(CameraInfo * info = 0);
@@ -67,14 +73,24 @@ private:
 private:

 #ifdef RTABMAP_K4A
-	void* playbackHandle_;
-	void* transformationHandle_;
+	k4a_device_t deviceHandle_;
+	k4a_device_configuration_t config_;
+	k4a_calibration_t calibration_;
+	k4a_transformation_t transformationHandle_;
+	k4a_capture_t captureHandle_;
+	k4a_playback_t playbackHandle_;
+	std::string serial_number_;
+
 	CameraModel model_;
 	int deviceId_;
 	std::string fileName_;
+	int rgb_resolution_;
+	int framerate_;
+	int depth_resolution_;
 	bool ir_;
 	double previousStamp_;
 	UTimer timer_;
+	Transform imuLocalTransform_;
 #endif

 };
@@ -66,7 +66,7 @@ public:
 	CameraK4W2(int deviceId = 0, // not used
 		Type type = kTypeDepth2ColorSD,
 		float imageRate = 0.0f,
-		const Transform & localTransform = Transform::getIdentity());
+		const Transform & localTransform = CameraModel::opticalRotation());
 	virtual ~CameraK4W2();

 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
@@ -52,7 +52,7 @@ public:
 	static bool available();

 public:
-	CameraMyntEye(const std::string & device = "", bool apiRectification = false, bool apiDepth = false, float imageRate = 0, const Transform & localTransform = Transform::getIdentity());
+	CameraMyntEye(const std::string & device = "", bool apiRectification = false, bool apiDepth = false, float imageRate = 0, const Transform & localTransform = CameraModel::opticalRotation());
 	virtual ~CameraMyntEye();

 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
@@ -61,6 +61,9 @@ public:
 	virtual bool odomProvided() const { return false; }

 	void publishInterIMU(bool enabled);
+	void setAutoExposure();
+	void setManualExposure(int gain=24, int brightness=120, int constrast=116);
+	void setIrControl(int value);

 protected:
 	/**
@@ -80,6 +83,11 @@ private:
 	std::string deviceName_;
 	bool apiRectification_;
 	bool apiDepth_;
+	bool autoExposure_;
+	int gain_;
+	int brightness_;
+	int contrast_;
+	int irControl_;
 	USemaphore dataReady_;
 	UMutex dataMutex_;
 	cv::Mat leftFrameBuffer_;
@@ -94,7 +102,7 @@ private:

 	double softTimeBegin_;
 	std::uint64_t hardTimeBegin_;
-	std::uint64_t unitHardTime;
+	std::uint64_t unitHardTime_;
 	std::vector<std::uint64_t> lastHardTimes_;
 	std::vector<std::uint64_t> acc_;
 #endif
@@ -54,7 +54,7 @@ public:
 	CameraOpenNI2(const std::string & deviceId = "",
 					Type type = kTypeColorDepth,
 					float imageRate = 0,
-					const Transform & localTransform = Transform::getIdentity());
+					const Transform & localTransform = CameraModel::opticalRotation());
 	virtual ~CameraOpenNI2();

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

 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
@@ -66,13 +66,20 @@ public:
 	// default local transform z in, x right, y down));
 	CameraOpenni(const std::string & deviceId="",
 			float imageRate = 0,
-			const Transform & localTransform = Transform::getIdentity());
+			const Transform & localTransform = CameraModel::opticalRotation());
 	virtual ~CameraOpenni();
 #ifdef RTABMAP_OPENNI
+#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
+	void image_cb (
+			const std::shared_ptr<openni_wrapper::Image>& rgb,
+			const std::shared_ptr<openni_wrapper::DepthImage>& depth,
+			float constant);
+#else
    void image_cb (
    		const boost::shared_ptr<openni_wrapper::Image>& rgb,
 			const boost::shared_ptr<openni_wrapper::DepthImage>& depth,
 			float constant);
+#endif
 #endif

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

 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;

 	virtual void setStartIndex(int index) {CameraImages::setStartIndex(index);cameraDepth_.setStartIndex(index);} // negative means last
 	virtual void setMaxFrames(int value) {CameraImages::setMaxFrames(value);cameraDepth_.setMaxFrames(value);}
@@ -64,7 +64,7 @@ public:
 		int presetDepth = 0, // 0=best quality, 1=largest image, 2=highest framerate
 		bool computeOdometry = false,
 		float imageRate = 0,
-		const Transform & localTransform = Transform::getIdentity());
+		const Transform & localTransform = CameraModel::opticalRotation());
 	virtual ~CameraRealSense();

 	void setDepthScaledToRGBSize(bool enabled);
@@ -62,7 +62,7 @@ public:
 	CameraRealSense2(
 		const std::string & deviceId = "",
 		float imageRate = 0,
-		const Transform & localTransform = Transform::getIdentity());
+		const Transform & localTransform = CameraModel::opticalRotation());
 	virtual ~CameraRealSense2();

 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
@@ -75,6 +75,7 @@ public:
 	void setEmitterEnabled(bool enabled);
 	void setIRFormat(bool enabled, bool useDepthInsteadOfRightImage);
 	void setResolution(int width, int height, int fps = 30);
+	void setGlobalTimeSync(bool enabled);
 	void publishInterIMU(bool enabled);
 	void setDualMode(bool enabled, const Transform & extrinsics);
 	void setJsonConfig(const std::string & json);
@@ -93,7 +94,7 @@ private:
 			Transform & pose,
 			unsigned int & poseConfidence,
 			IMU & imu,
-			int maxWaitTimeMs = 35) const;
+			int maxWaitTimeMs = 35);
 #endif

 protected:
@@ -121,6 +122,7 @@ private:
 	UMutex imuMutex_;
 	double lastImuStamp_;
 	bool clockSyncWarningShown_;
+	bool imuGlobalSyncWarningShown_;

 	bool emitterEnabled_;
 	bool ir_;
@@ -130,10 +132,13 @@ private:
 	int cameraWidth_;
 	int cameraHeight_;
 	int cameraFps_;
+	bool globalTimeSync_;
 	bool publishInterIMU_;
 	bool dualMode_;
 	Transform dualExtrinsics_;
 	std::string jsonConfig_;
+	bool closing_;
+	bool isL500_;

 	static Transform realsense2PoseRotation_;
 	static Transform realsense2PoseRotationInv_;
@@ -45,7 +45,7 @@ public:
 	static bool available();

 public:
-	CameraStereoDC1394( float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity());
+	CameraStereoDC1394( float imageRate=0.0f, const Transform & localTransform = CameraModel::opticalRotation());
 	virtual ~CameraStereoDC1394();

 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
@@ -47,7 +47,7 @@ public:
 	static bool available();

 public:
-	CameraStereoFlyCapture2( float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity());
+	CameraStereoFlyCapture2( float imageRate=0.0f, const Transform & localTransform = CameraModel::opticalRotation());
 	virtual ~CameraStereoFlyCapture2();

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

 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
@@ -53,7 +53,7 @@ public:
 			int device,
 			bool rectifyImages = false,
 			float imageRate = 0.0f,
-			const Transform & localTransform = Transform::getIdentity());
+			const Transform & localTransform = CameraModel::opticalRotation());

 	virtual ~CameraStereoTara();

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

 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
@@ -57,7 +57,7 @@ public:
 			int confidenceThr = 100,
 			bool computeOdometry = false,
 			float imageRate=0.0f,
-			const Transform & localTransform = Transform::getIdentity(),
+			const Transform & localTransform = CameraModel::opticalRotation(),
 			bool selfCalibration = true,
 			bool odomForce3DoF = false,
 			int texturenessConfidenceThr = 90); // introduced with ZED SDK 3
@@ -68,7 +68,7 @@ public:
 			int confidenceThr = 100,
 			bool computeOdometry = false,
 			float imageRate=0.0f,
-			const Transform & localTransform = Transform::getIdentity(),
+			const Transform & localTransform = CameraModel::opticalRotation(),
 			bool selfCalibration = true,
 			bool odomForce3DoF = false,
 			int texturenessConfidenceThr = 90); // introduced with ZED SDK 3
@@ -45,11 +45,11 @@ public:
 	CameraVideo(int usbDevice = 0,
 			bool rectifyImages = false,
 			float imageRate = 0,
-			const Transform & localTransform = Transform::getIdentity());
+			const Transform & localTransform = CameraModel::opticalRotation());
 	CameraVideo(const std::string & filePath,
 			bool rectifyImages = false,
 			float imageRate = 0,
-			const Transform & localTransform = Transform::getIdentity());
+			const Transform & localTransform = CameraModel::opticalRotation());
 	virtual ~CameraVideo();

 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
@@ -97,11 +97,35 @@ void segmentObstaclesFromGround(
 			// cluster all surfaces for which the centroid is in the Z-range of the bigger surface
 			if(clusteredFlatSurfaces.size())
 			{
-				ground = clusteredFlatSurfaces.at(biggestFlatSurfaceIndex);
-				Eigen::Vector4f min,max;
-				pcl::getMinMax3D(*cloud, *clusteredFlatSurfaces.at(biggestFlatSurfaceIndex), min, max);
+				Eigen::Vector4f biggestSurfaceMin,biggestSurfaceMax;
+				if(maxGroundHeight != 0.0f)
+				{
+					// Search for biggest surface under max ground height
+					size_t points = 0;
+					biggestFlatSurfaceIndex = -1;
+					for(size_t i=0;i<clusteredFlatSurfaces.size();++i)
+					{
+						Eigen::Vector4f min,max;
+						pcl::getMinMax3D(*cloud, *clusteredFlatSurfaces.at(i), min, max);
+						if(min[2]<maxGroundHeight && clusteredFlatSurfaces.size() > points)
+						{
+							points = clusteredFlatSurfaces.at(i)->size();
+							biggestFlatSurfaceIndex = i;
+							biggestSurfaceMin = min;
+							biggestSurfaceMax = max;
+						}
+					}
+				}
+				else
+				{
+					pcl::getMinMax3D(*cloud, *clusteredFlatSurfaces.at(biggestFlatSurfaceIndex), biggestSurfaceMin, biggestSurfaceMax);
+				}
+				if(biggestFlatSurfaceIndex>=0)
+				{
+					ground = clusteredFlatSurfaces.at(biggestFlatSurfaceIndex);
+				}

-				if(maxGroundHeight == 0.0f || min[2] < maxGroundHeight)
+				if(!ground->empty() && (maxGroundHeight == 0.0f || biggestSurfaceMin[2] < maxGroundHeight))
 				{
 					for(unsigned int i=0; i<clusteredFlatSurfaces.size(); ++i)
 					{
@@ -109,7 +133,7 @@ void segmentObstaclesFromGround(
 						{
 							Eigen::Vector4f centroid(0,0,0,1);
 							pcl::compute3DCentroid(*cloud, *clusteredFlatSurfaces.at(i), centroid);
-							if(maxGroundHeight==0.0f || centroid[2] <= maxGroundHeight || centroid[2] <= max[2]) // epsilon
+							if(maxGroundHeight==0.0f || centroid[2] <= maxGroundHeight || centroid[2] <= biggestSurfaceMax[2]) // epsilon
 							{
 								ground = util3d::concatenate(ground, clusteredFlatSurfaces.at(i));
 							}
@@ -9,6 +9,7 @@
 #define CORELIB_INCLUDE_RTABMAP_CORE_IMPL_UTIL3D_SURFACE_HPP_

 #include <pcl/search/kdtree.h>
+#include <pcl/conversions.h>
 #include <rtabmap/utilite/UConversion.h>

 namespace rtabmap {
@@ -78,8 +78,7 @@ private:
 	Signature * map_;
 	Signature * lastFrame_;
 	int lastFrameOldestNewId_;
-	std::vector<std::pair<pcl::PointCloud<pcl::PointNormal>::Ptr, pcl::IndicesPtr> > scansBuffer_;
-	std::map<double, Transform> imus_;
+	std::vector<std::pair<pcl::PointCloud<pcl::PointXYZINormal>::Ptr, pcl::IndicesPtr> > scansBuffer_;
 	bool initGravity_;

 	std::map<int, std::map<int, FeatureBA> > bundleWordReferences_; //<WordId, <FrameId, pt2D+depth>>
@@ -29,11 +29,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define ODOMETRYMONO_H_

 #include <rtabmap/core/Odometry.h>
+#include <rtabmap/core/Link.h>

 namespace rtabmap {

 class Memory;
-class Stereo;
+class Feature2D;

 class RTABMAP_EXP OdometryMono : public Odometry
 {
@@ -41,6 +42,7 @@ public:
 	OdometryMono(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
 	virtual ~OdometryMono();
 	virtual void reset(const Transform & initialPose);
+	virtual Odometry::Type getType() {return kTypeUndef;}

 private:
 	virtual Transform computeTransform(SensorData & data, const Transform & guess = Transform(), OdometryInfo * info = 0);
@@ -56,7 +58,7 @@ private:
 	int pnpFlags_;
 	int pnpRefineIterations_;

-	Stereo * stereo_;
+	Feature2D * feature2D_;

 	Memory * memory_;
 	int localHistoryMaxSize_;
@@ -66,12 +68,14 @@ private:
 	float fundMatrixReprojError_;
 	float fundMatrixConfidence_;

-	cv::Mat refDepthOrRight_;
-	std::map<int, cv::Point2f> cornersMap_;
+	std::map<int, cv::Point2f> firstFrameGuessCorners_;
 	std::map<int, cv::Point3f> localMap_;
 	std::map<int, std::map<int, cv::Point3f> > keyFrameWords3D_;
 	std::map<int, Transform> keyFramePoses_;
+	std::multimap<int, Link> keyFrameLinks_;
+	std::map<int, CameraModel> keyFrameModels_;
 	float maxVariance_;
+	float keyFrameThr_;
 };

 }
@@ -37,10 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <opencv2/calib3d/calib3d_c.h>

 #if CV_MAJOR_VERSION >= 4
-
-#if CV_MINOR_VERSION >= 3
 #include <opencv2/core/core_c.h>
-#endif

 // Opencv4 doesn't expose those functions below anymore, we should recopy all of them!
 int cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian CV_DEFAULT(0))
@@ -1192,8 +1189,8 @@ void stereoRectifyFisheye( cv::InputArray _cameraMatrix1, cv::InputArray _distCo
    cv::Mat cameraMatrix1 = _cameraMatrix1.getMat(), cameraMatrix2 = _cameraMatrix2.getMat();
    cv::Mat distCoeffs1 = _distCoeffs1.getMat(), distCoeffs2 = _distCoeffs2.getMat();
    cv::Mat Rmat = _Rmat.getMat(), Tmat = _Tmat.getMat();
-
-#if CV_MAJOR_VERSION > 3 or (CV_MAJOR_VERSION >= 3 and (CV_MINOR_VERSION>4 or CV_MINOR_VERSION>=4  and CV_SUBMINOR_VERSION>=4))
+	
+#if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION >= 3 && (CV_MINOR_VERSION>4 || (CV_MINOR_VERSION>=4 && CV_SUBMINOR_VERSION>=4)))
    CvMat c_cameraMatrix1 = cvMat(cameraMatrix1);
    CvMat c_cameraMatrix2 = cvMat(cameraMatrix2);
    CvMat c_distCoeffs1 = cvMat(distCoeffs1);
@@ -1213,7 +1210,7 @@ void stereoRectifyFisheye( cv::InputArray _cameraMatrix1, cv::InputArray _distCo
    _Pmat1.create(3, 4, rtype);
    _Pmat2.create(3, 4, rtype);
    cv::Mat R1 = _Rmat1.getMat(), R2 = _Rmat2.getMat(), P1 = _Pmat1.getMat(), P2 = _Pmat2.getMat(), Q;
-#if CV_MAJOR_VERSION > 3 or (CV_MAJOR_VERSION >= 3 and (CV_MINOR_VERSION>4 or CV_MINOR_VERSION>=4  and CV_SUBMINOR_VERSION>=4))
+#if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION >= 3 && (CV_MINOR_VERSION>4 || (CV_MINOR_VERSION>=4 && CV_SUBMINOR_VERSION>=4)))
    CvMat c_R1 = cvMat(R1), c_R2 = cvMat(R2), c_P1 = cvMat(P1), c_P2 = cvMat(P2);
 #else
    CvMat c_R1 = CvMat(R1), c_R2 = CvMat(R2), c_P1 = CvMat(P1), c_P2 = CvMat(P2);
@@ -1223,7 +1220,7 @@ void stereoRectifyFisheye( cv::InputArray _cameraMatrix1, cv::InputArray _distCo
    if( _Qmat.needed() )
    {
        _Qmat.create(4, 4, rtype);
-#if CV_MAJOR_VERSION > 3 or (CV_MAJOR_VERSION >= 3 and (CV_MINOR_VERSION>4 or CV_MINOR_VERSION>=4  and CV_SUBMINOR_VERSION>=4))
+#if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION >= 3 && (CV_MINOR_VERSION>4 || (CV_MINOR_VERSION>=4 && CV_SUBMINOR_VERSION>=4)))
        p_Q = &(c_Q = cvMat(Q = _Qmat.getMat()));
 #else
        p_Q = &(c_Q = CvMat(Q = _Qmat.getMat()));
@@ -1233,7 +1230,7 @@ void stereoRectifyFisheye( cv::InputArray _cameraMatrix1, cv::InputArray _distCo
    CvMat *p_distCoeffs1 = distCoeffs1.empty() ? NULL : &c_distCoeffs1;
    CvMat *p_distCoeffs2 = distCoeffs2.empty() ? NULL : &c_distCoeffs2;
    cvStereoRectifyFisheye( &c_cameraMatrix1, &c_cameraMatrix2, p_distCoeffs1, p_distCoeffs2,
-#if CV_MAJOR_VERSION > 3 or (CV_MAJOR_VERSION >= 3 and (CV_MINOR_VERSION>4 or CV_MINOR_VERSION>=4  and CV_SUBMINOR_VERSION>=4))
+#if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION >= 3 && (CV_MINOR_VERSION>4 || (CV_MINOR_VERSION>=4 && CV_SUBMINOR_VERSION>=4)))
        cvSize(imageSize), &c_R, &c_T, &c_R1, &c_R2, &c_P1, &c_P2, p_Q, flags, alpha,
        cvSize(newImageSize));
 #else
@@ -154,6 +154,8 @@ cv::Mat RTABMAP_EXP brightnessAndContrastAuto(
 cv::Mat RTABMAP_EXP exposureFusion(
 	const std::vector<cv::Mat> & images);

+void RTABMAP_EXP HSVtoRGB( float *r, float *g, float *b, float h, float s, float v );
+
 } // namespace util3d
 } // namespace rtabmap

@@ -78,14 +78,11 @@ std::map<int, cv::Point3f> RTABMAP_EXP generateWords3DMono(
 		const std::map<int, cv::KeyPoint> & previousKpts,
 		const CameraModel & cameraModel,
 		Transform & cameraTransform,
-		int pnpIterations = 100,
-		float pnpReprojError = 8.0f,
-		int pnpFlags = 0, // cv::SOLVEPNP_ITERATIVE
-		int pnpRefineIterations = 1,
-		float ransacParam1 = 3.0f,
-		float ransacParam2 = 0.99f,
+		float ransacReprojThreshold = 3.0f,
+		float ransacConfidence = 0.99f,
 		const std::map<int, cv::Point3f> & refGuess3D = std::map<int, cv::Point3f>(),
-		double * variance = 0);
+		double * variance = 0,
+		std::vector<int> * matchesOut = 0);

 std::multimap<int, cv::KeyPoint> RTABMAP_EXP aggregate(
 		const std::list<int> & wordIds,
@@ -56,7 +56,16 @@ LaserScan RTABMAP_EXP commonFiltering(
 		float voxelSize = 0.0f,
 		int normalK = 0,
 		float normalRadius = 0.0f,
-		bool forceGroundNormalsUp = false);
+		float groundNormalsUp = 0.0f);
+RTABMAP_DEPRECATED(LaserScan RTABMAP_EXP commonFiltering(
+		const LaserScan & scan,
+		int downsamplingStep,
+		float rangeMin,
+		float rangeMax,
+		float voxelSize,
+		int normalK,
+		float normalRadius,
+		bool forceGroundNormalsUp), "Use version with groundNormalsUp as float. For forceGroundNormalsUp=true, set groundNormalsUp=0.8, otherwise set groundNormalsUp=0.0.");

 LaserScan RTABMAP_EXP rangeFiltering(
 		const LaserScan & scan,
@@ -288,6 +297,12 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cropBox(
 		const Eigen::Vector4f & max,
 		const Transform & transform = Transform::getIdentity(),
 		bool negative = false);
+pcl::PointCloud<pcl::PointXYZINormal>::Ptr RTABMAP_EXP cropBox(
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		const Eigen::Vector4f & min,
+		const Eigen::Vector4f & max,
+		const Transform & transform = Transform::getIdentity(),
+		bool negative = false);
 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP cropBox(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		const Eigen::Vector4f & min,
@@ -451,6 +466,12 @@ pcl::IndicesPtr RTABMAP_EXP subtractFiltering(
 /**
 * For convenience.
 */
+pcl::PointCloud<pcl::PointXYZINormal>::Ptr RTABMAP_EXP subtractFiltering(
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & substractCloud,
+		float radiusSearch,
+		float maxAngle = M_PI/4.0f,
+		int minNeighborsInRadius = 1);
 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP subtractFiltering(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
@@ -467,6 +488,14 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP subtractFiltering(
 * @param radiusSearch the radius in meter.
 * @return the indices of the points satisfying the parameters.
 */
+pcl::IndicesPtr RTABMAP_EXP subtractFiltering(
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & substractCloud,
+		const pcl::IndicesPtr & substractIndices,
+		float radiusSearch,
+		float maxAngle = M_PI/4.0f,
+		int minNeighborsInRadius = 1);
 pcl::IndicesPtr RTABMAP_EXP subtractFiltering(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
@@ -66,12 +66,25 @@ void RTABMAP_EXP computeVarianceAndCorrespondences(
 		double maxCorrespondenceAngle, // <=0 means that we don't care about normal angle difference
 		double & variance,
 		int & correspondencesOut);
+void RTABMAP_EXP computeVarianceAndCorrespondences(
+		const pcl::PointCloud<pcl::PointXYZINormal>::ConstPtr & cloudA,
+		const pcl::PointCloud<pcl::PointXYZINormal>::ConstPtr & cloudB,
+		double maxCorrespondenceDistance,
+		double maxCorrespondenceAngle, // <=0 means that we don't care about normal angle difference
+		double & variance,
+		int & correspondencesOut);
 void RTABMAP_EXP computeVarianceAndCorrespondences(
 		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloudA,
 		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloudB,
 		double maxCorrespondenceDistance,
 		double & variance,
 		int & correspondencesOut);
+void RTABMAP_EXP computeVarianceAndCorrespondences(
+		const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloudA,
+		const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloudB,
+		double maxCorrespondenceDistance,
+		double & variance,
+		int & correspondencesOut);

 Transform RTABMAP_EXP icp(
 		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
@@ -82,6 +95,15 @@ Transform RTABMAP_EXP icp(
 		pcl::PointCloud<pcl::PointXYZ> & cloud_source_registered,
 		float epsilon = 0.0f,
 		bool icp2D = false);
+Transform RTABMAP_EXP icp(
+		const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloud_source,
+		const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloud_target,
+		double maxCorrespondenceDistance,
+		int maximumIterations,
+		bool & hasConverged,
+		pcl::PointCloud<pcl::PointXYZI> & cloud_source_registered,
+		float epsilon = 0.0f,
+		bool icp2D = false);

 Transform RTABMAP_EXP icpPointToPlane(
 		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_source,
@@ -92,6 +114,15 @@ Transform RTABMAP_EXP icpPointToPlane(
 		pcl::PointCloud<pcl::PointNormal> & cloud_source_registered,
 		float epsilon = 0.0f,
 		bool icp2D = false);
+Transform RTABMAP_EXP icpPointToPlane(
+		const pcl::PointCloud<pcl::PointXYZINormal>::ConstPtr & cloud_source,
+		const pcl::PointCloud<pcl::PointXYZINormal>::ConstPtr & cloud_target,
+		double maxCorrespondenceDistance,
+		int maximumIterations,
+		bool & hasConverged,
+		pcl::PointCloud<pcl::PointXYZINormal> & cloud_source_registered,
+		float epsilon = 0.0f,
+		bool icp2D = false);

 } // namespace util3d
 } // namespace rtabmap
@@ -39,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/CameraModel.h>
 #include <rtabmap/core/ProgressState.h>
 #include <rtabmap/core/LaserScan.h>
+#include <rtabmap/core/Version.h>
 #include <set>
 #include <list>

@@ -148,7 +149,8 @@ pcl::TextureMesh::Ptr RTABMAP_EXP createTextureMesh(
 		int minClusterSize = 50, // minimum size of polygons clusters textured
 		const std::vector<float> & roiRatios = std::vector<float>(), // [left, right, top, bottom] region of interest (in ratios) of the image projected.
 		const ProgressState * state = 0,
-		std::vector<std::map<int, pcl::PointXY> > * vertexToPixels = 0);
+		std::vector<std::map<int, pcl::PointXY> > * vertexToPixels = 0, // For each point, we have a list of cameras with corresponding pixel in it. Beware that the camera ids don't correspond to pose ids, they are indexes from 0 to total camera models and texture's materials.
+		bool distanceToCamPolicy = false);
 pcl::TextureMesh::Ptr RTABMAP_EXP createTextureMesh(
 		const pcl::PolygonMesh::Ptr & mesh,
 		const std::map<int, Transform> & poses,
@@ -160,7 +162,8 @@ pcl::TextureMesh::Ptr RTABMAP_EXP createTextureMesh(
 		int minClusterSize = 50, // minimum size of polygons clusters textured
 		const std::vector<float> & roiRatios = std::vector<float>(), // [left, right, top, bottom] region of interest (in ratios) of the image projected.
 		const ProgressState * state = 0,
-		std::vector<std::map<int, pcl::PointXY> > * vertexToPixels = 0);
+		std::vector<std::map<int, pcl::PointXY> > * vertexToPixels = 0, // For each point, we have a list of cameras with corresponding pixel in it. Beware that the camera ids don't correspond to pose ids, they are indexes from 0 to total camera models and texture's materials.
+		bool distanceToCamPolicy = false);

 /**
 * Remove not textured polygon clusters. If minClusterSize<0, only the largest cluster is kept.
@@ -175,18 +178,18 @@ pcl::TextureMesh::Ptr RTABMAP_EXP concatenateTextureMeshes(
 void RTABMAP_EXP concatenateTextureMaterials(
 		pcl::TextureMesh & mesh, const cv::Size & imageSize, int textureSize, int maxTextures, float & scale, std::vector<bool> * materialsKept=0);

-std::vector<std::vector<unsigned int> > RTABMAP_EXP convertPolygonsFromPCL(
+std::vector<std::vector<RTABMAP_PCL_INDEX> > RTABMAP_EXP convertPolygonsFromPCL(
 		const std::vector<pcl::Vertices> & polygons);
-std::vector<std::vector<std::vector<unsigned int> > > RTABMAP_EXP convertPolygonsFromPCL(
+std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > RTABMAP_EXP convertPolygonsFromPCL(
 		const std::vector<std::vector<pcl::Vertices> > & polygons);
 std::vector<pcl::Vertices> RTABMAP_EXP convertPolygonsToPCL(
-		const std::vector<std::vector<unsigned int> > & polygons);
+		const std::vector<std::vector<RTABMAP_PCL_INDEX> > & polygons);
 std::vector<std::vector<pcl::Vertices> > RTABMAP_EXP convertPolygonsToPCL(
-		const std::vector<std::vector<std::vector<unsigned int> > > & tex_polygons);
+		const std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > & tex_polygons);

 pcl::TextureMesh::Ptr RTABMAP_EXP assembleTextureMesh(
 		const cv::Mat & cloudMat,
-		const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
+		const std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > & polygons,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 		const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
 #else
@@ -197,7 +200,7 @@ pcl::TextureMesh::Ptr RTABMAP_EXP assembleTextureMesh(

 pcl::PolygonMesh::Ptr RTABMAP_EXP assemblePolygonMesh(
 		const cv::Mat & cloudMat,
-		const std::vector<std::vector<unsigned int> > & polygons);
+		const std::vector<std::vector<RTABMAP_PCL_INDEX> > & polygons);

 /**
 * Merge all textures in the mesh into "textureCount" textures of size "textureSize".
@@ -263,8 +266,9 @@ bool RTABMAP_EXP multiBandTexturing(
 		int textureSize = 8192,
 		const std::string & textureFormat = "jpg",    // png, jpg
 		const std::map<int, std::map<int, cv::Vec4d> > & gains = std::map<int, std::map<int, cv::Vec4d> >(),       // optional output of util3d::mergeTextures()
-		const std::map<int, std::map<int, cv::Mat> > & blendingGains = std::map<int, std::map<int, cv::Mat> >(),    // optional output of util3d::mergeTextures()
-		const std::pair<float, float> & contrastValues = std::pair<float, float>(0,0));               // optional output of util3d::mergeTextures()
+		const std::map<int, std::map<int, cv::Mat> > & blendingGains = std::map<int, std::map<int, cv::Mat> >(),   // optional output of util3d::mergeTextures()
+		const std::pair<float, float> & contrastValues = std::pair<float, float>(0,0),                             // optional output of util3d::mergeTextures()
+		bool gainRGB = true);

 cv::Mat RTABMAP_EXP computeNormals(
 		const cv::Mat & laserScan,
@@ -354,6 +358,12 @@ float RTABMAP_EXP computeNormalsComplexity(
 		bool is2d = false,
 		cv::Mat * pcaEigenVectors = 0,
 		cv::Mat * pcaEigenValues = 0);
+float RTABMAP_EXP computeNormalsComplexity(
+		const pcl::PointCloud<pcl::PointXYZINormal> & cloud,
+		const Transform & t = Transform::getIdentity(),
+		bool is2d = false,
+		cv::Mat * pcaEigenVectors = 0,
+		cv::Mat * pcaEigenValues = 0);
 float RTABMAP_EXP computeNormalsComplexity(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
 		const Transform & t = Transform::getIdentity(),
@@ -383,18 +393,39 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP mls(
 		float dilationVoxelSize = 1.0f,  // VOXEL_GRID_DILATION
 		int dilationIterations = 0);     // VOXEL_GRID_DILATION

-LaserScan RTABMAP_EXP adjustNormalsToViewPoint(
+RTABMAP_DEPRECATED(LaserScan RTABMAP_EXP adjustNormalsToViewPoint(
 		const LaserScan & scan,
 		const Eigen::Vector3f & viewpoint,
-		bool forceGroundNormalsUp);
+		bool forceGroundNormalsUp), "Use version with groundNormalsUp as float. For forceGroundNormalsUp=true, set groundNormalsUp to 0.8f, otherwise set groundNormalsUp to 0.0f.");
+LaserScan RTABMAP_EXP adjustNormalsToViewPoint(
+		const LaserScan & scan,
+		const Eigen::Vector3f & viewpoint = Eigen::Vector3f(0,0,0),
+		float groundNormalsUp = 0.0f);
+RTABMAP_DEPRECATED(void RTABMAP_EXP adjustNormalsToViewPoint(
+		pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const Eigen::Vector3f & viewpoint,
+		bool forceGroundNormalsUp), "Use version with groundNormalsUp as float. For forceGroundNormalsUp=true, set groundNormalsUp to 0.8f, otherwise set groundNormalsUp to 0.0f.");
 void RTABMAP_EXP adjustNormalsToViewPoint(
 		pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
 		const Eigen::Vector3f & viewpoint = Eigen::Vector3f(0,0,0),
-		bool forceGroundNormalsUp = false);
+		float groundNormalsUp = 0.0f);
+RTABMAP_DEPRECATED(void RTABMAP_EXP adjustNormalsToViewPoint(
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const Eigen::Vector3f & viewpoint,
+		bool forceGroundNormalsUp), "Use version with groundNormalsUp as float. For forceGroundNormalsUp=true, set groundNormalsUp to 0.8f, otherwise set groundNormalsUp to 0.0f.");
 void RTABMAP_EXP adjustNormalsToViewPoint(
 		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		const Eigen::Vector3f & viewpoint = Eigen::Vector3f(0,0,0),
-		bool forceGroundNormalsUp = false);
+		float groundNormalsUp = 0.0f);
+RTABMAP_DEPRECATED(void RTABMAP_EXP adjustNormalsToViewPoint(
+		pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		const Eigen::Vector3f & viewpoint,
+		bool forceGroundNormalsUp), "Use version with groundNormalsUp as float. For forceGroundNormalsUp=true, set groundNormalsUp to 0.8f, otherwise set groundNormalsUp to 0.0f.");
+void RTABMAP_EXP adjustNormalsToViewPoint(
+		pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		const Eigen::Vector3f & viewpoint = Eigen::Vector3f(0,0,0),
+		float groundNormalsUp = 0.0f);
+
 void RTABMAP_EXP adjustNormalsToViewPoints(
 		const std::map<int, Transform> & poses,
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,
@@ -417,6 +417,20 @@ cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector
 	return prediction;
 }

+unsigned long BayesFilter::getMemoryUsed() const
+{
+	long memoryUsage = sizeof(BayesFilter);
+	memoryUsage += _posterior.size() * (sizeof(float)+sizeof(int)+sizeof(std::map<int, float>::iterator)) + sizeof(std::map<int, float>);
+	memoryUsage += _prediction.total() * _prediction.elemSize();
+	memoryUsage += _predictionLC.size() * sizeof(double);
+	memoryUsage += _neighborsIndex.size() * (sizeof(int)+sizeof(std::map<int, int>)+sizeof(std::map<int, std::map<int, int> >::iterator)) + sizeof(std::map<int, std::map<int, int> >);
+	for(std::map<int, std::map<int, int> >::const_iterator iter=_neighborsIndex.begin(); iter!=_neighborsIndex.end(); ++iter)
+	{
+		memoryUsage += iter->second.size() * (sizeof(int)*2+sizeof(std::map<int, int>::iterator)) + sizeof(std::map<int, int>);
+	}
+	return memoryUsage;
+}
+
 void BayesFilter::normalize(cv::Mat & prediction, unsigned int index, float addedProbabilitiesSum, bool virtualPlaceUsed) const
 {
 	UASSERT(index < (unsigned int)prediction.rows && index < (unsigned int)prediction.cols);
@@ -117,6 +117,8 @@ SET(SRC_FILES
    clams/slam_calibrator.cpp
    
    opencv/ORBextractor.cc
+    opencv/solvepnp.cpp
+    opencv/five-point.cpp
 )

 IF(OpenCV_VERSION_MAJOR EQUAL 2)
@@ -125,10 +127,6 @@ SET(SRC_FILES
    opencv/Orb.cpp
 )
 ENDIF(OpenCV_VERSION_MAJOR EQUAL 2)
-SET(SRC_FILES
-    ${SRC_FILES}
-    opencv/solvepnp.cpp
-)

 # to get includes in visual studio
 IF(MSVC)
@@ -190,6 +188,24 @@ IF(TORCH_FOUND)
 	)
 ENDIF(TORCH_FOUND)

+IF(Python3_FOUND)
+	SET(LIBRARIES
+		${LIBRARIES}
+		Python3::Python
+	)
+	SET(SRC_FILES
+		${SRC_FILES}
+		pymatcher/PyMatcher.cpp
+	)
+	 SET(INCLUDE_DIRS
+	    ${TORCH_INCLUDE_DIRS}
+	    ${CMAKE_CURRENT_SOURCE_DIR}/pymatcher
+		${INCLUDE_DIRS}
+	)
+ENDIF(Python3_FOUND)
+
+
+
 IF(Freenect_FOUND)
 	IF(Freenect_DASH_INCLUDES)
 	   ADD_DEFINITIONS("-DFREENECT_DASH_INCLUDES")
@@ -277,6 +293,11 @@ IF(realsense2_FOUND)
 			${LIBRARIES}
 			${RealSense2_LIBRARIES}
 		)
+	ELSEIF(APPLE)
+	    SET(LIBRARIES
+			${LIBRARIES}
+			${realsense2_LIBRARIES}
+		)
 	ELSE()
 		SET(LIBRARIES
 			${LIBRARIES}
@@ -571,18 +592,24 @@ ENDIF(WITH_MADGWICK)
 ####################################
 CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/resources/DatabaseSchema.sql.in ${CMAKE_CURRENT_SOURCE_DIR}/resources/DatabaseSchema.sql)

-SET(R
+SET(RESOURCES
 	${CMAKE_CURRENT_SOURCE_DIR}/resources/DatabaseSchema.sql
+	${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_18_3.sql
+	${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_18_0.sql
+	${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_17_0.sql
+	${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_16_2.sql
+	${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_16_1.sql
+	${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_16_0.sql
 )

-#replace semicolons by spaces
-foreach(arg ${R})
-   set(RESOURCES "${RESOURCES}" "${arg}")
-endforeach(arg ${R})
+foreach(arg ${RESOURCES})
+   get_filename_component(filename ${arg} NAME)
+   string(REPLACE "." "_" output ${filename})
+   set(RESOURCES_HEADERS "${RESOURCES_HEADERS}" "${CMAKE_CURRENT_BINARY_DIR}/${output}.h")
+endforeach(arg ${RESOURCES})

-SET(RESOURCES_HEADERS 
-	${CMAKE_CURRENT_BINARY_DIR}/DatabaseSchema_sql.h 
-)
+MESSAGE(STATUS "RESOURCES = ${RESOURCES}")
+MESSAGE(STATUS "RESOURCES_HEADERS = ${RESOURCES_HEADERS}")

 IF(ANDROID)

@@ -597,14 +624,14 @@ IF(ANDROID)
      OUTPUT ${RESOURCES_HEADERS}
      COMMAND ${RTABMAP_RES_TOOL} -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${RESOURCES}
      COMMENT "[Creating resources]"
-      DEPENDS ${R}
+      DEPENDS ${RESOURCES}
   )
 ELSE()
   ADD_CUSTOM_COMMAND(
      OUTPUT ${RESOURCES_HEADERS}
      COMMAND ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}/rtabmap-res_tool -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${RESOURCES}
      COMMENT "[Creating resources]"
-      DEPENDS ${R} res_tool
+      DEPENDS ${RESOURCES} res_tool
   )
 ENDIF()

@@ -64,6 +64,11 @@ void Camera::resetTimer()
 	_frameRateTimer->start();
 }

+bool Camera::initFromFile(const std::string & calibrationPath)
+{
+	return init(UDirectory::getDir(calibrationPath), uSplit(UFile::getName(calibrationPath), '.').front());
+}
+
 SensorData Camera::takeImage(CameraInfo * info)
 {
 	bool warnFrameRateTooHigh = false;
@@ -37,7 +37,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 namespace rtabmap {

-CameraModel::CameraModel()
+CameraModel::CameraModel() :
+	localTransform_(0,0,1,0, -1,0,0,0, 0,-1,0,0)
 {

 }
@@ -210,7 +211,7 @@ void CameraModel::setImageSize(const cv::Size & size)
 	}
 }

-bool CameraModel::load(const std::string & directory, const std::string & cameraName)
+bool CameraModel::load(const std::string & filePath)
 {
 	K_ = cv::Mat();
 	D_ = cv::Mat();
@@ -221,7 +222,6 @@ bool CameraModel::load(const std::string & directory, const std::string & camera
 	name_.clear();
 	imageSize_ = cv::Size();

-	std::string filePath = directory+"/"+cameraName+".yaml";
 	if(UFile::exists(filePath))
 	{
 		try
@@ -360,6 +360,11 @@ bool CameraModel::load(const std::string & directory, const std::string & camera
 	return false;
 }

+bool CameraModel::load(const std::string & directory, const std::string & cameraName)
+{
+	return load(directory+"/"+cameraName+".yaml");
+}
+
 bool CameraModel::save(const std::string & directory) const
 {
 	std::string filePath = directory+"/"+name_+".yaml";
@@ -635,22 +640,23 @@ CameraModel CameraModel::roi(const cv::Rect & roi) const
 	return roiModel;
 }

+double CameraModel::fovX() const
+{
+	return imageSize_.width>0 && fx()>0?2.0*atan(imageSize_.width/(fx()*2.0)):0.0;
+}
+double CameraModel::fovY() const
+{
+	return imageSize_.height>0 && fy()>0?2.0*atan(imageSize_.height/(fy()*2.0)):0.0;
+}
+
 double CameraModel::horizontalFOV() const
 {
-	if(imageWidth() > 0 && fx() > 0.0)
-	{
-		return atan((double(imageWidth())/2.0)/fx())*2.0*180.0/CV_PI;
-	}
-	return 0.0;
+	return fovX()*180.0/CV_PI;
 }

 double CameraModel::verticalFOV() const
 {
-	if(imageHeight() > 0 && fy() > 0.0)
-	{
-		return atan((double(imageHeight())/2.0)/fy())*2.0*180.0/CV_PI;
-	}
-	return 0.0;
+	return fovY()*180.0/CV_PI;
 }

 cv::Mat CameraModel::rectifyImage(const cv::Mat & raw, int interpolation) const
@@ -759,4 +765,16 @@ bool CameraModel::inFrame(int u, int v) const
 	return uIsInBounds(u, 0, imageWidth()) && uIsInBounds(v, 0, imageHeight());
 }

+std::ostream& operator<<(std::ostream& os, const CameraModel& model)
+{
+	os << "Name: " << model.name() << std::endl
+	   << "Size: " << model.imageWidth() << "x" << model.imageHeight() << std::endl
+	   << "K= " << model.K_raw() << std::endl
+	   << "D= " << model.D_raw() << std::endl
+	   << "R= " << model.R() << std::endl
+	   << "P= " << model.P() << std::endl
+	   << "LocalTransform= " << model.localTransform();
+	return os;
+}
+
 } /* namespace rtabmap */
@@ -74,7 +74,6 @@ CameraThread::CameraThread(Camera * camera, const ParametersMap & parameters) :

 CameraThread::~CameraThread()
 {
-	UDEBUG("");
 	join(true);
 	delete _camera;
 	delete _distortionModel;
@@ -130,6 +129,39 @@ void CameraThread::disableIMUFiltering()
 	_imuFilter = 0;
 }

+void CameraThread::setScanParameters(
+	bool fromDepth,
+	int downsampleStep,
+	float rangeMin,
+	float rangeMax,
+	float voxelSize,
+	int normalsK,
+	int normalsRadius,
+	bool forceGroundNormalsUp)
+{
+	setScanParameters(fromDepth, downsampleStep, rangeMin, rangeMax, voxelSize, normalsK, normalsRadius, forceGroundNormalsUp?0.8f:0.0f);
+}
+
+void CameraThread::setScanParameters(
+			bool fromDepth,
+			int downsampleStep, // decimation of the depth image in case the scan is from depth image
+			float rangeMin,
+			float rangeMax,
+			float voxelSize,
+			int normalsK,
+			int normalsRadius,
+			float groundNormalsUp)
+{
+	_scanFromDepth = fromDepth;
+	_scanDownsampleStep=downsampleStep;
+	_scanRangeMin = rangeMin;
+	_scanRangeMax = rangeMax;
+	_scanVoxelSize = voxelSize;
+	_scanNormalsK = normalsK;
+	_scanNormalsRadius = normalsRadius;
+	_scanForceGroundNormalsUp = groundNormalsUp;
+}
+
 void CameraThread::mainLoopBegin()
 {
 	ULogger::registerCurrentThread("Camera");
@@ -139,7 +171,6 @@ void CameraThread::mainLoopBegin()
 void CameraThread::mainLoop()
 {
 	UTimer totalTime;
-	UDEBUG("");
 	CameraInfo info;
 	SensorData data = _camera->takeImage(&info);

@@ -161,7 +192,6 @@ void CameraThread::mainLoop()

 void CameraThread::mainLoopKill()
 {
-	UDEBUG("");
 	if(dynamic_cast<CameraFreenect2*>(_camera) != 0)
 	{
 		int i=20;
@@ -240,7 +270,26 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
 		else
 		{
 			cv::Mat image = util2d::decimate(data.imageRaw(), _imageDecimation);
-			cv::Mat depthOrRight = util2d::decimate(data.depthOrRightRaw(), _imageDecimation);
+
+			int depthDecimation = _imageDecimation;
+			if(data.depthOrRightRaw().rows <= image.rows || data.depthOrRightRaw().cols <= image.cols)
+			{
+				depthDecimation = 1;
+			}
+			else
+			{
+				depthDecimation = 2;
+				while(data.depthOrRightRaw().rows / depthDecimation > image.rows ||
+					  data.depthOrRightRaw().cols / depthDecimation > image.cols ||
+					  data.depthOrRightRaw().rows % depthDecimation != 0 ||
+					  data.depthOrRightRaw().cols % depthDecimation != 0)
+				{
+					++depthDecimation;
+				}
+				UDEBUG("depthDecimation=%d", depthDecimation);
+			}
+			cv::Mat depthOrRight = util2d::decimate(data.depthOrRightRaw(), depthDecimation);
+
 			std::vector<CameraModel> models = data.cameraModels();
 			for(unsigned int i=0; i<models.size(); ++i)
 			{
@@ -40,7 +40,6 @@ namespace rtabmap {

 DBDriver * DBDriver::create(const ParametersMap & parameters)
 {
-	// well, we only have Sqlite3 database type for now :P
 	return new DBDriverSqlite3(parameters);
 }

@@ -59,6 +58,7 @@ DBDriver::~DBDriver()

 void DBDriver::parseParameters(const ParametersMap & parameters)
 {
+	Parameters::parse(parameters, Parameters::kDbTargetVersion(), _targetVersion);
 }

 void DBDriver::closeConnection(bool save, const std::string & outputUrl)
@@ -107,9 +107,9 @@ bool DBDriver::isConnected() const
 }

 // In bytes
-long DBDriver::getMemoryUsed() const
+unsigned long DBDriver::getMemoryUsed() const
 {
-	long bytes;
+	unsigned long bytes;
 	_dbSafeAccessMutex.lock();
 	bytes = getMemoryUsedQuery();
 	_dbSafeAccessMutex.unlock();
@@ -683,13 +683,29 @@ void DBDriver::getNodeData(
 	if(uContains(_trashSignatures, signatureId))
 	{
 		const Signature * s = _trashSignatures.at(signatureId);
-		if(!s->sensorData().imageCompressed().empty() ||
-			!s->sensorData().laserScanCompressed().isEmpty() ||
-			!s->sensorData().userDataCompressed().empty() ||
-			s->sensorData().gridCellSize() != 0.0f ||
-			!s->isSaved())
+		if((!s->isSaved() ||
+			((!images || !s->sensorData().imageCompressed().empty()) &&
+			 (!scan || !s->sensorData().laserScanCompressed().isEmpty()) &&
+			 (!userData || !s->sensorData().userDataCompressed().empty()) &&
+			 (!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;
 		}
 	}
@@ -1140,10 +1156,10 @@ void DBDriver::addInfoAfterRun(
 	}
 }

-void DBDriver::addStatistics(const Statistics & statistics) const
+void DBDriver::addStatistics(const Statistics & statistics, bool saveWmState) const
 {
 	_dbSafeAccessMutex.lock();
-	addStatisticsQuery(statistics);
+	addStatisticsQuery(statistics, saveWmState);
 	_dbSafeAccessMutex.unlock();
 }

@@ -1193,7 +1209,7 @@ cv::Mat DBDriver::load2DMap(float & xMin, float & yMin, float & cellSize) const

 void DBDriver::saveOptimizedMesh(
 			const cv::Mat & cloud,
-			const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
+			const std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > & polygons,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
 #else
@@ -1207,7 +1223,7 @@ void DBDriver::saveOptimizedMesh(
 }

 cv::Mat DBDriver::loadOptimizedMesh(
-				std::vector<std::vector<std::vector<unsigned int> > > * polygons,
+				std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > * polygons,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 				std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords,
 #else
@@ -34,6 +34,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/util3d.h"
 #include "rtabmap/core/Compression.h"
 #include "DatabaseSchema_sql.h"
+#include "DatabaseSchema_0_18_3_sql.h"
+#include "DatabaseSchema_0_18_0_sql.h"
+#include "DatabaseSchema_0_17_0_sql.h"
+#include "DatabaseSchema_0_16_2_sql.h"
+#include "DatabaseSchema_0_16_1_sql.h"
+#include "DatabaseSchema_0_16_0_sql.h"
+
+
 #include <set>

 #include "rtabmap/utilite/UtiLite.h"
@@ -383,6 +391,34 @@ bool DBDriverSqlite3::connectDatabaseQuery(const std::string & url, bool overwri
 		}
 		// Create the database
 		std::string schema = DATABASESCHEMA_SQL;
+		std::string targetVersion = this->getTargetVersion();
+		if(!targetVersion.empty())
+		{
+			// search for schema with version <= target version
+			std::vector<std::pair<std::string, std::string> > schemas;
+			schemas.push_back(std::make_pair("0.16.0", DATABASESCHEMA_0_16_0_SQL));
+			schemas.push_back(std::make_pair("0.16.1", DATABASESCHEMA_0_16_1_SQL));
+			schemas.push_back(std::make_pair("0.16.2", DATABASESCHEMA_0_16_2_SQL));
+			schemas.push_back(std::make_pair("0.17.0", DATABASESCHEMA_0_17_0_SQL));
+			schemas.push_back(std::make_pair("0.18.0", DATABASESCHEMA_0_18_0_SQL));
+			schemas.push_back(std::make_pair("0.18.3", DATABASESCHEMA_0_18_3_SQL));
+			schemas.push_back(std::make_pair(uNumber2Str(RTABMAP_VERSION_MAJOR)+"."+uNumber2Str(RTABMAP_VERSION_MINOR), DATABASESCHEMA_SQL));
+			for(size_t i=0; i<schemas.size(); ++i)
+			{
+				if(uStrNumCmp(targetVersion, schemas[i].first) < 0)
+				{
+					if(i==0)
+					{
+						UERROR("Cannot create database with target version \"%s\" (not implemented), using latest version.", targetVersion.c_str());
+					}
+					break;
+				}
+				else
+				{
+					schema = schemas[i].second;
+				}
+			}
+		}
 		schema = uHex2Str(schema);
 		this->executeNoResultQuery(schema.c_str());
 	}
@@ -486,7 +522,7 @@ void DBDriverSqlite3::executeNoResultQuery(const std::string & sql) const
 	}
 }

-long DBDriverSqlite3::getMemoryUsedQuery() const
+unsigned long DBDriverSqlite3::getMemoryUsedQuery() const
 {
 	if(_dbInMemory)
 	{
@@ -842,22 +878,22 @@ long DBDriverSqlite3::getFeaturesMemoryUsedQuery() const
 		std::string query;
 		if(uStrNumCmp(_version, "0.13.0") >= 0)
 		{
-			query = "SELECT sum(length(word_id) + length(pos_x) + length(pos_y) + length(size) + length(dir) + length(response) + length(octave) + length(depth_x) + length(depth_y) + length(depth_z) + length(descriptor_size) + length(descriptor)) "
+			query = "SELECT sum(length(node_id) + length(word_id) + length(pos_x) + length(pos_y) + length(size) + length(dir) + length(response) + length(octave) + length(depth_x) + length(depth_y) + length(depth_z) + length(descriptor_size) + length(descriptor)) "
 					 "FROM Feature";
 		}
 		else if(uStrNumCmp(_version, "0.12.0") >= 0)
 		{
-			query = "SELECT sum(length(word_id) + length(pos_x) + length(pos_y) + length(size) + length(dir) + length(response) + length(octave) + length(depth_x) + length(depth_y) + length(depth_z) + length(descriptor_size) + length(descriptor)) "
+			query = "SELECT sum(length(node_id) + length(word_id) + length(pos_x) + length(pos_y) + length(size) + length(dir) + length(response) + length(octave) + length(depth_x) + length(depth_y) + length(depth_z) + length(descriptor_size) + length(descriptor)) "
 					 "FROM Map_Node_Word";
 		}
 		else if(uStrNumCmp(_version, "0.11.2") >= 0)
 		{
-			query = "SELECT sum(length(word_id) + length(pos_x) + length(pos_y) + length(size) + length(dir) + length(response) + length(depth_x) + length(depth_y) + length(depth_z) + length(descriptor_size) + length(descriptor)) "
+			query = "SELECT sum(length(node_id) + length(word_id) + length(pos_x) + length(pos_y) + length(size) + length(dir) + length(response) + length(depth_x) + length(depth_y) + length(depth_z) + length(descriptor_size) + length(descriptor)) "
 					 "FROM Map_Node_Word";
 		}
 		else
 		{
-			query = "SELECT sum(length(word_id) + length(pos_x) + length(pos_y) + length(size) + length(dir) + length(response) + length(depth_x) + length(depth_y) + length(depth_z)) "
+			query = "SELECT sum(length(node_id) + length(word_id) + length(pos_x) + length(pos_y) + length(size) + length(dir) + length(response) + length(depth_x) + length(depth_y) + length(depth_z)) "
 					 "FROM Map_Node_Word";
 		}

@@ -2377,6 +2413,7 @@ void DBDriverSqlite3::getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildre
 			query << "INNER JOIN Link ";
 			query << "ON id = to_id "; // use to_id to ignore all children (which don't have link pointing on them)
 			query << "WHERE from_id != to_id "; // ignore self referring links
+			query << "AND weight>-9 "; //ignore invalid nodes
 		}

 		if(ignoreBadSignatures)
@@ -3078,9 +3115,10 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 			const void * descriptor = 0;
 			int dRealSize = 0;
 			cv::KeyPoint kpt;
-			std::multimap<int, cv::KeyPoint> visualWords;
-			std::multimap<int, cv::Point3f> visualWords3;
-			std::multimap<int, cv::Mat> descriptors;
+			std::multimap<int, int> visualWords;
+			std::vector<cv::KeyPoint> visualWordsKpts;
+			std::vector<cv::Point3f> visualWords3;
+			cv::Mat descriptors;
 			bool allWords3NaN = true;
 			cv::Point3f depth(0,0,0);

@@ -3130,8 +3168,9 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 					depth.z = sqlite3_column_double(ppStmt, index++);
 				}

-				visualWords.insert(visualWords.end(), std::make_pair(visualWordId, kpt));
-				visualWords3.insert(visualWords3.end(), std::make_pair(visualWordId, depth));
+				visualWordsKpts.push_back(kpt);
+				visualWords.insert(visualWords.end(), std::make_pair(visualWordId, visualWordsKpts.size()-1));
+				visualWords3.push_back(depth);

 				if(allWords3NaN && util3d::isFinite(depth))
 				{
@@ -3164,7 +3203,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<

 						memcpy(d.data, descriptor, dRealSize);

-						descriptors.insert(descriptors.end(), std::make_pair(visualWordId, d));
+						descriptors.push_back(d);
 					}
 				}

@@ -3178,13 +3217,12 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 			}
 			else
 			{
-				(*iter)->setWords(visualWords);
-				if(!allWords3NaN)
+				if(allWords3NaN)
 				{
-					(*iter)->setWords3(visualWords3);
+					visualWords3.clear();
 				}
-				(*iter)->setWordsDescriptors(descriptors);
-				ULOGGER_DEBUG("Add %d keypoints, %d 3d points and %d descriptors to node %d", (int)visualWords.size(), allWords3NaN?0:(int)visualWords3.size(), (int)descriptors.size(), (*iter)->id());
+				(*iter)->setWords(visualWords, visualWordsKpts, visualWords3, descriptors);
+				ULOGGER_DEBUG("Add %d keypoints, %d 3d points and %d descriptors to node %d", (int)visualWords.size(), allWords3NaN?0:(int)visualWords3.size(), (int)descriptors.rows, (*iter)->id());
 			}

 			//reset
@@ -3384,6 +3422,74 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<

 			ULOGGER_DEBUG("Time load %d calibrations=%fs", (int)nodes.size(), timer.ticks());
 		}
+
+		// load global descriptors
+		if(nodes.size() && uStrNumCmp(_version, "0.20.0") >= 0)
+		{
+			std::stringstream query3;
+			query3 << "SELECT type, info, data "
+					 "FROM GlobalDescriptor "
+					 "WHERE node_id = ? ";
+
+			rc = sqlite3_prepare_v2(_ppDb, query3.str().c_str(), -1, &ppStmt, 0);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+			for(std::list<Signature*>::const_iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
+			{
+				// bind id
+				rc = sqlite3_bind_int(ppStmt, 1, (*iter)->id());
+				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+				std::vector<GlobalDescriptor> globalDescriptors;
+
+				rc = sqlite3_step(ppStmt);
+				while(rc == SQLITE_ROW)
+				{
+					int index=0;
+					const void * data = 0;
+					int dataSize = 0;
+					int type = -1;
+					cv::Mat info;
+					cv::Mat dataMat;
+
+					type = sqlite3_column_int(ppStmt, index++);
+					data = sqlite3_column_blob(ppStmt, index);
+					dataSize = sqlite3_column_bytes(ppStmt, index++);
+					if(dataSize && data)
+					{
+						info = rtabmap::uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone());
+					}
+					data = sqlite3_column_blob(ppStmt, index);
+					dataSize = sqlite3_column_bytes(ppStmt, index++);
+					if(dataSize && data)
+					{
+						dataMat = rtabmap::uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone());
+					}
+
+					UASSERT(!dataMat.empty());
+					globalDescriptors.push_back(GlobalDescriptor(type, dataMat, info));
+
+					rc = sqlite3_step(ppStmt);
+				}
+				UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+				if(!globalDescriptors.empty())
+				{
+					(*iter)->sensorData().setGlobalDescriptors(globalDescriptors);
+					ULOGGER_DEBUG("Add %d global descriptors to node %d", (int)globalDescriptors.size(), (*iter)->id());
+				}
+
+				//reset
+				rc = sqlite3_reset(ppStmt);
+				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+			}
+			// Finalize (delete) the statement
+			rc = sqlite3_finalize(ppStmt);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+			ULOGGER_DEBUG("Time load %d global descriptors=%fs", (int)nodes.size(), timer.ticks());
+		}
+
 		if(ids.size() != loaded)
 		{
 			UERROR("Some signatures not found in database");
@@ -3554,6 +3660,7 @@ void DBDriverSqlite3::loadWordsQuery(const std::set<int> & wordIds, std::list<Vi
 		int descriptorSize;
 		const void * descriptor;
 		int dRealSize;
+		unsigned long dRealSizeTotal = 0;
 		for(std::set<int>::const_iterator iter=wordIds.begin(); iter!=wordIds.end(); ++iter)
 		{
 			// bind id
@@ -3586,6 +3693,7 @@ void DBDriverSqlite3::loadWordsQuery(const std::set<int> & wordIds, std::list<Vi
 				}

 				memcpy(d.data, descriptor, dRealSize);
+				dRealSizeTotal+=dRealSize;
 				VisualWord * vw = new VisualWord(*iter, d);
 				if(vw)
 				{
@@ -3607,7 +3715,7 @@ void DBDriverSqlite3::loadWordsQuery(const std::set<int> & wordIds, std::list<Vi
 		rc = sqlite3_finalize(ppStmt);
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());

-		ULOGGER_DEBUG("Time=%fs", timer.ticks());
+		UDEBUG("Time=%fs (%d words, %lu MB)", timer.ticks(), (int)vws.size(), dRealSizeTotal/1000000);

 		if(wordIds.size() != loaded.size())
 		{
@@ -3658,7 +3766,7 @@ void DBDriverSqlite3::loadLinksQuery(
 			query << "SELECT to_id, type, transform FROM Link ";
 		}
 		query << "WHERE from_id = " << signatureId;
-		if(typeIn != Link::kUndef)
+		if(typeIn < Link::kEnd)
 		{
 			if(uStrNumCmp(_version, "0.7.4") >= 0)
 			{
@@ -4204,30 +4312,25 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures)
 		float nanFloat = std::numeric_limits<float>::quiet_NaN ();
 		for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
 		{
+			UASSERT((*i)->getWords().size() == (*i)->getWordsKpts().size());
 			UASSERT((*i)->getWords3().empty() || (*i)->getWords().size() == (*i)->getWords3().size());
-			UASSERT((*i)->getWordsDescriptors().empty() || (*i)->getWords().size() == (*i)->getWordsDescriptors().size());
+			UASSERT((*i)->getWordsDescriptors().empty() || (int)(*i)->getWords().size() == (*i)->getWordsDescriptors().rows);

-			std::multimap<int, cv::Point3f>::const_iterator p=(*i)->getWords3().begin();
-			std::multimap<int, cv::Mat>::const_iterator d=(*i)->getWordsDescriptors().begin();
-			for(std::multimap<int, cv::KeyPoint>::const_iterator w=(*i)->getWords().begin(); w!=(*i)->getWords().end(); ++w)
+			for(std::multimap<int, int>::const_iterator w=(*i)->getWords().begin(); w!=(*i)->getWords().end(); ++w)
 			{
 				cv::Point3f pt(nanFloat,nanFloat,nanFloat);
-				if(p!=(*i)->getWords3().end())
+				if(!(*i)->getWords3().empty())
 				{
-					UASSERT(w->first == p->first); // must be same id!
-					pt = p->second;
-					++p;
+					pt = (*i)->getWords3()[w->second];
 				}

 				cv::Mat descriptor;
-				if(d!=(*i)->getWordsDescriptors().end())
+				if(!(*i)->getWordsDescriptors().empty())
 				{
-					UASSERT(w->first == d->first); // must be same id!
-					descriptor = d->second;
-					++d;
+					descriptor = (*i)->getWordsDescriptors().row(w->second);
 				}

-				stepKeypoint(ppStmt, (*i)->id(), w->first, w->second, pt, descriptor);
+				stepKeypoint(ppStmt, (*i)->id(), w->first, (*i)->getWordsKpts()[w->second], pt, descriptor);
 			}
 		}
 		// Finalize (delete) the statement
@@ -4235,6 +4338,27 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures)
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 		UDEBUG("Time=%fs", timer.ticks());

+		if(uStrNumCmp(_version, "0.20.0") >= 0)
+		{
+			// Global descriptor table
+			std::string query = queryStepGlobalDescriptor();
+			rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+			for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
+			{
+				for(size_t d=0; d<(*i)->sensorData().globalDescriptors().size(); ++d)
+				{
+					stepGlobalDescriptor(ppStmt, (*i)->id(), (*i)->sensorData().globalDescriptors()[d]);
+				}
+			}
+			// Finalize (delete) the statement
+			rc = sqlite3_finalize(ppStmt);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+			UDEBUG("Time=%fs", timer.ticks());
+		}
+
 		if(uStrNumCmp(_version, "0.10.0") >= 0)
 		{
 			// Add SensorData
@@ -4519,7 +4643,7 @@ void DBDriverSqlite3::updateLaserScanQuery(
 	}
 }

-void DBDriverSqlite3::addStatisticsQuery(const Statistics & statistics) const
+void DBDriverSqlite3::addStatisticsQuery(const Statistics & statistics, bool saveWmState) const
 {
 	UDEBUG("Ref ID = %d", statistics.refImageId());
 	if(_ppDb)
@@ -4570,7 +4694,7 @@ void DBDriverSqlite3::addStatisticsQuery(const Statistics & statistics) const
 				cv::Mat compressedWmState;
 				if(uStrNumCmp(this->getDatabaseVersion(), "0.16.2") >= 0)
 				{
-					if(!statistics.wmState().empty())
+					if(saveWmState && !statistics.wmState().empty())
 					{
 						compressedWmState = compressData2(cv::Mat(1, statistics.wmState().size(), CV_32SC1, (void *)statistics.wmState().data()));
 						rc = sqlite3_bind_blob(ppStmt, index++, compressedWmState.data, compressedWmState.cols, SQLITE_STATIC);
@@ -4969,7 +5093,7 @@ cv::Mat DBDriverSqlite3::load2DMapQuery(float & xMin, float & yMin, float & cell

 void DBDriverSqlite3::saveOptimizedMeshQuery(
 			const cv::Mat & cloud,
-			const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
+			const std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > & polygons,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
 #else
@@ -5164,7 +5288,7 @@ void DBDriverSqlite3::saveOptimizedMeshQuery(
 }

 cv::Mat DBDriverSqlite3::loadOptimizedMeshQuery(
-			std::vector<std::vector<std::vector<unsigned int> > > * polygons,
+			std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > * polygons,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords,
 #else
@@ -5226,7 +5350,7 @@ cv::Mat DBDriverSqlite3::loadOptimizedMeshQuery(
 					for(int t=0; t<serializedPolygons.cols; ++t)
 					{
 						UASSERT(serializedPolygons.at<int>(t) > 0);
-						std::vector<std::vector<unsigned int> > materialPolygons(serializedPolygons.at<int>(t), std::vector<unsigned int>(polygonSize));
+						std::vector<std::vector<RTABMAP_PCL_INDEX> > materialPolygons(serializedPolygons.at<int>(t), std::vector<RTABMAP_PCL_INDEX>(polygonSize));
 						++t;
 						UASSERT(t < serializedPolygons.cols);
 						UDEBUG("materialPolygons=%d", (int)materialPolygons.size());
@@ -6364,6 +6488,61 @@ void DBDriverSqlite3::stepKeypoint(sqlite3_stmt * ppStmt,
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 }

+std::string DBDriverSqlite3::queryStepGlobalDescriptor() const
+{
+	UASSERT(uStrNumCmp(_version, "0.20.0") >= 0);
+	return "INSERT INTO GlobalDescriptor(node_id, type, info, data) VALUES(?,?,?,?);";
+}
+void DBDriverSqlite3::stepGlobalDescriptor(sqlite3_stmt * ppStmt,
+		int nodeId,
+		const GlobalDescriptor & descriptor) const
+{
+	if(!ppStmt)
+	{
+		UFATAL("");
+	}
+	int rc = SQLITE_OK;
+	int index = 1;
+
+	//node_if
+	rc = sqlite3_bind_int(ppStmt, index++, nodeId);
+	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+	//type
+	rc = sqlite3_bind_int(ppStmt, index++, nodeId);
+	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+	//info
+	std::vector<unsigned char> infoBytes = rtabmap::compressData(descriptor.info());
+	if(infoBytes.empty())
+	{
+		rc = sqlite3_bind_null(ppStmt, index++);
+	}
+	else
+	{
+		rc = sqlite3_bind_blob(ppStmt, index++, infoBytes.data(), infoBytes.size(), SQLITE_STATIC);
+	}
+	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+	//data
+	std::vector<unsigned char> dataBytes = rtabmap::compressData(descriptor.data());
+	if(infoBytes.empty())
+	{
+		rc = sqlite3_bind_null(ppStmt, index++);
+	}
+	else
+	{
+		rc = sqlite3_bind_blob(ppStmt, index++, dataBytes.data(), dataBytes.size(), SQLITE_STATIC);
+	}
+	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+	rc=sqlite3_step(ppStmt);
+	UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+	rc = sqlite3_reset(ppStmt);
+	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+}
+
 std::string DBDriverSqlite3::queryStepOccupancyGridUpdate() const
 {
 	UASSERT(uStrNumCmp(_version, "0.11.10") >= 0);
@@ -47,16 +47,16 @@ DBReader::DBReader(const std::string & databasePath,
 				   bool odometryIgnored,
 				   bool ignoreGoalDelay,
 				   bool goalsIgnored,
-				   int stopId,
+				   int startId,
 				   int cameraIndex,
-				   int endId) :
+				   int stopId) :
 	Camera(frameRate),
 	_paths(uSplit(databasePath, ';')),
 	_odometryIgnored(odometryIgnored),
 	_ignoreGoalDelay(ignoreGoalDelay),
 	_goalsIgnored(goalsIgnored),
-	_startId(stopId),
-	_stopId(endId),
+	_startId(startId),
+	_stopId(stopId),
 	_cameraIndex(cameraIndex),
 	_dbDriver(0),
 	_currentId(_ids.end()),
@@ -76,16 +76,16 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
 				   bool odometryIgnored,
 				   bool ignoreGoalDelay,
 				   bool goalsIgnored,
-				   int stopId,
+				   int startId,
 				   int cameraIndex,
-				   int endId) :
+				   int stopId) :
 	Camera(frameRate),
   _paths(databasePaths),
 	_odometryIgnored(odometryIgnored),
 	_ignoreGoalDelay(ignoreGoalDelay),
 	_goalsIgnored(goalsIgnored),
-	_startId(stopId),
-	_stopId(endId),
+	_startId(startId),
+	_stopId(stopId),
 	_cameraIndex(cameraIndex),
 	_dbDriver(0),
 	_currentId(_ids.end()),
@@ -510,23 +510,9 @@ SensorData DBReader::getNextData(CameraInfo * info)
 					data.gps().stamp()!=0.0?1:0,
 					gravityTransform.isNull()?0:1);

-			cv::Mat descriptors;
-			if(!s->getWordsDescriptors().empty())
-			{
-				descriptors = cv::Mat(
-					s->getWordsDescriptors().size(),
-					s->getWordsDescriptors().begin()->second.cols,
-					s->getWordsDescriptors().begin()->second.type());
-				int i=0;
-				for(std::multimap<int, cv::Mat>::const_iterator iter=s->getWordsDescriptors().begin();
-					iter!=s->getWordsDescriptors().end();
-					++iter, ++i)
-				{
-					iter->second.copyTo(descriptors.row(i));
-				}
-			}
-			std::vector<cv::KeyPoint> keypoints = uValues(s->getWords());
-			std::vector<cv::Point3f> keypoints3D = uValues(s->getWords3());
+			cv::Mat descriptors = s->getWordsDescriptors().clone();
+			const std::vector<cv::KeyPoint> & keypoints = s->getWordsKpts();
+			const std::vector<cv::Point3f> & keypoints3D = s->getWords3();
 			if(!keypoints.empty() &&
 			   (keypoints3D.empty() || keypoints.size() == keypoints3D.size()) &&
 			   (descriptors.empty() || (int)keypoints.size() == descriptors.rows))
@@ -70,15 +70,21 @@ bool EpipolarGeometry::check(const Signature * ssA, const Signature * ssB)
 	}
 	ULOGGER_DEBUG("id(%d,%d)", ssA->id(), ssB->id());

-	std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
+	std::list<std::pair<int, std::pair<int, int> > > pairsId;

-	findPairsUnique(ssA->getWords(), ssB->getWords(), pairs);
+	findPairsUnique(ssA->getWords(), ssB->getWords(), pairsId);

-	if((int)pairs.size()<_matchCountMinAccepted)
+	if((int)pairsId.size()<_matchCountMinAccepted)
 	{
 		return false;
 	}

+	std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
+	for(std::list<std::pair<int, std::pair<int, int> > >::iterator iter = pairsId.begin(); iter!=pairsId.end(); ++iter)
+	{
+		pairs.push_back(std::make_pair(iter->first, std::make_pair(ssA->getWordsKpts()[iter->second.first], ssB->getWordsKpts()[iter->second.second])));
+	}
+
 	std::vector<uchar> status;
 	cv::Mat f = findFFromWords(pairs, status, _ransacParam1, _ransacParam2);

@@ -292,8 +298,8 @@ cv::Mat EpipolarGeometry::findPFromE(const cv::Mat & E,
 cv::Mat EpipolarGeometry::findFFromWords(
 		const std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs, // id, kpt1, kpt2
 		std::vector<uchar> & status,
-		double ransacParam1,
-		double ransacParam2)
+		double ransacReprojThreshold,
+		double ransacConfidence)
 {

 	status = std::vector<uchar>(pairs.size(), 0);
@@ -329,8 +335,8 @@ cv::Mat EpipolarGeometry::findFFromWords(
 				points2,
 				status,
 				cv::FM_RANSAC,
-				ransacParam1,
-				ransacParam2);
+				ransacReprojThreshold,
+				ransacConfidence);

 	ULOGGER_DEBUG("Find fundamental matrix (OpenCV) time = %fs", timer.ticks());

@@ -406,136 +412,6 @@ cv::Mat EpipolarGeometry::findFFromCalibratedStereoCameras(double fx, double fy,
 	return K.inv().t()*E*K.inv();
 }

-/**
- * if a=[1 2 3 4 6], b=[1 2 4 5 6], results= [(1,1) (2,2) (4,4) (6,6)]
- * realPairsCount = 4
- */
-int EpipolarGeometry::findPairs(
-		const std::map<int, cv::KeyPoint> & wordsA,
-		const std::map<int, cv::KeyPoint> & wordsB,
-		std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
-		bool ignoreInvalidIds)
-{
-	int realPairsCount = 0;
-	pairs.clear();
-	for(std::map<int, cv::KeyPoint>::const_iterator i=wordsA.begin(); i!=wordsA.end(); ++i)
-	{
-		if(!ignoreInvalidIds || (ignoreInvalidIds && i->first>=0))
-		{
-			std::map<int, cv::KeyPoint>::const_iterator ptB = wordsB.find(i->first);
-			if(ptB != wordsB.end())
-			{
-				pairs.push_back(std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> >(i->first, std::pair<cv::KeyPoint, cv::KeyPoint>(i->second, ptB->second)));
-				++realPairsCount;
-			}
-		}
-	}
-	return realPairsCount;
-}
-
-/**
- * if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(1,1a) (2,2) (4,4) (6a,6a) (6b,6b)]
- * realPairsCount = 5
- */
-int EpipolarGeometry::findPairs(const std::multimap<int, cv::KeyPoint> & wordsA,
-		const std::multimap<int, cv::KeyPoint> & wordsB,
-		std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
-		bool ignoreInvalidIds)
-{
-	const std::list<int> & ids = uUniqueKeys(wordsA);
-	std::multimap<int, cv::KeyPoint>::const_iterator iterA;
-	std::multimap<int, cv::KeyPoint>::const_iterator iterB;
-	pairs.clear();
-	int realPairsCount = 0;
-	for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
-	{
-		if(!ignoreInvalidIds || (ignoreInvalidIds && *i >= 0))
-		{
-			iterA = wordsA.find(*i);
-			iterB = wordsB.find(*i);
-			while(iterA != wordsA.end() && iterB != wordsB.end() && (*iterA).first == (*iterB).first && (*iterA).first == *i)
-			{
-				pairs.push_back(std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> >(*i, std::pair<cv::KeyPoint, cv::KeyPoint>((*iterA).second, (*iterB).second)));
-				++iterA;
-				++iterB;
-				++realPairsCount;
-			}
-		}
-	}
-	return realPairsCount;
-}
-
-/**
- * if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(2,2) (4,4)]
- * realPairsCount = 5
- */
-int EpipolarGeometry::findPairsUnique(
-		const std::multimap<int, cv::KeyPoint> & wordsA,
-		const std::multimap<int, cv::KeyPoint> & wordsB,
-		std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
-		bool ignoreInvalidIds)
-{
-	const std::list<int> & ids = uUniqueKeys(wordsA);
-	int realPairsCount = 0;
-	pairs.clear();
-	for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
-	{
-		if(!ignoreInvalidIds || (ignoreInvalidIds && *i>=0))
-		{
-			std::list<cv::KeyPoint> ptsA = uValues(wordsA, *i);
-			std::list<cv::KeyPoint> ptsB = uValues(wordsB, *i);
-			if(ptsA.size() == 1 && ptsB.size() == 1)
-			{
-				pairs.push_back(std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> >(*i, std::pair<cv::KeyPoint, cv::KeyPoint>(ptsA.front(), ptsB.front())));
-				++realPairsCount;
-			}
-			else if(ptsA.size()>1 && ptsB.size()>1)
-			{
-				// just update the count
-				realPairsCount += ptsA.size() > ptsB.size() ? ptsB.size() : ptsA.size();
-			}
-		}
-	}
-	return realPairsCount;
-}
-
-/**
- * if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(1,1a) (1,1b) (2,2) (4,4) (6a,6a) (6a,6b) (6b,6a) (6b,6b)]
- * realPairsCount = 5
- */
-int EpipolarGeometry::findPairsAll(const std::multimap<int, cv::KeyPoint> & wordsA,
-		const std::multimap<int, cv::KeyPoint> & wordsB,
-		std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
-		bool ignoreInvalidIds)
-{
-	UTimer timer;
-	timer.start();
-	const std::list<int> & ids = uUniqueKeys(wordsA);
-	pairs.clear();
-	int realPairsCount = 0;;
-	for(std::list<int>::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter)
-	{
-		if(!ignoreInvalidIds || (ignoreInvalidIds && *iter>=0))
-		{
-			std::list<cv::KeyPoint> ptsA = uValues(wordsA, *iter);
-			std::list<cv::KeyPoint> ptsB = uValues(wordsB, *iter);
-
-			realPairsCount += ptsA.size() > ptsB.size() ? ptsB.size() : ptsA.size();
-
-			for(std::list<cv::KeyPoint>::iterator jter=ptsA.begin(); jter!=ptsA.end(); ++jter)
-			{
-				for(std::list<cv::KeyPoint>::iterator kter=ptsB.begin(); kter!=ptsB.end(); ++kter)
-				{
-					pairs.push_back(std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> >(*iter, std::pair<cv::KeyPoint, cv::KeyPoint>(*jter, *kter)));
-				}
-			}
-		}
-	}
-	ULOGGER_DEBUG("time = %f", timer.ticks());
-	return realPairsCount;
-}
-
-

 /**
 source = SfM toy library: https://github.com/royshil/SfM-Toy-Library
@@ -44,7 +44,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "opencv/ORBextractor.h"
 #endif

-#ifdef RTABMAP_SP_TORCH
+#ifdef RTABMAP_SUPERPOINT_TORCH
 #include "superpoint_torch/SuperPoint.h"
 #endif

@@ -339,7 +339,7 @@ void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std:
 	if(maxKeypoints > 0 && (int)keypoints.size() > maxKeypoints)
 	{
 		UTimer timer;
-		ULOGGER_DEBUG("too much words (%d), removing words with the hessian threshold", keypoints.size());
+		ULOGGER_DEBUG("too much words (%d), removing words with the hessian threshold", (int)keypoints.size());
 		// Remove words under the new hessian threshold

 		// Sort words by hessian
@@ -365,10 +365,50 @@ void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std:
 	}
 	else
 	{
+		ULOGGER_DEBUG("keeping all %d keypoints", (int)keypoints.size());
 		inliers.resize(keypoints.size(), true);
 	}
 }

+void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints, const cv::Size & imageSize, int gridRows, int gridCols)
+{
+	if(maxKeypoints <= 0 || (int)keypoints.size() <= maxKeypoints)
+	{
+		inliers.resize(keypoints.size(), true);
+		return;
+	}
+	UASSERT(gridCols>=1 && gridRows >=1);
+	UASSERT(imageSize.height>gridRows && imageSize.width>gridCols);
+	int rowSize = imageSize.height / gridRows;
+	int colSize = imageSize.width / gridCols;
+	int maxKeypointsPerCell = maxKeypoints / (gridRows * gridCols);
+	std::vector<std::vector<cv::KeyPoint> > keypointsPerCell(gridRows * gridCols);
+	std::vector<std::vector<int> > indexesPerCell(gridRows * gridCols);
+	for(size_t i=0; i<keypoints.size(); ++i)
+	{
+		int cellRow = int(keypoints[i].pt.y)/rowSize;
+		int cellCol = int(keypoints[i].pt.x)/colSize;
+		UASSERT(cellRow >=0 && cellRow < gridRows);
+		UASSERT(cellCol >=0 && cellCol < gridCols);
+
+		keypointsPerCell[cellRow*gridCols + cellCol].push_back(keypoints[i]);
+		indexesPerCell[cellRow*gridCols + cellCol].push_back(i);
+	}
+	inliers.resize(keypoints.size(), false);
+	for(size_t i=0; i<keypointsPerCell.size(); ++i)
+	{
+		std::vector<bool> inliersCell;
+		limitKeypoints(keypointsPerCell[i], inliersCell, maxKeypointsPerCell);
+		for(size_t j=0; j<inliersCell.size(); ++j)
+		{
+			if(inliersCell[j])
+			{
+				inliers.at(indexesPerCell[i][j]) = true;
+			}
+		}
+	}
+}
+
 cv::Rect Feature2D::computeRoi(const cv::Mat & image, const std::string & roiRatios)
 {
 	return util2d::computeRoi(image, roiRatios);
@@ -414,10 +454,6 @@ void Feature2D::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kKpGridCols(), gridCols_);

 	UASSERT(gridRows_ >= 1 && gridCols_>=1);
-	if(maxFeatures_ > 0)
-	{
-		maxFeatures_ =	maxFeatures_ / (gridRows_ * gridCols_);
-	}

 	// convert ROI from string to vector
 	ParametersMap::const_iterator iter;
@@ -472,41 +508,70 @@ Feature2D * Feature2D::create(const ParametersMap & parameters)
 }
 Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parameters)
 {
-#ifndef RTABMAP_NONFREE
-	if(type == Feature2D::kFeatureSurf || type == Feature2D::kFeatureSift)
+
+// NONFREE checks
+#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <= 3) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION < 4 || (CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<11)))
+
+  #ifndef RTABMAP_NONFREE
+	if(type == Feature2D::kFeatureSurf || type == Feature2D::kFeatureSift || type == Feature2D::kFeatureSurfFreak || type == Feature2D::kFeatureSurfDaisy)
 	{
-#if CV_MAJOR_VERSION < 3
+    #if CV_MAJOR_VERSION < 3
 		UWARN("SURF and SIFT features cannot be used because OpenCV was not built with nonfree module. GFTT/ORB is used instead.");
-#else
+    #else
 		UWARN("SURF and SIFT features cannot be used because OpenCV was not built with xfeatures2d module. GFTT/ORB is used instead.");
-#endif
+    #endif
 		type = Feature2D::kFeatureGfttOrb;
 	}
-#if CV_MAJOR_VERSION == 3
+  #endif
+
+#else // >= 4.4.0 >= 3.4.11
+
+  #ifndef RTABMAP_NONFREE
+	if(type == Feature2D::kFeatureSurf)
+	{
+		UWARN("SURF features cannot be used because OpenCV was not built with nonfree module. SIFT is used instead.");
+		type = Feature2D::kFeatureSift;
+	}
+	else if(type == Feature2D::kFeatureSurfFreak || type == Feature2D::kFeatureSurfDaisy)
+	{
+		UWARN("SURF detector cannot be used because OpenCV was not built with nonfree module. GFTT/ORB is used instead.");
+		type = Feature2D::kFeatureGfttOrb;
+	}
+  #endif
+
+#endif // >= 4.4.0 >= 3.4.11
+
+#if !defined(HAVE_OPENCV_XFEATURES2D) && CV_MAJOR_VERSION >= 3
 	if(type == Feature2D::kFeatureFastBrief ||
 	   type == Feature2D::kFeatureFastFreak ||
 	   type == Feature2D::kFeatureGfttBrief ||
-	   type == Feature2D::kFeatureGfttFreak)
+	   type == Feature2D::kFeatureGfttFreak ||
+	   type == Feature2D::kFeatureSurfFreak ||
+	   type == Feature2D::kFeatureGfttDaisy ||
+	   type == Feature2D::kFeatureSurfDaisy)
 	{
-		UWARN("BRIEF and FREAK features cannot be used because OpenCV was not built with xfeatures2d module. GFTT/ORB is used instead.");
+		UWARN("BRIEF, FREAK and DAISY features cannot be used because OpenCV was not built with xfeatures2d module. GFTT/ORB is used instead.");
 		type = Feature2D::kFeatureGfttOrb;
 	}
-#endif
-#endif
-
-#if CV_MAJOR_VERSION < 3
+#elif CV_MAJOR_VERSION < 3
 	if(type == Feature2D::kFeatureKaze)
 	{
-#ifdef RTABMAP_NONFREE
+  #ifdef RTABMAP_NONFREE
 		UWARN("KAZE detector/descriptor can be used only with OpenCV3. SURF is used instead.");
 		type = Feature2D::kFeatureSurf;
-#else
+  #else
 		UWARN("KAZE detector/descriptor can be used only with OpenCV3. GFTT/ORB is used instead.");
 		type = Feature2D::kFeatureGfttOrb;
-#endif
+  #endif
+	}
+	if(type == Feature2D::kFeatureGfttDaisy || type == Feature2D::kFeatureSurfDaisy)
+	{
+		UWARN("DAISY detector/descriptor can be used only with OpenCV3. GFTT/BRIEF is used instead.");
+		type = Feature2D::kFeatureGfttBrief;
 	}
 #endif

+
 #ifndef RTABMAP_ORB_OCTREE
 	if(type == Feature2D::kFeatureOrbOctree)
 	{
@@ -515,7 +580,7 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame
 	}
 #endif

-#ifndef RTABMAP_SP_TORCH
+#ifndef RTABMAP_SUPERPOINT_TORCH
 	if(type == Feature2D::kFeatureSuperPointTorch)
 	{
 		UWARN("SupertPoint Torch feature cannot be used as RTAB-Map is not built with the option enabled. GFTT/ORB is used instead.");
@@ -559,11 +624,20 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame
 	case Feature2D::kFeatureOrbOctree:
 		feature2D = new ORBOctree(parameters);
 		break;
-#ifdef RTABMAP_SP_TORCH
+#ifdef RTABMAP_SUPERPOINT_TORCH
 	case Feature2D::kFeatureSuperPointTorch:
 		feature2D = new SuperPointTorch(parameters);
 		break;
 #endif
+	case Feature2D::kFeatureSurfFreak:
+		feature2D = new SURF_FREAK(parameters);
+		break;
+	case Feature2D::kFeatureGfttDaisy:
+		feature2D = new GFTT_DAISY(parameters);
+		break;
+	case Feature2D::kFeatureSurfDaisy:
+		feature2D = new SURF_DAISY(parameters);
+		break;
 #ifdef RTABMAP_NONFREE
 	default:
 		feature2D = new SURF(parameters);
@@ -572,7 +646,7 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame
 #else
 	default:
 		feature2D = new ORB(parameters);
-		type = Feature2D::kFeatureOrb;
+		type = Feature2D::kFeatureGfttOrb;
 		break;
 #endif

@@ -639,6 +713,7 @@ std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, co
 	// Get keypoints
 	int rowSize = globalRoi.height / gridRows_;
 	int colSize = globalRoi.width / gridCols_;
+	int maxFeatures =	maxFeatures_ / (gridRows_ * gridCols_);
 	for (int i = 0; i<gridRows_; ++i)
 	{
 		for (int j = 0; j<gridCols_; ++j)
@@ -646,7 +721,7 @@ std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, co
 			cv::Rect roi(globalRoi.x + j*colSize, globalRoi.y + i*rowSize, colSize, rowSize);
 			std::vector<cv::KeyPoint> sub_keypoints;
 			sub_keypoints = this->generateKeypointsImpl(image, roi, mask);
-			limitKeypoints(sub_keypoints, maxFeatures_);
+			limitKeypoints(sub_keypoints, maxFeatures);
 			if(roi.x || roi.y)
 			{
 				// Adjust keypoint position to raw image
@@ -659,7 +734,8 @@ std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, co
 			keypoints.insert( keypoints.end(), sub_keypoints.begin(), sub_keypoints.end() );
 		}
 	}
-	UDEBUG("Keypoints extraction time = %f s, keypoints extracted = %d (mask empty=%d)", timer.ticks(), keypoints.size(), mask.empty()?1:0);
+	UDEBUG("Keypoints extraction time = %f s, keypoints extracted = %d (grid=%dx%d, mask empty=%d)",
+			timer.ticks(), keypoints.size(), gridCols_, gridRows_,  mask.empty()?1:0);

 	if(keypoints.size() && _subPixWinSize > 0 && _subPixIterations > 0)
 	{
@@ -891,7 +967,8 @@ SIFT::SIFT(const ParametersMap & parameters) :
 	nOctaveLayers_(Parameters::defaultSIFTNOctaveLayers()),
 	contrastThreshold_(Parameters::defaultSIFTContrastThreshold()),
 	edgeThreshold_(Parameters::defaultSIFTEdgeThreshold()),
-	sigma_(Parameters::defaultSIFTSigma())
+	sigma_(Parameters::defaultSIFTSigma()),
+	rootSIFT_(Parameters::defaultSIFTRootSIFT())
 {
 	parseParameters(parameters);
 }
@@ -908,7 +985,9 @@ void SIFT::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kSIFTEdgeThreshold(), edgeThreshold_);
 	Parameters::parse(parameters, Parameters::kSIFTNOctaveLayers(), nOctaveLayers_);
 	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)))
 #ifdef RTABMAP_NONFREE
 #if CV_MAJOR_VERSION < 3
 	_sift = cv::Ptr<CV_SIFT>(new CV_SIFT(this->getMaxFeatures(), nOctaveLayers_, contrastThreshold_, edgeThreshold_, sigma_));
@@ -918,22 +997,29 @@ void SIFT::parseParameters(const ParametersMap & parameters)
 #else
 	UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
 #endif
+#else // >=4.4, >=3.4.11
+	_sift = CV_SIFT::create(this->getMaxFeatures(), nOctaveLayers_, contrastThreshold_, edgeThreshold_, sigma_);
+#endif
 }

 std::vector<cv::KeyPoint> SIFT::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask)
 {
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	std::vector<cv::KeyPoint> keypoints;
-#ifdef RTABMAP_NONFREE
 	cv::Mat imgRoi(image, roi);
 	cv::Mat maskRoi;
 	if(!mask.empty())
 	{
 		maskRoi = cv::Mat(mask, roi);
 	}
+#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <= 3) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION < 4 || (CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<11)))
+#ifdef RTABMAP_NONFREE
 	_sift->detect(imgRoi, keypoints, maskRoi); // Opencv keypoints
 #else
 	UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
+#endif
+#else // >=4.4, >=3.4.11
+	_sift->detect(imgRoi, keypoints, maskRoi); // Opencv keypoints
 #endif
 	return keypoints;
 }
@@ -942,11 +1028,30 @@ cv::Mat SIFT::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::Key
 {
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	cv::Mat descriptors;
+#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <= 3) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION < 4 || (CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<11)))
 #ifdef RTABMAP_NONFREE
 	_sift->compute(image, keypoints, descriptors);
 #else
 	UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
 #endif
+#else // >=4.4, >=3.4.11
+	_sift->compute(image, keypoints, descriptors);
+#endif
+	if( rootSIFT_ && !descriptors.empty())
+	{
+		UDEBUG("Performing RootSIFT...");
+		// see http://www.pyimagesearch.com/2015/04/13/implementing-rootsift-in-python-and-opencv/
+		// apply the Hellinger kernel by first L1-normalizing and taking the
+		// square-root
+		for(int i=0; i<descriptors.rows; ++i)
+		{
+			// By taking the L1 norm, followed by the square-root, we have
+			// already L2 normalized the feature vector and further normalization
+			// is not needed.
+			descriptors.row(i) = descriptors.row(i) / cv::sum(descriptors.row(i))[0];
+			cv::sqrt(descriptors.row(i), descriptors.row(i));
+		}
+	}
 	return descriptors;
 }

@@ -1649,6 +1754,59 @@ cv::Mat GFTT_FREAK::generateDescriptorsImpl(const cv::Mat & image, std::vector<c
 	return descriptors;
 }

+//////////////////////////
+//SURF-FREAK
+//////////////////////////
+SURF_FREAK::SURF_FREAK(const ParametersMap & parameters) :
+	SURF(parameters),
+	orientationNormalized_(Parameters::defaultFREAKOrientationNormalized()),
+	scaleNormalized_(Parameters::defaultFREAKScaleNormalized()),
+	patternScale_(Parameters::defaultFREAKPatternScale()),
+	nOctaves_(Parameters::defaultFREAKNOctaves())
+{
+	parseParameters(parameters);
+}
+
+SURF_FREAK::~SURF_FREAK()
+{
+}
+
+void SURF_FREAK::parseParameters(const ParametersMap & parameters)
+{
+	SURF::parseParameters(parameters);
+
+	Parameters::parse(parameters, Parameters::kFREAKOrientationNormalized(), orientationNormalized_);
+	Parameters::parse(parameters, Parameters::kFREAKScaleNormalized(), scaleNormalized_);
+	Parameters::parse(parameters, Parameters::kFREAKPatternScale(), patternScale_);
+	Parameters::parse(parameters, Parameters::kFREAKNOctaves(), nOctaves_);
+
+#if CV_MAJOR_VERSION < 3
+	_freak = cv::Ptr<CV_FREAK>(new CV_FREAK(orientationNormalized_, scaleNormalized_, patternScale_, nOctaves_));
+#else
+#ifdef HAVE_OPENCV_XFEATURES2D
+	_freak = CV_FREAK::create(orientationNormalized_, scaleNormalized_, patternScale_, nOctaves_);
+#else
+	UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so Freak cannot be used!");
+#endif
+#endif
+}
+
+cv::Mat SURF_FREAK::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
+{
+	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
+	cv::Mat descriptors;
+#if CV_MAJOR_VERSION < 3
+	_freak->compute(image, keypoints, descriptors);
+#else
+#ifdef HAVE_OPENCV_XFEATURES2D
+	_freak->compute(image, keypoints, descriptors);
+#else
+	UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so Freak cannot be used!");
+#endif
+#endif
+	return descriptors;
+}
+
 //////////////////////////
 //GFTT-ORB
 //////////////////////////
@@ -1801,7 +1959,10 @@ cv::Mat KAZE::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::Key
 ORBOctree::ORBOctree(const ParametersMap & parameters) :
 		scaleFactor_(Parameters::defaultORBScaleFactor()),
 		nLevels_(Parameters::defaultORBNLevels()),
-		fastThreshold_(Parameters::defaultFASTThreshold())
+		patchSize_(Parameters::defaultORBPatchSize()),
+		edgeThreshold_(Parameters::defaultORBEdgeThreshold()),
+		fastThreshold_(Parameters::defaultFASTThreshold()),
+		fastMinThreshold_(Parameters::defaultFASTMinThreshold())
 {
 	parseParameters(parameters);
 }
@@ -1816,12 +1977,14 @@ void ORBOctree::parseParameters(const ParametersMap & parameters)

 	Parameters::parse(parameters, Parameters::kORBScaleFactor(), scaleFactor_);
 	Parameters::parse(parameters, Parameters::kORBNLevels(), nLevels_);
+	Parameters::parse(parameters, Parameters::kORBPatchSize(), patchSize_);
+	Parameters::parse(parameters, Parameters::kORBEdgeThreshold(), edgeThreshold_);

 	Parameters::parse(parameters, Parameters::kFASTThreshold(), fastThreshold_);
 	Parameters::parse(parameters, Parameters::kFASTMinThreshold(), fastMinThreshold_);

 #ifdef RTABMAP_ORB_OCTREE
-	_orb = cv::Ptr<ORBextractor>(new ORBextractor(this->getMaxFeatures(), scaleFactor_, nLevels_, fastThreshold_, fastMinThreshold_));
+	_orb = cv::Ptr<ORBextractor>(new ORBextractor(this->getMaxFeatures(), scaleFactor_, nLevels_, fastThreshold_, fastMinThreshold_, patchSize_, edgeThreshold_));
 #else
 	UWARN("RTAB-Map is not built with ORB OcTree option enabled so ORB OcTree feature cannot be used!");
 #endif
@@ -1866,11 +2029,11 @@ cv::Mat ORBOctree::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv
 //SuperPointTorch
 //////////////////////////
 SuperPointTorch::SuperPointTorch(const ParametersMap & parameters) :
-		path_(Parameters::defaultSPTorchModelPath()),
-		threshold_(Parameters::defaultSPTorchThreshold()),
-		nms_(Parameters::defaultSPTorchNMS()),
-		minDistance_(Parameters::defaultSPTorchMinDistance()),
-		cuda_(Parameters::defaultSPTorchCuda())
+		path_(Parameters::defaultSuperPointModelPath()),
+		threshold_(Parameters::defaultSuperPointThreshold()),
+		nms_(Parameters::defaultSuperPointNMS()),
+		minDistance_(Parameters::defaultSuperPointNMSRadius()),
+		cuda_(Parameters::defaultSuperPointCuda())
 {
 	parseParameters(parameters);
 }
@@ -1884,14 +2047,16 @@ void SuperPointTorch::parseParameters(const ParametersMap & parameters)
 	Feature2D::parseParameters(parameters);

 	std::string previousPath = path_;
+#ifdef RTABMAP_SUPERPOINT_TORCH
 	bool previousCuda = cuda_;
-	Parameters::parse(parameters, Parameters::kSPTorchModelPath(), path_);
-	Parameters::parse(parameters, Parameters::kSPTorchThreshold(), threshold_);
-	Parameters::parse(parameters, Parameters::kSPTorchNMS(), nms_);
-	Parameters::parse(parameters, Parameters::kSPTorchMinDistance(), minDistance_);
-	Parameters::parse(parameters, Parameters::kSPTorchCuda(), cuda_);
+#endif
+	Parameters::parse(parameters, Parameters::kSuperPointModelPath(), path_);
+	Parameters::parse(parameters, Parameters::kSuperPointThreshold(), threshold_);
+	Parameters::parse(parameters, Parameters::kSuperPointNMS(), nms_);
+	Parameters::parse(parameters, Parameters::kSuperPointNMSRadius(), minDistance_);
+	Parameters::parse(parameters, Parameters::kSuperPointCuda(), cuda_);

-#ifdef RTABMAP_SP_TORCH
+#ifdef RTABMAP_SUPERPOINT_TORCH
 	if(superPoint_.get() == 0 || path_.compare(previousPath) != 0 || previousCuda != cuda_)
 	{
 		superPoint_ = cv::Ptr<SPDetector>(new SPDetector(path_, threshold_, nms_, minDistance_, cuda_));
@@ -1909,10 +2074,10 @@ void SuperPointTorch::parseParameters(const ParametersMap & parameters)

 std::vector<cv::KeyPoint> SuperPointTorch::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask)
 {
-#ifdef RTABMAP_SP_TORCH
+#ifdef RTABMAP_SUPERPOINT_TORCH
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	UASSERT_MSG(roi.x==0 && roi.y ==0, "Not supporting ROI");
-	return superPoint_->detect(image);
+	return superPoint_->detect(image, mask);
 #else
 	UWARN("RTAB-Map is not built with SuperPoint Torch support so SuperPoint Torch feature cannot be used!");
 	return std::vector<cv::KeyPoint>();
@@ -1921,7 +2086,7 @@ std::vector<cv::KeyPoint> SuperPointTorch::generateKeypointsImpl(const cv::Mat &

 cv::Mat SuperPointTorch::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
 {
-#ifdef RTABMAP_SP_TORCH
+#ifdef RTABMAP_SUPERPOINT_TORCH
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	return superPoint_->compute(keypoints);
 #else
@@ -1930,4 +2095,95 @@ cv::Mat SuperPointTorch::generateDescriptorsImpl(const cv::Mat & image, std::vec
 #endif
 }

+
+//////////////////////////
+//GFTT-DAISY
+//////////////////////////
+GFTT_DAISY::GFTT_DAISY(const ParametersMap & parameters) :
+	GFTT(parameters),
+	orientationNormalized_(Parameters::defaultFREAKOrientationNormalized()),
+	scaleNormalized_(Parameters::defaultFREAKScaleNormalized()),
+	patternScale_(Parameters::defaultFREAKPatternScale()),
+	nOctaves_(Parameters::defaultFREAKNOctaves())
+{
+	parseParameters(parameters);
+}
+
+GFTT_DAISY::~GFTT_DAISY()
+{
+}
+
+void GFTT_DAISY::parseParameters(const ParametersMap & parameters)
+{
+	GFTT::parseParameters(parameters);
+
+	Parameters::parse(parameters, Parameters::kFREAKOrientationNormalized(), orientationNormalized_);
+	Parameters::parse(parameters, Parameters::kFREAKScaleNormalized(), scaleNormalized_);
+	Parameters::parse(parameters, Parameters::kFREAKPatternScale(), patternScale_);
+	Parameters::parse(parameters, Parameters::kFREAKNOctaves(), nOctaves_);
+
+#ifdef HAVE_OPENCV_XFEATURES2D
+	_daisy = CV_DAISY::create();
+#else
+	UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so DAISY cannot be used!");
+#endif
+}
+
+cv::Mat GFTT_DAISY::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
+{
+	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
+	cv::Mat descriptors;
+#ifdef HAVE_OPENCV_XFEATURES2D
+	_daisy->compute(image, keypoints, descriptors);
+#else
+	UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so DAISY cannot be used!");
+#endif
+	return descriptors;
+}
+
+//////////////////////////
+//SURF-DAISY
+//////////////////////////
+SURF_DAISY::SURF_DAISY(const ParametersMap & parameters) :
+	SURF(parameters),
+	orientationNormalized_(Parameters::defaultFREAKOrientationNormalized()),
+	scaleNormalized_(Parameters::defaultFREAKScaleNormalized()),
+	patternScale_(Parameters::defaultFREAKPatternScale()),
+	nOctaves_(Parameters::defaultFREAKNOctaves())
+{
+	parseParameters(parameters);
+}
+
+SURF_DAISY::~SURF_DAISY()
+{
+}
+
+void SURF_DAISY::parseParameters(const ParametersMap & parameters)
+{
+	SURF::parseParameters(parameters);
+
+	Parameters::parse(parameters, Parameters::kFREAKOrientationNormalized(), orientationNormalized_);
+	Parameters::parse(parameters, Parameters::kFREAKScaleNormalized(), scaleNormalized_);
+	Parameters::parse(parameters, Parameters::kFREAKPatternScale(), patternScale_);
+	Parameters::parse(parameters, Parameters::kFREAKNOctaves(), nOctaves_);
+
+#ifdef HAVE_OPENCV_XFEATURES2D
+	_daisy = CV_DAISY::create();
+#else
+	UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so DAISY cannot be used!");
+#endif
+}
+
+cv::Mat SURF_DAISY::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
+{
+	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
+	cv::Mat descriptors;
+#ifdef HAVE_OPENCV_XFEATURES2D
+	_daisy->compute(image, keypoints, descriptors);
+#else
+	UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so DAISY cannot be used!");
+#endif
+	return descriptors;
+}
+
 }
@@ -107,32 +107,36 @@ unsigned int FlannIndex::indexedFeatures() const
 	}
 }

-// return KB
-unsigned int FlannIndex::memoryUsed() const
+// return Bytes
+unsigned long FlannIndex::memoryUsed() const
 {
 	if(!index_)
 	{
 		return 0;
 	}
+	unsigned long memoryUsage = sizeof(FlannIndex);
+	memoryUsage += addedDescriptors_.size() * (sizeof(int) + sizeof(cv::Mat) + sizeof(std::map<int, cv::Mat>::iterator)) + sizeof(std::map<int, cv::Mat>);
+	memoryUsage += sizeof(std::list<int>) + removedIndexes_.size() * sizeof(int);
 	if(featuresType_ == CV_8UC1)
 	{
-		return ((const rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->usedMemory()/1000;
+		memoryUsage += ((const rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->usedMemory();
 	}
 	else
 	{
 		if(useDistanceL1_)
 		{
-			return ((const rtflann::Index<rtflann::L1<float> >*)index_)->usedMemory()/1000;
+			memoryUsage += ((const rtflann::Index<rtflann::L1<float> >*)index_)->usedMemory();
 		}
 		else if(featuresDim_ <= 3)
 		{
-			return ((const rtflann::Index<rtflann::L2_Simple<float> >*)index_)->usedMemory()/1000;
+			memoryUsage += ((const rtflann::Index<rtflann::L2_Simple<float> >*)index_)->usedMemory();
 		}
 		else
 		{
-			return ((const rtflann::Index<rtflann::L2<float> >*)index_)->usedMemory()/1000;
+			memoryUsage += ((const rtflann::Index<rtflann::L2<float> >*)index_)->usedMemory();
 		}
 	}
+	return memoryUsage;
 }

 void FlannIndex::buildLinearIndex(
@@ -177,10 +181,21 @@ void FlannIndex::buildLinearIndex(
 		}
 	}

-	// incremental FLANN
-	addedDescriptors_.insert(std::make_pair(nextIndex_, features));
-
-	nextIndex_ = features.rows;
+	// incremental FLANN: we should add all headers separately in case we remove
+	// some indexes (to keep underlying matrix data allocated)
+	if(rebalancingFactor_ > 1.0f)
+	{
+		for(int i=0; i<features.rows; ++i)
+		{
+			addedDescriptors_.insert(std::make_pair(nextIndex_++, features.row(i)));
+		}
+	}
+	else
+	{
+		// tree won't ever be rebalanced, so just keep only one header for the data
+		addedDescriptors_.insert(std::make_pair(nextIndex_, features));
+		nextIndex_ += features.rows;
+	}
 	UDEBUG("");
 }

@@ -227,10 +242,21 @@ void FlannIndex::buildKDTreeIndex(
 		}
 	}

-	// incremental FLANN
-	addedDescriptors_.insert(std::make_pair(nextIndex_, features));
-
-	nextIndex_ = features.rows;
+	// incremental FLANN: we should add all headers separately in case we remove
+	// some indexes (to keep underlying matrix data allocated)
+	if(rebalancingFactor_ > 1.0f)
+	{
+		for(int i=0; i<features.rows; ++i)
+		{
+			addedDescriptors_.insert(std::make_pair(nextIndex_++, features.row(i)));
+		}
+	}
+	else
+	{
+		// tree won't ever be rebalanced, so just keep only one header for the data
+		addedDescriptors_.insert(std::make_pair(nextIndex_, features));
+		nextIndex_ += features.rows;
+	}
 	UDEBUG("");
 }

@@ -278,10 +304,21 @@ void FlannIndex::buildKDTreeSingleIndex(
 		}
 	}

-	// incremental FLANN
-	addedDescriptors_.insert(std::make_pair(nextIndex_, features));
-
-	nextIndex_ = features.rows;
+	// incremental FLANN: we should add all headers separately in case we remove
+	// some indexes (to keep underlying matrix data allocated)
+	if(rebalancingFactor_ > 1.0f)
+	{
+		for(int i=0; i<features.rows; ++i)
+		{
+			addedDescriptors_.insert(std::make_pair(nextIndex_++, features.row(i)));
+		}
+	}
+	else
+	{
+		// tree won't ever be rebalanced, so just keep only one header for the data
+		addedDescriptors_.insert(std::make_pair(nextIndex_, features));
+		nextIndex_ += features.rows;
+	}
 	UDEBUG("");
 }

@@ -305,10 +342,21 @@ void FlannIndex::buildLSHIndex(
 	index_ = new rtflann::Index<rtflann::Hamming<unsigned char> >(dataset, rtflann::LshIndexParams(12, 20, 2));
 	((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->buildIndex();

-	// incremental FLANN
-	addedDescriptors_.insert(std::make_pair(nextIndex_, features));
-
-	nextIndex_ = features.rows;
+	// incremental FLANN: we should add all headers separately in case we remove
+	// some indexes (to keep underlying matrix data allocated)
+	if(rebalancingFactor_ > 1.0f)
+	{
+		for(int i=0; i<features.rows; ++i)
+		{
+			addedDescriptors_.insert(std::make_pair(nextIndex_++, features.row(i)));
+		}
+	}
+	else
+	{
+		// tree won't ever be rebalanced, so just keep only one header for the data
+		addedDescriptors_.insert(std::make_pair(nextIndex_, features));
+		nextIndex_ += features.rows;
+	}
 	UDEBUG("");
 }

@@ -317,12 +365,12 @@ bool FlannIndex::isBuilt()
 	return index_!=0;
 }

-unsigned int FlannIndex::addPoints(const cv::Mat & features)
+std::vector<unsigned int> FlannIndex::addPoints(const cv::Mat & features)
 {
 	if(!index_)
 	{
 		UERROR("Flann index not yet created!");
-		return 0;
+		return std::vector<unsigned int>();
 	}
 	UASSERT(features.type() == featuresType_);
 	UASSERT(features.cols == featuresDim_);
@@ -401,11 +449,16 @@ unsigned int FlannIndex::addPoints(const cv::Mat & features)
 		removedIndexes_.clear();
 	}

-	addedDescriptors_.insert(std::make_pair(nextIndex_, features));
+	// incremental FLANN: we should add all headers separately in case we remove
+	// some indexes (to keep underlying matrix data allocated)
+	std::vector<unsigned int> indexes;
+	for(int i=0; i<features.rows; ++i)
+	{
+		indexes.push_back(nextIndex_);
+		addedDescriptors_.insert(std::make_pair(nextIndex_++, features.row(i)));
+	}

-	int r = nextIndex_;
-	nextIndex_ += features.rows;
-	return r;
+	return indexes;
 }

 void FlannIndex::removePoint(unsigned int index)
@@ -39,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/search/kdtree.h>
 #include <pcl/common/eigen.h>
 #include <pcl/common/common.h>
+#include <pcl/common/point_tests.h>
 #include <set>
 #include <queue>
 #include <fstream>
@@ -168,7 +169,7 @@ bool exportPoses(

 bool importPoses(
 		const std::string & filePath,
-		int format, // 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame), 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe, 9=EuRoC MAC
+		int format, // 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame), 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe, 9=EuRoC MAV
 		std::map<int, Transform> & poses,
 		std::multimap<int, Link> * constraints, // optional for formats 3 and 4
 		std::map<int, double> * stamps) // optional for format 1 and 9
@@ -1128,19 +1129,22 @@ std::multimap<int, Link> filterDuplicateLinks(

 std::multimap<int, Link> filterLinks(
 		const std::multimap<int, Link> & links,
-		Link::Type filteredType)
+		Link::Type filteredType,
+		bool inverted)
 {
 	std::multimap<int, Link> output;
 	for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
 	{
 		if(filteredType == Link::kSelfRefLink)
 		{
-			if(iter->second.from() != iter->second.to())
+			if((!inverted && iter->second.from() != iter->second.to())||
+			   (inverted && iter->second.from() == iter->second.to()))
 			{
 				output.insert(*iter);
 			}
 		}
-		else if(iter->second.type() != filteredType)
+		else if((!inverted && iter->second.type() != filteredType)||
+				(inverted && iter->second.type() == filteredType))
 		{
 			output.insert(*iter);
 		}
@@ -1150,19 +1154,22 @@ std::multimap<int, Link> filterLinks(

 std::map<int, Link> filterLinks(
 		const std::map<int, Link> & links,
-		Link::Type filteredType)
+		Link::Type filteredType,
+		bool inverted)
 {
 	std::map<int, Link> output;
 	for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
 	{
 		if(filteredType == Link::kSelfRefLink)
 		{
-			if(iter->second.from() != iter->second.to())
+			if((!inverted && iter->second.from() != iter->second.to())||
+			   (inverted && iter->second.from() == iter->second.to()))
 			{
 				output.insert(*iter);
 			}
 		}
-		else if(iter->second.type() != filteredType)
+		else if((!inverted && iter->second.type() != filteredType)||
+				(inverted && iter->second.type() == filteredType))
 		{
 			output.insert(*iter);
 		}
@@ -2053,23 +2060,28 @@ std::list<std::pair<int, Transform> > computePath(

 int findNearestNode(
 		const std::map<int, rtabmap::Transform> & nodes,
-		const rtabmap::Transform & targetPose)
+		const rtabmap::Transform & targetPose,
+		float * distance)
 {
 	int id = 0;
-	std::vector<int> nearestNodes = findNearestNodes(nodes, targetPose, 1);
-	if(nearestNodes.size())
+	std::map<int, float> nearestNodes = findNearestNodes(nodes, targetPose, 1);
+	if(!nearestNodes.empty())
 	{
-		id = nearestNodes[0];
+		id = nearestNodes.begin()->first;
+		if(distance)
+		{
+			*distance = nearestNodes.begin()->second;
+		}
 	}
 	return id;
 }

-std::vector<int> findNearestNodes(
+std::map<int, float> findNearestNodes(
 		const std::map<int, rtabmap::Transform> & nodes,
 		const rtabmap::Transform & targetPose,
 		int k)
 {
-	std::vector<int> nearestIds;
+	std::map<int, float> nearestIds;
 	if(nodes.size() && !targetPose.isNull())
 	{
 		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
@@ -2089,10 +2101,9 @@ std::vector<int> findNearestNodes(
 		pcl::PointXYZ pt(targetPose.x(), targetPose.y(), targetPose.z());
 		kdTree->nearestKSearch(pt, k, ind, dist);

-		nearestIds.resize(ind.size());
 		for(unsigned int i=0; i<ind.size(); ++i)
 		{
-			nearestIds[i] = ids[ind[i]];
+			nearestIds.insert(std::make_pair(ids[ind[i]], dist[i]));
 		}
 	}
 	return nearestIds;
@@ -2105,8 +2116,21 @@ std::map<int, float> getNodesInRadius(
 		float radius)
 {
 	UASSERT(uContains(nodes, nodeId));
+
+	std::map<int, Transform> nodesMinusTarget = nodes;
+	Transform targetPose = nodes.at(nodeId);
+	nodesMinusTarget.erase(nodeId);
+	return getNodesInRadius(targetPose, nodesMinusTarget, radius);
+}
+
+// return <id, sqrd distance>, excluding query
+std::map<int, float> getNodesInRadius(
+		const Transform & targetPose,
+		const std::map<int, Transform> & nodes,
+		float radius)
+{
 	std::map<int, float> foundNodes;
-	if(nodes.size() <= 1)
+	if(nodes.empty())
 	{
 		return foundNodes;
 	}
@@ -2117,26 +2141,21 @@ std::map<int, float> getNodesInRadius(
 	int oi = 0;
 	for(std::map<int, Transform>::const_iterator iter = nodes.begin(); iter!=nodes.end(); ++iter)
 	{
-		if(iter->first != nodeId)
-		{
-			(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
-			UASSERT_MSG(pcl::isFinite((*cloud)[oi]), uFormat("Invalid pose (%d) %s", iter->first, iter->second.prettyPrint().c_str()).c_str());
-			ids[oi] = iter->first;
-			++oi;
-		}
+		(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
+		UASSERT_MSG(pcl::isFinite((*cloud)[oi]), uFormat("Invalid pose (%d) %s", iter->first, iter->second.prettyPrint().c_str()).c_str());
+		ids[oi] = iter->first;
+		++oi;
 	}
 	cloud->resize(oi);
 	ids.resize(oi);

-	Transform fromT = nodes.at(nodeId);
-
 	if(cloud->size())
 	{
 		pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
 		kdTree->setInputCloud(cloud);
 		std::vector<int> ind;
 		std::vector<float> sqrdDist;
-		pcl::PointXYZ pt(fromT.x(), fromT.y(), fromT.z());
+		pcl::PointXYZ pt(targetPose.x(), targetPose.y(), targetPose.z());
 		kdTree->radiusSearch(pt, radius, ind, sqrdDist, 0);
 		for(unsigned int i=0; i<ind.size(); ++i)
 		{
@@ -2158,8 +2177,21 @@ std::map<int, Transform> getPosesInRadius(
 		float angle)
 {
 	UASSERT(uContains(nodes, nodeId));
+
+	std::map<int, Transform> nodesMinusTarget = nodes;
+	Transform targetPose = nodes.at(nodeId);
+	nodesMinusTarget.erase(nodeId);
+	return getPosesInRadius(targetPose, nodesMinusTarget, radius, angle);
+}
+// return <id, Transform>, excluding query
+std::map<int, Transform> getPosesInRadius(
+		const Transform & targetPose,
+		const std::map<int, Transform> & nodes,
+		float radius,
+		float angle)
+{
 	std::map<int, Transform> foundNodes;
-	if(nodes.size() <= 1)
+	if(nodes.empty())
 	{
 		return foundNodes;
 	}
@@ -2170,29 +2202,24 @@ std::map<int, Transform> getPosesInRadius(
 	int oi = 0;
 	for(std::map<int, Transform>::const_iterator iter = nodes.begin(); iter!=nodes.end(); ++iter)
 	{
-		if(iter->first != nodeId)
-		{
-			(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
-			UASSERT_MSG(pcl::isFinite((*cloud)[oi]), uFormat("Invalid pose (%d) %s", iter->first, iter->second.prettyPrint().c_str()).c_str());
-			ids[oi] = iter->first;
-			++oi;
-		}
+		(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
+		UASSERT_MSG(pcl::isFinite((*cloud)[oi]), uFormat("Invalid pose (%d) %s", iter->first, iter->second.prettyPrint().c_str()).c_str());
+		ids[oi] = iter->first;
+		++oi;
 	}
 	cloud->resize(oi);
 	ids.resize(oi);

-	Transform fromT = nodes.at(nodeId);
-
 	if(cloud->size())
 	{
 		pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
 		kdTree->setInputCloud(cloud);
 		std::vector<int> ind;
 		std::vector<float> sqrdDist;
-		pcl::PointXYZ pt(fromT.x(), fromT.y(), fromT.z());
+		pcl::PointXYZ pt(targetPose.x(), targetPose.y(), targetPose.z());
 		kdTree->radiusSearch(pt, radius, ind, sqrdDist, 0);

-		Eigen::Vector3f vA = fromT.toEigen3f().linear()*Eigen::Vector3f(1,0,0);
+		Eigen::Vector3f vA = targetPose.toEigen3f().linear()*Eigen::Vector3f(1,0,0);

 		for(unsigned int i=0; i<ind.size(); ++i)
 		{
@@ -36,6 +36,8 @@ MarkerDetector::MarkerDetector(const ParametersMap & parameters)
 #ifdef HAVE_OPENCV_ARUCO
 	markerLength_ = Parameters::defaultMarkerLength();
 	maxDepthError_ = Parameters::defaultMarkerMaxDepthError();
+	maxRange_ = Parameters::defaultMarkerMaxRange();
+	minRange_ = Parameters::defaultMarkerMinRange();
 	dictionaryId_ = Parameters::defaultMarkerDictionary();
 #if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=2)
 	detectorParams_ = cv::aruco::DetectorParameters::create();
@@ -87,6 +89,8 @@ void MarkerDetector::parseParameters(const ParametersMap & parameters)

 	Parameters::parse(parameters, Parameters::kMarkerLength(), markerLength_);
 	Parameters::parse(parameters, Parameters::kMarkerMaxDepthError(), maxDepthError_);
+	Parameters::parse(parameters, Parameters::kMarkerMaxRange(), maxRange_);
+	Parameters::parse(parameters, Parameters::kMarkerMinRange(), minRange_);
 	Parameters::parse(parameters, Parameters::kMarkerDictionary(), dictionaryId_);
 #if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION <4 || (CV_MINOR_VERSION ==4 && CV_SUBMINOR_VERSION<2)))
 	if(dictionaryId_ >= 17)
@@ -191,15 +195,20 @@ std::map<int, Transform> MarkerDetector::detect(const cv::Mat & image, const Cam
 				}
 			}

-			cv::Mat R;
-			cv::Rodrigues(rvecs[i], R);
-			Transform t(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvecs[i].val[0],
-						   R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvecs[i].val[1],
-						   R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvecs[i].val[2]);
-
-			Transform pose = model.localTransform() * t;
-			detections.insert(std::make_pair(ids[i], pose));
-			UDEBUG("Marker %d detected at %s (%s)", ids[i], pose.prettyPrint().c_str(), t.prettyPrint().c_str());
+			// Limit the detection range to be between the min / max range.
+			// If the ranges are -1, allow any detection within that direction.
+			if((maxRange_ <= 0 || tvecs[i].val[2] < maxRange_) &&
+				(minRange_ <= 0 || tvecs[i].val[2] > minRange_))
+			{
+				cv::Mat R;
+				cv::Rodrigues(rvecs[i], R);
+				Transform t(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvecs[i].val[0],
+								 R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvecs[i].val[1],
+								 R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvecs[i].val[2]);
+				Transform pose = model.localTransform() * t;
+				detections.insert(std::make_pair(ids[i], pose));
+				UDEBUG("Marker %d detected at %s (%s)", ids[i], pose.prettyPrint().c_str(), t.prettyPrint().c_str());
+			}
 		}
 		if(markerLength_ == 0)
 		{
@@ -261,4 +270,3 @@ std::map<int, Transform> MarkerDetector::detect(const cv::Mat & image, const Cam


 } /* namespace rtabmap */
-
@@ -1532,4 +1532,36 @@ bool OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 	return updated;
 }

+unsigned long OccupancyGrid::getMemoryUsed() const
+{
+	unsigned long memoryUsage = sizeof(OccupancyGrid);
+	memoryUsage += parameters_.size()*(sizeof(std::string)*2+sizeof(ParametersMap::iterator)) + sizeof(ParametersMap);
+
+	memoryUsage += cache_.size()*(sizeof(int) + sizeof(std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat>) + sizeof(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator)) + sizeof(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >);
+	for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::const_iterator iter=cache_.begin(); iter!=cache_.end(); ++iter)
+	{
+		memoryUsage += iter->second.first.first.total() * iter->second.first.first.elemSize();
+		memoryUsage += iter->second.first.second.total() * iter->second.first.second.elemSize();
+		memoryUsage += iter->second.second.total() * iter->second.second.elemSize();
+	}
+	memoryUsage += map_.total() * map_.elemSize();
+	memoryUsage += mapInfo_.total() * mapInfo_.elemSize();
+	memoryUsage += cellCount_.size()*(sizeof(int)*3 + sizeof(std::pair<int, int>) + sizeof(std::map<int, std::pair<int, int> >::iterator)) + sizeof(std::map<int, std::pair<int, int> >);
+	memoryUsage += addedNodes_.size()*(sizeof(int) + sizeof(Transform)+ sizeof(float)*12 + sizeof(std::map<int, Transform>::iterator)) + sizeof(std::map<int, Transform>);
+
+	if(assembledGround_.get())
+	{
+		memoryUsage += assembledGround_->points.size() * sizeof(pcl::PointXYZRGB);
+	}
+	if(assembledObstacles_.get())
+	{
+		memoryUsage += assembledObstacles_->points.size() * sizeof(pcl::PointXYZRGB);
+	}
+	if(assembledEmptyCells_.get())
+	{
+		memoryUsage += assembledEmptyCells_->points.size() * sizeof(pcl::PointXYZRGB);
+	}
+	return memoryUsage;
+}
+
 }
--- a/Show More
+++ b/Show More