merged master->humble-devel

* New feature: Depth confidence

* iOS app updated to save depth confidence, added util2d::depthBleedingFiltering function

* Updated tools to show/extract depth confidence

* Android: moved smoothing in post-processing, fixed confidence registration, added depth bleeding error option.

* Fixed warning

* removed debug log

* Added new feature types, fixed rendering when exporting texture >4096 (#1469), added depth bleeding filter option to iOS

* fixed some warnings, android: added bleeding error option

* CI: try updating ros2 key

* added sudo

* antoher test

* bump ios app version

.github/workflows/cmake-ros.yml

@@ -3,7 +3,7 @@ name: CMake-ROS
 on:
   push:
     branches: 
-      - master
+      - humble-devel
   pull_request:
     branches: 
       - '**'
@@ -23,14 +23,29 @@ jobs:
     strategy:
       fail-fast: false
       matrix:
-        ros_distribution: [ noetic, humble ]
+        ros_distribution: [ humble ]
         include:
-        - ros_distribution: 'noetic'
-          os: ubuntu-20.04
         - ros_distribution: 'humble'
           os: ubuntu-22.04
+        - ros_distribution: 'jazzy'
+          os: ubuntu-24.04
+        - ros_distribution: 'kilted'
+          os: ubuntu-24.04
+        - ros_distribution: 'rolling'
+          os: ubuntu-24.04
 
     steps:   
+    - name: Setup ROS2
+      # https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debs.html
+      run: |
+        sudo apt install software-properties-common
+        sudo add-apt-repository universe
+        sudo apt update && sudo apt install curl -y
+        export ROS_APT_SOURCE_VERSION=$(curl -s https://api.github.com/repos/ros-infrastructure/ros-apt-source/releases/latest | grep -F "tag_name" | awk -F\" '{print $4}')
+        curl -L -o /tmp/ros2-apt-source.deb "https://github.com/ros-infrastructure/ros-apt-source/releases/download/${ROS_APT_SOURCE_VERSION}/ros2-apt-source_${ROS_APT_SOURCE_VERSION}.$(. /etc/os-release && echo $VERSION_CODENAME)_all.deb"
+        sudo apt install /tmp/ros2-apt-source.deb
+        sudo apt update
+
     - uses: ros-tooling/setup-ros@v0.7
       with:
         required-ros-distributions: ${{ matrix.ros_distribution }}

.github/workflows/cmake.yml

@@ -18,7 +18,7 @@ jobs:
     strategy:
       fail-fast: false
       matrix:
-        os: [ubuntu-24.04, ubuntu-22.04, ubuntu-20.04]
+        os: [ubuntu-24.04, ubuntu-22.04]
 
     steps:    
     - name: Install dependencies

.github/workflows/docker.yml

@@ -7,12 +7,22 @@ on:
 
 jobs:
   docker_deps:
+
+    # Disabling ###-deps step from CI because it is too flaky (seg faults, arm64 build timeout...)
+    # Only way I was able to build all images is to do it from a ubuntu 20.04 computer with:
+    #    $ sudo add-apt-repository ppa:canonical-server/server-backports
+    #    $ sudo apt-get update
+    #    $ sudo apt-get upgrade qemu-user-static
+    #    $ docker run --rm --privileged multiarch/qemu-user-static --reset -p yes -c yes
+    # More info: https://github.com/introlab/rtabmap/issues/1454
+    # if: false
+
     runs-on: ubuntu-latest
     
     strategy:
       fail-fast: false
       matrix:
-        docker_tag: [focal-deps, jammy-deps, jammy-iron-deps, noble-deps]
+        docker_tag: [focal-deps, jammy-deps, noble-deps, noble-kilted-deps]
         include:
         - docker_tag: focal-deps
           docker_tags: |
@@ -20,7 +30,6 @@ jobs:
           docker_platforms: |
             linux/amd64
             linux/arm64
-            linux/arm/v7
           docker_path: 'focal/deps'
         - docker_tag: jammy-deps
           docker_tags: |
@@ -29,12 +38,6 @@ jobs:
             linux/amd64
             linux/arm64
           docker_path: 'jammy/deps'
-        - docker_tag: jammy-iron-deps
-          docker_tags: |
-            introlab3it/rtabmap:jammy-iron-deps
-          docker_platforms: |
-            linux/amd64
-          docker_path: 'jammy-iron/deps'
         - docker_tag: noble-deps
           docker_tags: |
             introlab3it/rtabmap:noble-deps
@@ -42,6 +45,13 @@ jobs:
             linux/amd64
             linux/arm64
           docker_path: 'noble/deps'
+        - docker_tag: noble-kilted-deps
+          docker_tags: |
+            introlab3it/rtabmap:noble-kilted-deps
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'noble-kilted/deps'
     
     steps:
       -
@@ -49,21 +59,21 @@ jobs:
         uses: actions/checkout@v2
       - 
         name: Set up QEMU
-        uses: docker/setup-qemu-action@v1
+        uses: docker/setup-qemu-action@v3
         with:
           platforms: all
       -
         name: Set up Docker Buildx
-        uses: docker/setup-buildx-action@v1
+        uses: docker/setup-buildx-action@v3
       -
         name: Login to DockerHub
-        uses: docker/login-action@v1
+        uses: docker/login-action@v3
         with:
           username: ${{ secrets.DOCKERHUB_USERNAME }}
           password: ${{ secrets.DOCKERHUB_TOKEN }}
       -
         name: Build and push
-        uses: docker/build-push-action@v2
+        uses: docker/build-push-action@v6
         with:
           context: .
           push: true
@@ -80,7 +90,7 @@ jobs:
     strategy:
       fail-fast: false
       matrix:
-        docker_tag: [bionic, focal, jammy, jammy-iron, noble, android23, android24, android26, android30]
+        docker_tag: [bionic, focal, jammy, noble, noble-kilted, android23, android24, android26, android30]
         include:
         - docker_tag: bionic
           docker_tags: |
@@ -102,7 +112,6 @@ jobs:
           docker_platforms: |
             linux/amd64
             linux/arm64
-            linux/arm/v7
           docker_path: 'focal'
         - docker_tag: jammy
           docker_tags: |
@@ -114,14 +123,6 @@ jobs:
             linux/amd64
             linux/arm64
           docker_path: 'jammy'
-        - docker_tag: jammy-iron
-          docker_tags: |
-            introlab3it/rtabmap:jammy-iron
-          docker_args: |
-            NOT_USED=0
-          docker_platforms: |
-            linux/amd64
-          docker_path: 'jammy-iron'
         - docker_tag: noble
           docker_tags: |
             introlab3it/rtabmap:noble
@@ -132,6 +133,15 @@ jobs:
             linux/amd64
             linux/arm64
           docker_path: 'noble'
+        - docker_tag: noble-kilted
+          docker_tags: |
+            introlab3it/rtabmap:noble-kilted
+          docker_args: |
+            NOT_USED=0
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'noble-kilted'
         - docker_tag: android23
           docker_tags: |
             introlab3it/rtabmap:android23
@@ -172,21 +182,21 @@ jobs:
         uses: actions/checkout@v2
       - 
         name: Set up QEMU
-        uses: docker/setup-qemu-action@v1
+        uses: docker/setup-qemu-action@v3
         with:
           platforms: all
       -
         name: Set up Docker Buildx
-        uses: docker/setup-buildx-action@v1
+        uses: docker/setup-buildx-action@v3
       -
         name: Login to DockerHub
-        uses: docker/login-action@v1
+        uses: docker/login-action@v3
         with:
           username: ${{ secrets.DOCKERHUB_USERNAME }}
           password: ${{ secrets.DOCKERHUB_TOKEN }}
       -
         name: Build and push
-        uses: docker/build-push-action@v2
+        uses: docker/build-push-action@v6
         with:
           context: .
           push: true

CMakeLists.txt

@@ -19,8 +19,8 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 # VERSION
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
-SET(RTABMAP_MINOR_VERSION 21)
-SET(RTABMAP_PATCH_VERSION 9)
+SET(RTABMAP_MINOR_VERSION 22)
+SET(RTABMAP_PATCH_VERSION 0)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   
@@ -208,7 +208,7 @@ option(WITH_MYNTEYE       "Include mynteye-s support"            ON)
 option(WITH_DEPTHAI       "Include depthai-core support"         OFF)
 option(WITH_XVSDK         "Include XVisio SDK support"           OFF)
 option(WITH_OCTOMAP       "Include OctoMap support"              ON)
-option(WITH_GRIDMAP       "Include GridMap support"              ON)
+option(WITH_GRIDMAP       "Include GridMap support"              OFF)
 option(WITH_CPUTSDF       "Include CPUTSDF support"              OFF)
 option(WITH_OPENCHISEL    "Include open_chisel support"          OFF)
 option(WITH_ALICE_VISION  "Include AliceVision support"          OFF)

LICENSE

@@ -1,5 +1,5 @@
 RTAB-Map - https://github.com/introlab/rtabmap
-Copyright (c) 2010-2021, Mathieu Labbe - IntRoLab - Universite de Sherbrooke, all rights reserved.
+Copyright (c) 2010-2025, Mathieu Labbe - IntRoLab - Universite de Sherbrooke, all rights reserved.
 Copyright (c) XXX, contributors, all rights reserved.
 
 Redistribution and use in source and binary forms, with or without

README.md

@@ -64,8 +64,8 @@ This project is supported by [IntRoLab - Intelligent / Interactive / Integrated
             <td><a href="http://build.ros2.org/job/Hbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary" alt="Build Status"/></td>
         </tr>
         <tr>
-            <td>Iron</td>
-            <td><a href="http://build.ros2.org/job/Ibin_uJ64__rtabmap__ubuntu_jammy_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__rtabmap__ubuntu_jammy_amd64__binary" alt="Build Status"/></td>
+            <td>Jazzy</td>
+            <td><a href="http://build.ros2.org/job/Jbin_uN64__rtabmap__ubuntu_noble_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Jbin_uN64__rtabmap__ubuntu_noble_amd64__binary" alt="Build Status"/></td>
         </tr>
         <tr>
             <td>Rolling</td>
@@ -76,7 +76,7 @@ This project is supported by [IntRoLab - Intelligent / Interactive / Integrated
            <td>
              <a href="https://hub.docker.com/r/introlab3it/rtabmap">rtabmap</a>
            </td>
-           <td><img src="https://img.shields.io/docker/pulls/introlab3it/rtabmap.svg?label=pulls" alt="Docker Pulls"/></td>
+           <td><img src="https://img.shields.io/docker/pulls/introlab3it/rtabmap" alt="Docker Pulls"/></td>
         </tr>
     </tbody>
 </table>

app/android/jni/CMakeLists.txt

@@ -3,9 +3,10 @@ SET(INCLUDE_DIRS
     ${CMAKE_CURRENT_SOURCE_DIR}
     ${CMAKE_CURRENT_SOURCE_DIR}/tango-gl/include
     ${CMAKE_CURRENT_SOURCE_DIR}/third-party/include
-	${PROJECT_BINARY_DIR}/corelib/include
-	${PROJECT_SOURCE_DIR}/corelib/include
-	${PROJECT_SOURCE_DIR}/utilite/include
+	  ${PROJECT_BINARY_DIR}/corelib/include
+	  ${PROJECT_SOURCE_DIR}/corelib/include
+	  ${PROJECT_SOURCE_DIR}/utilite/include
+    ${CMAKE_CURRENT_BINARY_DIR}
     ${OpenCV_INCLUDE_DIRS}
     ${PCL_INCLUDE_DIRS}
     "${ANDROID_NDK}/platforms/android-${ANDROID_NATIVE_API_LEVEL}/arch-${ANDROID_ARCH_NAME}/usr/include"
@@ -24,14 +25,19 @@ set(sources
   point_cloud_drawable.cpp
   graph_drawable.cpp
   background_renderer.cc
+  text_drawable.cpp
+  quad_color.cpp
+  tango-gl/bounding_box.cpp 
   tango-gl/axis.cpp 
   tango-gl/camera.cpp 
+  tango-gl/circle.cpp 
   tango-gl/conversions.cpp 
   tango-gl/drawable_object.cpp 
   tango-gl/frustum.cpp 
   tango-gl/gesture_camera.cpp 
   tango-gl/grid.cpp 
   tango-gl/line.cpp 
+  tango-gl/mesh.cpp 
   tango-gl/shaders.cpp 
   tango-gl/trace.cpp 
   tango-gl/transform.cpp 
@@ -114,7 +120,35 @@ add_definitions(${PCL_DEFINITIONS})
 
 INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
 
-add_library(NativeRTABMap SHARED ${sources})        
+####################################
+# Generate resources files
+####################################
+SET(RESOURCES
+    ${CMAKE_CURRENT_SOURCE_DIR}/resources/text_atlas.png
+)
+
+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})
+
+find_host_program(RTABMAP_RES_TOOL rtabmap-res_tool PATHS ${CMAKE_RUNTIME_OUTPUT_DIRECTORY})
+IF(NOT RTABMAP_RES_TOOL)
+  MESSAGE( FATAL_ERROR "RTABMAP_RES_TOOL is not defined (it is the path to \"rtabmap-res_tool\" application created by a non-Android build)." )
+ENDIF(NOT RTABMAP_RES_TOOL)
+
+ADD_CUSTOM_COMMAND(
+      OUTPUT ${RESOURCES_HEADERS}
+      COMMAND ${RTABMAP_RES_TOOL} -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${RESOURCES}
+      COMMENT "[Creating resources]"
+      DEPENDS ${RESOURCES}
+)
+####################################
+# Generate resources files END
+####################################
+
+add_library(NativeRTABMap SHARED ${sources} ${RESOURCES_HEADERS})        
 target_link_libraries(NativeRTABMap ${LIBRARIES}
   android
   log

app/android/jni/CameraARCore.cpp

@@ -37,8 +37,8 @@ namespace rtabmap {
 //////////////////////////////
 // CameraARCore
 //////////////////////////////
-CameraARCore::CameraARCore(void* env, void* context, void* activity, bool depthFromMotion, bool smoothing, float upstreamRelocalizationAccThr):
-	CameraMobile(smoothing, upstreamRelocalizationAccThr),
+CameraARCore::CameraARCore(void* env, void* context, void* activity, bool depthFromMotion, float upstreamRelocalizationAccThr):
+	CameraMobile(upstreamRelocalizationAccThr),
 	env_(env),
 	context_(context),
 	activity_(activity),

app/android/jni/CameraARCore.h

@@ -50,7 +50,7 @@ namespace rtabmap {
 
 class CameraARCore : public CameraMobile {
 public:
-	CameraARCore(void* env, void* context, void* activity, bool depthFromMotion = false, bool smoothing = false, float upstreamRelocalizationAccThr = 0.0f);
+	CameraARCore(void* env, void* context, void* activity, bool depthFromMotion = false, float upstreamRelocalizationAccThr = 0.0f);
 	virtual ~CameraARCore();
 
 	virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height);

app/android/jni/CameraAREngine.cpp

@@ -40,8 +40,8 @@ namespace rtabmap {
 //////////////////////////////
 // CameraAREngine
 //////////////////////////////
-CameraAREngine::CameraAREngine(void* env, void* context, void* activity, bool smoothing, float upstreamRelocalizationAccThr):
-	CameraMobile(smoothing, upstreamRelocalizationAccThr),
+CameraAREngine::CameraAREngine(void* env, void* context, void* activity, float upstreamRelocalizationAccThr):
+	CameraMobile(upstreamRelocalizationAccThr),
 	env_(env),
 	context_(context),
 	activity_(activity),

app/android/jni/CameraAREngine.h

@@ -46,7 +46,7 @@ namespace rtabmap {
 
 class CameraAREngine : public CameraMobile {
 public:
-	CameraAREngine(void* env, void* context, void* activity, bool smoothing = false, float upstreamRelocalizationAccThr = 0.0f);
+	CameraAREngine(void* env, void* context, void* activity, float upstreamRelocalizationAccThr = 0.0f);
 	virtual ~CameraAREngine();
 
 	virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height);

app/android/jni/CameraMobile.cpp

@@ -41,9 +41,6 @@ namespace rtabmap {
 //////////////////////////////
 // CameraMobile
 //////////////////////////////
-const float CameraMobile::bilateralFilteringSigmaS = 2.0f;
-const float CameraMobile::bilateralFilteringSigmaR = 0.075f;
-
 const rtabmap::Transform CameraMobile::opticalRotation = Transform(
 		0.0f,  0.0f,  1.0f, 0.0f,
 	   -1.0f,  0.0f,  0.0f, 0.0f,
@@ -53,13 +50,12 @@ const rtabmap::Transform CameraMobile::opticalRotationInv = Transform(
 	    0.0f,  0.0f, -1.0f, 0.0f,
 		1.0f,  0.0f,  0.0f, 0.0f);
 
-CameraMobile::CameraMobile(bool smoothing, float upstreamRelocalizationAccThr) :
+CameraMobile::CameraMobile(float upstreamRelocalizationAccThr) :
 		Camera(10),
 		deviceTColorCamera_(Transform::getIdentity()),
 		textureId_(0),
 		uvs_initialized_(false),
 		stampEpochOffset_(0.0),
-		smoothing_(smoothing),
 		colorCameraToDisplayRotation_(ROTATION_0),
 		originUpdate_(true),
 		upstreamRelocalizationAccThr_(upstreamRelocalizationAccThr),
@@ -108,8 +104,9 @@ void CameraMobile::close()
     dataReady_.release();
 }
 
-void CameraMobile::resetOrigin()
+void CameraMobile::resetOrigin(const rtabmap::Transform & offset)
 {
+    manualOriginOffset_ = offset;
 	originUpdate_ = true;
 }
 
@@ -193,7 +190,7 @@ void CameraMobile::poseReceived(const Transform & pose, double deviceStamp)
 			previousAnchorPose_.setNull();
 			previousAnchorLinearVelocity_.clear();
 			previousAnchorStamp_ = 0.0;
-			originOffset_ = pose.translation().inverse();
+            originOffset_ = manualOriginOffset_.isNull() ? pose.translation().inverse() : manualOriginOffset_;
 			originUpdate_ = false;
 		}
         
@@ -393,124 +390,135 @@ SensorData CameraMobile::updateDataOnRender(Transform & pose)
 void CameraMobile::postUpdate()
 {
 	if(data_.isValid())
-	{
-		// adjust origin
-		if(!originOffset_.isNull())
-		{
-			dataPose_ = originOffset_ * dataPose_;
-			viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * originOffset_ *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_));
-			occlusionModel_.setLocalTransform(originOffset_ * occlusionModel_.localTransform());
-		}
-
-		if(lastKnownGPS_.stamp() > 0.0 && data_.stamp()-lastKnownGPS_.stamp()<1.0)
-		{
-			data_.setGPS(lastKnownGPS_);
-		}
-		else if(lastKnownGPS_.stamp()>0.0)
-		{
-			LOGD("GPS too old (current time=%f, gps time = %f)", data_.stamp(), lastKnownGPS_.stamp());
-		}
-
-		if(lastEnvSensors_.size())
-		{
-			data_.setEnvSensors(lastEnvSensors_);
-			lastEnvSensors_.clear();
-		}
-
-		if(smoothing_ && !data_.depthRaw().empty())
-		{
-			//UTimer t;
-			data_.setDepthOrRightRaw(rtabmap::util2d::fastBilateralFiltering(data_.depthRaw(), bilateralFilteringSigmaS, bilateralFilteringSigmaR));
-			//LOGD("Bilateral filtering, time=%fs", t.ticks());
-		}
-
-		// 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);
-			cv::transpose(rgb,rgbt);
-			rgb = rgbt;
-			cv::Mat deptht(data_.depthRaw().cols, data_.depthRaw().rows, data_.depthRaw().type());
-			cv::flip(data_.depthRaw(),depth,1);
-			cv::transpose(depth,deptht);
-			depth = deptht;
-			CameraModel model = data_.cameraModels()[0];
-			cv::Size sizet(model.imageHeight(), model.imageWidth());
-			model = CameraModel(
-					model.fy(),
-					model.fx(),
-					model.cy(),
-					model.cx()>0?model.imageWidth()-model.cx():0,
-					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);
-			cv::flip(data_.depthOrRightRaw(),depth,1);
-			cv::flip(depth,depth,0);
-			CameraModel model = data_.cameraModels()[0];
-			cv::Size sizet(model.imageWidth(), model.imageHeight());
-			model = CameraModel(
-					model.fx(),
-					model.fy(),
-					model.cx()>0?model.imageWidth()-model.cx():0,
-					model.cy()>0?model.imageHeight()-model.cy():0,
-					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);
-			cv::Mat depth(data_.depthOrRightRaw().cols, data_.depthOrRightRaw().rows, data_.depthOrRightRaw().type());
-			cv::transpose(data_.depthOrRightRaw(),depth);
-			cv::flip(depth,depth,1);
-			CameraModel model = data_.cameraModels()[0];
-			cv::Size sizet(model.imageHeight(), model.imageWidth());
-			model = CameraModel(
-					model.fy(),
-					model.fx(),
-					model.cy()>0?model.imageHeight()-model.cy():0,
-					model.cx(),
-					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());
-		}
+    {
+        // adjust origin
+        if(!originOffset_.isNull())
+        {
+            dataPose_ = originOffset_ * dataPose_;
+            viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * originOffset_ *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_));
+            occlusionModel_.setLocalTransform(originOffset_ * occlusionModel_.localTransform());
+        }
+        
+        if(lastKnownGPS_.stamp() > 0.0 && data_.stamp()-lastKnownGPS_.stamp()<1.0)
+        {
+            data_.setGPS(lastKnownGPS_);
+        }
+        else if(lastKnownGPS_.stamp()>0.0)
+        {
+            LOGD("GPS too old (current time=%f, gps time = %f)", data_.stamp(), lastKnownGPS_.stamp());
+        }
+        
+        if(lastEnvSensors_.size())
+        {
+            data_.setEnvSensors(lastEnvSensors_);
+            lastEnvSensors_.clear();
+        }
+        
+        
+        // Rotate image depending on the camera orientation
+        if(colorCameraToDisplayRotation_ == ROTATION_90)
+        {
+            UDEBUG("ROTATION_90");
+            cv::Mat rgb, depth, confidence;
+            cv::Mat rgbt;
+            cv::flip(data_.imageRaw(),rgb,1);
+            cv::transpose(rgb,rgbt);
+            rgb = rgbt;
+            cv::Mat deptht;
+            cv::flip(data_.depthRaw(),depth,1);
+            cv::transpose(depth,deptht);
+            depth = deptht;
+            if(!data_.depthConfidenceRaw().empty()) {
+                cv::Mat conft;
+                cv::flip(data_.depthConfidenceRaw(),confidence,1);
+                cv::transpose(confidence,conft);
+                confidence = conft;
+            }
+            CameraModel model = data_.cameraModels()[0];
+            cv::Size sizet(model.imageHeight(), model.imageWidth());
+            model = CameraModel(
+                                model.fy(),
+                                model.fx(),
+                                model.cy(),
+                                model.cx()>0?model.imageWidth()-model.cx():0,
+                                model.localTransform()*rtabmap::Transform(0,-1,0,0, 1,0,0,0, 0,0,1,0));
+            model.setImageSize(sizet);
+            data_.setRGBDImage(rgb, depth, confidence, 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, confidence;
+            cv::flip(data_.imageRaw(),rgb,1);
+            cv::flip(rgb,rgb,0);
+            cv::flip(data_.depthOrRightRaw(),depth,1);
+            cv::flip(depth,depth,0);
+            if(!data_.depthConfidenceRaw().empty()) {
+                cv::flip(data_.depthConfidenceRaw(),confidence,1);
+                cv::flip(confidence,confidence,0);
+            }
+            CameraModel model = data_.cameraModels()[0];
+            cv::Size sizet(model.imageWidth(), model.imageHeight());
+            model = CameraModel(
+                                model.fx(),
+                                model.fy(),
+                                model.cx()>0?model.imageWidth()-model.cx():0,
+                                model.cy()>0?model.imageHeight()-model.cy():0,
+                                model.localTransform()*rtabmap::Transform(0,0,0,0,0,1,0));
+            model.setImageSize(sizet);
+            data_.setRGBDImage(rgb, depth, confidence, 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, depth, confidence;
+            cv::transpose(data_.imageRaw(),rgb);
+            cv::flip(rgb,rgb,1);
+            cv::transpose(data_.depthOrRightRaw(),depth);
+            cv::flip(depth,depth,1);
+            if(!data_.depthConfidenceRaw().empty()) {
+                cv::transpose(data_.depthConfidenceRaw(),confidence);
+                cv::flip(confidence,confidence,1);
+            }
+            CameraModel model = data_.cameraModels()[0];
+            cv::Size sizet(model.imageHeight(), model.imageWidth());
+            model = CameraModel(
+                                model.fy(),
+                                model.fx(),
+                                model.cy()>0?model.imageHeight()-model.cy():0,
+                                model.cx(),
+                                model.localTransform()*rtabmap::Transform(0,1,0,0, -1,0,0,0, 0,0,1,0));
+            model.setImageSize(sizet);
+            data_.setRGBDImage(rgb, depth, confidence, 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());
+        }
+        else
+        {
+            UDEBUG("ROTATION_0");
+        }
 	}
 }
 

app/android/jni/CameraMobile.h

@@ -70,9 +70,6 @@ private:
 
 class CameraMobile : public Camera, public UEventsSender {
 public:
-	static const float bilateralFilteringSigmaS;
-	static const float bilateralFilteringSigmaR;
-
 	static const rtabmap::Transform opticalRotation;
 	static const rtabmap::Transform opticalRotationInv;
     
@@ -87,7 +84,7 @@ public:
             int kptsSize = 3);
 
 public:
-	CameraMobile(bool smoothing = false, float upstreamRelocalizationAccThr = 0.0f);
+	CameraMobile(float upstreamRelocalizationAccThr = 0.0f);
 	virtual ~CameraMobile();
 
 	// abstract functions
@@ -99,7 +96,7 @@ public:
 	void update(const SensorData & data, const Transform & pose, const glm::mat4 & viewMatrix, const glm::mat4 & projectionMatrix, const float * texCoord);
 	void updateOnRender();
 
-	void resetOrigin();
+	void resetOrigin(const rtabmap::Transform & offset = rtabmap::Transform());
 	virtual bool isCalibrated() const;
 
 	virtual bool odomProvided() const { return true; }
@@ -110,7 +107,6 @@ public:
 
 	const CameraModel & getCameraModel() const {return model_;}
 	const Transform & getDeviceTColorCamera() const {return deviceTColorCamera_;}
-	void setSmoothing(bool enabled) {smoothing_ = enabled;}
 	virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height) {colorCameraToDisplayRotation_ = colorCameraToDisplayRotation;}
 	void setGPS(const GPS & gps);
 	void addEnvSensor(int type, float value);
@@ -144,12 +140,12 @@ protected:
 private:
 	bool firstFrame_;
 	double stampEpochOffset_;
-	bool smoothing_;
 	ScreenRotation colorCameraToDisplayRotation_;
 	GPS lastKnownGPS_;
 	EnvSensors lastEnvSensors_;
 	Transform originOffset_;
 	bool originUpdate_;
+    rtabmap::Transform manualOriginOffset_;
 	float upstreamRelocalizationAccThr_;
 	rtabmap::Transform previousAnchorPose_;
 	std::vector<float> previousAnchorLinearVelocity_;

app/android/jni/CameraTango.cpp

@@ -114,8 +114,7 @@ void onTangoEventAvailableRouter(void* context, const TangoEvent* event)
 //////////////////////////////
 // CameraTango
 //////////////////////////////
-CameraTango::CameraTango(bool colorCamera, int decimation, bool publishRawScan, bool smoothing) :
-		CameraMobile(smoothing),
+CameraTango::CameraTango(bool colorCamera, int decimation, bool publishRawScan) :
 		tango_config_(0),
 		colorCamera_(colorCamera),
 		decimation_(decimation),

app/android/jni/CameraTango.h

@@ -45,7 +45,7 @@ namespace rtabmap {
 
 class CameraTango : public CameraMobile {
 public:
-	CameraTango(bool colorCamera, int decimation, bool publishRawScan, bool smoothing);
+	CameraTango(bool colorCamera, int decimation, bool publishRawScan);
 	virtual ~CameraTango();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");

app/android/jni/Measure.h

@@ -0,0 +1,65 @@
+/*
+Copyright (c) 2010-2025, 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.
+*/
+
+#ifndef MEASURE_H_
+#define MEASURE_H_
+
+#include <opencv2/opencv.hpp>
+
+class Measure
+{
+public:
+	Measure(
+			const cv::Point3f & pt1,
+			const cv::Point3f & pt2,
+			const cv::Vec3f & n1,
+			const cv::Vec3f & n2) :
+				pt1_(pt1),
+				pt2_(pt2),
+				n1_(n1),
+				n2_(n2)
+	{
+		length_ = cv::norm(pt2_ - pt1_);
+	}
+	virtual ~Measure() {}
+
+	float length() const {return length_;}
+	const cv::Point3f & pt1() const {return pt1_;}
+	const cv::Point3f & pt2() const {return pt2_;}
+	const cv::Vec3f & n1() const {return n1_;}
+	const cv::Vec3f & n2() const {return n2_;}
+
+private:
+	cv::Point3f pt1_;
+	cv::Point3f pt2_;
+	cv::Vec3f n1_;
+	cv::Vec3f n2_;
+	float length_;
+};
+
+
+#endif /* MEASURE_H_ */

app/android/jni/RTABMapApp.h

@@ -48,6 +48,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/pcl_base.h>
 #include <pcl/TextureMesh.h>
 
+#include "Measure.h"
 
 // RTABMapApp handles the application lifecycle and resources.
 class RTABMapApp : public UEventsHandler {
@@ -84,7 +85,7 @@ class RTABMapApp : public UEventsHandler {
 #ifdef __ANDROID__
   bool startCamera(JNIEnv* env, jobject iBinder, jobject context, jobject activity, int driver);
 #else // __APPLE__
-    bool startCamera();
+  bool startCamera();
 #endif
   // Allocate OpenGL resources for rendering, mainly for initializing the Scene.
   void InitializeGLContent();
@@ -127,6 +128,7 @@ class RTABMapApp : public UEventsHandler {
   void setLighting(bool enabled);
   void setBackfaceCulling(bool enabled);
   void setWireframe(bool enabled);
+  void setTextureColorSeamsHidden(bool hidden);
   void setLocalizationMode(bool enabled);
   void setTrajectoryMode(bool enabled);
   void setGraphOptimization(bool enabled);
@@ -137,6 +139,7 @@ class RTABMapApp : public UEventsHandler {
   void setCameraColor(bool enabled);
   void setFullResolution(bool enabled);
   void setSmoothing(bool enabled);
+  void setDepthBleedingError(float value);
   void setDepthFromMotion(bool enabled);
   void setAppendMode(bool enabled);
   void setUpstreamRelocalizationAccThr(float value);
@@ -176,10 +179,19 @@ class RTABMapApp : public UEventsHandler {
 		  int optimizedMinClusterSize,
 		  float optimizedMaxTextureDistance,
 		  int optimizedMinTextureClusterSize,
+      int textureVertexColorPolicy,
 		  bool blockRendering);
   bool postExportation(bool visualize);
   bool writeExportedMesh(const std::string & directory, const std::string & name);
   int postProcessing(int approach);
+  void clearMeasures();
+  void showMeasures(bool x, bool y, bool z, bool custom);
+  void setMeasuringMode(int mode);
+  void addMeasureButtonClicked();
+  void teleportButtonClicked();
+  void removeMeasure();
+  void setMetricSystem(bool enabled);
+  void setMeasuringTextSize(float size);
 
   void postOdometryEvent(
 		rtabmap::Transform pose,
@@ -203,6 +215,7 @@ class RTABMapApp : public UEventsHandler {
  private:
   int updateMeshDecimation(int width, int height);
   rtabmap::ParametersMap getRtabmapParameters();
+  void updateMeasuringState();
   bool smoothMesh(int id, rtabmap::Mesh & mesh);
   void gainCompensation(bool full = false);
   std::vector<pcl::Vertices> filterOrganizedPolygons(const std::vector<pcl::Vertices> & polygons, int cloudSize) const;
@@ -223,6 +236,7 @@ class RTABMapApp : public UEventsHandler {
   bool trajectoryMode_;
   bool rawScanSaved_;
   bool smoothing_;
+  float depthBleedingError_;
   bool depthFromMotion_;
   bool cameraColor_;
   bool fullResolution_;
@@ -238,7 +252,7 @@ class RTABMapApp : public UEventsHandler {
   float maxGainRadius_;
   int renderingTextureDecimation_;
   float backgroundColor_;
-  int depthConfidence_;
+  unsigned char depthConfidence_;
   float upstreamRelocalizationMaxAcc_;
   std::string exportPointCloudFormat_;
 
@@ -264,11 +278,26 @@ class RTABMapApp : public UEventsHandler {
 
   bool visualizingMesh_;
   bool exportedMeshUpdated_;
-  pcl::TextureMesh::Ptr optMesh_;
+  pcl::TextureMesh::Ptr optTextureMesh_;
   cv::Mat optTexture_;
+  rtabmap::Mesh optMesh_;
   int optRefId_;
   rtabmap::Transform * optRefPose_; // App crashes when loading native library if not dynamic
-
+  std::list<Measure> measures_; // In opengl frame
+  bool measuresUpdated_;
+  bool metricSystem_;
+  float measuringTextSize_;
+  float snapAxisThr_;
+  std::vector<cv::Vec3f> snapAxes_;
+  int measuringMode_;
+  bool addMeasureClicked_;
+  bool teleportClicked_;
+  bool removeMeasureClicked_;
+  std::vector<cv::Point3f> measuringTmpPts_; // In opengl frame
+  std::vector<cv::Point3f> measuringTmpNormals_; // In opengl frame
+  pcl::PointCloud<pcl::PointXYZRGB>::Ptr targetPoint_;
+  pcl::PointCloud<pcl::PointXYZ>::Ptr quadSample_;
+  std::vector<pcl::Vertices> quadSamplePolygons_;
   // main_scene_ includes all drawable object for visualizing Tango device's
   // movement and point cloud.
   Scene main_scene_;

app/android/jni/jni_interface.cpp

@@ -512,6 +512,19 @@ Java_com_introlab_rtabmap_RTABMapLib_setSmoothing(
 	}
 }
 JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setDepthBleedingError(
+		JNIEnv*, jclass, jlong native_application, float value)
+{
+	if(native_application)
+	{
+		return native(native_application)->setDepthBleedingError(value);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setDepthFromMotion(
 		JNIEnv*, jclass, jlong native_application, bool enabled)
 {
@@ -821,6 +834,7 @@ Java_com_introlab_rtabmap_RTABMapLib_exportMesh(
 					optimizedMinClusterSize,
 					optimizedMaxTextureDistance,
 					optimizedMinTextureClusterSize,
+					0,
 					blockRendering);
 	}
 	else

app/android/jni/point_cloud_drawable.cpp

@@ -170,10 +170,12 @@ const std::string kTextureMeshVertexShader =
     "precision highp float;\n"
     "precision mediump int;\n"
     "attribute vec3 aVertex;\n"
+	"attribute vec3 aColor;\n"
     "attribute vec2 aTexCoord;\n"
 
     "uniform mat4 uMVP;\n"
 
+	"varying vec3 vColor;\n"
     "varying vec2 vTexCoord;\n"
     "varying float vLightWeighting;\n"
 
@@ -187,12 +189,14 @@ const std::string kTextureMeshVertexShader =
     "    vTexCoord = aTexCoord;\n"
     "  }\n"
 
+	"  vColor = aColor;\n"
     "  vLightWeighting = 1.0;\n"
     "}\n";
 const std::string kTextureMeshLightingVertexShader =
     "precision highp float;\n"
     "precision mediump int;\n"
     "attribute vec3 aVertex;\n"
+	"attribute vec3 aColor;\n"
 	"attribute vec3 aNormal;\n"
     "attribute vec2 aTexCoord;\n"
 
@@ -200,6 +204,7 @@ const std::string kTextureMeshLightingVertexShader =
     "uniform mat3 uN;\n"
     "uniform vec3 uLightingDirection;\n"
 
+	"varying vec3 vColor;\n"
     "varying vec2 vTexCoord;\n"
     "varying float vLightWeighting;\n"
 
@@ -213,6 +218,7 @@ const std::string kTextureMeshLightingVertexShader =
     "    vTexCoord = aTexCoord;\n"
     "  }\n"
 
+	"  vColor = aColor;\n"
     "  vec3 transformedNormal = uN * aNormal;\n"
     "  vLightWeighting = max(dot(transformedNormal, uLightingDirection)*0.5+0.5, 0.0);\n"
     "  if(vLightWeighting<0.5) \n"
@@ -225,11 +231,22 @@ const std::string kTextureMeshFragmentShader =
 	"uniform float uGainR;\n"
 	"uniform float uGainG;\n"
 	"uniform float uGainB;\n"
+	"uniform int uHideSeams;\n"
+	"varying vec3 vColor;\n"
     "varying vec2 vTexCoord;\n"
 	"varying float vLightWeighting;\n"
 	""
     "void main() {\n"
-    "  vec4 textureColor = texture2D(uTexture, vTexCoord);\n"
+	"  vec4 textureColor;\n"
+	"  if(uHideSeams==1) {\n"
+	"    if(vTexCoord.x>0.99 && vTexCoord.y>0.99) {\n"
+	"      textureColor = vec4(vColor.z, vColor.y, vColor.x, 1.0);\n"
+	"    } else {\n"
+    "      textureColor = texture2D(uTexture, vTexCoord);\n"
+	"    }\n"
+    "  } else {\n"
+    "    textureColor = texture2D(uTexture, vTexCoord) * vec4(vColor.z, vColor.y, vColor.x, 1.0);\n"
+    "  }\n"
 	"  gl_FragColor = vec4(textureColor.r * uGainR * vLightWeighting, textureColor.g * uGainG * vLightWeighting, textureColor.b * uGainB * vLightWeighting, textureColor.a);\n"
     "}\n";
 const std::string kTextureMeshBlendingFragmentShader =
@@ -243,11 +260,12 @@ const std::string kTextureMeshBlendingFragmentShader =
 	"uniform vec2 uScreenScale;\n"
 	"uniform float uNearZ;\n"
 	"uniform float uFarZ;\n"
+	"varying vec3 vColor;\n"
     "varying vec2 vTexCoord;\n"
 	"varying float vLightWeighting;\n"
 	""
     "void main() {\n"
-    "  vec4 textureColor = texture2D(uTexture, vTexCoord);\n"
+	"  vec4 textureColor = texture2D(uTexture, vTexCoord);\n"
     "  float alpha = 1.0;\n"
 	"  vec2 coord = uScreenScale * gl_FragCoord.xy;\n;"
 	"  vec4 depthPacked = texture2D(uDepthTexture, coord);\n"
@@ -1008,7 +1026,8 @@ void PointCloudDrawable::Render(
 		float nearClipPlane,
 	    float farClipPlane,
 		bool packDepthToColorChannel,
-		bool wireFrame) const
+		bool wireFrame,
+        bool hideSeams) const
 {
 	if(vertex_buffer_ && nPoints_ && visible_ && !shaderPrograms_.empty())
 	{
@@ -1127,12 +1146,16 @@ void PointCloudDrawable::Render(
 
 				attribute_texture = glGetAttribLocation(program, "aTexCoord");
 				glEnableVertexAttribArray(attribute_texture);
+                
+                if(depthTexture == 0)
+                {
+                    GLuint hideSeams_handle = glGetUniformLocation(program, "uHideSeams");
+                    glUniform1i(hideSeams_handle, hideSeams?1:0);
+                }
 			}
-			else
-			{
-				attribute_color = glGetAttribLocation(program, "aColor");
-				glEnableVertexAttribArray(attribute_color);
-			}
+
+            attribute_color = glGetAttribLocation(program, "aColor");
+            glEnableVertexAttribArray(attribute_color);
 		}
 		tango_gl::util::CheckGlError("Pointcloud::Render() common");
 
@@ -1144,7 +1167,7 @@ void PointCloudDrawable::Render(
 			{
 				glVertexAttribPointer(attribute_texture, 2, GL_FLOAT, GL_FALSE, (hasNormals_?9:6)*sizeof(GLfloat), (GLvoid*) (4 * sizeof(GLfloat)));
 			}
-			else if(!packDepthToColorChannel)
+			if(!packDepthToColorChannel)
 			{
 				glVertexAttribPointer(attribute_color, 3, GL_UNSIGNED_BYTE, GL_TRUE,  (hasNormals_?9:6)*sizeof(GLfloat), (GLvoid*) (3 * sizeof(GLfloat)));
 			}

app/android/jni/point_cloud_drawable.h

@@ -98,7 +98,8 @@ private:
 		  float nearClipPlane = 0, // nonnull if depthTexture>0
 		  float farClipPlane = 0,  // nonnull if depthTexture>0
 		  bool packDepthToColorChannel = false,
-		  bool wireFrame = false) const;
+		  bool wireFrame = false,
+          bool hideSeams = false) const;
 
  private:
   template<class PointT>

app/android/jni/quad_color.cpp

@@ -0,0 +1,69 @@
+/*
+ * Copyright 2014 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.
+ */
+
+#include "quad_color.h"
+#include "tango-gl/util.h"
+
+static const float vertices[] = {-1.0f, -1.0f, 1.0f, -1.0f,
+                                 -1.0f, 1.0f,  1.0f, 1.0f};
+
+QuadColor::QuadColor(float size) {
+	SetShader();
+
+	vertices_.resize(8);
+	for(int i=0; i<8; ++i)
+	{
+		vertices_[i] = vertices[i]*size;
+	}
+}
+
+QuadColor::QuadColor(
+		float widthLeft,
+		float widthRight,
+		float heightBottom,
+		float heightTop) {
+	SetShader();
+
+	vertices_.resize(8);
+	vertices_[0] = vertices[0]*widthLeft;
+	vertices_[1] = vertices[1]*heightBottom;
+	vertices_[2] = vertices[2]*widthRight;
+	vertices_[3] = vertices[3]*heightBottom;
+	vertices_[4] = vertices[4]*widthLeft;
+	vertices_[5] = vertices[5]*heightTop;
+	vertices_[6] = vertices[6]*widthRight;
+	vertices_[7] = vertices[7]*heightTop;
+}
+
+void QuadColor::Render(const glm::mat4& projection_mat,
+                  const glm::mat4& view_mat) const {
+  glUseProgram(shader_program_);
+
+  // Calculate MVP matrix and pass it to shader.
+  glm::mat4 model_mat = GetTransformationMatrix();
+  glm::mat4 mvp_mat = projection_mat * view_mat * model_mat;
+  glUniformMatrix4fv(uniform_mvp_mat_, 1, GL_FALSE, glm::value_ptr(mvp_mat));
+
+  glUniform4f(uniform_color_, red_, green_, blue_, alpha_);
+
+  // Vertice binding
+  glEnableVertexAttribArray(attrib_vertices_);
+  glVertexAttribPointer(attrib_vertices_, 2, GL_FLOAT, GL_FALSE, 0, &vertices_[0]);
+  glBindBuffer(GL_ARRAY_BUFFER, 0);
+
+  glDrawArrays(GL_TRIANGLE_STRIP, 0, 4);
+  glUseProgram(0);
+}

app/android/jni/quad_color.h

@@ -0,0 +1,35 @@
+/*
+ * Copyright 2014 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 TANGO_GL_QUADCOLOR_H_
+#define TANGO_GL_QUADCOLOR_H_
+
+#include "tango-gl/drawable_object.h"
+
+class QuadColor : public tango_gl::DrawableObject {
+ public:
+	QuadColor(float size);
+	QuadColor(float widthLeft,
+			float widthRight,
+			float heightBottom,
+			float heightTop);
+	QuadColor(const QuadColor& other) = delete;
+	QuadColor& operator=(const QuadColor&) = delete;
+	virtual ~QuadColor() {}
+
+	void Render(const glm::mat4& projection_mat, const glm::mat4& view_mat) const;
+};
+#endif  // TANGO_GL_QUADCOLOR_H_

app/android/jni/scene.cpp

@@ -36,7 +36,7 @@
 // add an offset in z to our origin. We'll set this offset to 1.3 meters based
 // on the average height of a human standing with a Tango device. This allows us
 // to place a grid roughly on the ground for most users.
-const glm::vec3 kHeightOffset = glm::vec3(0.0f, -1.3f, 0.0f);
+const glm::vec3 Scene::kHeightOffset = glm::vec3(0.0f, -1.3f, 0.0f);
 
 // Color of the motion tracking trajectory.
 const tango_gl::Color kTraceColor(0.66f, 0.66f, 0.66f);
@@ -93,6 +93,7 @@ Scene::Scene() :
 		lighting_(false),
 		backfaceCulling_(true),
 		wireFrame_(false),
+		textureColorSeamsHidden_(true),
 		r_(0.0f),
 		g_(0.0f),
 		b_(0.0f),
@@ -124,6 +125,7 @@ void Scene::InitGLContent()
 
 	UASSERT(axis_ == 0);
 
+    TextDrawable::createShaderProgram();
 
 	axis_ = new tango_gl::Axis();
 	frustum_ = new tango_gl::Frustum();
@@ -166,6 +168,7 @@ void Scene::DeleteResources() {
 		background_renderer_ = 0;
 	}
 
+    TextDrawable::releaseShaderProgram();
 	PointCloudDrawable::releaseShaderPrograms();
 
 	if (graph_shader_program_) {
@@ -198,6 +201,10 @@ void Scene::clear()
 	{
 		delete iter->second;
 	}
+    clearLines();
+    clearQuads();
+    clearTexts();
+    clearCircles();
 	if(trace_)
 	{
 		trace_->ClearVertexArray();
@@ -215,6 +222,39 @@ void Scene::clear()
 	}
 }
 
+void Scene::clearLines()
+{
+    for(std::map<int, tango_gl::Line*>::iterator iter=lines_.begin(); iter!=lines_.end(); ++iter)
+    {
+        delete iter->second;
+    }
+    lines_.clear();
+}
+void Scene::clearTexts()
+{
+    for(std::map<int, TextDrawable*>::iterator iter=texts_.begin(); iter!=texts_.end(); ++iter)
+    {
+        delete iter->second;
+    }
+    texts_.clear();
+}
+void Scene::clearQuads()
+{
+    for(std::map<int, QuadColor*>::iterator iter=quads_.begin(); iter!=quads_.end(); ++iter)
+    {
+        delete iter->second;
+    }
+    quads_.clear();
+}
+void Scene::clearCircles()
+{
+    for(std::map<int, tango_gl::Circle*>::iterator iter=circles_.begin(); iter!=circles_.end(); ++iter)
+    {
+        delete iter->second;
+    }
+    circles_.clear();
+}
+
 //Should only be called in OpenGL thread!
 void Scene::SetupViewPort(int w, int h) {
 	if (h == 0) {
@@ -444,8 +484,14 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
 	std::vector<glm::vec4> planes = computeFrustumPlanes(projectionMatrix*viewMatrix, true);
 	std::vector<PointCloudDrawable*> cloudsToDraw(pointClouds_.size());
 	int oi=0;
+    int positiveCloudIds = 0;
 	for(std::map<int, PointCloudDrawable*>::const_iterator iter=pointClouds_.begin(); iter!=pointClouds_.end(); ++iter)
 	{
+        if(iter->first > 0)
+        {
+            positiveCloudIds++;
+        }
+        
 		if(!mapRendering_ && iter->first > 0)
 		{
 			break;
@@ -487,7 +533,7 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
         (blending_ &&
          gesture_camera_->GetCameraType()!=tango_gl::GestureCamera::kTopOrtho &&
          mapRendering_ && meshRendering_ &&
-         (cloudsToDraw.size() > 1 || (renderBackgroundCamera && wireFrame_)));
+         (positiveCloudIds > 1 || (renderBackgroundCamera && wireFrame_)));
 
 	if(onlineBlending && fboId_)
 	{
@@ -629,8 +675,17 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
 				cloud->getPose().z() - openglCamera.z());
 		float distanceToCameraSqr = cloudToCamera[0]*cloudToCamera[0] + cloudToCamera[1]*cloudToCamera[1] + cloudToCamera[2]*cloudToCamera[2];
 
-		cloud->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, meshRenderingTexture_, lighting_, distanceToCameraSqr, onlineBlending?depthTexture_:0, screenWidth_, screenHeight_, gesture_camera_->getNearClipPlane(), gesture_camera_->getFarClipPlane(), false, wireFrame_);
+		cloud->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, meshRenderingTexture_, lighting_, distanceToCameraSqr, onlineBlending?depthTexture_:0, screenWidth_, screenHeight_, gesture_camera_->getNearClipPlane(), gesture_camera_->getFarClipPlane(), false, wireFrame_, textureColorSeamsHidden_);
 	}
+    
+    if(quads_.find(55556)!=quads_.end())
+    {
+        glEnable(GL_BLEND);
+        glDisable(GL_CULL_FACE);
+        const QuadColor * quad = quads_.at(55556);
+        quad->Render(projectionMatrix, viewMatrix);
+        glEnable(GL_CULL_FACE);
+    }
 
 	if(onlineBlending)
 	{
@@ -643,6 +698,57 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
 		glDepthMask(GL_TRUE);
 	}
     
+    ///////
+    glDisable (GL_DEPTH_TEST);
+    if(lines_.size())
+    {
+        for(std::map<int, tango_gl::Line*>::const_iterator iter=lines_.begin(); iter!=lines_.end(); ++iter)
+        {
+            const tango_gl::Line * line = iter->second;
+            line->Render(projectionMatrix, viewMatrix);
+        }
+    }
+
+    if(quads_.size())
+    {
+        glEnable(GL_BLEND);
+        glDisable(GL_CULL_FACE);
+        for(std::map<int, QuadColor*>::const_iterator iter=quads_.begin(); iter!=quads_.end(); ++iter)
+        {
+            if(iter->first!=55556)
+            {
+                const QuadColor * quad = iter->second;
+                quad->Render(projectionMatrix, viewMatrix);
+            }
+        }
+    }
+    if(circles_.size())
+    {
+        glEnable(GL_BLEND);
+        glDisable(GL_CULL_FACE);
+        for(std::map<int, tango_gl::Circle*>::const_iterator iter=circles_.begin(); iter!=circles_.end(); ++iter)
+        {
+            const tango_gl::Circle * circle = iter->second;
+            circle->Render(projectionMatrix, viewMatrix);
+        }
+    }
+    if(texts_.size())
+    {
+        glDisable(GL_CULL_FACE);
+        glEnable(GL_BLEND);
+
+        glm::mat4 viewMatrixRotInv = viewMatrix;
+        viewMatrixRotInv[3][0] = 0;
+        viewMatrixRotInv[3][1] = 0;
+        viewMatrixRotInv[3][2] = 0;
+        viewMatrixRotInv = glm::inverse(viewMatrixRotInv);
+        for(std::map<int, TextDrawable*>::const_iterator iter=texts_.begin(); iter!=texts_.end(); ++iter)
+        {
+            const TextDrawable * text = iter->second;
+            text->Render(projectionMatrix, viewMatrix, viewMatrixRotInv);
+        }
+    }
+    
 	//draw markers on foreground
 	for(std::map<int, tango_gl::Axis*>::const_iterator iter=markers_.begin(); iter!=markers_.end(); ++iter)
 	{
@@ -800,12 +906,7 @@ void Scene::addCloud(
 		const rtabmap::Transform & pose)
 {
 	LOGI("add cloud %d (%d points %d indices)", id, (int)cloud->size(), indices.get()?(int)indices->size():0);
-	std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
-	if(iter != pointClouds_.end())
-	{
-		delete iter->second;
-		pointClouds_.erase(iter);
-	}
+    removeCloudOrMesh(id);
 
 	//create
 	PointCloudDrawable * drawable = new PointCloudDrawable(cloud, indices);
@@ -813,6 +914,16 @@ void Scene::addCloud(
 	pointClouds_.insert(std::make_pair(id, drawable));
 }
 
+void Scene::removeCloudOrMesh(int id)
+{
+    std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
+    if(iter != pointClouds_.end())
+    {
+        delete iter->second;
+        pointClouds_.erase(iter);
+    }
+}
+
 void Scene::addMesh(
 		int id,
 		const rtabmap::Mesh & mesh,
@@ -820,12 +931,7 @@ void Scene::addMesh(
 		bool createWireframe)
 {
 	LOGI("add mesh %d", id);
-	std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
-	if(iter != pointClouds_.end())
-	{
-		delete iter->second;
-		pointClouds_.erase(iter);
-	}
+    removeCloudOrMesh(id);
 
 	//create
 	PointCloudDrawable * drawable = new PointCloudDrawable(mesh, createWireframe);
@@ -885,6 +991,160 @@ void Scene::addMesh(
 	}
 }
 
+void Scene::addLine(
+        int id,
+        const cv::Point3f & pt1,
+        const cv::Point3f & pt2,
+        const tango_gl::Color & color)
+{
+    LOGI("add line %d", id);
+    removeLine(id);
+
+    //create
+    tango_gl::Line * line = new tango_gl::Line(2.0f, GL_LINES);
+    line->SetShader();
+    std::vector<glm::vec3> vertices(2);
+    vertices[0].x = pt1.x;
+    vertices[0].y = pt1.y;
+    vertices[0].z = pt1.z;
+    vertices[1].x = pt2.x;
+    vertices[1].y = pt2.y;
+    vertices[1].z = pt2.z;
+    line->UpdateLineVertices(vertices);
+    line->SetColor(color);
+    lines_.insert(std::make_pair(id, line));
+}
+
+void Scene::removeLine(int id)
+{
+    std::map<int, tango_gl::Line*>::iterator iter=lines_.find(id);
+    if(iter != lines_.end())
+    {
+        delete iter->second;
+        lines_.erase(iter);
+    }
+}
+
+void Scene::addText(
+        int id,
+        const std::string & text,
+        const rtabmap::Transform & pose,
+        float size,
+        const tango_gl::Color & color)
+{
+    LOGI("add text %d", id);
+    removeText(id);
+
+    //create
+    TextDrawable * textD = new TextDrawable(text, pose, size, color);
+    texts_.insert(std::make_pair(id, textD));
+}
+void Scene::removeText(int id)
+{
+    std::map<int, TextDrawable*>::iterator iter=texts_.find(id);
+    if(iter != texts_.end())
+    {
+        delete iter->second;
+        texts_.erase(iter);
+    }
+}
+
+void Scene::addQuad(
+        int id,
+        float size,
+        const rtabmap::Transform & pose,
+        const tango_gl::Color & color,
+        float alpha)
+{
+    //LOGI("add quad %d", id);
+    std::map<int, QuadColor*>::iterator iter=quads_.find(id);
+    if(iter != quads_.end())
+    {
+        delete iter->second;
+        quads_.erase(iter);
+    }
+
+    //create
+    QuadColor * quad = new QuadColor(size);
+    quad->SetTransformationMatrix(glmFromTransform(pose));
+    quad->SetColor(color);
+    quad->SetAlpha(alpha);
+    quads_.insert(std::make_pair(id, quad));
+}
+
+void Scene::addQuad(
+        int id,
+        float widthLeft,
+        float widthRight,
+        float heightBottom,
+        float heightTop,
+        const rtabmap::Transform & pose,
+        const tango_gl::Color & color,
+        float alpha)
+{
+    //LOGI("add quad %d", id);
+    removeQuad(id);
+
+    //create
+    QuadColor * quad = new QuadColor(widthLeft, widthRight, heightBottom, heightTop);
+    quad->SetTransformationMatrix(glmFromTransform(pose));
+    quad->SetColor(color);
+    quad->SetAlpha(alpha);
+    quads_.insert(std::make_pair(id, quad));
+}
+
+void Scene::removeQuad(int id)
+{
+    std::map<int, QuadColor*>::iterator iter=quads_.find(id);
+    if(iter != quads_.end())
+    {
+        delete iter->second;
+        quads_.erase(iter);
+    }
+}
+
+bool Scene::hasQuad(int id) const
+{
+    return quads_.find(id) != quads_.end();
+}
+
+void Scene::addCircle(
+        int id,
+        float radius,
+        const rtabmap::Transform & pose,
+        const tango_gl::Color & color,
+        float alpha)
+{
+    //LOGI("add quad %d", id);
+    std::map<int, tango_gl::Circle*>::iterator iter=circles_.find(id);
+    if(iter != circles_.end())
+    {
+        delete iter->second;
+        circles_.erase(iter);
+    }
+
+    //create
+    tango_gl::Circle * circle = new tango_gl::Circle(radius, 12);
+    circle->SetTransformationMatrix(glmFromTransform(pose));
+    circle->SetColor(color);
+    circle->SetAlpha(alpha);
+    circles_.insert(std::make_pair(id, circle));
+}
+
+void Scene::removeCircle(int id)
+{
+    std::map<int, tango_gl::Circle*>::iterator iter=circles_.find(id);
+    if(iter != circles_.end())
+    {
+        delete iter->second;
+        circles_.erase(iter);
+    }
+}
+
+bool Scene::hasCircle(int id) const
+{
+    return circles_.find(id) != circles_.end();
+}
 
 void Scene::setCloudPose(int id, const rtabmap::Transform & pose)
 {

app/android/jni/scene.h

@@ -33,6 +33,7 @@
 #include <tango-gl/trace.h>
 #include <tango-gl/transform.h>
 #include <tango-gl/util.h>
+#include <tango-gl/circle.h>
 
 #include <rtabmap/core/Transform.h>
 #include <rtabmap/core/Link.h>
@@ -41,12 +42,16 @@
 #include "graph_drawable.h"
 #include "bounding_box_drawable.h"
 #include "background_renderer.h"
+#include "text_drawable.h"
+#include "quad_color.h"
 
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 
 // Scene provides OpenGL drawable objects and renders them for visualization.
 class Scene {
+ public:
+    static const glm::vec3 kHeightOffset;
  public:
   // Constructor and destructor.
   Scene();
@@ -67,6 +72,10 @@ class Scene {
   void setScreenRotation(rtabmap::ScreenRotation colorCameraToDisplayRotation) {color_camera_to_display_rotation_ = colorCameraToDisplayRotation;}
 
   void clear(); // removed all point clouds
+  void clearLines();
+  void clearTexts();
+  void clearQuads();
+  void clearCircles();
 
   // Render loop.
   // @param: cur_pose_transformation, latest pose's transformation.
@@ -119,11 +128,50 @@ class Scene {
   		  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		  const pcl::IndicesPtr & indices,
   		  const rtabmap::Transform & pose);
+  void removeCloudOrMesh(int id);
   void addMesh(
   		int id,
   		const rtabmap::Mesh & mesh,
   		const rtabmap::Transform & pose,
 		bool createWireframe = false);
+  void addLine(
+        int id,
+        const cv::Point3f & pt1,
+        const cv::Point3f & pt2,
+        const tango_gl::Color & color = tango_gl::Color(1.0f, 1.0f, 1.0f));
+  void removeLine(int id);
+  void addText(
+        int id,
+        const std::string & text,
+        const rtabmap::Transform & pose,
+        float size,
+        const tango_gl::Color & color);
+  void removeText(int id);
+  void addQuad(
+        int id,
+        float size,
+        const rtabmap::Transform & pose,
+        const tango_gl::Color & color,
+        float alpha = 1.0f);
+  void addQuad(
+        int id,
+        float widthLeft,
+        float widthRight,
+        float heightBottom,
+        float heightTop,
+        const rtabmap::Transform & pose,
+        const tango_gl::Color & color,
+        float alpha =1.0f);
+  void removeQuad(int id);
+  bool hasQuad(int id) const;
+  void addCircle(
+          int id,
+          float radius,
+          const rtabmap::Transform & pose,
+          const tango_gl::Color & color,
+          float alpha = 1.0f);
+  void removeCircle(int id);
+  bool hasCircle(int id) const;
 
   void setCloudPose(int id, const rtabmap::Transform & pose);
   void setCloudVisible(int id, bool visible);
@@ -145,6 +193,7 @@ class Scene {
   void setLighting(bool enabled) {lighting_ = enabled;}
   void setBackfaceCulling(bool enabled) {backfaceCulling_ = enabled;}
   void setWireframe(bool enabled) {wireFrame_ = enabled;}
+  void setTextureColorSeamsHidden(bool hidden) {textureColorSeamsHidden_ = hidden;}
   void setBackgroundColor(float r, float g, float b) {r_=r; g_=g; b_=b;} // 0.0f <> 1.0f
   void setGridColor(float r, float g, float b);
 
@@ -188,6 +237,10 @@ class Scene {
   rtabmap::ScreenRotation color_camera_to_display_rotation_;
 
   std::map<int, PointCloudDrawable*> pointClouds_;
+  std::map<int, tango_gl::Line*> lines_;
+  std::map<int, TextDrawable*> texts_;
+  std::map<int, QuadColor*> quads_;
+  std::map<int, tango_gl::Circle*> circles_;
 
   rtabmap::Transform * currentPose_;
 
@@ -203,6 +256,7 @@ class Scene {
   bool lighting_;
   bool backfaceCulling_;
   bool wireFrame_;
+  bool textureColorSeamsHidden_;
   float r_;
   float g_;
   float b_;

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

@@ -28,8 +28,8 @@ Circle::Circle(float radius, int resolution) : Mesh(GL_TRIANGLE_FAN){
   for (int i = resolution; i >= 0; i--) {
     float theta = delta_theta * static_cast<float>(i);
     vertices.push_back(cos(theta) * radius);
-    vertices.push_back(0);
     vertices.push_back(sin(theta) * radius);
+    vertices.push_back(0);
   }
   SetVertices(vertices);
 }

app/android/jni/text_drawable.cpp

@@ -0,0 +1,366 @@
+/*
+Copyright (c) 2010-2025, 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.
+*/
+
+#ifdef __ANDROID__
+#include <jni.h>
+#endif
+
+#include <tango-gl/util.h>
+#include <vector>
+#include <rtabmap/core/Transform.h>
+#include "util.h"
+#include "text_drawable.h"
+#include "text_atlas_png.h"
+#include <rtabmap/utilite/UConversion.h>
+
+const std::string kTextVertexShader =
+	"uniform mat4 uMVPMatrix;"
+	"attribute vec4 vPosition;"
+	"attribute vec2 a_texCoord;"
+	"varying vec2 v_texCoord;"
+	"void main() {"
+	"  gl_Position = uMVPMatrix * vPosition;"
+	"  v_texCoord = a_texCoord;"
+	"}";
+const std::string kTextFragmentShader =
+	"precision mediump float;"
+	"uniform vec3 uColor;"
+	"varying vec2 v_texCoord;"
+	"uniform sampler2D s_texture;"
+	"void main() {"
+	"  gl_FragColor = texture2D( s_texture, v_texCoord );"
+	"  gl_FragColor.rgb = uColor;"
+	"}";
+
+const int RI_TEXT_TEXTURE_SIZE = 512; // 512
+const float RI_TEXT_HEIGHT_BASE = 32.0f;
+const char RI_TEXT_START = ' ';
+const char RI_TEXT_STOP = '~'+1;
+
+GLuint TextDrawable::textProgram_ = 0;
+GLuint TextDrawable::textTextureId_ = 0;
+float TextDrawable::textUVWidth_ = 0;
+float TextDrawable::textUVHeight_ = 0;
+float TextDrawable::textHeight_ = 0;
+std::vector<float> TextDrawable::textCharacterWidths_;
+
+void TextDrawable::createShaderProgram()
+{
+	releaseShaderProgram();
+
+	// hard-coded with text_atlas.png resource
+	textCharacterWidths_.resize(95);
+	textCharacterWidths_[0]=8;
+	textCharacterWidths_[1]=9.000000;
+	textCharacterWidths_[2]=10.000000;
+	textCharacterWidths_[3]=19.000000;
+	textCharacterWidths_[4]=18.000000;
+	textCharacterWidths_[5]=24.000000;
+	textCharacterWidths_[6]=21.000000;
+	textCharacterWidths_[7]=5.000000;
+	textCharacterWidths_[8]=11.000000;
+	textCharacterWidths_[9]=11.000000;
+	textCharacterWidths_[10]=15.000000;
+	textCharacterWidths_[11]=17.000000;
+	textCharacterWidths_[12]=8.000000;
+	textCharacterWidths_[13]=12.000000;
+	textCharacterWidths_[14]=9.000000;
+	textCharacterWidths_[15]=12.000000;
+	textCharacterWidths_[16]=18.000000;
+	textCharacterWidths_[17]=18.000000;
+	textCharacterWidths_[18]=18.000000;
+	textCharacterWidths_[19]=18.000000;
+	textCharacterWidths_[20]=18.000000;
+	textCharacterWidths_[21]=18.000000;
+	textCharacterWidths_[22]=18.000000;
+	textCharacterWidths_[23]=18.000000;
+	textCharacterWidths_[24]=18.000000;
+	textCharacterWidths_[25]=18.000000;
+	textCharacterWidths_[26]=9.000000;
+	textCharacterWidths_[27]=8.000000;
+	textCharacterWidths_[28]=16.000000;
+	textCharacterWidths_[29]=18.000000;
+	textCharacterWidths_[30]=17.000000;
+	textCharacterWidths_[31]=16.000000;
+	textCharacterWidths_[32]=29.000000;
+	textCharacterWidths_[33]=22.000000;
+	textCharacterWidths_[34]=20.000000;
+	textCharacterWidths_[35]=21.000000;
+	textCharacterWidths_[36]=21.000000;
+	textCharacterWidths_[37]=18.000000;
+	textCharacterWidths_[38]=18.000000;
+	textCharacterWidths_[39]=22.000000;
+	textCharacterWidths_[40]=23.000000;
+	textCharacterWidths_[41]=9.000000;
+	textCharacterWidths_[42]=18.000000;
+	textCharacterWidths_[43]=20.000000;
+	textCharacterWidths_[44]=17.000000;
+	textCharacterWidths_[45]=28.000000;
+	textCharacterWidths_[46]=23.000000;
+	textCharacterWidths_[47]=22.000000;
+	textCharacterWidths_[48]=21.000000;
+	textCharacterWidths_[49]=22.000000;
+	textCharacterWidths_[50]=20.000000;
+	textCharacterWidths_[51]=20.000000;
+	textCharacterWidths_[52]=20.000000;
+	textCharacterWidths_[53]=21.000000;
+	textCharacterWidths_[54]=21.000000;
+	textCharacterWidths_[55]=28.000000;
+	textCharacterWidths_[56]=20.000000;
+	textCharacterWidths_[57]=20.000000;
+	textCharacterWidths_[58]=19.000000;
+	textCharacterWidths_[59]=9.000000;
+	textCharacterWidths_[60]=14.000000;
+	textCharacterWidths_[61]=9.000000;
+	textCharacterWidths_[62]=14.000000;
+	textCharacterWidths_[63]=14.000000;
+	textCharacterWidths_[64]=11.000000;
+	textCharacterWidths_[65]=17.000000;
+	textCharacterWidths_[66]=18.000000;
+	textCharacterWidths_[67]=17.000000;
+	textCharacterWidths_[68]=18.000000;
+	textCharacterWidths_[69]=17.000000;
+	textCharacterWidths_[70]=11.000000;
+	textCharacterWidths_[71]=18.000000;
+	textCharacterWidths_[72]=18.000000;
+	textCharacterWidths_[73]=8.000000;
+	textCharacterWidths_[74]=8.000000;
+	textCharacterWidths_[75]=17.000000;
+	textCharacterWidths_[76]=8.000000;
+	textCharacterWidths_[77]=28.000000;
+	textCharacterWidths_[78]=18.000000;
+	textCharacterWidths_[79]=18.000000;
+	textCharacterWidths_[80]=18.000000;
+	textCharacterWidths_[81]=18.000000;
+	textCharacterWidths_[82]=12.000000;
+	textCharacterWidths_[83]=16.000000;
+	textCharacterWidths_[84]=11.000000;
+	textCharacterWidths_[85]=18.000000;
+	textCharacterWidths_[86]=16.000000;
+	textCharacterWidths_[87]=24.000000;
+	textCharacterWidths_[88]=16.000000;
+	textCharacterWidths_[89]=16.000000;
+	textCharacterWidths_[90]=16.000000;
+	textCharacterWidths_[91]=11.000000;
+	textCharacterWidths_[92]=8.000000;
+	textCharacterWidths_[93]=11.000000;
+	textCharacterWidths_[94]=21.000000;
+	textUVWidth_=0.062500;
+	textUVHeight_=0.082031;
+	textHeight_=42.000000;
+
+	textProgram_ = tango_gl::util::CreateProgram(kTextVertexShader.c_str(), kTextFragmentShader.c_str());
+	UASSERT(textProgram_ != 0);
+
+	glGenTextures(1, &textTextureId_);
+	UASSERT(textTextureId_);
+
+	// gen texture from image
+	glBindTexture(GL_TEXTURE_2D, textTextureId_);
+	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
+	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
+	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+
+	std::vector<char> data = uHex2Bytes(rtabmap::TEXT_ATLAS_PNG);
+
+	cv::Mat rgbImage = cv::imdecode(data, cv::IMREAD_UNCHANGED);
+
+	glPixelStorei(GL_UNPACK_ALIGNMENT, 4);
+	glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, rgbImage.cols, rgbImage.rows, 0, GL_RGBA, GL_UNSIGNED_BYTE, rgbImage.data);
+
+	GLint error = glGetError();
+	UASSERT(error == GL_NO_ERROR);
+}
+
+void TextDrawable::releaseShaderProgram()
+{
+	if(textProgram_)
+	{
+		glDeleteShader(textProgram_);
+		textProgram_ = 0;
+	}
+}
+
+TextDrawable::TextDrawable(
+		const std::string & text,
+		const rtabmap::Transform & pose,
+		float textSize, // meters
+		const tango_gl::Color & color) :
+		poseGl_(glmFromTransform(pose)),
+		color_(color)
+{
+	if(textProgram_ > 0)
+	{
+		vertexBuffer_ = std::vector<float>(text.size() * 12);
+		textureBuffer_ = std::vector<float>(text.size() * 8);
+		drawListBuffer_ = std::vector<unsigned short>(text.size() * 6);
+
+		int index_vecs = 0;
+		int index_indices = 0;
+		int index_uvs = 0;
+		float x=0.0f;
+		float y=0.0f;
+		float uniformscale = textSize/textHeight_;
+		for(unsigned int i=0; i<text.size(); ++i)
+		{
+			// get ascii value
+			char c = text[i];
+			int c_val = (int)c;
+
+			int indx = c_val-RI_TEXT_START;
+
+			if(indx<0 || indx>=textCharacterWidths_.size()) {
+				// unknown character, we will add a space for it to be save.
+				indx = 0;
+			}
+
+			int colCount = RI_TEXT_TEXTURE_SIZE/(int)RI_TEXT_HEIGHT_BASE;
+
+			// Calculate the uv parts
+			int row = indx / colCount;
+			int col = indx % colCount;
+
+			float v = row * textUVHeight_;
+			float v2 = v + textUVHeight_;
+			float u = col * textUVWidth_;
+			float u2 = u + textCharacterWidths_[indx]/(float)RI_TEXT_TEXTURE_SIZE;
+
+			// Creating the triangle information
+			std::vector<float> vec(12);
+			std::vector<float> uv(8);
+
+			vec[0] = x;
+			vec[1] = y + (textHeight_ * uniformscale);
+			vec[2] = 0;
+			vec[3] = x;
+			vec[4] = y;
+			vec[5] = 0;
+			vec[6] = x + (textCharacterWidths_[indx] * uniformscale);
+			vec[7] = y;
+			vec[8] = 0;
+			vec[9] = x + (textCharacterWidths_[indx] * uniformscale);
+			vec[10] = y + (textHeight_ * uniformscale);
+			vec[11] = 0;
+
+			// 0.001f = texture bleeding hack/fix
+			uv[0] = u+0.001f;
+			uv[1] = v+0.001f;
+			uv[2] = u+0.001f;
+			uv[3] = v2-0.001f;
+			uv[4] = u2-0.001f;
+			uv[5] = v2-0.001f;
+			uv[6] = u2-0.001f;
+			uv[7] = v+0.001f;
+
+			unsigned short inds[6] = {0, 1, 2, 0, 2, 3};
+
+			// We need a base value because the object has indices related to
+			// that object and not to this collection so basicly we need to
+			// translate the indices to align with the vertexlocation in ou
+			// vecs array of vectors.
+			short base = (short) (index_vecs / 3);
+
+			// We should add the vec, translating the indices to our saved vector
+			for(int i=0;i<vec.size();i++)
+			{
+				vertexBuffer_[index_vecs] = vec[i];
+				index_vecs++;
+			}
+
+			// We should add the uvs
+			for(int i=0;i<uv.size();i++)
+			{
+				textureBuffer_[index_uvs] = uv[i];
+				index_uvs++;
+			}
+
+			// We handle the indices
+			for(int j=0;j<6;j++)
+			{
+				drawListBuffer_[index_indices] = (unsigned short) (base + inds[j]);
+				index_indices++;
+			}
+
+			// Calculate the new position
+			x += (textCharacterWidths_[indx]  * uniformscale);
+		}
+	}
+}
+
+void TextDrawable::Render(
+		const glm::mat4 & projectionMatrix,
+		const glm::mat4 & viewMatrix,
+		const glm::mat4 & viewMatrixRotInv) const
+{
+	glUseProgram(textProgram_);
+
+	// get handle to vertex shader's vPosition member
+	int mPositionHandle = glGetAttribLocation(textProgram_, "vPosition");
+
+	// Enable a handle to the triangle vertices
+	glEnableVertexAttribArray(mPositionHandle);
+
+	// Prepare the background coordinate data
+	glVertexAttribPointer(mPositionHandle, 3, GL_FLOAT, false, 0, &vertexBuffer_[0]);
+
+	int mTexCoordLoc = glGetAttribLocation(textProgram_, "a_texCoord" );
+
+	// Prepare the texturecoordinates
+	glVertexAttribPointer ( mTexCoordLoc, 2, GL_FLOAT, false, 0, &textureBuffer_[0]);
+
+	glEnableVertexAttribArray ( mPositionHandle );
+	glEnableVertexAttribArray ( mTexCoordLoc );
+
+	// get handle to shape's transformation matrix
+	int mtrxhandle = glGetUniformLocation(textProgram_, "uMVPMatrix");
+
+	// Apply the projection and view transformation
+	//glUniformMatrix4fv(mtrxhandle, 1, false, m, 0);
+	glm::mat4 mvp_mat = projectionMatrix * viewMatrix * poseGl_ * viewMatrixRotInv;//glm::toMat4(gesture_camera_->GetParent()->GetRotation());
+	glUniformMatrix4fv(mtrxhandle, 1, GL_FALSE, glm::value_ptr(mvp_mat));
+
+	// get handle to color value
+	int colorhandle = glGetUniformLocation(textProgram_, "uColor");
+	glUniform3f(colorhandle, color_.r, color_.g, color_.b);
+
+	int mSamplerLoc = glGetUniformLocation (textProgram_, "s_texture" );
+
+	// Texture activate unit 0
+	glActiveTexture(GL_TEXTURE0);
+	// Bind the texture to this unit.
+	glBindTexture(GL_TEXTURE_2D, textTextureId_);
+	// Set the sampler texture unit to our selected id
+	glUniform1i ( mSamplerLoc, 0);
+
+	// Draw the triangle
+	glDrawElements(GL_TRIANGLES, drawListBuffer_.size(), GL_UNSIGNED_SHORT, &drawListBuffer_[0]);
+
+	// Disable vertex array
+	glDisableVertexAttribArray(mPositionHandle);
+	glDisableVertexAttribArray(mTexCoordLoc);
+}

app/android/jni/text_drawable.h

@@ -0,0 +1,71 @@
+/*
+Copyright (c) 2010-2025, 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.
+*/
+
+#ifndef TANGO_TEXT_DRAWABLE_H_
+#define TANGO_TEXT_DRAWABLE_H_
+
+#include <vector>
+#include <rtabmap/core/Transform.h>
+#include <tango-gl/color.h>
+
+// PointCloudDrawable is responsible for the point cloud rendering.
+class TextDrawable {
+private:
+	static GLuint textProgram_;
+	static GLuint textTextureId_;
+	static float textUVWidth_;
+	static float textUVHeight_;
+	static float textHeight_;
+	static std::vector<float> textCharacterWidths_;
+
+public:
+	static void createShaderProgram();
+	static void releaseShaderProgram();
+
+public:
+	TextDrawable(
+			const std::string & text,
+			const rtabmap::Transform & pose,
+			float textSize = 0.1f, // meters
+			const tango_gl::Color & color = tango_gl::Color(1,0,0));
+
+  virtual ~TextDrawable() {}
+
+  void Render(
+		  const glm::mat4 & projectionMatrix,
+		  const glm::mat4 & viewMatrix,
+		  const glm::mat4 & viewMatrixRotInv) const;
+
+ private:
+	std::vector<float> vertexBuffer_;
+	std::vector<float> textureBuffer_;
+	std::vector<unsigned short> drawListBuffer_;
+	glm::mat4 poseGl_;
+	tango_gl::Color color_;
+};
+
+#endif  // TANGO_POINT_CLOUD_POINT_CLOUD_DRAWABLE_H_

app/android/res/layout/activity_settings.xml

@@ -114,6 +114,13 @@
 		        android:title="@string/pref_title_smoothing"
 		        android:summary="@string/pref_summary_smoothing"
 		        android:defaultValue="@string/pref_default_smoothing"/>
+			<ListPreference
+		        android:key="@string/pref_key_depth_bleeding_error"
+		        android:title="@string/pref_title_depth_bleeding_error"
+		        android:summary="@string/pref_summary_depth_bleeding_error"
+		        android:entries="@array/pref_depth_bleeding_error_keys"
+		   	    android:entryValues="@array/pref_depth_bleeding_error_values"
+		   		android:defaultValue="@string/pref_default_depth_bleeding_error"/>
 		    <com.introlab.rtabmap.CustomSwitchPreference
 		        android:key="@string/pref_key_fisheye"
 		        android:title="@string/pref_title_fisheye"

app/android/res/values/strings.xml

@@ -76,6 +76,8 @@
     <string name="pref_default_resolution">false</string>
     <string name="pref_key_smoothing">pref_key_smoothing</string>
     <string name="pref_default_smoothing">false</string>
+    <string name="pref_key_depth_bleeding_error">pref_key_depth_bleeding_error</string>
+    <string name="pref_default_depth_bleeding_error">0.0</string>
     <string name="pref_key_fisheye">pref_key_fisheye</string>
     <string name="pref_default_fisheye">false</string>
     
@@ -337,6 +339,8 @@
     <string name="pref_summary_resolution">Save HD images of the color camera if you want very detailed textures. More memory will be required.</string>
     <string name="pref_title_smoothing">Smoothing</string>
     <string name="pref_summary_smoothing">Smooth the point clouds.</string>
+    <string name="pref_title_depth_bleeding_error">Depth Bleeding Filtering Error</string>
+    <string name="pref_summary_depth_bleeding_error">Filter noisy depth points on the edges.</string>
     <string name="pref_title_fisheye">Fish Eye Camera</string>
     <string name="pref_summary_fisheye">Use fish eye camera instead of the color camera. May not work on some devices.</string>
     <string name="pref_title_update_rate">Update Rate</string>
@@ -436,6 +440,26 @@
         <item>"29.41995"</item>
         <item>"19.6133"</item>
     </string-array>
+    <string-array name="pref_depth_bleeding_error_keys">
+        <item>"Disabled"</item>
+        <item>"2.5 cm"</item>
+        <item>"5 cm"</item>
+        <item>"10 cm"</item>
+        <item>"15 cm"</item>
+        <item>"20 cm"</item>
+        <item>"25 cm"</item>
+        <item>"30 cm"</item>
+    </string-array>
+    <string-array name="pref_depth_bleeding_error_values">
+        <item>"0"</item>
+        <item>"0.025"</item>
+        <item>"0.05"</item>
+        <item>"0.10"</item>
+        <item>"0.15"</item>
+        <item>"0.20"</item>
+        <item>"0.25"</item>
+        <item>"0.30"</item>
+    </string-array>
     <string-array name="pref_max_speed_keys">
         <item>"No Limit"</item>
         <item>"High"</item>

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

@@ -1140,6 +1140,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.setDepthBleedingError(nativeApplication, Float.parseFloat(sharedPref.getString(getString(R.string.pref_key_depth_bleeding_error), getString(R.string.pref_default_depth_bleeding_error))));
 			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))));

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

@@ -73,6 +73,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 setDepthBleedingError(long nativeApplication, float value);
     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);

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

@@ -222,7 +222,8 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         ((Preference)findPreference(getString(R.string.pref_key_background_color))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_background_color))).getEntry() + ") "+getString(R.string.pref_summary_background_color));
         ((Preference)findPreference(getString(R.string.pref_key_rendering_texture_decimation))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_rendering_texture_decimation))).getEntry() + ") "+getString(R.string.pref_summary_rendering_texture_decimation));
           
-        ((Preference)findPreference(getString(R.string.pref_key_arcore_relocalization_acc_thr))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_arcore_relocalization_acc_thr))).getEntry() + ") "+getString(R.string.pref_summary_arcore_relocalization_acc_thr));
+        ((Preference)findPreference(getString(R.string.pref_key_depth_bleeding_error))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_depth_bleeding_error))).getEntry() + ") "+getString(R.string.pref_summary_depth_bleeding_error));
+		((Preference)findPreference(getString(R.string.pref_key_arcore_relocalization_acc_thr))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_arcore_relocalization_acc_thr))).getEntry() + ") "+getString(R.string.pref_summary_arcore_relocalization_acc_thr));
         ((Preference)findPreference(getString(R.string.pref_key_update_rate))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_update_rate))).getEntry() + ") "+getString(R.string.pref_summary_update_rate));
         ((Preference)findPreference(getString(R.string.pref_key_max_speed))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_max_speed))).getEntry() + ") "+getString(R.string.pref_summary_max_speed));
         ((Preference)findPreference(getString(R.string.pref_key_time_thr))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_time_thr))).getEntry() + ") "+getString(R.string.pref_summary_time_thr));
@@ -285,7 +286,8 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         	if(key.compareTo(getString(R.string.pref_key_background_color))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_background_color));
         	if(key.compareTo(getString(R.string.pref_key_rendering_texture_decimation))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_rendering_texture_decimation));
         	
-        	if(key.compareTo(getString(R.string.pref_key_arcore_relocalization_acc_thr))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_arcore_relocalization_acc_thr));
+        	if(key.compareTo(getString(R.string.pref_key_depth_bleeding_error))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_depth_bleeding_error));
+			if(key.compareTo(getString(R.string.pref_key_arcore_relocalization_acc_thr))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_arcore_relocalization_acc_thr));
         	if(key.compareTo(getString(R.string.pref_key_update_rate))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_update_rate));
         	if(key.compareTo(getString(R.string.pref_key_max_speed))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_max_speed));
         	if(key.compareTo(getString(R.string.pref_key_time_thr))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_time_thr));

app/ios/RTABMapApp.xcodeproj/project.pbxproj

@@ -8,10 +8,82 @@
 
 /* Begin PBXBuildFile section */
 		4E0D83832621F52C00C879AC /* Settings.bundle in Resources */ = {isa = PBXBuildFile; fileRef = 4E0D83822621F52C00C879AC /* Settings.bundle */; };
+		4E10D89C2DE409870015926D /* librtabmap_core.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EB0259D5BE500575D88 /* librtabmap_core.a */; };
+		4E10D89D2DE409870015926D /* librtabmap_utilite.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EAF259D5BE500575D88 /* librtabmap_utilite.a */; };
+		4E10D89E2DE409A60015926D /* libopencv_xfeatures2d.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0ED9259D5BFC00575D88 /* libopencv_xfeatures2d.a */; };
+		4E10D89F2DE409B20015926D /* libopencv_video.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EB4259D5BFB00575D88 /* libopencv_video.a */; };
+		4E10D8A02DE409BB0015926D /* libopencv_tracking.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0ECC259D5BFC00575D88 /* libopencv_tracking.a */; };
+		4E10D8A12DE409C00015926D /* libopencv_optflow.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EC3259D5BFC00575D88 /* libopencv_optflow.a */; };
+		4E10D8A22DE409C70015926D /* libopencv_imgproc.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EBE259D5BFC00575D88 /* libopencv_imgproc.a */; };
+		4E10D8A32DE409CA0015926D /* libopencv_features2d.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EB8259D5BFB00575D88 /* libopencv_features2d.a */; };
+		4E10D8A42DE409CE0015926D /* libopencv_core.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0ED1259D5BFC00575D88 /* libopencv_core.a */; };
+		4E10D8A52DE409D10015926D /* libopencv_calib3d.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0ED6259D5BFC00575D88 /* libopencv_calib3d.a */; };
+		4E10D8A62DE409D50015926D /* libopencv_aruco.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EB5259D5BFB00575D88 /* libopencv_aruco.a */; };
+		4E10D8A72DE409F40015926D /* libopencv_flann.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EBC259D5BFB00575D88 /* libopencv_flann.a */; };
+		4E10D8A82DE40A1B0015926D /* libopencv_ximgproc.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EDC259D5BFC00575D88 /* libopencv_ximgproc.a */; };
+		4E10D8A92DE40A3A0015926D /* libopencv_photo.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EB6259D5BFB00575D88 /* libopencv_photo.a */; };
+		4E10D8AA2DE40A6C0015926D /* libopencv_objdetect.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EBB259D5BFB00575D88 /* libopencv_objdetect.a */; };
+		4E10D8AB2DE40A8C0015926D /* libopencv_stitching.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EBD259D5BFB00575D88 /* libopencv_stitching.a */; };
+		4E10D8AC2DE40AA20015926D /* libopencv_imgcodecs.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EC4259D5BFC00575D88 /* libopencv_imgcodecs.a */; };
+		4E10D8AD2DE40E990015926D /* libg2o_core.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F0D259D5C2200575D88 /* libg2o_core.a */; };
+		4E10D8AE2DE40E990015926D /* libg2o_solver_dense.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F07259D5C2100575D88 /* libg2o_solver_dense.a */; };
+		4E10D8AF2DE40E990015926D /* libg2o_solver_eigen.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F12259D5C2200575D88 /* libg2o_solver_eigen.a */; };
+		4E10D8B02DE40E990015926D /* libg2o_solver_pcg.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F0B259D5C2200575D88 /* libg2o_solver_pcg.a */; };
+		4E10D8B12DE40E990015926D /* libg2o_solver_slam2d_linear.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F09259D5C2100575D88 /* libg2o_solver_slam2d_linear.a */; };
+		4E10D8B22DE40E990015926D /* libg2o_solver_structure_only.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F11259D5C2200575D88 /* libg2o_solver_structure_only.a */; };
+		4E10D8B32DE40E990015926D /* libg2o_stuff.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F14259D5C2200575D88 /* libg2o_stuff.a */; };
+		4E10D8B42DE40E990015926D /* libg2o_types_data.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F0C259D5C2200575D88 /* libg2o_types_data.a */; };
+		4E10D8B52DE40E990015926D /* libg2o_types_icp.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F0F259D5C2200575D88 /* libg2o_types_icp.a */; };
+		4E10D8B62DE40E990015926D /* libg2o_types_sba.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F15259D5C2200575D88 /* libg2o_types_sba.a */; };
+		4E10D8B72DE40E990015926D /* libg2o_types_sclam2d.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F1A259D5C2200575D88 /* libg2o_types_sclam2d.a */; };
+		4E10D8B82DE40E990015926D /* libg2o_types_sim3.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F1B259D5C2200575D88 /* libg2o_types_sim3.a */; };
+		4E10D8B92DE40E990015926D /* libg2o_types_slam2d_addons.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F16259D5C2200575D88 /* libg2o_types_slam2d_addons.a */; };
+		4E10D8BA2DE40E990015926D /* libg2o_types_slam2d.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F0A259D5C2100575D88 /* libg2o_types_slam2d.a */; };
+		4E10D8BB2DE40E990015926D /* libg2o_types_slam3d_addons.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F17259D5C2200575D88 /* libg2o_types_slam3d_addons.a */; };
+		4E10D8BC2DE40E990015926D /* libg2o_types_slam3d.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F18259D5C2200575D88 /* libg2o_types_slam3d.a */; };
+		4E10D8BD2DE40EC70015926D /* libpcl_common.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B4D259D4DF100575D88 /* libpcl_common.a */; };
+		4E10D8BE2DE40EC70015926D /* libpcl_features.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B43259D4DF100575D88 /* libpcl_features.a */; };
+		4E10D8BF2DE40EC70015926D /* libpcl_filters.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B44259D4DF100575D88 /* libpcl_filters.a */; };
+		4E10D8C02DE40EC70015926D /* libpcl_io_ply.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B4E259D4DF100575D88 /* libpcl_io_ply.a */; };
+		4E10D8C12DE40EC70015926D /* libpcl_io.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B4C259D4DF100575D88 /* libpcl_io.a */; };
+		4E10D8C22DE40EC70015926D /* libpcl_kdtree.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B46259D4DF100575D88 /* libpcl_kdtree.a */; };
+		4E10D8C32DE40EC70015926D /* libpcl_keypoints.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B49259D4DF100575D88 /* libpcl_keypoints.a */; };
+		4E10D8C42DE40EC70015926D /* libpcl_ml.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B4F259D4DF100575D88 /* libpcl_ml.a */; };
+		4E10D8C52DE40EC70015926D /* libpcl_octree.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B4B259D4DF100575D88 /* libpcl_octree.a */; };
+		4E10D8C62DE40EC70015926D /* libpcl_registration.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B42259D4DF100575D88 /* libpcl_registration.a */; };
+		4E10D8C72DE40EC70015926D /* libpcl_sample_consensus.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B45259D4DF100575D88 /* libpcl_sample_consensus.a */; };
+		4E10D8C82DE40EC70015926D /* libpcl_search.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B47259D4DF100575D88 /* libpcl_search.a */; };
+		4E10D8C92DE40EC70015926D /* libpcl_segmentation.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B50259D4DF100575D88 /* libpcl_segmentation.a */; };
+		4E10D8CA2DE40EC70015926D /* libpcl_stereo.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B41259D4DF100575D88 /* libpcl_stereo.a */; };
+		4E10D8CB2DE40EC70015926D /* libpcl_surface.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B48259D4DF100575D88 /* libpcl_surface.a */; };
+		4E10D8CC2DE40EE30015926D /* libgtsam_unstable.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F0E259D5C2200575D88 /* libgtsam_unstable.a */; };
+		4E10D8CD2DE40EE30015926D /* libgtsam.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F1C259D5C2200575D88 /* libgtsam.a */; };
+		4E10D8CE2DE40EE30015926D /* libmetis-gtsam.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4E10D8622DE408EE0015926D /* libmetis-gtsam.a */; };
+		4E10D8CF2DE40EF90015926D /* libboost_serialization.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B30259D4DE900575D88 /* libboost_serialization.a */; };
+		4E10D8D02DE40F060015926D /* libboost_timer.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B34259D4DE900575D88 /* libboost_timer.a */; };
+		4E10D8D12DE40F190015926D /* liblas_c.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4E10D8752DE408EF0015926D /* liblas_c.a */; };
+		4E10D8D22DE40F190015926D /* liblas.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFAA9422CAE4E960055DA51 /* liblas.a */; };
+		4E10D8D32DE40F190015926D /* liblaszip.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4ED73AD02D7FECF3007899C2 /* liblaszip.a */; };
+		4E10D8D42DE40F2A0015926D /* libboost_thread.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B33259D4DE900575D88 /* libboost_thread.a */; };
+		4E10D8E12DE4101B0015926D /* liblibjpeg-turbo.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4E10D8D52DE40FBE0015926D /* liblibjpeg-turbo.a */; };
+		4E10D8E22DE4101B0015926D /* liblibpng.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4E10D8D72DE40FCA0015926D /* liblibpng.a */; };
+		4E1E9CF72D618E69000CB881 /* quad_color.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 4E1E9CF62D618079000CB881 /* quad_color.cpp */; };
+		4E1E9CF82D618E69000CB881 /* text_drawable.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 4E1E9CF52D618079000CB881 /* text_drawable.cpp */; };
 		4E20B24E266AB94300316EE6 /* Images.xcassets in Resources */ = {isa = PBXBuildFile; fileRef = 4E20B24D266AB94300316EE6 /* Images.xcassets */; };
 		4E20B250266AB95600316EE6 /* Images.xcassets in Resources */ = {isa = PBXBuildFile; fileRef = 4E20B24F266AB95600316EE6 /* Images.xcassets */; };
 		4E2C516725A63119005CEDBD /* DatabaseView.swift in Sources */ = {isa = PBXBuildFile; fileRef = 4E2C516525A63119005CEDBD /* DatabaseView.swift */; };
 		4E2C516825A63119005CEDBD /* VerticalScrollerView.swift in Sources */ = {isa = PBXBuildFile; fileRef = 4E2C516625A63119005CEDBD /* VerticalScrollerView.swift */; };
+		4E3525542DE4C6E500F39D8F /* libg2o_csparse_extension.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F13259D5C2200575D88 /* libg2o_csparse_extension.a */; };
+		4E3525552DE4C6E500F39D8F /* libg2o_solver_csparse.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F10259D5C2200575D88 /* libg2o_solver_csparse.a */; };
+		4E3525572DE4C6E500F39D8F /* libg2o_solver_cholmod.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4E3525562DE4C6E500F39D8F /* libg2o_solver_cholmod.a */; };
+		4E3525592DE4C6F600F39D8F /* libcholmod.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4E3525582DE4C6F600F39D8F /* libcholmod.a */; };
+		4E35255E2DE4C71000F39D8F /* libcxsparse.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4E35255A2DE4C71000F39D8F /* libcxsparse.a */; };
+		4E35255F2DE4C71000F39D8F /* libcolamd.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4E35255B2DE4C71000F39D8F /* libcolamd.a */; };
+		4E3525602DE4C71000F39D8F /* libcamd.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4E35255C2DE4C71000F39D8F /* libcamd.a */; };
+		4E3525612DE4C71000F39D8F /* libccolamd.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4E35255D2DE4C71000F39D8F /* libccolamd.a */; };
+		4E3525642DE4C71C00F39D8F /* libsuitesparseconfig.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4E3525622DE4C71C00F39D8F /* libsuitesparseconfig.a */; };
+		4E3525652DE4C71C00F39D8F /* libspqr.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4E3525632DE4C71C00F39D8F /* libspqr.a */; };
+		4E3525672DE4C72D00F39D8F /* libamd.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4E3525662DE4C72D00F39D8F /* libamd.a */; };
 		4E8B155426273A580037FC53 /* Zip in Frameworks */ = {isa = PBXBuildFile; productRef = 4E8B155326273A580037FC53 /* Zip */; };
 		4EB1A66625A0D90B0043C7BF /* RTABMap.swift in Sources */ = {isa = PBXBuildFile; fileRef = 4EB1A66525A0D90B0043C7BF /* RTABMap.swift */; };
 		4EE016B3259BE441008CCE65 /* ViewController.swift in Sources */ = {isa = PBXBuildFile; fileRef = 4EE016AF259BE441008CCE65 /* ViewController.swift */; };
@@ -20,34 +92,6 @@
 		4EE016C3259BE464008CCE65 /* Main.storyboard in Resources */ = {isa = PBXBuildFile; fileRef = 4EE016BF259BE464008CCE65 /* Main.storyboard */; };
 		4EE016C4259BE464008CCE65 /* LaunchScreen.storyboard in Resources */ = {isa = PBXBuildFile; fileRef = 4EE016C1259BE464008CCE65 /* LaunchScreen.storyboard */; };
 		4EE016C7259BE46F008CCE65 /* Assets.xcassets in Resources */ = {isa = PBXBuildFile; fileRef = 4EE016C6259BE46F008CCE65 /* Assets.xcassets */; };
-		4EFAA9432CAE4E960055DA51 /* liblas.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFAA9422CAE4E960055DA51 /* liblas.a */; };
-		4EFD0B36259D4DE900575D88 /* libboost_filesystem.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B2B259D4DE900575D88 /* libboost_filesystem.a */; };
-		4EFD0B37259D4DE900575D88 /* libboost_program_options.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B2C259D4DE900575D88 /* libboost_program_options.a */; };
-		4EFD0B38259D4DE900575D88 /* libboost_regex.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B2D259D4DE900575D88 /* libboost_regex.a */; };
-		4EFD0B39259D4DE900575D88 /* libboost_iostreams.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B2E259D4DE900575D88 /* libboost_iostreams.a */; };
-		4EFD0B3A259D4DE900575D88 /* libboost_date_time.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B2F259D4DE900575D88 /* libboost_date_time.a */; };
-		4EFD0B3B259D4DE900575D88 /* libboost_serialization.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B30259D4DE900575D88 /* libboost_serialization.a */; };
-		4EFD0B3C259D4DE900575D88 /* libboost_system.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B31259D4DE900575D88 /* libboost_system.a */; };
-		4EFD0B3D259D4DE900575D88 /* libboost_signals.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B32259D4DE900575D88 /* libboost_signals.a */; };
-		4EFD0B3E259D4DE900575D88 /* libboost_thread.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B33259D4DE900575D88 /* libboost_thread.a */; };
-		4EFD0B3F259D4DE900575D88 /* libboost_timer.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B34259D4DE900575D88 /* libboost_timer.a */; };
-		4EFD0B40259D4DE900575D88 /* libboost_chrono.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B35259D4DE900575D88 /* libboost_chrono.a */; };
-		4EFD0B51259D4DF100575D88 /* libpcl_stereo.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B41259D4DF100575D88 /* libpcl_stereo.a */; };
-		4EFD0B52259D4DF100575D88 /* libpcl_registration.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B42259D4DF100575D88 /* libpcl_registration.a */; };
-		4EFD0B53259D4DF100575D88 /* libpcl_features.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B43259D4DF100575D88 /* libpcl_features.a */; };
-		4EFD0B54259D4DF100575D88 /* libpcl_filters.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B44259D4DF100575D88 /* libpcl_filters.a */; };
-		4EFD0B55259D4DF100575D88 /* libpcl_sample_consensus.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B45259D4DF100575D88 /* libpcl_sample_consensus.a */; };
-		4EFD0B56259D4DF100575D88 /* libpcl_kdtree.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B46259D4DF100575D88 /* libpcl_kdtree.a */; };
-		4EFD0B57259D4DF100575D88 /* libpcl_search.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B47259D4DF100575D88 /* libpcl_search.a */; };
-		4EFD0B58259D4DF100575D88 /* libpcl_surface.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B48259D4DF100575D88 /* libpcl_surface.a */; };
-		4EFD0B59259D4DF100575D88 /* libpcl_keypoints.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B49259D4DF100575D88 /* libpcl_keypoints.a */; };
-		4EFD0B5A259D4DF100575D88 /* libpcl_recognition.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B4A259D4DF100575D88 /* libpcl_recognition.a */; };
-		4EFD0B5B259D4DF100575D88 /* libpcl_octree.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B4B259D4DF100575D88 /* libpcl_octree.a */; };
-		4EFD0B5C259D4DF100575D88 /* libpcl_io.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B4C259D4DF100575D88 /* libpcl_io.a */; };
-		4EFD0B5D259D4DF100575D88 /* libpcl_common.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B4D259D4DF100575D88 /* libpcl_common.a */; };
-		4EFD0B5E259D4DF100575D88 /* libpcl_io_ply.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B4E259D4DF100575D88 /* libpcl_io_ply.a */; };
-		4EFD0B5F259D4DF100575D88 /* libpcl_ml.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B4F259D4DF100575D88 /* libpcl_ml.a */; };
-		4EFD0B60259D4DF100575D88 /* libpcl_segmentation.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B50259D4DF100575D88 /* libpcl_segmentation.a */; };
 		4EFD0B97259D501E00575D88 /* line.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 4EFD0B63259D501E00575D88 /* line.cpp */; };
 		4EFD0B98259D501E00575D88 /* util.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 4EFD0B64259D501E00575D88 /* util.cpp */; };
 		4EFD0B99259D501E00575D88 /* obj_loader.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 4EFD0B65259D501E00575D88 /* obj_loader.cpp */; };
@@ -75,93 +119,74 @@
 		4EFD0BBC259D509300575D88 /* point_cloud_drawable.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 4EFD0BBA259D509300575D88 /* point_cloud_drawable.cpp */; };
 		4EFD0BC0259D50A800575D88 /* scene.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 4EFD0BBE259D50A800575D88 /* scene.cpp */; };
 		4EFD0EAB259D581800575D88 /* OpenGLES.framework in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EAA259D581800575D88 /* OpenGLES.framework */; };
-		4EFD0EB1259D5BE500575D88 /* librtabmap_utilite.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EAF259D5BE500575D88 /* librtabmap_utilite.a */; };
-		4EFD0EB2259D5BE500575D88 /* librtabmap_core.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EB0259D5BE500575D88 /* librtabmap_core.a */; };
-		4EFD0EDD259D5BFC00575D88 /* libopencv_video.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EB4259D5BFB00575D88 /* libopencv_video.a */; };
-		4EFD0EDE259D5BFC00575D88 /* libopencv_aruco.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EB5259D5BFB00575D88 /* libopencv_aruco.a */; };
-		4EFD0EDF259D5BFC00575D88 /* libopencv_photo.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EB6259D5BFB00575D88 /* libopencv_photo.a */; };
-		4EFD0EE0259D5BFC00575D88 /* libopencv_line_descriptor.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EB7259D5BFB00575D88 /* libopencv_line_descriptor.a */; };
-		4EFD0EE1259D5BFC00575D88 /* libopencv_features2d.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EB8259D5BFB00575D88 /* libopencv_features2d.a */; };
-		4EFD0EE2259D5BFC00575D88 /* libopencv_bgsegm.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EB9259D5BFB00575D88 /* libopencv_bgsegm.a */; };
-		4EFD0EE3259D5BFC00575D88 /* libopencv_dnn_objdetect.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EBA259D5BFB00575D88 /* libopencv_dnn_objdetect.a */; };
-		4EFD0EE4259D5BFC00575D88 /* libopencv_objdetect.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EBB259D5BFB00575D88 /* libopencv_objdetect.a */; };
-		4EFD0EE5259D5BFC00575D88 /* libopencv_flann.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EBC259D5BFB00575D88 /* libopencv_flann.a */; };
-		4EFD0EE6259D5BFC00575D88 /* libopencv_stitching.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EBD259D5BFB00575D88 /* libopencv_stitching.a */; };
-		4EFD0EE7259D5BFC00575D88 /* libopencv_imgproc.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EBE259D5BFC00575D88 /* libopencv_imgproc.a */; };
-		4EFD0EE8259D5BFC00575D88 /* libopencv_ml.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EBF259D5BFC00575D88 /* libopencv_ml.a */; };
-		4EFD0EE9259D5BFC00575D88 /* libopencv_rgbd.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EC0259D5BFC00575D88 /* libopencv_rgbd.a */; };
-		4EFD0EEA259D5BFC00575D88 /* libopencv_xphoto.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EC1259D5BFC00575D88 /* libopencv_xphoto.a */; };
-		4EFD0EEB259D5BFC00575D88 /* libopencv_phase_unwrapping.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EC2259D5BFC00575D88 /* libopencv_phase_unwrapping.a */; };
-		4EFD0EEC259D5BFC00575D88 /* libopencv_optflow.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EC3259D5BFC00575D88 /* libopencv_optflow.a */; };
-		4EFD0EED259D5BFC00575D88 /* libopencv_imgcodecs.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EC4259D5BFC00575D88 /* libopencv_imgcodecs.a */; };
-		4EFD0EEE259D5BFC00575D88 /* libopencv_fuzzy.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EC5259D5BFC00575D88 /* libopencv_fuzzy.a */; };
-		4EFD0EEF259D5BFC00575D88 /* libopencv_dpm.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EC6259D5BFC00575D88 /* libopencv_dpm.a */; };
-		4EFD0EF0259D5BFC00575D88 /* libopencv_plot.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EC7259D5BFC00575D88 /* libopencv_plot.a */; };
-		4EFD0EF1259D5BFC00575D88 /* libopencv_dnn.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EC8259D5BFC00575D88 /* libopencv_dnn.a */; };
-		4EFD0EF2259D5BFC00575D88 /* libopencv_saliency.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EC9259D5BFC00575D88 /* libopencv_saliency.a */; };
-		4EFD0EF3259D5BFC00575D88 /* libopencv_videoio.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0ECA259D5BFC00575D88 /* libopencv_videoio.a */; };
-		4EFD0EF4259D5BFC00575D88 /* libopencv_hfs.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0ECB259D5BFC00575D88 /* libopencv_hfs.a */; };
-		4EFD0EF5259D5BFC00575D88 /* libopencv_tracking.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0ECC259D5BFC00575D88 /* libopencv_tracking.a */; };
-		4EFD0EF6259D5BFC00575D88 /* libopencv_text.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0ECD259D5BFC00575D88 /* libopencv_text.a */; };
-		4EFD0EF7259D5BFC00575D88 /* libopencv_face.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0ECE259D5BFC00575D88 /* libopencv_face.a */; };
-		4EFD0EF8259D5BFC00575D88 /* libopencv_bioinspired.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0ECF259D5BFC00575D88 /* libopencv_bioinspired.a */; };
-		4EFD0EF9259D5BFC00575D88 /* libopencv_highgui.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0ED0259D5BFC00575D88 /* libopencv_highgui.a */; };
-		4EFD0EFA259D5BFC00575D88 /* libopencv_core.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0ED1259D5BFC00575D88 /* libopencv_core.a */; };
-		4EFD0EFB259D5BFC00575D88 /* libopencv_shape.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0ED2259D5BFC00575D88 /* libopencv_shape.a */; };
-		4EFD0EFC259D5BFC00575D88 /* libopencv_datasets.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0ED3259D5BFC00575D88 /* libopencv_datasets.a */; };
-		4EFD0EFD259D5BFC00575D88 /* libopencv_img_hash.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0ED4259D5BFC00575D88 /* libopencv_img_hash.a */; };
-		4EFD0EFE259D5BFC00575D88 /* libopencv_surface_matching.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0ED5259D5BFC00575D88 /* libopencv_surface_matching.a */; };
-		4EFD0EFF259D5BFC00575D88 /* libopencv_calib3d.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0ED6259D5BFC00575D88 /* libopencv_calib3d.a */; };
-		4EFD0F00259D5BFC00575D88 /* libopencv_structured_light.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0ED7259D5BFC00575D88 /* libopencv_structured_light.a */; };
-		4EFD0F01259D5BFC00575D88 /* libopencv_reg.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0ED8259D5BFC00575D88 /* libopencv_reg.a */; };
-		4EFD0F02259D5BFC00575D88 /* libopencv_xfeatures2d.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0ED9259D5BFC00575D88 /* libopencv_xfeatures2d.a */; };
-		4EFD0F03259D5BFC00575D88 /* libopencv_videostab.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EDA259D5BFC00575D88 /* libopencv_videostab.a */; };
-		4EFD0F04259D5BFC00575D88 /* libopencv_ccalib.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EDB259D5BFC00575D88 /* libopencv_ccalib.a */; };
-		4EFD0F05259D5BFC00575D88 /* libopencv_ximgproc.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EDC259D5BFC00575D88 /* libopencv_ximgproc.a */; };
-		4EFD0F1D259D5C2200575D88 /* libg2o_solver_dense.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F07259D5C2100575D88 /* libg2o_solver_dense.a */; };
-		4EFD0F1E259D5C2200575D88 /* libg2o_ext_csparse.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F08259D5C2100575D88 /* libg2o_ext_csparse.a */; };
-		4EFD0F1F259D5C2200575D88 /* libg2o_solver_slam2d_linear.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F09259D5C2100575D88 /* libg2o_solver_slam2d_linear.a */; };
-		4EFD0F20259D5C2200575D88 /* libg2o_types_slam2d.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F0A259D5C2100575D88 /* libg2o_types_slam2d.a */; };
-		4EFD0F21259D5C2200575D88 /* libg2o_solver_pcg.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F0B259D5C2200575D88 /* libg2o_solver_pcg.a */; };
-		4EFD0F22259D5C2200575D88 /* libg2o_types_data.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F0C259D5C2200575D88 /* libg2o_types_data.a */; };
-		4EFD0F23259D5C2200575D88 /* libg2o_core.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F0D259D5C2200575D88 /* libg2o_core.a */; };
-		4EFD0F24259D5C2200575D88 /* libgtsam_unstable.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F0E259D5C2200575D88 /* libgtsam_unstable.a */; };
-		4EFD0F25259D5C2200575D88 /* libg2o_types_icp.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F0F259D5C2200575D88 /* libg2o_types_icp.a */; };
-		4EFD0F26259D5C2200575D88 /* libg2o_solver_csparse.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F10259D5C2200575D88 /* libg2o_solver_csparse.a */; };
-		4EFD0F27259D5C2200575D88 /* libg2o_solver_structure_only.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F11259D5C2200575D88 /* libg2o_solver_structure_only.a */; };
-		4EFD0F28259D5C2200575D88 /* libg2o_solver_eigen.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F12259D5C2200575D88 /* libg2o_solver_eigen.a */; };
-		4EFD0F29259D5C2200575D88 /* libg2o_csparse_extension.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F13259D5C2200575D88 /* libg2o_csparse_extension.a */; };
-		4EFD0F2A259D5C2200575D88 /* libg2o_stuff.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F14259D5C2200575D88 /* libg2o_stuff.a */; };
-		4EFD0F2B259D5C2200575D88 /* libg2o_types_sba.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F15259D5C2200575D88 /* libg2o_types_sba.a */; };
-		4EFD0F2C259D5C2200575D88 /* libg2o_types_slam2d_addons.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F16259D5C2200575D88 /* libg2o_types_slam2d_addons.a */; };
-		4EFD0F2D259D5C2200575D88 /* libg2o_types_slam3d_addons.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F17259D5C2200575D88 /* libg2o_types_slam3d_addons.a */; };
-		4EFD0F2E259D5C2200575D88 /* libg2o_types_slam3d.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F18259D5C2200575D88 /* libg2o_types_slam3d.a */; };
-		4EFD0F2F259D5C2200575D88 /* libmetis.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F19259D5C2200575D88 /* libmetis.a */; };
-		4EFD0F30259D5C2200575D88 /* libg2o_types_sclam2d.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F1A259D5C2200575D88 /* libg2o_types_sclam2d.a */; };
-		4EFD0F31259D5C2200575D88 /* libg2o_types_sim3.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F1B259D5C2200575D88 /* libg2o_types_sim3.a */; };
-		4EFD0F32259D5C2200575D88 /* libgtsam.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F1C259D5C2200575D88 /* libgtsam.a */; };
-		4EFD0F34259D5C2900575D88 /* libflann_cpp_s.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F33259D5C2900575D88 /* libflann_cpp_s.a */; };
 		4EFD0F36259D5C7300575D88 /* vtk.framework in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F35259D5C7300575D88 /* vtk.framework */; };
 		4EFD0F3A259D5D1500575D88 /* CameraMobile.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 4EFD0F38259D5D1500575D88 /* CameraMobile.cpp */; };
-		4EFD0F3D259D5D5C00575D88 /* libsqlite3.tbd in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F3C259D5D5C00575D88 /* libsqlite3.tbd */; };
-		4EFD0F50259D67D900575D88 /* liblibwebp.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F4B259D67D900575D88 /* liblibwebp.a */; };
-		4EFD0F51259D67D900575D88 /* liblibpng.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F4C259D67D900575D88 /* liblibpng.a */; };
-		4EFD0F52259D67D900575D88 /* liblibjpeg-turbo.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F4D259D67D900575D88 /* liblibjpeg-turbo.a */; };
-		4EFD0F53259D67D900575D88 /* libzlib.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F4E259D67D900575D88 /* libzlib.a */; };
-		4EFD0F54259D67D900575D88 /* liblibprotobuf.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F4F259D67D900575D88 /* liblibprotobuf.a */; };
 		4EFD0F62259E341C00575D88 /* RTABMapApp.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 4EFD0F61259E341C00575D88 /* RTABMapApp.cpp */; };
 		4EFD0F67259E38DE00575D88 /* NativeWrapper.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 4EFD0F66259E38DE00575D88 /* NativeWrapper.cpp */; };
 		4EFD0F70259F847300575D88 /* background_renderer.cc in Sources */ = {isa = PBXBuildFile; fileRef = 4EFD0F6E259F847300575D88 /* background_renderer.cc */; };
 /* End PBXBuildFile section */
 
 /* Begin PBXFileReference section */
+		44D4681D2D538A4100B094BA /* RTABMapApp.entitlements */ = {isa = PBXFileReference; lastKnownFileType = text.plist.entitlements; name = RTABMapApp.entitlements; path = RTABMapApp/RTABMapApp.entitlements; sourceTree = "<group>"; };
 		4E0D83822621F52C00C879AC /* Settings.bundle */ = {isa = PBXFileReference; lastKnownFileType = "wrapper.plug-in"; path = Settings.bundle; sourceTree = "<group>"; };
+		4E10D8602DE408EE0015926D /* libboost_stacktrace_noop.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libboost_stacktrace_noop.a; path = RTABMapApp/Libraries/lib/libboost_stacktrace_noop.a; sourceTree = "<group>"; };
+		4E10D8612DE408EE0015926D /* libboost_wserialization.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libboost_wserialization.a; path = RTABMapApp/Libraries/lib/libboost_wserialization.a; sourceTree = "<group>"; };
+		4E10D8622DE408EE0015926D /* libmetis-gtsam.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = "libmetis-gtsam.a"; path = "RTABMapApp/Libraries/lib/libmetis-gtsam.a"; sourceTree = "<group>"; };
+		4E10D8632DE408EE0015926D /* libopencv_wechat_qrcode.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libopencv_wechat_qrcode.a; path = RTABMapApp/Libraries/lib/libopencv_wechat_qrcode.a; sourceTree = "<group>"; };
+		4E10D8642DE408EE0015926D /* libboost_container.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libboost_container.a; path = RTABMapApp/Libraries/lib/libboost_container.a; sourceTree = "<group>"; };
+		4E10D8652DE408EE0015926D /* libboost_graph.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libboost_graph.a; path = RTABMapApp/Libraries/lib/libboost_graph.a; sourceTree = "<group>"; };
+		4E10D8662DE408EE0015926D /* libboost_json.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libboost_json.a; path = RTABMapApp/Libraries/lib/libboost_json.a; sourceTree = "<group>"; };
+		4E10D8672DE408EE0015926D /* libboost_stacktrace_basic.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libboost_stacktrace_basic.a; path = RTABMapApp/Libraries/lib/libboost_stacktrace_basic.a; sourceTree = "<group>"; };
+		4E10D8682DE408EE0015926D /* libboost_atomic.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libboost_atomic.a; path = RTABMapApp/Libraries/lib/libboost_atomic.a; sourceTree = "<group>"; };
+		4E10D8692DE408EE0015926D /* libopencv_intensity_transform.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libopencv_intensity_transform.a; path = RTABMapApp/Libraries/lib/libopencv_intensity_transform.a; sourceTree = "<group>"; };
+		4E10D86A2DE408EF0015926D /* libboost_locale.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libboost_locale.a; path = RTABMapApp/Libraries/lib/libboost_locale.a; sourceTree = "<group>"; };
+		4E10D86B2DE408EF0015926D /* liblz4.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = liblz4.a; path = RTABMapApp/Libraries/lib/liblz4.a; sourceTree = "<group>"; };
+		4E10D86C2DE408EF0015926D /* libopencv_dnn_superres.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libopencv_dnn_superres.a; path = RTABMapApp/Libraries/lib/libopencv_dnn_superres.a; sourceTree = "<group>"; };
+		4E10D86D2DE408EF0015926D /* libopencv_rapid.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libopencv_rapid.a; path = RTABMapApp/Libraries/lib/libopencv_rapid.a; sourceTree = "<group>"; };
+		4E10D86E2DE408EF0015926D /* libopencv_quality.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libopencv_quality.a; path = RTABMapApp/Libraries/lib/libopencv_quality.a; sourceTree = "<group>"; };
+		4E10D86F2DE408EF0015926D /* libboost_url.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libboost_url.a; path = RTABMapApp/Libraries/lib/libboost_url.a; sourceTree = "<group>"; };
+		4E10D8702DE408EF0015926D /* libboost_unit_test_framework.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libboost_unit_test_framework.a; path = RTABMapApp/Libraries/lib/libboost_unit_test_framework.a; sourceTree = "<group>"; };
+		4E10D8712DE408EF0015926D /* libboost_random.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libboost_random.a; path = RTABMapApp/Libraries/lib/libboost_random.a; sourceTree = "<group>"; };
+		4E10D8722DE408EF0015926D /* libboost_wave.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libboost_wave.a; path = RTABMapApp/Libraries/lib/libboost_wave.a; sourceTree = "<group>"; };
+		4E10D8732DE408EF0015926D /* libboost_charconv.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libboost_charconv.a; path = RTABMapApp/Libraries/lib/libboost_charconv.a; sourceTree = "<group>"; };
+		4E10D8742DE408EF0015926D /* libboost_type_erasure.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libboost_type_erasure.a; path = RTABMapApp/Libraries/lib/libboost_type_erasure.a; sourceTree = "<group>"; };
+		4E10D8752DE408EF0015926D /* liblas_c.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = liblas_c.a; path = RTABMapApp/Libraries/lib/liblas_c.a; sourceTree = "<group>"; };
+		4E10D8762DE408EF0015926D /* libboost_contract.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libboost_contract.a; path = RTABMapApp/Libraries/lib/libboost_contract.a; sourceTree = "<group>"; };
+		4E10D8772DE408EF0015926D /* libopencv_gapi.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libopencv_gapi.a; path = RTABMapApp/Libraries/lib/libopencv_gapi.a; sourceTree = "<group>"; };
+		4E10D8782DE408EF0015926D /* libopencv_signal.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libopencv_signal.a; path = RTABMapApp/Libraries/lib/libopencv_signal.a; sourceTree = "<group>"; };
+		4E10D8792DE408EF0015926D /* libboost_prg_exec_monitor.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libboost_prg_exec_monitor.a; path = RTABMapApp/Libraries/lib/libboost_prg_exec_monitor.a; sourceTree = "<group>"; };
+		4E10D87A2DE408EF0015926D /* libboost_stacktrace_addr2line.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libboost_stacktrace_addr2line.a; path = RTABMapApp/Libraries/lib/libboost_stacktrace_addr2line.a; sourceTree = "<group>"; };
+		4E10D87B2DE408EF0015926D /* libboost_nowide.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libboost_nowide.a; path = RTABMapApp/Libraries/lib/libboost_nowide.a; sourceTree = "<group>"; };
+		4E10D87C2DE408EF0015926D /* libboost_test_exec_monitor.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libboost_test_exec_monitor.a; path = RTABMapApp/Libraries/lib/libboost_test_exec_monitor.a; sourceTree = "<group>"; };
+		4E10D87D2DE408EF0015926D /* libopencv_mcc.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libopencv_mcc.a; path = RTABMapApp/Libraries/lib/libopencv_mcc.a; sourceTree = "<group>"; };
+		4E10D8D52DE40FBE0015926D /* liblibjpeg-turbo.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = "liblibjpeg-turbo.a"; path = "RTABMapApp/Libraries/lib/opencv4/3rdparty/liblibjpeg-turbo.a"; sourceTree = "<group>"; };
+		4E10D8D72DE40FCA0015926D /* liblibpng.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = liblibpng.a; path = RTABMapApp/Libraries/lib/opencv4/3rdparty/liblibpng.a; sourceTree = "<group>"; };
+		4E10D8D92DE40FF60015926D /* libzlib.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libzlib.a; path = RTABMapApp/Libraries/lib/opencv4/3rdparty/libzlib.a; sourceTree = "<group>"; };
+		4E10D8DA2DE40FF60015926D /* libIlmImf.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libIlmImf.a; path = RTABMapApp/Libraries/lib/opencv4/3rdparty/libIlmImf.a; sourceTree = "<group>"; };
+		4E10D8DB2DE40FF60015926D /* libade.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libade.a; path = RTABMapApp/Libraries/lib/opencv4/3rdparty/libade.a; sourceTree = "<group>"; };
+		4E10D8DC2DE40FF60015926D /* liblibprotobuf.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = liblibprotobuf.a; path = RTABMapApp/Libraries/lib/opencv4/3rdparty/liblibprotobuf.a; sourceTree = "<group>"; };
+		4E1E9CF22D617D08000CB881 /* Measure.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; name = Measure.h; path = ../android/jni/Measure.h; sourceTree = "<group>"; };
+		4E1E9CF32D618079000CB881 /* quad_color.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; name = quad_color.h; path = ../android/jni/quad_color.h; sourceTree = "<group>"; };
+		4E1E9CF42D618079000CB881 /* text_drawable.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; name = text_drawable.h; path = ../android/jni/text_drawable.h; sourceTree = "<group>"; };
+		4E1E9CF52D618079000CB881 /* text_drawable.cpp */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.cpp.cpp; name = text_drawable.cpp; path = ../android/jni/text_drawable.cpp; sourceTree = "<group>"; };
+		4E1E9CF62D618079000CB881 /* quad_color.cpp */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.cpp.cpp; name = quad_color.cpp; path = ../android/jni/quad_color.cpp; sourceTree = "<group>"; };
+		4E1E9CF92D619025000CB881 /* text_atlas_png.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; name = text_atlas_png.h; path = RTABMapApp/text_atlas_png.h; sourceTree = "<group>"; };
 		4E20B24D266AB94300316EE6 /* Images.xcassets */ = {isa = PBXFileReference; lastKnownFileType = folder.assetcatalog; name = Images.xcassets; path = RTABMapApp/Images.xcassets; sourceTree = SOURCE_ROOT; };
 		4E20B24F266AB95600316EE6 /* Images.xcassets */ = {isa = PBXFileReference; lastKnownFileType = folder.assetcatalog; name = Images.xcassets; path = RTABMapApp/Images.xcassets; sourceTree = "<group>"; };
 		4E2C516525A63119005CEDBD /* DatabaseView.swift */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.swift; name = DatabaseView.swift; path = RTABMapApp/DatabaseView.swift; sourceTree = "<group>"; };
 		4E2C516625A63119005CEDBD /* VerticalScrollerView.swift */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.swift; name = VerticalScrollerView.swift; path = RTABMapApp/VerticalScrollerView.swift; sourceTree = "<group>"; };
+		4E3525562DE4C6E500F39D8F /* libg2o_solver_cholmod.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libg2o_solver_cholmod.a; path = RTABMapApp/Libraries/lib/libg2o_solver_cholmod.a; sourceTree = "<group>"; };
+		4E3525582DE4C6F600F39D8F /* libcholmod.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libcholmod.a; path = RTABMapApp/Libraries/lib/libcholmod.a; sourceTree = "<group>"; };
+		4E35255A2DE4C71000F39D8F /* libcxsparse.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libcxsparse.a; path = RTABMapApp/Libraries/lib/libcxsparse.a; sourceTree = "<group>"; };
+		4E35255B2DE4C71000F39D8F /* libcolamd.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libcolamd.a; path = RTABMapApp/Libraries/lib/libcolamd.a; sourceTree = "<group>"; };
+		4E35255C2DE4C71000F39D8F /* libcamd.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libcamd.a; path = RTABMapApp/Libraries/lib/libcamd.a; sourceTree = "<group>"; };
+		4E35255D2DE4C71000F39D8F /* libccolamd.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libccolamd.a; path = RTABMapApp/Libraries/lib/libccolamd.a; sourceTree = "<group>"; };
+		4E3525622DE4C71C00F39D8F /* libsuitesparseconfig.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libsuitesparseconfig.a; path = RTABMapApp/Libraries/lib/libsuitesparseconfig.a; sourceTree = "<group>"; };
+		4E3525632DE4C71C00F39D8F /* libspqr.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libspqr.a; path = RTABMapApp/Libraries/lib/libspqr.a; sourceTree = "<group>"; };
+		4E3525662DE4C72D00F39D8F /* libamd.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libamd.a; path = RTABMapApp/Libraries/lib/libamd.a; sourceTree = "<group>"; };
 		4E401BE825CC92C100240A56 /* ProgressionStatus.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; name = ProgressionStatus.h; path = ../android/jni/ProgressionStatus.h; sourceTree = "<group>"; };
 		4EB1A66525A0D90B0043C7BF /* RTABMap.swift */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.swift; name = RTABMap.swift; path = RTABMapApp/RTABMap.swift; sourceTree = "<group>"; };
+		4ED73AD02D7FECF3007899C2 /* liblaszip.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = liblaszip.a; path = RTABMapApp/Libraries/lib/liblaszip.a; sourceTree = "<group>"; };
 		4EE015C2259A2AF0008CCE65 /* RTABMapApp.app */ = {isa = PBXFileReference; explicitFileType = wrapper.application; includeInIndex = 0; path = RTABMapApp.app; sourceTree = BUILT_PRODUCTS_DIR; };
 		4EE016AF259BE441008CCE65 /* ViewController.swift */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.swift; name = ViewController.swift; path = RTABMapApp/ViewController.swift; sourceTree = "<group>"; };
 		4EE016B0259BE441008CCE65 /* SceneDelegate.swift */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.swift; name = SceneDelegate.swift; path = RTABMapApp/SceneDelegate.swift; sourceTree = "<group>"; };
@@ -345,109 +370,79 @@
 			isa = PBXFrameworksBuildPhase;
 			buildActionMask = 2147483647;
 			files = (
-				4EFAA9432CAE4E960055DA51 /* liblas.a in Frameworks */,
-				4EFD0F50259D67D900575D88 /* liblibwebp.a in Frameworks */,
-				4EFD0F51259D67D900575D88 /* liblibpng.a in Frameworks */,
-				4EFD0F52259D67D900575D88 /* liblibjpeg-turbo.a in Frameworks */,
-				4EFD0F53259D67D900575D88 /* libzlib.a in Frameworks */,
-				4EFD0F54259D67D900575D88 /* liblibprotobuf.a in Frameworks */,
-				4EFD0F3D259D5D5C00575D88 /* libsqlite3.tbd in Frameworks */,
-				4EFD0F36259D5C7300575D88 /* vtk.framework in Frameworks */,
-				4EFD0F34259D5C2900575D88 /* libflann_cpp_s.a in Frameworks */,
-				4EFD0F1D259D5C2200575D88 /* libg2o_solver_dense.a in Frameworks */,
-				4EFD0F1E259D5C2200575D88 /* libg2o_ext_csparse.a in Frameworks */,
-				4EFD0F1F259D5C2200575D88 /* libg2o_solver_slam2d_linear.a in Frameworks */,
-				4EFD0F20259D5C2200575D88 /* libg2o_types_slam2d.a in Frameworks */,
-				4EFD0F21259D5C2200575D88 /* libg2o_solver_pcg.a in Frameworks */,
-				4EFD0F22259D5C2200575D88 /* libg2o_types_data.a in Frameworks */,
-				4EFD0F23259D5C2200575D88 /* libg2o_core.a in Frameworks */,
-				4EFD0F24259D5C2200575D88 /* libgtsam_unstable.a in Frameworks */,
-				4EFD0F25259D5C2200575D88 /* libg2o_types_icp.a in Frameworks */,
-				4EFD0F26259D5C2200575D88 /* libg2o_solver_csparse.a in Frameworks */,
-				4EFD0F27259D5C2200575D88 /* libg2o_solver_structure_only.a in Frameworks */,
-				4EFD0F28259D5C2200575D88 /* libg2o_solver_eigen.a in Frameworks */,
-				4EFD0F29259D5C2200575D88 /* libg2o_csparse_extension.a in Frameworks */,
-				4EFD0F2A259D5C2200575D88 /* libg2o_stuff.a in Frameworks */,
-				4EFD0F2B259D5C2200575D88 /* libg2o_types_sba.a in Frameworks */,
-				4EFD0F2C259D5C2200575D88 /* libg2o_types_slam2d_addons.a in Frameworks */,
-				4EFD0F2D259D5C2200575D88 /* libg2o_types_slam3d_addons.a in Frameworks */,
-				4EFD0F2E259D5C2200575D88 /* libg2o_types_slam3d.a in Frameworks */,
-				4EFD0F2F259D5C2200575D88 /* libmetis.a in Frameworks */,
-				4EFD0F30259D5C2200575D88 /* libg2o_types_sclam2d.a in Frameworks */,
-				4EFD0F31259D5C2200575D88 /* libg2o_types_sim3.a in Frameworks */,
-				4EFD0F32259D5C2200575D88 /* libgtsam.a in Frameworks */,
 				4E8B155426273A580037FC53 /* Zip in Frameworks */,
-				4EFD0EDD259D5BFC00575D88 /* libopencv_video.a in Frameworks */,
-				4EFD0EDE259D5BFC00575D88 /* libopencv_aruco.a in Frameworks */,
-				4EFD0EDF259D5BFC00575D88 /* libopencv_photo.a in Frameworks */,
-				4EFD0EE0259D5BFC00575D88 /* libopencv_line_descriptor.a in Frameworks */,
-				4EFD0EE1259D5BFC00575D88 /* libopencv_features2d.a in Frameworks */,
-				4EFD0EE2259D5BFC00575D88 /* libopencv_bgsegm.a in Frameworks */,
-				4EFD0EE3259D5BFC00575D88 /* libopencv_dnn_objdetect.a in Frameworks */,
-				4EFD0EE4259D5BFC00575D88 /* libopencv_objdetect.a in Frameworks */,
-				4EFD0EE5259D5BFC00575D88 /* libopencv_flann.a in Frameworks */,
-				4EFD0EE6259D5BFC00575D88 /* libopencv_stitching.a in Frameworks */,
-				4EFD0EE7259D5BFC00575D88 /* libopencv_imgproc.a in Frameworks */,
-				4EFD0EE8259D5BFC00575D88 /* libopencv_ml.a in Frameworks */,
-				4EFD0EE9259D5BFC00575D88 /* libopencv_rgbd.a in Frameworks */,
-				4EFD0EEA259D5BFC00575D88 /* libopencv_xphoto.a in Frameworks */,
-				4EFD0EEB259D5BFC00575D88 /* libopencv_phase_unwrapping.a in Frameworks */,
-				4EFD0EEC259D5BFC00575D88 /* libopencv_optflow.a in Frameworks */,
-				4EFD0EED259D5BFC00575D88 /* libopencv_imgcodecs.a in Frameworks */,
-				4EFD0EEE259D5BFC00575D88 /* libopencv_fuzzy.a in Frameworks */,
-				4EFD0EEF259D5BFC00575D88 /* libopencv_dpm.a in Frameworks */,
-				4EFD0EF0259D5BFC00575D88 /* libopencv_plot.a in Frameworks */,
-				4EFD0EF1259D5BFC00575D88 /* libopencv_dnn.a in Frameworks */,
-				4EFD0EF2259D5BFC00575D88 /* libopencv_saliency.a in Frameworks */,
-				4EFD0EF3259D5BFC00575D88 /* libopencv_videoio.a in Frameworks */,
-				4EFD0EF4259D5BFC00575D88 /* libopencv_hfs.a in Frameworks */,
-				4EFD0EF5259D5BFC00575D88 /* libopencv_tracking.a in Frameworks */,
-				4EFD0EF6259D5BFC00575D88 /* libopencv_text.a in Frameworks */,
-				4EFD0EF7259D5BFC00575D88 /* libopencv_face.a in Frameworks */,
-				4EFD0EF8259D5BFC00575D88 /* libopencv_bioinspired.a in Frameworks */,
-				4EFD0EF9259D5BFC00575D88 /* libopencv_highgui.a in Frameworks */,
-				4EFD0EFA259D5BFC00575D88 /* libopencv_core.a in Frameworks */,
-				4EFD0EFB259D5BFC00575D88 /* libopencv_shape.a in Frameworks */,
-				4EFD0EFC259D5BFC00575D88 /* libopencv_datasets.a in Frameworks */,
-				4EFD0EFD259D5BFC00575D88 /* libopencv_img_hash.a in Frameworks */,
-				4EFD0EFE259D5BFC00575D88 /* libopencv_surface_matching.a in Frameworks */,
-				4EFD0EFF259D5BFC00575D88 /* libopencv_calib3d.a in Frameworks */,
-				4EFD0F00259D5BFC00575D88 /* libopencv_structured_light.a in Frameworks */,
-				4EFD0F01259D5BFC00575D88 /* libopencv_reg.a in Frameworks */,
-				4EFD0F02259D5BFC00575D88 /* libopencv_xfeatures2d.a in Frameworks */,
-				4EFD0F03259D5BFC00575D88 /* libopencv_videostab.a in Frameworks */,
-				4EFD0F04259D5BFC00575D88 /* libopencv_ccalib.a in Frameworks */,
-				4EFD0F05259D5BFC00575D88 /* libopencv_ximgproc.a in Frameworks */,
-				4EFD0EB1259D5BE500575D88 /* librtabmap_utilite.a in Frameworks */,
-				4EFD0EB2259D5BE500575D88 /* librtabmap_core.a in Frameworks */,
+				4EFD0F36259D5C7300575D88 /* vtk.framework in Frameworks */,
 				4EFD0EAB259D581800575D88 /* OpenGLES.framework in Frameworks */,
-				4EFD0B36259D4DE900575D88 /* libboost_filesystem.a in Frameworks */,
-				4EFD0B37259D4DE900575D88 /* libboost_program_options.a in Frameworks */,
-				4EFD0B38259D4DE900575D88 /* libboost_regex.a in Frameworks */,
-				4EFD0B39259D4DE900575D88 /* libboost_iostreams.a in Frameworks */,
-				4EFD0B3A259D4DE900575D88 /* libboost_date_time.a in Frameworks */,
-				4EFD0B3B259D4DE900575D88 /* libboost_serialization.a in Frameworks */,
-				4EFD0B3C259D4DE900575D88 /* libboost_system.a in Frameworks */,
-				4EFD0B3D259D4DE900575D88 /* libboost_signals.a in Frameworks */,
-				4EFD0B3E259D4DE900575D88 /* libboost_thread.a in Frameworks */,
-				4EFD0B3F259D4DE900575D88 /* libboost_timer.a in Frameworks */,
-				4EFD0B40259D4DE900575D88 /* libboost_chrono.a in Frameworks */,
-				4EFD0B51259D4DF100575D88 /* libpcl_stereo.a in Frameworks */,
-				4EFD0B52259D4DF100575D88 /* libpcl_registration.a in Frameworks */,
-				4EFD0B53259D4DF100575D88 /* libpcl_features.a in Frameworks */,
-				4EFD0B54259D4DF100575D88 /* libpcl_filters.a in Frameworks */,
-				4EFD0B55259D4DF100575D88 /* libpcl_sample_consensus.a in Frameworks */,
-				4EFD0B56259D4DF100575D88 /* libpcl_kdtree.a in Frameworks */,
-				4EFD0B57259D4DF100575D88 /* libpcl_search.a in Frameworks */,
-				4EFD0B58259D4DF100575D88 /* libpcl_surface.a in Frameworks */,
-				4EFD0B59259D4DF100575D88 /* libpcl_keypoints.a in Frameworks */,
-				4EFD0B5A259D4DF100575D88 /* libpcl_recognition.a in Frameworks */,
-				4EFD0B5B259D4DF100575D88 /* libpcl_octree.a in Frameworks */,
-				4EFD0B5C259D4DF100575D88 /* libpcl_io.a in Frameworks */,
-				4EFD0B5D259D4DF100575D88 /* libpcl_common.a in Frameworks */,
-				4EFD0B5E259D4DF100575D88 /* libpcl_io_ply.a in Frameworks */,
-				4EFD0B5F259D4DF100575D88 /* libpcl_ml.a in Frameworks */,
-				4EFD0B60259D4DF100575D88 /* libpcl_segmentation.a in Frameworks */,
+				4E10D89C2DE409870015926D /* librtabmap_core.a in Frameworks */,
+				4E10D89D2DE409870015926D /* librtabmap_utilite.a in Frameworks */,
+				4E10D89E2DE409A60015926D /* libopencv_xfeatures2d.a in Frameworks */,
+				4E10D89F2DE409B20015926D /* libopencv_video.a in Frameworks */,
+				4E10D8A02DE409BB0015926D /* libopencv_tracking.a in Frameworks */,
+				4E10D8A12DE409C00015926D /* libopencv_optflow.a in Frameworks */,
+				4E10D8A22DE409C70015926D /* libopencv_imgproc.a in Frameworks */,
+				4E10D8A32DE409CA0015926D /* libopencv_features2d.a in Frameworks */,
+				4E10D8A42DE409CE0015926D /* libopencv_core.a in Frameworks */,
+				4E10D8A52DE409D10015926D /* libopencv_calib3d.a in Frameworks */,
+				4E10D8A62DE409D50015926D /* libopencv_aruco.a in Frameworks */,
+				4E10D8A72DE409F40015926D /* libopencv_flann.a in Frameworks */,
+				4E10D8A82DE40A1B0015926D /* libopencv_ximgproc.a in Frameworks */,
+				4E10D8A92DE40A3A0015926D /* libopencv_photo.a in Frameworks */,
+				4E10D8AA2DE40A6C0015926D /* libopencv_objdetect.a in Frameworks */,
+				4E10D8AB2DE40A8C0015926D /* libopencv_stitching.a in Frameworks */,
+				4E10D8AC2DE40AA20015926D /* libopencv_imgcodecs.a in Frameworks */,
+				4E3525672DE4C72D00F39D8F /* libamd.a in Frameworks */,
+				4E3525642DE4C71C00F39D8F /* libsuitesparseconfig.a in Frameworks */,
+				4E3525652DE4C71C00F39D8F /* libspqr.a in Frameworks */,
+				4E35255E2DE4C71000F39D8F /* libcxsparse.a in Frameworks */,
+				4E35255F2DE4C71000F39D8F /* libcolamd.a in Frameworks */,
+				4E3525602DE4C71000F39D8F /* libcamd.a in Frameworks */,
+				4E3525612DE4C71000F39D8F /* libccolamd.a in Frameworks */,
+				4E3525592DE4C6F600F39D8F /* libcholmod.a in Frameworks */,
+				4E10D8AD2DE40E990015926D /* libg2o_core.a in Frameworks */,
+				4E3525572DE4C6E500F39D8F /* libg2o_solver_cholmod.a in Frameworks */,
+				4E3525542DE4C6E500F39D8F /* libg2o_csparse_extension.a in Frameworks */,
+				4E3525552DE4C6E500F39D8F /* libg2o_solver_csparse.a in Frameworks */,
+				4E10D8AE2DE40E990015926D /* libg2o_solver_dense.a in Frameworks */,
+				4E10D8AF2DE40E990015926D /* libg2o_solver_eigen.a in Frameworks */,
+				4E10D8B02DE40E990015926D /* libg2o_solver_pcg.a in Frameworks */,
+				4E10D8B12DE40E990015926D /* libg2o_solver_slam2d_linear.a in Frameworks */,
+				4E10D8B22DE40E990015926D /* libg2o_solver_structure_only.a in Frameworks */,
+				4E10D8B32DE40E990015926D /* libg2o_stuff.a in Frameworks */,
+				4E10D8B42DE40E990015926D /* libg2o_types_data.a in Frameworks */,
+				4E10D8B52DE40E990015926D /* libg2o_types_icp.a in Frameworks */,
+				4E10D8B62DE40E990015926D /* libg2o_types_sba.a in Frameworks */,
+				4E10D8B72DE40E990015926D /* libg2o_types_sclam2d.a in Frameworks */,
+				4E10D8B82DE40E990015926D /* libg2o_types_sim3.a in Frameworks */,
+				4E10D8B92DE40E990015926D /* libg2o_types_slam2d_addons.a in Frameworks */,
+				4E10D8BA2DE40E990015926D /* libg2o_types_slam2d.a in Frameworks */,
+				4E10D8BB2DE40E990015926D /* libg2o_types_slam3d_addons.a in Frameworks */,
+				4E10D8BC2DE40E990015926D /* libg2o_types_slam3d.a in Frameworks */,
+				4E10D8BD2DE40EC70015926D /* libpcl_common.a in Frameworks */,
+				4E10D8BE2DE40EC70015926D /* libpcl_features.a in Frameworks */,
+				4E10D8BF2DE40EC70015926D /* libpcl_filters.a in Frameworks */,
+				4E10D8C02DE40EC70015926D /* libpcl_io_ply.a in Frameworks */,
+				4E10D8C12DE40EC70015926D /* libpcl_io.a in Frameworks */,
+				4E10D8C22DE40EC70015926D /* libpcl_kdtree.a in Frameworks */,
+				4E10D8C32DE40EC70015926D /* libpcl_keypoints.a in Frameworks */,
+				4E10D8C42DE40EC70015926D /* libpcl_ml.a in Frameworks */,
+				4E10D8C52DE40EC70015926D /* libpcl_octree.a in Frameworks */,
+				4E10D8C62DE40EC70015926D /* libpcl_registration.a in Frameworks */,
+				4E10D8C72DE40EC70015926D /* libpcl_sample_consensus.a in Frameworks */,
+				4E10D8C82DE40EC70015926D /* libpcl_search.a in Frameworks */,
+				4E10D8C92DE40EC70015926D /* libpcl_segmentation.a in Frameworks */,
+				4E10D8CA2DE40EC70015926D /* libpcl_stereo.a in Frameworks */,
+				4E10D8CB2DE40EC70015926D /* libpcl_surface.a in Frameworks */,
+				4E10D8CC2DE40EE30015926D /* libgtsam_unstable.a in Frameworks */,
+				4E10D8CD2DE40EE30015926D /* libgtsam.a in Frameworks */,
+				4E10D8CE2DE40EE30015926D /* libmetis-gtsam.a in Frameworks */,
+				4E10D8CF2DE40EF90015926D /* libboost_serialization.a in Frameworks */,
+				4E10D8D02DE40F060015926D /* libboost_timer.a in Frameworks */,
+				4E10D8D42DE40F2A0015926D /* libboost_thread.a in Frameworks */,
+				4E10D8D12DE40F190015926D /* liblas_c.a in Frameworks */,
+				4E10D8D22DE40F190015926D /* liblas.a in Frameworks */,
+				4E10D8D32DE40F190015926D /* liblaszip.a in Frameworks */,
+				4E10D8E12DE4101B0015926D /* liblibjpeg-turbo.a in Frameworks */,
+				4E10D8E22DE4101B0015926D /* liblibpng.a in Frameworks */,
 			);
 			runOnlyForDeploymentPostprocessing = 0;
 		};
@@ -475,6 +470,7 @@
 		4EE01634259BDCC7008CCE65 /* RTABMapApp */ = {
 			isa = PBXGroup;
 			children = (
+				44D4681D2D538A4100B094BA /* RTABMapApp.entitlements */,
 				4E20B24F266AB95600316EE6 /* Images.xcassets */,
 				4EFD0BB0259D503200575D88 /* NativeWrapper */,
 				4EFD0B62259D501E00575D88 /* tango-gl */,
@@ -496,6 +492,52 @@
 		4EFD0B2A259D4DE900575D88 /* Frameworks */ = {
 			isa = PBXGroup;
 			children = (
+				4E3525662DE4C72D00F39D8F /* libamd.a */,
+				4E3525632DE4C71C00F39D8F /* libspqr.a */,
+				4E3525622DE4C71C00F39D8F /* libsuitesparseconfig.a */,
+				4E35255C2DE4C71000F39D8F /* libcamd.a */,
+				4E35255D2DE4C71000F39D8F /* libccolamd.a */,
+				4E35255B2DE4C71000F39D8F /* libcolamd.a */,
+				4E35255A2DE4C71000F39D8F /* libcxsparse.a */,
+				4E3525582DE4C6F600F39D8F /* libcholmod.a */,
+				4E3525562DE4C6E500F39D8F /* libg2o_solver_cholmod.a */,
+				4E10D8DB2DE40FF60015926D /* libade.a */,
+				4E10D8DA2DE40FF60015926D /* libIlmImf.a */,
+				4E10D8DC2DE40FF60015926D /* liblibprotobuf.a */,
+				4E10D8D92DE40FF60015926D /* libzlib.a */,
+				4E10D8D72DE40FCA0015926D /* liblibpng.a */,
+				4E10D8D52DE40FBE0015926D /* liblibjpeg-turbo.a */,
+				4E10D8682DE408EE0015926D /* libboost_atomic.a */,
+				4E10D8732DE408EF0015926D /* libboost_charconv.a */,
+				4E10D8642DE408EE0015926D /* libboost_container.a */,
+				4E10D8762DE408EF0015926D /* libboost_contract.a */,
+				4E10D8652DE408EE0015926D /* libboost_graph.a */,
+				4E10D8662DE408EE0015926D /* libboost_json.a */,
+				4E10D86A2DE408EF0015926D /* libboost_locale.a */,
+				4E10D87B2DE408EF0015926D /* libboost_nowide.a */,
+				4E10D8792DE408EF0015926D /* libboost_prg_exec_monitor.a */,
+				4E10D8712DE408EF0015926D /* libboost_random.a */,
+				4E10D87A2DE408EF0015926D /* libboost_stacktrace_addr2line.a */,
+				4E10D8672DE408EE0015926D /* libboost_stacktrace_basic.a */,
+				4E10D8602DE408EE0015926D /* libboost_stacktrace_noop.a */,
+				4E10D87C2DE408EF0015926D /* libboost_test_exec_monitor.a */,
+				4E10D8742DE408EF0015926D /* libboost_type_erasure.a */,
+				4E10D8702DE408EF0015926D /* libboost_unit_test_framework.a */,
+				4E10D86F2DE408EF0015926D /* libboost_url.a */,
+				4E10D8722DE408EF0015926D /* libboost_wave.a */,
+				4E10D8612DE408EE0015926D /* libboost_wserialization.a */,
+				4E10D8752DE408EF0015926D /* liblas_c.a */,
+				4E10D86B2DE408EF0015926D /* liblz4.a */,
+				4E10D8622DE408EE0015926D /* libmetis-gtsam.a */,
+				4E10D86C2DE408EF0015926D /* libopencv_dnn_superres.a */,
+				4E10D8772DE408EF0015926D /* libopencv_gapi.a */,
+				4E10D8692DE408EE0015926D /* libopencv_intensity_transform.a */,
+				4E10D87D2DE408EF0015926D /* libopencv_mcc.a */,
+				4E10D86E2DE408EF0015926D /* libopencv_quality.a */,
+				4E10D86D2DE408EF0015926D /* libopencv_rapid.a */,
+				4E10D8782DE408EF0015926D /* libopencv_signal.a */,
+				4E10D8632DE408EE0015926D /* libopencv_wechat_qrcode.a */,
+				4ED73AD02D7FECF3007899C2 /* liblaszip.a */,
 				4EFAA9422CAE4E960055DA51 /* liblas.a */,
 				4EFD0F4D259D67D900575D88 /* liblibjpeg-turbo.a */,
 				4EFD0F4C259D67D900575D88 /* liblibpng.a */,
@@ -679,6 +721,12 @@
 		4EFD0BB0259D503200575D88 /* NativeWrapper */ = {
 			isa = PBXGroup;
 			children = (
+				4E1E9CF92D619025000CB881 /* text_atlas_png.h */,
+				4E1E9CF62D618079000CB881 /* quad_color.cpp */,
+				4E1E9CF32D618079000CB881 /* quad_color.h */,
+				4E1E9CF52D618079000CB881 /* text_drawable.cpp */,
+				4E1E9CF42D618079000CB881 /* text_drawable.h */,
+				4E1E9CF22D617D08000CB881 /* Measure.h */,
 				4EFD0F6E259F847300575D88 /* background_renderer.cc */,
 				4EFD0F6F259F847300575D88 /* background_renderer.h */,
 				4EFD0F66259E38DE00575D88 /* NativeWrapper.cpp */,
@@ -791,7 +839,7 @@
 			);
 			runOnlyForDeploymentPostprocessing = 0;
 			shellPath = /bin/sh;
-			shellScript = "# Type a script or drag a script file from your workspace to insert its path.\nversion=\"$MARKETING_VERSION\"\n/usr/libexec/PlistBuddy -c \"Set PreferenceSpecifiers:21:DefaultValue $version\" \"${SRCROOT}/Settings.bundle/Root.plist\"\n";
+			shellScript = "# Type a script or drag a script file from your workspace to insert its path.\nversion=\"$MARKETING_VERSION\"\n/usr/libexec/PlistBuddy -c \"Set PreferenceSpecifiers:24:DefaultValue $version\" \"${SRCROOT}/Settings.bundle/Root.plist\"\n";
 		};
 /* End PBXShellScriptBuildPhase section */
 
@@ -800,6 +848,8 @@
 			isa = PBXSourcesBuildPhase;
 			buildActionMask = 2147483647;
 			files = (
+				4E1E9CF72D618E69000CB881 /* quad_color.cpp in Sources */,
+				4E1E9CF82D618E69000CB881 /* text_drawable.cpp in Sources */,
 				4EFD0F70259F847300575D88 /* background_renderer.cc in Sources */,
 				4EFD0BC0259D50A800575D88 /* scene.cpp in Sources */,
 				4E2C516825A63119005CEDBD /* VerticalScrollerView.swift in Sources */,
@@ -867,7 +917,7 @@
 				ALWAYS_SEARCH_USER_PATHS = NO;
 				CLANG_ANALYZER_NONNULL = YES;
 				CLANG_ANALYZER_NUMBER_OBJECT_CONVERSION = YES_AGGRESSIVE;
-				CLANG_CXX_LANGUAGE_STANDARD = "gnu++14";
+				CLANG_CXX_LANGUAGE_STANDARD = "gnu++17";
 				CLANG_CXX_LIBRARY = "libc++";
 				CLANG_ENABLE_MODULES = YES;
 				CLANG_ENABLE_OBJC_ARC = YES;
@@ -898,7 +948,7 @@
 				DEBUG_INFORMATION_FORMAT = dwarf;
 				ENABLE_STRICT_OBJC_MSGSEND = YES;
 				ENABLE_TESTABILITY = YES;
-				GCC_C_LANGUAGE_STANDARD = gnu11;
+				GCC_C_LANGUAGE_STANDARD = gnu17;
 				GCC_DYNAMIC_NO_PIC = NO;
 				GCC_NO_COMMON_BLOCKS = YES;
 				GCC_OPTIMIZATION_LEVEL = 0;
@@ -928,7 +978,7 @@
 				ALWAYS_SEARCH_USER_PATHS = NO;
 				CLANG_ANALYZER_NONNULL = YES;
 				CLANG_ANALYZER_NUMBER_OBJECT_CONVERSION = YES_AGGRESSIVE;
-				CLANG_CXX_LANGUAGE_STANDARD = "gnu++14";
+				CLANG_CXX_LANGUAGE_STANDARD = "gnu++17";
 				CLANG_CXX_LIBRARY = "libc++";
 				CLANG_ENABLE_MODULES = YES;
 				CLANG_ENABLE_OBJC_ARC = YES;
@@ -959,7 +1009,7 @@
 				DEBUG_INFORMATION_FORMAT = "dwarf-with-dsym";
 				ENABLE_NS_ASSERTIONS = NO;
 				ENABLE_STRICT_OBJC_MSGSEND = YES;
-				GCC_C_LANGUAGE_STANDARD = gnu11;
+				GCC_C_LANGUAGE_STANDARD = gnu17;
 				GCC_NO_COMMON_BLOCKS = YES;
 				GCC_PREPROCESSOR_DEFINITIONS = "";
 				GCC_WARN_64_TO_32_BIT_CONVERSION = YES;
@@ -986,6 +1036,7 @@
 				ASSETCATALOG_COMPILER_GLOBAL_ACCENT_COLOR_NAME = AccentColor;
 				CLANG_CXX_LIBRARY = "libc++";
 				CLANG_USE_OPTIMIZATION_PROFILE = NO;
+				CODE_SIGN_ENTITLEMENTS = RTABMapApp/RTABMapApp.entitlements;
 				CODE_SIGN_IDENTITY = "Apple Development";
 				CODE_SIGN_STYLE = Automatic;
 				CURRENT_PROJECT_VERSION = 1;
@@ -1012,8 +1063,9 @@
 					"$(inherited)",
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/lib",
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/share/OpenCV/3rdparty/lib",
+					"$(PROJECT_DIR)/RTABMapApp/Libraries/lib/opencv4/3rdparty",
 				);
-				MARKETING_VERSION = 0.21.8;
+				MARKETING_VERSION = 0.22.0;
 				OTHER_CFLAGS = "";
 				PRODUCT_BUNDLE_IDENTIFIER = com.introlab.rtabmap;
 				PRODUCT_NAME = "$(TARGET_NAME)";
@@ -1025,11 +1077,12 @@
 				SYSTEM_HEADER_SEARCH_PATHS = (
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include/eigen3\"",
-					"\"$(SRCROOT)/RTABMapApp/Libraries/include/pcl-1.11\"",
-					"\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.21\"",
+					"\"$(SRCROOT)/RTABMapApp/Libraries/include/pcl-1.15\"",
+					"\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.22\"",
 					"\"$(SRCROOT)/../android/jni/tango-gl/include\"",
 					"\"$(SRCROOT)/../android/jni/third-party/include\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/lib/vtk.framework/Headers\"",
+					"\"$(SRCROOT)/RTABMapApp/Libraries/include/opencv4\"",
 				);
 				TARGETED_DEVICE_FAMILY = "1,2";
 				VALIDATE_WORKSPACE = NO;
@@ -1043,6 +1096,7 @@
 				ASSETCATALOG_COMPILER_GLOBAL_ACCENT_COLOR_NAME = AccentColor;
 				CLANG_CXX_LIBRARY = "libc++";
 				CLANG_USE_OPTIMIZATION_PROFILE = NO;
+				CODE_SIGN_ENTITLEMENTS = RTABMapApp/RTABMapApp.entitlements;
 				CODE_SIGN_IDENTITY = "Apple Development";
 				CODE_SIGN_STYLE = Automatic;
 				CURRENT_PROJECT_VERSION = 1;
@@ -1069,8 +1123,9 @@
 					"$(inherited)",
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/lib",
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/share/OpenCV/3rdparty/lib",
+					"$(PROJECT_DIR)/RTABMapApp/Libraries/lib/opencv4/3rdparty",
 				);
-				MARKETING_VERSION = 0.21.8;
+				MARKETING_VERSION = 0.22.0;
 				ONLY_ACTIVE_ARCH = YES;
 				OTHER_CFLAGS = "";
 				PRODUCT_BUNDLE_IDENTIFIER = com.introlab.rtabmap;
@@ -1083,11 +1138,12 @@
 				SYSTEM_HEADER_SEARCH_PATHS = (
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include/eigen3\"",
-					"\"$(SRCROOT)/RTABMapApp/Libraries/include/pcl-1.11\"",
-					"\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.21\"",
+					"\"$(SRCROOT)/RTABMapApp/Libraries/include/pcl-1.15\"",
+					"\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.22\"",
 					"\"$(SRCROOT)/../android/jni/tango-gl/include\"",
 					"\"$(SRCROOT)/../android/jni/third-party/include\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/lib/vtk.framework/Headers\"",
+					"\"$(SRCROOT)/RTABMapApp/Libraries/include/opencv4\"",
 				);
 				TARGETED_DEVICE_FAMILY = "1,2";
 				VALIDATE_WORKSPACE = NO;

app/ios/RTABMapApp/Base.lproj/Main.storyboard

@@ -18,8 +18,8 @@
                         <rect key="frame" x="0.0" y="0.0" width="393" height="852"/>
                         <autoresizingMask key="autoresizingMask" widthSizable="YES" heightSizable="YES"/>
                         <subviews>
-                            <stackView opaque="NO" contentMode="scaleToFill" axis="vertical" translatesAutoresizingMaskIntoConstraints="NO" id="kPv-DO-hpd">
-                                <rect key="frame" x="323" y="366" width="60" height="120"/>
+                            <stackView opaque="NO" contentMode="scaleToFill" axis="vertical" spacing="30" translatesAutoresizingMaskIntoConstraints="NO" id="kPv-DO-hpd">
+                                <rect key="frame" x="323" y="261" width="60" height="330"/>
                                 <subviews>
                                     <button opaque="NO" contentMode="scaleToFill" contentHorizontalAlignment="center" contentVerticalAlignment="center" buttonType="system" lineBreakMode="middleTruncation" translatesAutoresizingMaskIntoConstraints="NO" id="mJN-Ru-yPg" userLabel="StopButton">
                                         <rect key="frame" x="0.0" y="1" width="60" height="58.666666666666671"/>
@@ -34,7 +34,7 @@
                                         </connections>
                                     </button>
                                     <button opaque="NO" contentMode="scaleToFill" contentHorizontalAlignment="center" contentVerticalAlignment="center" lineBreakMode="middleTruncation" translatesAutoresizingMaskIntoConstraints="NO" id="7Lv-u6-mGh" userLabel="RecordButton">
-                                        <rect key="frame" x="0.0" y="61.000000000000007" width="60" height="58.666666666666664"/>
+                                        <rect key="frame" x="0.0" y="91" width="60" height="58.666666666666686"/>
                                         <constraints>
                                             <constraint firstAttribute="height" constant="60" id="DQI-eQ-v3X"/>
                                             <constraint firstAttribute="width" constant="60" id="Oeu-YT-wMz"/>
@@ -47,6 +47,32 @@
                                             <action selector="recordAction:" destination="zah-iI-EPt" eventType="touchUpInside" id="dzY-yL-YBg"/>
                                         </connections>
                                     </button>
+                                    <button opaque="NO" contentMode="scaleToFill" contentHorizontalAlignment="center" contentVerticalAlignment="center" buttonType="system" lineBreakMode="middleTruncation" translatesAutoresizingMaskIntoConstraints="NO" id="lFB-Lm-zFS" userLabel="AddMeasureButton">
+                                        <rect key="frame" x="0.0" y="181.33333333333334" width="60" height="57.999999999999972"/>
+                                        <constraints>
+                                            <constraint firstAttribute="width" constant="60" id="Wtb-vg-lgB"/>
+                                            <constraint firstAttribute="height" constant="60" id="p0E-BN-poT"/>
+                                        </constraints>
+                                        <color key="tintColor" systemColor="systemYellowColor"/>
+                                        <inset key="imageEdgeInsets" minX="0.0" minY="0.0" maxX="2.2250738585072014e-308" maxY="0.0"/>
+                                        <state key="normal" backgroundImage="plus.viewfinder" catalog="system"/>
+                                        <connections>
+                                            <action selector="addMeasureAction:" destination="zah-iI-EPt" eventType="touchUpInside" id="lUX-tm-SgY"/>
+                                        </connections>
+                                    </button>
+                                    <button opaque="NO" contentMode="scaleToFill" contentHorizontalAlignment="center" contentVerticalAlignment="center" buttonType="system" lineBreakMode="middleTruncation" translatesAutoresizingMaskIntoConstraints="NO" id="c1k-0i-64A" userLabel="RemoveMeasureButton">
+                                        <rect key="frame" x="0.0" y="271" width="60" height="58.666666666666686"/>
+                                        <constraints>
+                                            <constraint firstAttribute="height" constant="60" id="0gd-nd-i5y"/>
+                                            <constraint firstAttribute="width" constant="60" id="mSi-rI-NoN"/>
+                                        </constraints>
+                                        <color key="tintColor" white="1" alpha="1" colorSpace="custom" customColorSpace="genericGamma22GrayColorSpace"/>
+                                        <inset key="imageEdgeInsets" minX="0.0" minY="0.0" maxX="2.2250738585072014e-308" maxY="0.0"/>
+                                        <state key="normal" backgroundImage="minus.circle" catalog="system"/>
+                                        <connections>
+                                            <action selector="removeMeasureAction:" destination="zah-iI-EPt" eventType="touchUpInside" id="2RW-qr-kvb"/>
+                                        </connections>
+                                    </button>
                                 </subviews>
                             </stackView>
                             <stackView opaque="NO" contentMode="scaleToFill" spacing="20" translatesAutoresizingMaskIntoConstraints="NO" id="ozY-dg-YOd">
@@ -79,22 +105,10 @@
                             </stackView>
                             <label opaque="NO" userInteractionEnabled="NO" contentMode="left" horizontalHuggingPriority="251" verticalHuggingPriority="251" text="Label" textAlignment="natural" lineBreakMode="wordWrap" baselineAdjustment="alignBaselines" adjustsFontSizeToFit="NO" translatesAutoresizingMaskIntoConstraints="NO" id="KlF-4H-k6I">
                                 <rect key="frame" x="16" y="79" width="31" height="14.333333333333329"/>
-                                <color key="backgroundColor" red="0.0" green="0.0" blue="0.0" alpha="0.29820365646258501" colorSpace="custom" customColorSpace="sRGB"/>
                                 <fontDescription key="fontDescription" type="system" pointSize="12"/>
                                 <color key="textColor" white="1" alpha="1" colorSpace="custom" customColorSpace="genericGamma22GrayColorSpace"/>
                                 <nil key="highlightedColor"/>
                             </label>
-                            <button opaque="NO" contentMode="scaleToFill" contentHorizontalAlignment="center" contentVerticalAlignment="center" lineBreakMode="middleTruncation" translatesAutoresizingMaskIntoConstraints="NO" id="773-n4-lob" userLabel="ViewButton">
-                                <rect key="frame" x="321" y="756.33333333333337" width="52" height="40"/>
-                                <constraints>
-                                    <constraint firstAttribute="width" constant="52" id="epT-x2-WRc"/>
-                                    <constraint firstAttribute="height" constant="44" id="wAe-I1-qhs"/>
-                                </constraints>
-                                <color key="tintColor" white="1" alpha="1" colorSpace="custom" customColorSpace="genericGamma22GrayColorSpace"/>
-                                <state key="normal" backgroundImage="eye" catalog="system">
-                                    <color key="titleColor" white="1" alpha="1" colorSpace="custom" customColorSpace="genericGamma22GrayColorSpace"/>
-                                </state>
-                            </button>
                             <button opaque="NO" contentMode="scaleToFill" contentHorizontalAlignment="center" contentVerticalAlignment="center" lineBreakMode="middleTruncation" translatesAutoresizingMaskIntoConstraints="NO" id="012-W3-dOS" userLabel="CloseVisualization">
                                 <rect key="frame" x="111.33333333333333" y="772" width="170.33333333333337" height="22"/>
                                 <color key="tintColor" white="1" alpha="1" colorSpace="custom" customColorSpace="genericGamma22GrayColorSpace"/>
@@ -146,18 +160,8 @@
                                     <action selector="clipDistanceAction:" destination="zah-iI-EPt" eventType="valueChanged" id="iKj-nz-0KW"/>
                                 </connections>
                             </slider>
-                            <slider opaque="NO" contentMode="scaleToFill" placeholderIntrinsicWidth="114" placeholderIntrinsicHeight="30" contentHorizontalAlignment="center" contentVerticalAlignment="center" value="90" minValue="0.0" maxValue="180" translatesAutoresizingMaskIntoConstraints="NO" id="Z1x-Af-hdf" userLabel="GridRotationSlider">
-                                <rect key="frame" x="135.66666666666666" y="727" width="121.99999999999997" height="31"/>
-                                <constraints>
-                                    <constraint firstAttribute="width" constant="118" id="KDl-lP-Jj7"/>
-                                </constraints>
-                                <connections>
-                                    <action selector="rotateGridAction:" destination="zah-iI-EPt" eventType="valueChanged" id="eiC-QR-TYH"/>
-                                </connections>
-                            </slider>
                             <label opaque="NO" userInteractionEnabled="NO" contentMode="left" horizontalHuggingPriority="251" verticalHuggingPriority="251" text="Toast Label" textAlignment="center" lineBreakMode="wordWrap" numberOfLines="4" baselineAdjustment="alignBaselines" adjustsFontSizeToFit="NO" translatesAutoresizingMaskIntoConstraints="NO" id="JMv-UE-cvK" userLabel="ToastLabel">
-                                <rect key="frame" x="51" y="668.33333333333337" width="291" height="20.333333333333371"/>
-                                <color key="backgroundColor" white="0.0" alpha="0.3002232142857143" colorSpace="custom" customColorSpace="genericGamma22GrayColorSpace"/>
+                                <rect key="frame" x="51" y="637.33333333333337" width="291" height="20.333333333333371"/>
                                 <accessibility key="accessibilityConfiguration">
                                     <accessibilityTraits key="traits" staticText="YES" notEnabled="YES"/>
                                 </accessibility>
@@ -165,31 +169,93 @@
                                 <color key="textColor" white="1" alpha="1" colorSpace="custom" customColorSpace="genericGamma22GrayColorSpace"/>
                                 <nil key="highlightedColor"/>
                             </label>
+                            <slider opaque="NO" contentMode="scaleToFill" placeholderIntrinsicWidth="114" placeholderIntrinsicHeight="30" contentHorizontalAlignment="center" contentVerticalAlignment="center" value="90" minValue="0.0" maxValue="180" translatesAutoresizingMaskIntoConstraints="NO" id="Z1x-Af-hdf" userLabel="GridRotationSlider">
+                                <rect key="frame" x="132" y="727" width="122" height="31"/>
+                                <constraints>
+                                    <constraint firstAttribute="width" constant="118" id="KDl-lP-Jj7"/>
+                                </constraints>
+                                <connections>
+                                    <action selector="rotateGridAction:" destination="zah-iI-EPt" eventType="valueChanged" id="eiC-QR-TYH"/>
+                                </connections>
+                            </slider>
+                            <button opaque="NO" contentMode="scaleToFill" contentHorizontalAlignment="center" contentVerticalAlignment="center" buttonType="system" lineBreakMode="middleTruncation" translatesAutoresizingMaskIntoConstraints="NO" id="aWA-YQ-w5B" userLabel="TeleportButton">
+                                <rect key="frame" x="10" y="396.66666666666669" width="62.666666666666671" height="60"/>
+                                <constraints>
+                                    <constraint firstAttribute="height" constant="60" id="fe3-pR-D97"/>
+                                    <constraint firstAttribute="width" constant="60" id="t1H-Yp-Ayy"/>
+                                </constraints>
+                                <color key="tintColor" systemColor="systemGreenColor"/>
+                                <inset key="imageEdgeInsets" minX="0.0" minY="0.0" maxX="2.2250738585072014e-308" maxY="0.0"/>
+                                <state key="normal" backgroundImage="accessibility.badge.arrow.up.right" catalog="system"/>
+                                <connections>
+                                    <action selector="teleportButtonAction:" destination="zah-iI-EPt" eventType="touchUpInside" id="acD-kc-XA8"/>
+                                </connections>
+                            </button>
+                            <button opaque="NO" contentMode="scaleToFill" contentHorizontalAlignment="center" contentVerticalAlignment="center" lineBreakMode="middleTruncation" translatesAutoresizingMaskIntoConstraints="NO" id="773-n4-lob" userLabel="ViewButton">
+                                <rect key="frame" x="321" y="756.33333333333337" width="52" height="40"/>
+                                <constraints>
+                                    <constraint firstAttribute="width" constant="52" id="epT-x2-WRc"/>
+                                    <constraint firstAttribute="height" constant="44" id="wAe-I1-qhs"/>
+                                </constraints>
+                                <color key="tintColor" white="1" alpha="1" colorSpace="custom" customColorSpace="genericGamma22GrayColorSpace"/>
+                                <state key="normal" backgroundImage="eye" catalog="system">
+                                    <color key="titleColor" white="1" alpha="1" colorSpace="custom" customColorSpace="genericGamma22GrayColorSpace"/>
+                                </state>
+                            </button>
+                            <button opaque="NO" contentMode="scaleToFill" contentHorizontalAlignment="center" contentVerticalAlignment="center" buttonType="system" lineBreakMode="middleTruncation" translatesAutoresizingMaskIntoConstraints="NO" id="Z7f-kc-Rob" userLabel="MeasuringModeButton">
+                                <rect key="frame" x="293" y="687.33333333333337" width="80" height="32.333333333333371"/>
+                                <constraints>
+                                    <constraint firstAttribute="width" secondItem="Z7f-kc-Rob" secondAttribute="height" multiplier="52:27" id="78b-kS-OdV"/>
+                                    <constraint firstAttribute="width" constant="80" id="PFZ-dH-x12"/>
+                                </constraints>
+                                <color key="tintColor" systemColor="systemYellowColor"/>
+                                <inset key="imageEdgeInsets" minX="0.0" minY="0.0" maxX="2.2250738585072014e-308" maxY="0.0"/>
+                                <state key="normal" backgroundImage="ruler" catalog="system"/>
+                                <connections>
+                                    <action selector="startMeasuring:" destination="zah-iI-EPt" eventType="touchUpInside" id="uAe-Qu-Wwb"/>
+                                </connections>
+                            </button>
+                            <button opaque="NO" contentMode="scaleToFill" contentHorizontalAlignment="center" contentVerticalAlignment="center" lineBreakMode="middleTruncation" translatesAutoresizingMaskIntoConstraints="NO" id="wOV-tT-ZlN" userLabel="StopMeasuring">
+                                <rect key="frame" x="123.33333333333333" y="771" width="146.33333333333343" height="22"/>
+                                <color key="tintColor" white="1" alpha="1" colorSpace="custom" customColorSpace="genericGamma22GrayColorSpace"/>
+                                <state key="normal" title="Stop Measuring" image="xmark.square" catalog="system">
+                                    <color key="titleColor" white="1" alpha="1" colorSpace="custom" customColorSpace="genericGamma22GrayColorSpace"/>
+                                </state>
+                                <connections>
+                                    <action selector="stopCameraAction:" destination="zah-iI-EPt" eventType="touchUpInside" id="UY3-Dh-T9g"/>
+                                </connections>
+                            </button>
                         </subviews>
                         <viewLayoutGuide key="safeArea" id="qAR-pI-uT2"/>
                         <constraints>
                             <constraint firstItem="KlF-4H-k6I" firstAttribute="leading" secondItem="qAR-pI-uT2" secondAttribute="leading" constant="16" id="14l-HO-Yqo"/>
                             <constraint firstItem="kPv-DO-hpd" firstAttribute="centerY" secondItem="sqF-4e-2HU" secondAttribute="centerY" id="4S8-5c-tBm"/>
+                            <constraint firstItem="qAR-pI-uT2" firstAttribute="trailing" secondItem="773-n4-lob" secondAttribute="trailing" constant="20" id="6TZ-Iy-b1B"/>
+                            <constraint firstItem="773-n4-lob" firstAttribute="top" secondItem="Z7f-kc-Rob" secondAttribute="bottom" constant="30" id="7cF-fT-hfI"/>
                             <constraint firstItem="JMv-UE-cvK" firstAttribute="leading" secondItem="sqF-4e-2HU" secondAttribute="leadingMargin" constant="35" id="Awg-pU-JkT"/>
                             <constraint firstItem="KlF-4H-k6I" firstAttribute="top" secondItem="qAR-pI-uT2" secondAttribute="top" constant="20" id="CS3-Rk-Wxs"/>
                             <constraint firstItem="012-W3-dOS" firstAttribute="centerX" secondItem="sqF-4e-2HU" secondAttribute="centerX" id="IsC-yB-kNs"/>
-                            <constraint firstItem="qAR-pI-uT2" firstAttribute="trailing" secondItem="773-n4-lob" secondAttribute="trailing" constant="20" id="N8s-Yg-VsL"/>
+                            <constraint firstItem="qAR-pI-uT2" firstAttribute="bottom" secondItem="wOV-tT-ZlN" secondAttribute="bottom" constant="25" id="Kab-84-pPU"/>
                             <constraint firstItem="JMv-UE-cvK" firstAttribute="centerX" secondItem="sqF-4e-2HU" secondAttribute="centerX" id="NIf-UH-gZ7"/>
-                            <constraint firstItem="qAR-pI-uT2" firstAttribute="bottom" secondItem="773-n4-lob" secondAttribute="bottom" constant="20" id="O9t-O0-69d"/>
                             <constraint firstItem="Jj4-ow-MwD" firstAttribute="centerX" secondItem="sqF-4e-2HU" secondAttribute="centerX" id="OLx-LR-8xj"/>
+                            <constraint firstItem="aWA-YQ-w5B" firstAttribute="leading" secondItem="sqF-4e-2HU" secondAttribute="leading" constant="10" id="Rlo-Ir-NqY"/>
                             <constraint firstItem="012-W3-dOS" firstAttribute="top" secondItem="Z1x-Af-hdf" secondAttribute="bottom" constant="15" id="RpS-k0-btM"/>
                             <constraint firstItem="ozY-dg-YOd" firstAttribute="top" secondItem="qAR-pI-uT2" secondAttribute="top" constant="20" id="TFT-we-noh"/>
+                            <constraint firstItem="qAR-pI-uT2" firstAttribute="trailing" secondItem="Z7f-kc-Rob" secondAttribute="trailing" constant="20" id="Tbc-4P-ryD"/>
+                            <constraint firstItem="aWA-YQ-w5B" firstAttribute="centerY" secondItem="sqF-4e-2HU" secondAttribute="centerY" id="YXX-yd-QGL"/>
                             <constraint firstAttribute="trailingMargin" secondItem="JMv-UE-cvK" secondAttribute="trailing" constant="35" id="Ya5-GD-1rM"/>
                             <constraint firstItem="30J-Tx-NRW" firstAttribute="leading" secondItem="sqF-4e-2HU" secondAttribute="leading" id="ZBG-GX-P1X"/>
+                            <constraint firstItem="wOV-tT-ZlN" firstAttribute="centerX" secondItem="sqF-4e-2HU" secondAttribute="centerX" id="aPp-NO-q0A"/>
                             <constraint firstItem="3Sc-lx-YKH" firstAttribute="top" secondItem="ozY-dg-YOd" secondAttribute="bottom" constant="16" id="abR-Tj-Nua"/>
                             <constraint firstItem="qAR-pI-uT2" firstAttribute="bottom" secondItem="012-W3-dOS" secondAttribute="bottom" constant="24" id="dcl-mV-bH1"/>
                             <constraint firstItem="9oq-op-NGV" firstAttribute="centerX" secondItem="sqF-4e-2HU" secondAttribute="centerX" id="g5b-gW-Amg"/>
-                            <constraint firstItem="Z1x-Af-hdf" firstAttribute="centerX" secondItem="sqF-4e-2HU" secondAttribute="centerX" id="i2j-zQ-m1V"/>
-                            <constraint firstItem="Z1x-Af-hdf" firstAttribute="top" secondItem="JMv-UE-cvK" secondAttribute="bottom" constant="38.5" id="icg-DR-ZpX"/>
+                            <constraint firstItem="qAR-pI-uT2" firstAttribute="bottom" secondItem="773-n4-lob" secondAttribute="bottom" constant="20" id="h09-nI-8yH"/>
+                            <constraint firstItem="9oq-op-NGV" firstAttribute="top" secondItem="JMv-UE-cvK" secondAttribute="bottom" constant="113.33" id="icg-DR-ZpX"/>
                             <constraint firstItem="qAR-pI-uT2" firstAttribute="bottom" secondItem="9oq-op-NGV" secondAttribute="bottom" constant="25" id="lFI-66-D3N"/>
                             <constraint firstAttribute="trailing" secondItem="kPv-DO-hpd" secondAttribute="trailing" constant="10" id="lFv-p3-LIS"/>
                             <constraint firstItem="qAR-pI-uT2" firstAttribute="trailing" secondItem="ozY-dg-YOd" secondAttribute="trailing" constant="20" id="ov4-d2-OMy"/>
                             <constraint firstItem="012-W3-dOS" firstAttribute="top" secondItem="30J-Tx-NRW" secondAttribute="bottom" constant="60" id="tbj-nG-nfI"/>
+                            <constraint firstItem="Z1x-Af-hdf" firstAttribute="leading" secondItem="30J-Tx-NRW" secondAttribute="trailing" constant="16" id="uMj-kM-hlH"/>
                             <constraint firstItem="qAR-pI-uT2" firstAttribute="trailing" secondItem="3Sc-lx-YKH" secondAttribute="trailing" constant="10" id="v2K-Dp-Kyn"/>
                             <constraint firstItem="Jj4-ow-MwD" firstAttribute="centerY" secondItem="sqF-4e-2HU" secondAttribute="centerY" id="vmc-4u-BtF"/>
                         </constraints>
@@ -198,24 +264,29 @@
                         </connections>
                     </glkView>
                     <connections>
+                        <outlet property="addMeasureButton" destination="lFB-Lm-zFS" id="iOG-iO-mMQ"/>
                         <outlet property="closeVisualizationButton" destination="012-W3-dOS" id="qSV-q9-eOP"/>
                         <outlet property="exportOBJPLYButton" destination="3Sc-lx-YKH" id="ZvS-VQ-nsg"/>
                         <outlet property="libraryButton" destination="9kh-4C-KyP" id="Mvv-uQ-kmi"/>
+                        <outlet property="measuringModeButton" destination="Z7f-kc-Rob" id="C0D-Pc-wdr"/>
                         <outlet property="menuButton" destination="y53-OI-O76" id="v3g-JB-SsY"/>
                         <outlet property="newScanButtonLarge" destination="Jj4-ow-MwD" id="0Hv-3K-Xjh"/>
                         <outlet property="orthoDistanceSlider" destination="30J-Tx-NRW" id="4J1-8u-tDY"/>
                         <outlet property="orthoGridSlider" destination="Z1x-Af-hdf" id="Zsn-xz-jn0"/>
                         <outlet property="recordButton" destination="7Lv-u6-mGh" id="xTu-EK-FH2"/>
+                        <outlet property="removeMeasureButton" destination="c1k-0i-64A" id="E9z-yH-Vye"/>
                         <outlet property="statusLabel" destination="KlF-4H-k6I" id="NmA-K5-UOw"/>
                         <outlet property="stopButton" destination="mJN-Ru-yPg" id="DND-gz-jLg"/>
                         <outlet property="stopCameraButton" destination="9oq-op-NGV" id="sxO-jc-luC"/>
+                        <outlet property="stopMeasuringButton" destination="wOV-tT-ZlN" id="iK5-hG-9q0"/>
+                        <outlet property="teleportButton" destination="aWA-YQ-w5B" id="KTE-cK-RCI"/>
                         <outlet property="toastLabel" destination="JMv-UE-cvK" id="9a7-QF-h02"/>
                         <outlet property="viewButton" destination="773-n4-lob" id="deU-Na-mBk"/>
                     </connections>
                 </glkViewController>
                 <placeholder placeholderIdentifier="IBFirstResponder" id="plN-WG-seA" userLabel="First Responder" customClass="UIResponder" sceneMemberID="firstResponder"/>
             </objects>
-            <point key="canvasLocation" x="-234.375" y="-586.25"/>
+            <point key="canvasLocation" x="-235.87786259541983" y="-586.61971830985919"/>
         </scene>
         <!--Unsupported View Controller-->
         <scene sceneID="sJB-xy-u63">
@@ -226,14 +297,14 @@
                         <autoresizingMask key="autoresizingMask" widthSizable="YES" heightSizable="YES"/>
                         <subviews>
                             <label opaque="NO" userInteractionEnabled="NO" contentMode="left" horizontalHuggingPriority="251" verticalHuggingPriority="251" text="Unsupported Device" textAlignment="center" lineBreakMode="tailTruncation" baselineAdjustment="alignBaselines" enabled="NO" adjustsFontSizeToFit="NO" translatesAutoresizingMaskIntoConstraints="NO" id="kZt-ct-4KD">
-                                <rect key="frame" x="28" y="341.33333333333331" width="337.33333333333331" height="33.666666666666686"/>
+                                <rect key="frame" x="21" y="352.33333333333331" width="351.33333333333331" height="33.666666666666686"/>
                                 <fontDescription key="fontDescription" style="UICTFontTextStyleTitle1"/>
                                 <nil key="textColor"/>
                                 <nil key="highlightedColor"/>
                             </label>
                             <label opaque="NO" userInteractionEnabled="NO" contentMode="left" horizontalHuggingPriority="251" verticalHuggingPriority="251" textAlignment="center" lineBreakMode="tailTruncation" numberOfLines="0" baselineAdjustment="alignBaselines" enabled="NO" adjustsFontSizeToFit="NO" translatesAutoresizingMaskIntoConstraints="NO" id="4Js-Hw-3QU">
-                                <rect key="frame" x="28" y="383" width="337.33333333333331" height="86.333333333333314"/>
-                                <string key="text">This sample app requires a LiDAR-capable device, such as the second-generation iPad Pro 11-inch and fourth-generation iPad Pro 12.9-inch.</string>
+                                <rect key="frame" x="21" y="394" width="351.33333333333331" height="64.333333333333314"/>
+                                <string key="text">This app requires a LiDAR-capable device, such as the second-generation iPad Pro 11-inch and fourth-generation iPad Pro 12.9-inch.</string>
                                 <fontDescription key="fontDescription" style="UICTFontTextStyleBody"/>
                                 <nil key="textColor"/>
                                 <nil key="highlightedColor"/>
@@ -258,16 +329,26 @@
         </scene>
     </scenes>
     <resources>
+        <image name="accessibility.badge.arrow.up.right" catalog="system" width="128" height="114"/>
         <image name="ellipsis.circle" catalog="system" width="128" height="123"/>
         <image name="eye" catalog="system" width="128" height="79"/>
         <image name="folder" catalog="system" width="128" height="96"/>
+        <image name="minus.circle" catalog="system" width="128" height="123"/>
         <image name="plus.app" catalog="system" width="128" height="114"/>
+        <image name="plus.viewfinder" catalog="system" width="128" height="115"/>
         <image name="record.circle" catalog="system" width="128" height="123"/>
+        <image name="ruler" catalog="system" width="128" height="57"/>
         <image name="square.and.arrow.up" catalog="system" width="108" height="128"/>
         <image name="stop.circle" catalog="system" width="128" height="123"/>
         <image name="xmark.square" catalog="system" width="128" height="114"/>
+        <systemColor name="systemGreenColor">
+            <color red="0.20392156859999999" green="0.78039215689999997" blue="0.34901960780000002" alpha="1" colorSpace="custom" customColorSpace="sRGB"/>
+        </systemColor>
         <systemColor name="systemRedColor">
             <color red="1" green="0.23137254900000001" blue="0.18823529410000001" alpha="1" colorSpace="custom" customColorSpace="sRGB"/>
         </systemColor>
+        <systemColor name="systemYellowColor">
+            <color red="1" green="0.80000000000000004" blue="0.0" alpha="1" colorSpace="custom" customColorSpace="sRGB"/>
+        </systemColor>
     </resources>
 </document>

app/ios/RTABMapApp/DatabaseView.swift

@@ -55,11 +55,19 @@ class DatabaseView: UIView {
         super.init(frame: frame)
         self.databaseURL = databaseURL
         commonInit(databasePath: databaseURL.path)
-        DispatchQueue.global().async {
-            let downloadedImage = getPreviewImage(databasePath: databaseURL.path)
-          DispatchQueue.main.async {
-            self.coverImageView.image = downloadedImage
-          }
+        if let image = ViewController.previewImages[databaseURL.path]
+        {
+            self.coverImageView.image = image
+        }
+        else
+        {
+            DispatchQueue.global().async {
+                let downloadedImage = getPreviewImage(databasePath: databaseURL.path)
+                DispatchQueue.main.async {
+                    self.coverImageView.image = downloadedImage
+                    ViewController.previewImages[databaseURL.path] = downloadedImage
+                }
+            }
         }
     }
 

app/ios/RTABMapApp/NativeWrapper.cpp

@@ -150,11 +150,12 @@ bool exportMeshNative(
             int optimizedMinClusterSize,
             float optimizedMaxTextureDistance,
             int optimizedMinTextureClusterSize,
+            int textureVertexColorPolicy,
             bool blockRendering)
 {
     if(object)
     {
-        return native(object)->exportMesh(cloudVoxelSize, regenerateCloud, meshing, textureSize, textureCount, normalK, optimized, optimizedVoxelSize, optimizedDepth, optimizedMaxPolygons, optimizedColorRadius, optimizedCleanWhitePolygons, optimizedMinClusterSize, optimizedMaxTextureDistance, optimizedMinTextureClusterSize, blockRendering);
+        return native(object)->exportMesh(cloudVoxelSize, regenerateCloud, meshing, textureSize, textureCount, normalK, optimized, optimizedVoxelSize, optimizedDepth, optimizedMaxPolygons, optimizedColorRadius, optimizedCleanWhitePolygons, optimizedMinClusterSize, optimizedMaxTextureDistance, optimizedMinTextureClusterSize, textureVertexColorPolicy, blockRendering);
     }
     else
     {
@@ -434,6 +435,13 @@ void setWireframeNative(const void *object, bool enabled)
     else
         UERROR("object is null!");
 }
+void setTextureColorSeamsHiddenNative(const void *object, bool hidden)
+{
+    if(object)
+        native(object)->setTextureColorSeamsHidden(hidden);
+    else
+        UERROR("object is null!");
+}
 void setLocalizationModeNative(const void *object, bool enabled)
 {
     if(object)
@@ -497,6 +505,13 @@ void setSmoothingNative(const void *object, bool enabled)
     else
         UERROR("object is null!");
 }
+void setDepthBleedingErrorNative(const void *object, float value)
+{
+    if(object)
+        native(object)->setDepthBleedingError(value);
+    else
+        UERROR("object is null!");
+}
 void setAppendModeNative(const void *object, bool enabled)
 {
     if(object)
@@ -622,3 +637,53 @@ void addEnvSensorNative(const void *object, int type, float value)
     else
         UERROR("object is null!");
 }
+
+void removeMeasureNative(const void *object)
+{
+    if(object)
+        return native(object)->removeMeasure();
+    else
+        UERROR("object is null!");
+}
+void addMeasureNative(const void *object)
+{
+    if(object)
+        return native(object)->addMeasureButtonClicked();
+    else
+        UERROR("object is null!");
+}
+void teleportNative(const void *object)
+{
+    if(object)
+        return native(object)->teleportButtonClicked();
+    else
+        UERROR("object is null!");
+}
+void setMeasuringModeNative(const void *object, int mode)
+{
+    if(object)
+        return native(object)->setMeasuringMode(mode);
+    else
+        UERROR("object is null!");
+}
+void setMetricSystemNative(const void *object, bool enabled)
+{
+    if(object)
+        return native(object)->setMetricSystem(enabled);
+    else
+        UERROR("object is null!");
+}
+void setMeasuringTextSizeNative(const void *object, float size)
+{
+    if(object)
+        return native(object)->setMeasuringTextSize(size);
+    else
+        UERROR("object is null!");
+}
+void clearMeasuresNative(const void *object)
+{
+    if(object)
+        return native(object)->clearMeasures();
+    else
+        UERROR("object is null!");
+}

app/ios/RTABMapApp/NativeWrapper.hpp

@@ -55,6 +55,7 @@ bool exportMeshNative(
             int optimizedMinClusterSize,
             float optimizedMaxTextureDistance,
             int optimizedMinTextureClusterSize,
+            int textureVertexColorPolicy,
             bool blockRendering);
 bool postExportationNative(const void *object, bool visualize);
 bool writeExportedMeshNative(const void *object, const char * directory, const char * name);
@@ -90,6 +91,7 @@ void setGridRotationNative(const void *object, float value);
 void setLightingNative(const void *object, bool enabled);
 void setBackfaceCullingNative(const void *object, bool enabled);
 void setWireframeNative(const void *object, bool enabled);
+void setTextureColorSeamsHiddenNative(const void *object, bool hidden);
 void setLocalizationModeNative(const void *object, bool enabled);
 void setDataRecorderModeNative(const void *object, bool enabled);
 void setTrajectoryModeNative(const void *object, bool enabled);
@@ -99,6 +101,7 @@ void setGraphVisibleNative(const void *object, bool visible);
 void setGridVisibleNative(const void *object, bool visible);
 void setFullResolutionNative(const void *object, bool enabled);
 void setSmoothingNative(const void *object, bool enabled);
+void setDepthBleedingErrorNative(const void *object, float value);
 void setAppendModeNative(const void *object, bool enabled);
 void setUpstreamRelocalizationAccThrNative(const void *object, float value);
 void setMaxCloudDepthNative(const void *object, float value);
@@ -115,6 +118,14 @@ void setDepthConfidenceNative(const void *object, int value);
 void setExportPointCloudFormatNative(const void *object, const char * format);
 int setMappingParameterNative(const void *object, const char * key, const char * value);
 
+void removeMeasureNative(const void *object);
+void addMeasureNative(const void *object);
+void teleportNative(const void *object);
+void setMeasuringModeNative(const void *object, int mode);
+void setMetricSystemNative(const void *object, bool enabled);
+void setMeasuringTextSizeNative(const void *object, float size);
+void clearMeasuresNative(const void *object);
+
 typedef struct ImageNative
 {
     const void * objectPtr;

app/ios/RTABMapApp/RTABMap.swift

@@ -152,6 +152,27 @@ class RTABMap {
         }
     }
     
+    func removeMeasure() {
+        removeMeasureNative(native_rtabmap)
+    }
+    func addMeasureButtonClicked() {
+        addMeasureNative(native_rtabmap)
+    }
+    func teleportButtonClicked() {
+        teleportNative(native_rtabmap)
+    }
+    func setMeasuringMode(_ mode: Int) {
+        setMeasuringModeNative(native_rtabmap, Int32(mode))
+    }
+    func setMetricSystem(_ enabled: Bool) {
+        setMetricSystemNative(native_rtabmap, enabled)
+    }
+    func setMeasuringTextSize(_ size: Float32) {
+        setMeasuringTextSizeNative(native_rtabmap, size)
+    }
+    func clearMeasures() {
+        clearMeasuresNative(native_rtabmap)
+    }
     func cancelProcessing() {
         cancelProcessingNative(native_rtabmap);
     }
@@ -176,6 +197,7 @@ class RTABMap {
         optimizedMinClusterSize: Int,
         optimizedMaxTextureDistance: Float,
         optimizedMinTextureClusterSize: Int,
+        textureVertexColorPolicy: Int,
         blockRendering: Bool) -> Bool
     {
        return exportMeshNative(native_rtabmap,
@@ -194,6 +216,7 @@ class RTABMap {
                                     Int32(optimizedMinClusterSize),
                                     optimizedMaxTextureDistance,
                                     Int32(optimizedMinTextureClusterSize),
+                            		Int32(textureVertexColorPolicy),
                                     blockRendering)
     }
     
@@ -223,7 +246,7 @@ class RTABMap {
         return Int(renderNative(native_rtabmap))
     }
     
-    func startCamera() -> Bool {
+    func startCamera(imageOverlayInFirstPerson: Bool = true) -> Bool {
         return startCameraNative(native_rtabmap)
     }
     
@@ -403,6 +426,9 @@ class RTABMap {
     func setWireframe(enabled: Bool) {
         setWireframeNative(native_rtabmap, enabled)
     }
+    func setTextureColorSeamsHidden(hidden: Bool) {
+        setTextureColorSeamsHiddenNative(native_rtabmap, hidden)
+    }
     func setLocalizationMode(enabled: Bool) {
         setLocalizationModeNative(native_rtabmap, enabled)
     }
@@ -427,6 +453,9 @@ class RTABMap {
     func setSmoothing(enabled: Bool) {
         setSmoothingNative(native_rtabmap, enabled)
     }
+    func setDepthBleedingError(value: Float) {
+        setDepthBleedingErrorNative(native_rtabmap, value)
+    }
     func setAppendMode(enabled: Bool) {
         setAppendModeNative(native_rtabmap, enabled)
     }

app/ios/RTABMapApp/RTABMapApp.entitlements

@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE plist PUBLIC "-//Apple//DTD PLIST 1.0//EN" "http://www.apple.com/DTDs/PropertyList-1.0.dtd">
+<plist version="1.0">
+<dict>
+	<key>com.apple.developer.kernel.increased-memory-limit</key>
+	<true/>
+</dict>
+</plist>

app/ios/RTABMapApp/ViewController.swift

@@ -45,6 +45,8 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
     
     private var mReviewRequested = false
     
+    private var mMaximumMemory: Int = 0
+    
     // UI states
     private enum State {
         case STATE_WELCOME,    // Camera/Motion off - showing only buttons open and start new scan
@@ -53,8 +55,9 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         STATE_IDLE,            // Camera/Motion off
         STATE_PROCESSING,      // Camera/Motion off - post processing
         STATE_VISUALIZING,     // Camera/Motion off - Showing optimized mesh
-        STATE_VISUALIZING_CAMERA,     // Camera/Motion on  - Showing optimized mesh
-        STATE_VISUALIZING_WHILE_LOADING // Camera/Motion off - Loading data while showing optimized mesh
+        STATE_VISUALIZING_CAMERA,     // Camera/Motion on  - Showing optimized mesh and localizing
+        STATE_VISUALIZING_WHILE_LOADING, // Camera/Motion off - Loading data while showing optimized mesh
+        STATE_VISUALIZING_AND_MEASURING    // Camera/Motion on  - Showing optimized mesh without localizing and measuring tools enabled
     }
     private var mState: State = State.STATE_WELCOME;
     private func getStateString(state: State) -> String {
@@ -73,6 +76,8 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
             return "Visualizing with Camera"
         case .STATE_VISUALIZING_WHILE_LOADING:
             return "Visualizing while Loading"
+        case .STATE_VISUALIZING_AND_MEASURING:
+            return "Measuring"
         default: // IDLE
             return "Idle"
         }
@@ -93,9 +98,14 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
     private var wireframeShown: Bool = false
     private var backfaceShown: Bool = false
     private var lightingShown: Bool = false
+    private var textureColorSeamsShown: Bool = false
     private var mHudVisible: Bool = true
     private var mLastTimeHudShown: DispatchTime = .now()
     private var mMenuOpened: Bool = false
+    private var lowMemoryWarningShown: Bool = false
+    static var previewImages: [String: UIImage] = [:]
+    private var measuringMode: Int = 0
+    private var visualizationType: Int = 0 // 0=Cloud, 1=Mesh, 2=Texture Mesh
     
     @IBOutlet weak var stopButton: UIButton!
     @IBOutlet weak var recordButton: UIButton!
@@ -106,6 +116,11 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
     @IBOutlet weak var statusLabel: UILabel!
     @IBOutlet weak var closeVisualizationButton: UIButton!
     @IBOutlet weak var stopCameraButton: UIButton!
+    @IBOutlet weak var stopMeasuringButton: UIButton!
+    @IBOutlet weak var teleportButton: UIButton!
+    @IBOutlet weak var addMeasureButton: UIButton!
+    @IBOutlet weak var removeMeasureButton: UIButton!
+    @IBOutlet weak var measuringModeButton: UIButton!
     @IBOutlet weak var exportOBJPLYButton: UIButton!
     @IBOutlet weak var orthoDistanceSlider: UISlider!{
         didSet{
@@ -128,6 +143,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
     }
     
     @objc func defaultsChanged(){
+        print("defaultsChanged()")
         updateDisplayFromDefaults()
     }
     
@@ -146,7 +162,6 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
     func resetNoTouchTimer(_ showHud: Bool = false) {
         if(showHud)
         {
-            print("Show HUD")
             mMenuOpened = false
             mHudVisible = true
             setNeedsStatusBarAppearanceUpdate()
@@ -168,20 +183,52 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         }
         else if(mState != .STATE_WELCOME && mState != .STATE_CAMERA && presentedViewController as? UIAlertController == nil && !mMenuOpened)
         {
-            print("Hide HUD")
             self.mHudVisible = false
             self.setNeedsStatusBarAppearanceUpdate()
             self.updateState(state: self.mState)
         }
     }
     
+    func addShadow(_ view: UIView, _ offset: Int = 4)
+    {
+        view.layer.shadowColor = UIColor.black.cgColor
+        view.layer.shadowRadius = 3.0
+        view.layer.shadowOpacity = 1.0
+        view.layer.shadowOffset = CGSize(width: offset, height: offset)
+        view.layer.masksToBounds = false
+    }
+    
     override func viewDidLoad() {
+        
+        mMaximumMemory = getAvailableMemory()
+        
         super.viewDidLoad()
         // Do any additional setup after loading the view.
         
         self.toastLabel.isHidden = true
         session.delegate = self
         
+        addShadow(stopButton)
+        addShadow(recordButton)
+        addShadow(menuButton)
+        addShadow(viewButton)
+        addShadow(newScanButtonLarge)
+        addShadow(libraryButton)
+        addShadow(closeVisualizationButton)
+        addShadow(stopCameraButton)
+        addShadow(stopMeasuringButton)
+        addShadow(teleportButton)
+        addShadow(addMeasureButton)
+        addShadow(removeMeasureButton)
+        addShadow(measuringModeButton)
+        addShadow(exportOBJPLYButton)
+        
+        addShadow(statusLabel, 0)
+        addShadow(toastLabel, 0)
+        
+        addShadow(orthoDistanceSlider, 0)
+        addShadow(orthoGridSlider, 0)
+        
         depthSupported = ARWorldTrackingConfiguration.supportsFrameSemantics(.sceneDepth)
         
         rtabmap = RTABMap()
@@ -236,6 +283,20 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
             self.updateState(state: self.mState)
         }
     }
+
+    func progressStatusUpdate() {
+        DispatchQueue.main.asyncAfter(deadline: .now() + 1.0) {
+            if self.mState == .STATE_PROCESSING && self.statusShown
+            {
+                let availableMem = self.getAvailableMemory()
+                let usedMem = self.mMaximumMemory - availableMem;
+                self.statusLabel.text =
+                    "Status: \(self.getStateString(state: self.mState))\n" +
+                    "RAM Usage (MB): \(usedMem) / \(self.mMaximumMemory)"
+                self.progressStatusUpdate()
+            }
+        }
+    }
     
     func progressUpdated(_ rtabmap: RTABMap, count: Int, max: Int) {
         DispatchQueue.main.async {
@@ -262,14 +323,17 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
             {
                 if(optimizedMeshDetected==1)
                 {
+                    self.visualizationType = 0;
                     self.setMeshRendering(viewMode: 0)
                 }
                 else if(optimizedMeshDetected==2)
                 {
+                    self.visualizationType = 1;
                     self.setMeshRendering(viewMode: 1)
                 }
                 else // isOBJ
                 {
+                    self.visualizationType = 2;
                     self.setMeshRendering(viewMode: 2)
                 }
 
@@ -280,10 +344,11 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
                 self.showToast(message: "Optimized mesh detected in the database, it is shown while the database is loading...", seconds: 3)
             }
 
-            let usedMem = self.getMemoryUsage()
+            let availableMem = self.getAvailableMemory()
+            let usedMem = self.mMaximumMemory - availableMem;
             self.statusLabel.text =
                 "Status: " + (status == 1 && msg.isEmpty ? self.mState == State.STATE_CAMERA ? "Camera Preview" : "Idle" : msg) + "\n" +
-                "Memory Usage: \(usedMem) MB"
+            	"RAM Usage (MB): \(usedMem) / \(self.mMaximumMemory)"
         }
     }
         
@@ -316,7 +381,8 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
                            pitch: Float,
                            yaw: Float)
     {
-        let usedMem = self.getMemoryUsage()
+        let availableMem = self.getAvailableMemory()
+        let usedMem = self.mMaximumMemory - availableMem;
         
         if(loopClosureId > 0)
         {
@@ -339,7 +405,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
                 self.statusLabel.text =
                     self.statusLabel.text! +
                     "Status: \(self.getStateString(state: self.mState))\n" +
-                    "Memory Usage : \(usedMem) MB"
+                	"RAM Usage (MB): \(usedMem) / \(self.mMaximumMemory)"
             }
             if self.debugShown {
                 self.statusLabel.text =
@@ -426,6 +492,43 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
                     self.showToast(message: "Landmark \(landmarkDetected) detected!", seconds: 1);
                 }
             }
+            if(self.mState == .STATE_MAPPING)
+            {
+                if(availableMem < 400)
+                {
+                    let msg = "Scanning will be stopped because the free memory is too "
+                    + "low (\(availableMem) MB). You should be able to save the database but some post-processing and exporting options may fail. "
+                    + "\n\nNote that for large environments, you can save multiple databases and "
+                    + "merge them with RTAB-Map Desktop version."
+                    let alert = UIAlertController(title: "Memory is full!", message: msg, preferredStyle: .alert)
+                    let alertActionYes = UIAlertAction(title: "Ok", style: .default) {
+                        (UIAlertAction) -> Void in
+                        self.stopMapping(ignoreSaving: false, offerPostProcessing: false);
+                    }
+                    alert.addAction(alertActionYes)
+                    self.present(alert, animated: true, completion: nil)
+                }
+                else if(!self.lowMemoryWarningShown && usedMem*3 > availableMem && !self.mDataRecording)
+                {
+                    self.lowMemoryWarningShown = true
+                    let msg = "Available memory (\(availableMem) MB) should be at least 3 times the "
+                    + "memory used (\(usedMem) MB) so that some post-processing and exporting options "
+                    + "have enough memory to work correctly. If you just want to save the database "
+                    + "after scanning, you can continue until the next warning.\n\n"
+                    + "Note that showing only point clouds and/or decrease density reduce memory needed for rendering."
+                    let alert = UIAlertController(title: "Memory is full!", message: msg, preferredStyle: .alert)
+                    let alertActionYes = UIAlertAction(title: "Stop Now", style: .default) {
+                        (UIAlertAction) -> Void in
+                        self.stopMapping(ignoreSaving: false);
+                    }
+                    alert.addAction(alertActionYes)
+                    let alertActionNo = UIAlertAction(title: "Continue", style: .cancel) {
+                        (UIAlertAction) -> Void in
+                    }
+                    alert.addAction(alertActionNo)
+                    self.present(alert, animated: true, completion: nil)
+                }
+            }
         }
     }
     
@@ -439,34 +542,22 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         }
     }
     
-    func getMemoryUsage() -> UInt64 {
-        var taskInfo = mach_task_basic_info()
-        var count = mach_msg_type_number_t(MemoryLayout<mach_task_basic_info>.size)/4
-        let kerr: kern_return_t = withUnsafeMutablePointer(to: &taskInfo) {
-            $0.withMemoryRebound(to: integer_t.self, capacity: 1) {
-                task_info(mach_task_self_, task_flavor_t(MACH_TASK_BASIC_INFO), $0, &count)
-            }
-        }
-
-        if kerr == KERN_SUCCESS {
-            return taskInfo.resident_size / (1024*1024)
-        }
-        else {
-            print("Error with task_info(): " +
-                (String(cString: mach_error_string(kerr), encoding: String.Encoding.ascii) ?? "unknown error"))
-            return 0
-        }
+    func getAvailableMemory() -> Int {
+        return os_proc_available_memory()/(1024*1024)
     }
     
     @objc func appMovedToBackground() {
-        if(mState == .STATE_VISUALIZING_CAMERA || mState == .STATE_MAPPING || mState == .STATE_CAMERA)
+        print("appMovedToBackground()")
+        if(mState == .STATE_VISUALIZING_CAMERA || mState == .STATE_VISUALIZING_AND_MEASURING || mState == .STATE_MAPPING || mState == .STATE_CAMERA)
         {
             stopMapping(ignoreSaving: true)
         }
     }
     
     @objc func appMovedToForeground() {
+        print("appMovedToForeground()")
         updateDisplayFromDefaults()
+        updateState(state: mState)
         
         if(mMapNodes > 0 && self.openedDatabasePath == nil)
         {
@@ -510,29 +601,33 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         switch AVCaptureDevice.authorizationStatus(for: .video) {
             case .authorized: // The user has previously granted access to the camera.
                 print("Start Camera")
-                rtabmap!.startCamera()
+            	rtabmap!.startCamera()
                 let configuration = ARWorldTrackingConfiguration()
                 var message = ""
-                if(!UserDefaults.standard.bool(forKey: "LidarMode"))
-                {
-                    message = "LiDAR is disabled (Settings->Mapping->LiDAR Mode = OFF), only tracked features will be mapped."
-                    self.setMeshRendering(viewMode: 0)
-                }
-                else if !depthSupported
-                {
-                    message = "The device does not have a LiDAR, only tracked features will be mapped. A LiDAR is required for accurate 3D reconstruction."
-                    self.setMeshRendering(viewMode: 0)
-                }
-                else
-                {
-                    configuration.frameSemantics = .sceneDepth
-                }
+            	if(mState != .STATE_VISUALIZING_AND_MEASURING)
+            	{
+                	if(!UserDefaults.standard.bool(forKey: "LidarMode"))
+                	{
+       		        	message = "LiDAR is disabled (Settings->Mapping->LiDAR Mode = OFF), only tracked features will be mapped."
+       		        	self.setMeshRendering(viewMode: 0)
+        	    	}
+                	else if !depthSupported
+                	{
+                    	message = "The device does not have a LiDAR, only tracked features will be mapped. A LiDAR is required for accurate 3D reconstruction."
+                    	self.setMeshRendering(viewMode: 0)
+                	}
+                	else
+                	{
+                    	configuration.frameSemantics = .sceneDepth
+                	}
+            	}
                 
                 session.run(configuration, options: [.resetSceneReconstruction, .resetTracking, .removeExistingAnchors])
                 
                 switch mState {
-                case .STATE_VISUALIZING:
-                    updateState(state: .STATE_VISUALIZING_CAMERA)
+                case .STATE_VISUALIZING_AND_MEASURING,
+                    .STATE_VISUALIZING_CAMERA:
+                    break // State should be already set
                 default:
                     locationManager?.startUpdatingLocation()
                     updateState(state: .STATE_CAMERA)
@@ -599,6 +694,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         var actionExportEnabled: Bool
         var actionOptimizeEnabled: Bool
         var actionSettingsEnabled: Bool
+        var actionMeasuringEnabled: Bool
         
         switch mState {
         case .STATE_CAMERA:
@@ -611,9 +707,14 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
             stopButton.isHidden = true
             closeVisualizationButton.isHidden = true
             stopCameraButton.isHidden = false
+            stopMeasuringButton.isHidden = true
             exportOBJPLYButton.isHidden = true
             orthoDistanceSlider.isHidden = cameraMode != 3
             orthoGridSlider.isHidden = cameraMode != 3
+            teleportButton.isHidden = true
+            addMeasureButton.isHidden = true
+            removeMeasureButton.isHidden = true
+            measuringModeButton.isHidden = true
             actionNewScanEnabled = !mDataRecording
             actionNewDataRecording = mDataRecording
             actionSaveEnabled = false
@@ -621,6 +722,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
             actionExportEnabled = false
             actionOptimizeEnabled = false
             actionSettingsEnabled = false
+            actionMeasuringEnabled = false
         case .STATE_MAPPING:
             libraryButton.isEnabled = false
             libraryButton.isHidden = !mHudVisible
@@ -631,9 +733,14 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
             stopButton.isHidden = false
             closeVisualizationButton.isHidden = true
             stopCameraButton.isHidden = true
+            stopMeasuringButton.isHidden = true
             exportOBJPLYButton.isHidden = true
             orthoDistanceSlider.isHidden = cameraMode != 3 || !mHudVisible
             orthoGridSlider.isHidden = cameraMode != 3 || !mHudVisible
+            teleportButton.isHidden = true
+            addMeasureButton.isHidden = true
+            removeMeasureButton.isHidden = true
+            measuringModeButton.isHidden = true
             actionNewScanEnabled = !mDataRecording
             actionNewDataRecording = mDataRecording
             actionSaveEnabled = false
@@ -641,9 +748,11 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
             actionExportEnabled = false
             actionOptimizeEnabled = false
             actionSettingsEnabled = false
+            actionMeasuringEnabled = false
         case .STATE_PROCESSING,
              .STATE_VISUALIZING_WHILE_LOADING,
-             .STATE_VISUALIZING_CAMERA:
+             .STATE_VISUALIZING_CAMERA,
+             .STATE_VISUALIZING_AND_MEASURING:
             libraryButton.isEnabled = false
             libraryButton.isHidden = !mHudVisible
             menuButton.isHidden = !mHudVisible
@@ -653,9 +762,14 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
             stopButton.isHidden = true
             closeVisualizationButton.isHidden = true
             stopCameraButton.isHidden = mState != .STATE_VISUALIZING_CAMERA
+            stopMeasuringButton.isHidden = mState != .STATE_VISUALIZING_AND_MEASURING
             exportOBJPLYButton.isHidden = true
-            orthoDistanceSlider.isHidden = cameraMode != 3 || mState != .STATE_VISUALIZING_WHILE_LOADING
-            orthoGridSlider.isHidden = cameraMode != 3 || mState != .STATE_VISUALIZING_WHILE_LOADING
+            orthoDistanceSlider.isHidden = cameraMode != 3 || mState == .STATE_PROCESSING
+            orthoGridSlider.isHidden = cameraMode != 3 || mState == .STATE_PROCESSING
+            teleportButton.isHidden = mState != .STATE_VISUALIZING_AND_MEASURING || cameraMode != 0
+            addMeasureButton.isHidden = mState != .STATE_VISUALIZING_AND_MEASURING || cameraMode == 3
+            removeMeasureButton.isHidden = mState != .STATE_VISUALIZING_AND_MEASURING || cameraMode == 3
+            measuringModeButton.isHidden = true
             actionNewScanEnabled = false
             actionNewDataRecording = false
             actionSaveEnabled = false
@@ -663,6 +777,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
             actionExportEnabled = false
             actionOptimizeEnabled = false
             actionSettingsEnabled = false
+            actionMeasuringEnabled = mState == .STATE_VISUALIZING_AND_MEASURING
         case .STATE_VISUALIZING:
             libraryButton.isEnabled = !databases.isEmpty
             libraryButton.isHidden = !mHudVisible
@@ -673,9 +788,14 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
             stopButton.isHidden = true
             closeVisualizationButton.isHidden = !mHudVisible
             stopCameraButton.isHidden = true
+            stopMeasuringButton.isHidden = true
             exportOBJPLYButton.isHidden = !mHudVisible
             orthoDistanceSlider.isHidden = cameraMode != 3 || !mHudVisible
             orthoGridSlider.isHidden = cameraMode != 3 || !mHudVisible
+            teleportButton.isHidden = true
+            addMeasureButton.isHidden = true
+            removeMeasureButton.isHidden = true
+            measuringModeButton.isHidden = !mHudVisible || self.visualizationType==0
             actionNewScanEnabled = true
             actionNewDataRecording = true
             actionSaveEnabled = mMapNodes>0
@@ -683,6 +803,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
             actionExportEnabled = mMapNodes>0
             actionOptimizeEnabled = mMapNodes>0
             actionSettingsEnabled = true
+            actionMeasuringEnabled = true
         default: // IDLE // WELCOME
             libraryButton.isEnabled = !databases.isEmpty
             libraryButton.isHidden = mState != .STATE_WELCOME && !mHudVisible
@@ -693,9 +814,14 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
             stopButton.isHidden = true
             closeVisualizationButton.isHidden = true
             stopCameraButton.isHidden = true
+            stopMeasuringButton.isHidden = true
             exportOBJPLYButton.isHidden = true
             orthoDistanceSlider.isHidden = cameraMode != 3 || !mHudVisible
             orthoGridSlider.isHidden = cameraMode != 3 || !mHudVisible
+            teleportButton.isHidden = true
+            addMeasureButton.isHidden = true
+            removeMeasureButton.isHidden = true
+            measuringModeButton.isHidden = true
             actionNewScanEnabled = true
             actionNewDataRecording = true
             actionSaveEnabled = mState != .STATE_WELCOME && mMapNodes>0
@@ -703,21 +829,20 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
             actionExportEnabled = mState != .STATE_WELCOME && mMapNodes>0
             actionOptimizeEnabled = mState != .STATE_WELCOME && mMapNodes>0
             actionSettingsEnabled = true
+            actionMeasuringEnabled = false
         }
 
         let view = self.view as? GLKView
-        if(mState != .STATE_MAPPING && mState != .STATE_CAMERA && mState != .STATE_VISUALIZING_CAMERA)
+        if(mState != .STATE_MAPPING && mState != .STATE_CAMERA && mState != .STATE_VISUALIZING_CAMERA && mState != .STATE_VISUALIZING_AND_MEASURING)
         {
             self.isPaused = true
             view?.enableSetNeedsDisplay = true
             self.view.setNeedsDisplay()
-            print("enableSetNeedsDisplay")
         }
         else
         {
             view?.enableSetNeedsDisplay = false
             self.isPaused = false
-            print("diaableSetNeedsDisplay")
         }
         
         if !self.isPaused {
@@ -726,6 +851,23 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         
         // Update menus based on current state
         
+        if(!exportOBJPLYButton.isHidden) {
+            let format = UserDefaults.standard.string(forKey: "ExportPointCloudFormat")!;
+            var title = "Export "
+            if (self.visualizationType == 2) {
+                title += "OBJ";
+            }
+            else if (self.visualizationType == 1)
+            {
+                title += "PLY";
+            }
+            else
+            {
+                title += format == "las" ? "LAS" : format == "laz" ? "LAZ" : "PLY";
+            }
+            self.exportOBJPLYButton.setTitle(title, for: .normal)
+        }
+        
         // PointCloud menu
         let pointCloudMenu = UIMenu(title: "Point cloud...", children: [
             UIAction(title: "Current Density", handler: { _ in
@@ -786,6 +928,24 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
             	self.newScan(dataRecordingMode: true)
         	})
         ])
+        
+        // Measuring menu
+        let measuringMenu = UIMenu(title: "Measuring...", image: UIImage(systemName: "ruler"), children: [
+            UIAction(title: "Plane to Plane Mode", image: measuringMode == 0 ? UIImage(systemName: "checkmark.circle") : UIImage(systemName: "circle"), handler: { _ in
+                self.measuringMode = 0
+                self.rtabmap!.setMeasuringMode(self.measuringMode)
+                self.resetNoTouchTimer(true)
+            }),
+            UIAction(title: "Point to Point Mode", image: measuringMode == 2 ? UIImage(systemName: "checkmark.circle") : UIImage(systemName: "circle"), handler: { _ in
+                self.measuringMode = 2
+                self.rtabmap!.setMeasuringMode(self.measuringMode)
+                self.resetNoTouchTimer(true)
+            }),
+            UIAction(title: "Clear All Measures", image: UIImage(systemName: "trash"), state: .off, handler: { _ in
+                self.clearMeasures();
+                self.resetNoTouchTimer(true)
+            })
+        ])
                 
         var fileMenuChildren: [UIMenuElement] = []
         fileMenuChildren.append(UIAction(title: "New Mapping Session", image: UIImage(systemName: "plus.app"), attributes: actionNewScanEnabled ? [] : .disabled, state: .off, handler: { _ in
@@ -811,6 +971,13 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         fileMenuChildren.append(UIAction(title: "Append Scan", image: UIImage(systemName: "play.fill"), attributes: actionResumeEnabled ? [] : .disabled, state: .off, handler: { _ in
             self.resumeScan()
         }))
+        if(actionMeasuringEnabled) {
+            fileMenuChildren.append(measuringMenu)
+        }
+        else {
+            fileMenuChildren.append(UIAction(title: "Measuring...", image: UIImage(systemName: "ruler"), attributes: .disabled, state: .off, handler: { _ in
+            }))
+        }
         fileMenuChildren.append(advancedMenu)
         
         // File menu
@@ -826,7 +993,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
                 self.debugShown = !self.debugShown
                 self.resetNoTouchTimer(true)
             }),
-            UIAction(title: "Odom Visible", image: odomShown ? UIImage(systemName: "checkmark.circle") : UIImage(systemName: "circle"), attributes: (self.mState == .STATE_MAPPING || self.mState == .STATE_CAMERA || self.mState == .STATE_VISUALIZING_CAMERA) ? [] : .disabled, handler: { _ in
+            UIAction(title: "Odom Visible", image: odomShown ? UIImage(systemName: "checkmark.circle") : UIImage(systemName: "circle"), attributes: (self.mState == .STATE_MAPPING || self.mState == .STATE_CAMERA || self.mState == .STATE_VISUALIZING_CAMERA || self.mState == .STATE_VISUALIZING_AND_MEASURING) ? [] : .disabled, handler: { _ in
                 self.odomShown = !self.odomShown
                 self.rtabmap!.setOdomCloudShown(shown: self.odomShown)
                 self.resetNoTouchTimer(true)
@@ -865,7 +1032,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
                 let ac = UIAlertController(title: "Reset All Default Settings", message: "Do you want to reset all settings to default?", preferredStyle: .alert)
                 ac.addAction(UIAlertAction(title: "Yes", style: .default, handler: { _ in
                     let notificationCenter = NotificationCenter.default
-                    notificationCenter.removeObserver(self)
+                    notificationCenter.removeObserver(self, name: UserDefaults.didChangeNotification, object: nil)
                     UserDefaults.standard.reset()
                     self.registerSettingsBundle()
                     self.updateDisplayFromDefaults();
@@ -881,12 +1048,17 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         
         // Camera menu
         let renderingMenu = UIMenu(title: "Rendering", options: .displayInline, children: [
+            UIAction(title: "Texture/Color Blend", image: self.textureColorSeamsShown ? UIImage(systemName: "checkmark.circle") : UIImage(systemName: "circle"), attributes: self.mState == .STATE_VISUALIZING || self.mState == .STATE_VISUALIZING_CAMERA || self.mState == .STATE_VISUALIZING_AND_MEASURING || self.mState == .STATE_VISUALIZING_WHILE_LOADING ? [] : .disabled, handler: { _ in
+                self.textureColorSeamsShown = !self.textureColorSeamsShown
+                self.rtabmap!.setTextureColorSeamsHidden(hidden: !self.textureColorSeamsShown)
+                self.resetNoTouchTimer(true)
+            }),
             UIAction(title: "Wireframe", image: self.wireframeShown ? UIImage(systemName: "checkmark.circle") : UIImage(systemName: "circle"), handler: { _ in
                 self.wireframeShown = !self.wireframeShown
                 self.rtabmap!.setWireframe(enabled: self.wireframeShown)
                 self.resetNoTouchTimer(true)
             }),
-            UIAction(title: "Lighting", image: self.lightingShown ? UIImage(systemName: "checkmark.circle") : UIImage(systemName: "circle"), attributes: self.mState == .STATE_VISUALIZING || self.mState == .STATE_VISUALIZING_CAMERA || self.mState == .STATE_VISUALIZING_WHILE_LOADING ? [] : .disabled, handler: { _ in
+            UIAction(title: "Lighting", image: self.lightingShown ? UIImage(systemName: "checkmark.circle") : UIImage(systemName: "circle"), attributes: self.mState == .STATE_VISUALIZING || self.mState == .STATE_VISUALIZING_CAMERA || self.mState == .STATE_VISUALIZING_AND_MEASURING || self.mState == .STATE_VISUALIZING_WHILE_LOADING ? [] : .disabled, handler: { _ in
                 self.lightingShown = !self.lightingShown
                 self.rtabmap!.setLighting(enabled: self.lightingShown)
                 self.resetNoTouchTimer(true)
@@ -899,21 +1071,21 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         ])
         
         let cameraMenu = UIMenu(title: "View", options: .displayInline, children: [
-            UIAction(title: "First-P. View", image: cameraMode == 0 ? UIImage(systemName: "checkmark.circle") : UIImage(systemName: "circle"), attributes: (self.mState == .STATE_CAMERA || self.mState == .STATE_VISUALIZING || self.mState == .STATE_MAPPING || self.mState == .STATE_VISUALIZING_CAMERA) ? [] : .disabled, handler: { _ in
+            UIAction(title: "First-P. View", image: cameraMode == 0 ? UIImage(systemName: "checkmark.circle") : UIImage(systemName: "circle"), attributes: (self.mState == .STATE_CAMERA || self.mState == .STATE_VISUALIZING || self.mState == .STATE_MAPPING || self.mState == .STATE_VISUALIZING_CAMERA || self.mState == .STATE_VISUALIZING_AND_MEASURING) ? [] : .disabled, handler: { _ in
                 self.setGLCamera(type: 0)
                 if(self.mState == .STATE_VISUALIZING)
                 {
                     self.rtabmap?.setLocalizationMode(enabled: true)
                     self.rtabmap?.setPausedMapping(paused: false);
-                    self.startCamera()
                     self.updateState(state: .STATE_VISUALIZING_CAMERA)
+                    self.startCamera()
                 }
                 else
                 {
                     self.resetNoTouchTimer(true)
                 }
             }),
-            UIAction(title: "Third-P. View", image: cameraMode == 1 ? UIImage(systemName: "checkmark.circle") : UIImage(systemName: "circle"), handler: { _ in
+            UIAction(title: "Third-P. View", image: cameraMode == 1 ? UIImage(systemName: "checkmark.circle") : UIImage(systemName: "circle"), attributes: (self.mState != .STATE_VISUALIZING_AND_MEASURING) ? [] : .disabled, handler: { _ in
                 self.setGLCamera(type: 1)
                 self.resetNoTouchTimer(true)
             }),
@@ -927,7 +1099,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
             })
         ])
         
-        let showCloudMeshActions = mState != .STATE_VISUALIZING && mState != .STATE_VISUALIZING_CAMERA && mState != .STATE_PROCESSING && mState != .STATE_VISUALIZING_WHILE_LOADING
+        let showCloudMeshActions = mState != .STATE_VISUALIZING && mState != .STATE_VISUALIZING_CAMERA && mState != .STATE_VISUALIZING_AND_MEASURING && mState != .STATE_PROCESSING && mState != .STATE_VISUALIZING_WHILE_LOADING
         let cloudMeshMenu = UIMenu(title: "CloudMesh", options: .displayInline, children: [
             UIAction(title: "Point Cloud", image: viewMode == 0 ? UIImage(systemName: "checkmark.circle") : UIImage(systemName: "circle"), attributes: showCloudMeshActions ? [] : .disabled, handler: { _ in
                 self.setMeshRendering(viewMode: 0)
@@ -1339,6 +1511,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         rtabmap!.setNodesFiltering(enabled: defaults.bool(forKey: "NodesFiltering"));
         rtabmap!.setFullResolution(enabled: defaults.bool(forKey: "HDMode"));
         rtabmap!.setSmoothing(enabled: defaults.bool(forKey: "Smoothing"));
+        rtabmap!.setDepthBleedingError(value: defaults.float(forKey: "DepthBleedingError"));
         rtabmap!.setAppendMode(enabled: defaults.bool(forKey: "AppendMode"));
         rtabmap!.setUpstreamRelocalizationAccThr(value: defaults.float(forKey: "UpstreamRelocalizationFilteringAccThr"));
         rtabmap!.setExportPointCloudFormat(format: defaults.string(forKey: "ExportPointCloudFormat")!);
@@ -1378,6 +1551,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
             rtabmap!.setMappingParameter(key: "Marker/Dictionary", value: defaults.string(forKey: "ArUcoMarkerDetection")!);
             rtabmap!.setMappingParameter(key: "Marker/CornerRefinementMethod", value: (markerDetection > 16 ? "3":"0"));
             rtabmap!.setMappingParameter(key: "Marker/MaxDepthError", value: defaults.string(forKey: "MarkerDepthErrorEstimation")!);
+            rtabmap!.setMappingParameter(key: "Marker/MaxRange", value: defaults.string(forKey: "MarkerMaxRange")!);
             if let val = NumberFormatter().number(from: defaults.string(forKey: "MarkerSize")!)?.doubleValue
             {
                 rtabmap!.setMappingParameter(key: "Marker/Length", value: String(format: "%f", val/100.0))
@@ -1399,16 +1573,17 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         let bgColor = defaults.float(forKey: "BackgroundColor");
         rtabmap!.setBackgroundColor(gray: bgColor);
         
-        let format = defaults.string(forKey: "ExportPointCloudFormat")!;
         DispatchQueue.main.async {
             self.statusLabel.textColor = bgColor>=0.6 ? UIColor(white: 0.0, alpha: 1) : UIColor(white: 1.0, alpha: 1)
-            self.exportOBJPLYButton.setTitle("Export OBJ-\(format == "las" ? "LAS" : "PLY")", for: .normal)
         }
     
         rtabmap!.setClusterRatio(value: defaults.float(forKey: "NoiseFilteringRatio"));
         rtabmap!.setMaxGainRadius(value: defaults.float(forKey: "ColorCorrectionRadius"));
         rtabmap!.setRenderingTextureDecimation(value: defaults.integer(forKey: "TextureResolution"));
         
+        rtabmap!.setMetricSystem(defaults.integer(forKey: "MeasuringUnits") == 0);
+        rtabmap!.setMeasuringTextSize(defaults.float(forKey: "MeasuringTextSize"));
+        
         if(locationManager != nil && !defaults.bool(forKey: "SaveGPS"))
         {
             locationManager?.stopUpdatingLocation()
@@ -1717,6 +1892,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         let optimizedColorRadius = defaults.float(forKey: "ColorRadius")
         let optimizedCleanWhitePolygons = defaults.bool(forKey: "CleanMesh")
         let optimizedMinClusterSize = defaults.integer(forKey: "PolygonFiltering")
+        let textureVertexColorPolicy = defaults.integer(forKey: "TextureVertexColorPolicy")
         let blockRendering = false
         
         var indicator: UIActivityIndicatorView?
@@ -1738,7 +1914,9 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
             })
             
         }))
-
+	
+        let previousState = mState
+        
         updateState(state: .STATE_PROCESSING);
         
         present(alertView, animated: true, completion: {
@@ -1750,6 +1928,8 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
             self.progressView!.tintColor = self.view.tintColor
             alertView.view.addSubview(self.progressView!)
             
+            self.progressStatusUpdate() // This will update memory usage during post processing
+            
             var success : Bool = false
             DispatchQueue.background(background: {
                 
@@ -1769,6 +1949,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
                     optimizedMinClusterSize: optimizedMinClusterSize,
                     optimizedMaxTextureDistance: maxTextureDistance,
                     optimizedMinTextureClusterSize: minTextureClusterSize,
+                    textureVertexColorPolicy: textureVertexColorPolicy,
                     blockRendering: blockRendering)
                 
             }, completion:{
@@ -1789,22 +1970,28 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
                             
                             if(!meshing)
                             {
+                                self.visualizationType = 0;
                                 self.setMeshRendering(viewMode: 0)
                             }
                             else if(!isOBJ)
                             {
+                                self.visualizationType = 1;
                                 self.setMeshRendering(viewMode: 1)
                             }
                             else // isOBJ
                             {
+                                self.visualizationType = 2;
                                 self.setMeshRendering(viewMode: 2)
                             }
 
                             self.updateState(state: .STATE_VISUALIZING)
                             
                             self.rtabmap!.postExportation(visualize: true)
-
-                            self.setGLCamera(type: 2)
+							
+                            if previousState != .STATE_VISUALIZING
+                            {
+                                self.setGLCamera(type: 2)
+                            }
 
                             if self.openedDatabasePath == nil
                             {
@@ -1856,6 +2043,8 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
             self.progressView!.tintColor = self.view.tintColor
             alertView.view.addSubview(self.progressView!)
             
+            self.progressStatusUpdate() // This will update memory usage during post processing
+            
             var loopDetected : Int = -1
             DispatchQueue.background(background: {
                 loopDetected = self.rtabmap!.postProcessing(approach: approach);
@@ -1898,7 +2087,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         })
     }
     
-    func stopMapping(ignoreSaving: Bool)
+    func stopMapping(ignoreSaving: Bool, offerPostProcessing: Bool = true)
     {
         session.pause()
         locationManager?.stopUpdatingLocation()
@@ -1913,11 +2102,11 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         {
             self.rtabmap?.setLocalizationMode(enabled: false)
         }
-        updateState(state: mState == .STATE_VISUALIZING_CAMERA ? .STATE_VISUALIZING : .STATE_IDLE);
+        updateState(state: mState == .STATE_VISUALIZING_CAMERA || mState == .STATE_VISUALIZING_AND_MEASURING ? .STATE_VISUALIZING : .STATE_IDLE);
         
         if !ignoreSaving
         {
-            if(mDataRecording)
+            if(mDataRecording || !offerPostProcessing)
             {
                 // Go directly to save
                 self.save()
@@ -2013,6 +2202,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
             }
             else {
                 if(status >= 1 && status<=3) {
+                    self.visualizationType = status-1;
                     self.updateState(state: .STATE_VISUALIZING);
                     self.resetNoTouchTimer(true);
                 }
@@ -2108,10 +2298,10 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
             if textField.text != "" {
                 self.dismiss(animated: true)
                 //Read TextFields text data
-                let fileName = textField.text!+".zip"
+                let fileName = textField.text! + (self.exportOBJPLYButton.title(for: .normal)!.contains("LAZ") ? ".laz" : ".zip")
                 let filePath = self.getDocumentDirectory().appendingPathComponent(fileName).path
                 if FileManager.default.fileExists(atPath: filePath) {
-                    let alert = UIAlertController(title: "File Already Exists", message: "Do you want to overwrite the existing file?", preferredStyle: .alert)
+                    let alert = UIAlertController(title: "File Already Exists", message: "\(fileName) already exists, do you want to overwrite it?", preferredStyle: .alert)
                     let yes = UIAlertAction(title: "Yes", style: .default) {
                         (UIAlertAction) -> Void in
                         self.writeExportedFiles(fileName: textField.text!);
@@ -2158,7 +2348,9 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
 
     func writeExportedFiles(fileName: String)
     {
-        let alertView = UIAlertController(title: "Exporting", message: "Please wait while zipping data to \(fileName+".zip")...", preferredStyle: .alert)
+        let isLAZ = self.visualizationType==0 && self.exportOBJPLYButton.title(for: .normal)!.contains("LAZ")
+        
+        let alertView = UIAlertController(title: "Exporting", message: "Please wait while exporting data to \(fileName+(isLAZ ? ".laz" : ".zip"))...", preferredStyle: .alert)
         alertView.addAction(UIAlertAction(title: "Cancel", style: .cancel, handler: { _ in
             self.dismiss(animated: true)
             self.progressView = nil
@@ -2205,13 +2397,34 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
                         let fileURLs = try FileManager.default.contentsOfDirectory(at: exportDir, includingPropertiesForKeys: nil)
                         if(!fileURLs.isEmpty)
                         {
-                            do {
-                                zipFileUrl = try Zip.quickZipFiles(fileURLs, fileName: fileName) // Zip
-                                print("Zip file \(zipFileUrl.path) created (size=\(zipFileUrl.fileSizeString)")
-                                success = true
+                            if(isLAZ)
+                            {
+                                zipFileUrl = self.getDocumentDirectory().appendingPathComponent(fileName+".laz")
+                                do {
+                                    if FileManager.default.fileExists(atPath: zipFileUrl.path)
+                                    {
+                                        try FileManager.default.removeItem(at: zipFileUrl)
+                                    }
+                                    try FileManager.default.moveItem(at: fileURLs.first!, to: zipFileUrl)
+                                    print("LAZ file \(zipFileUrl.path) created (size=\(zipFileUrl.fileSizeString)")
+                                    success = true
+                                }
+                                catch
+                                {
+                                    print("Failed moving \(fileURLs.first!) to \(zipFileUrl.path)")
+                                    return
+                                }
                             }
-                            catch {
-                              print("Something went wrong while zipping")
+                            else
+                            {
+                                do {
+                                    zipFileUrl = try Zip.quickZipFiles(fileURLs, fileName: fileName) // Zip
+                                    print("Zip file \(zipFileUrl.path) created (size=\(zipFileUrl.fileSizeString)")
+                                    success = true
+                                }
+                                catch {
+                                    print("Something went wrong while zipping")
+                                }
                             }
                         }
                     } catch {
@@ -2227,7 +2440,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
                 }
                 if(success)
                 {
-                    let alertShare = UIAlertController(title: "Mesh/Cloud Saved!", message: "\(fileName+".zip") (\(zipFileUrl.fileSizeString) successfully exported in Documents of RTAB-Map! Share it?", preferredStyle: .alert)
+                    let alertShare = UIAlertController(title: "Mesh/Cloud Saved!", message: "\(fileName+(isLAZ ? ".laz" : ".zip")) (\(zipFileUrl.fileSizeString) successfully exported in Documents of RTAB-Map! Share it?", preferredStyle: .alert)
                     let alertActionYes = UIAlertAction(title: "Yes", style: .default) {
                         (UIAlertAction) -> Void in
                         self.shareFile(zipFileUrl)
@@ -2313,6 +2526,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
 
     @IBAction func recordAction(_ sender: UIButton) {
         rtabmap?.setPausedMapping(paused: false);
+        lowMemoryWarningShown = false
         updateState(state: .STATE_MAPPING)
     }
     
@@ -2344,6 +2558,26 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         rtabmap!.setOrthoCropFactor(Float(120-sender.value)/20.0 - 3.0)
         self.view.setNeedsDisplay()
     }
+    
+    @IBAction func removeMeasureAction(_ sender: UIButton) {
+        self.rtabmap!.removeMeasure()
+    }
+    @IBAction func addMeasureAction(_ sender: UIButton) {
+        self.rtabmap!.addMeasureButtonClicked()
+    }
+    @IBAction func teleportButtonAction(_ sender: UIButton) {
+        self.rtabmap!.teleportButtonClicked()
+    }
+    func clearMeasures()
+    {
+        self.rtabmap!.clearMeasures()
+        self.resetNoTouchTimer(true)
+    }
+    @IBAction func startMeasuring(_ sender: UIButton) {
+        self.setGLCamera(type: 0)
+        self.updateState(state: .STATE_VISUALIZING_AND_MEASURING)
+        self.startCamera()
+    }
 }
 
 func clearBackgroundColor(of view: UIView) {
@@ -2372,7 +2606,7 @@ extension ViewController: GLKViewControllerDelegate {
             let viewportSize = CGSize(width: rect.size.width * view.contentScaleFactor, height: rect.size.height * view.contentScaleFactor)
             rtabmap?.setupGraphic(size: viewportSize, orientation: rotation)
         }
-        
+
         let value = rtabmap?.render()
         
         DispatchQueue.main.async {

app/ios/RTABMapApp/install_deps.sh

@@ -2,7 +2,7 @@
 
 set -euxo pipefail
 
-# Tested on Apple Silicon Mac, with cmake 3.19.2.
+# Tested on Apple Silicon Mac, with cmake 4.0.2, XCode 16.3.
 
 mkdir -p Libraries
 cd Libraries
@@ -28,41 +28,65 @@ sysroot=iphoneos
 # Boost
 if [ ! -e $prefix/include/boost ]
 then
-if [ ! -e boost_1_59_0 ]
+if [ ! -e boost-1.88.0 ]
 then
   echo "wget boost..."
-  curl -L https://downloads.sourceforge.net/project/boost/boost/1.59.0/boost_1_59_0.tar.gz -o boost_1_59_0.tar.gz
-  tar -xzf boost_1_59_0.tar.gz
+  curl -L https://github.com/boostorg/boost/releases/download/boost-1.88.0/boost-1.88.0-cmake.tar.gz -o boost-1.88.0-cmake.tar.gz
+  tar -xzf boost-1.88.0-cmake.tar.gz
 fi
-cd boost_1_59_0
-curl -L https://gist.github.com/matlabbe/0bce8feeb73a499a76afbbcc5c687221/raw/489ff2869eccd6f8d03ffb9090ef839108762741/BoostConfig.cmake.in -o BoostConfig.cmake.in
-curl -L https://gist.github.com/matlabbe/0bce8feeb73a499a76afbbcc5c687221/raw/b07fe7d4e5dfe5f1d110c733e5cf660d79a26378/CMakeLists.txt -o CMakeLists.txt
+cd boost-1.88.0
 mkdir -p build
 cd build
-cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=12.0 -DCMAKE_INSTALL_PREFIX=$prefix ..
+cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=12.0 -DCMAKE_INSTALL_PREFIX=$prefix -DBOOST_INCOMPATIBLE_LIBRARIES="process;context;coroutine;fiber;fiber_numa;log_setup;log;cobalt" -DBOOST_IOSTREAMS_ENABLE_ZLIB=OFF -DBOOST_IOSTREAMS_ENABLE_BZIP2=OFF ..
 cmake --build . --config Release
 cmake --build . --config Release --target install
 cd $pwd
-#rm -r boost_1_59_0.tar.gz boost_1_59_0
+#rm -r boost-1.88.0-cmake.tar.gz boost-1.88.0
 fi
 
 # eigen
 if [ ! -e $prefix/include/eigen3 ]
 then
-if [ ! -e eigen-3.3.9 ]
+if [ ! -e eigen-3.4.0 ]
 then
   echo "wget eigen..."
-  curl -L https://gitlab.com/libeigen/eigen/-/archive/3.3.9/eigen-3.3.9.tar.gz -o 3.3.9.tar.gz
-  tar -xzf 3.3.9.tar.gz
+  curl -L https://gitlab.com/libeigen/eigen/-/archive/3.4.0/eigen-3.4.0.tar.gz -o 3.4.0.tar.gz
+  tar -xzf 3.4.0.tar.gz
 fi
-cd eigen-3.3.9
+cd eigen-3.4.0
 mkdir -p build
 cd build
 cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=12.0 -DCMAKE_INSTALL_PREFIX=$prefix ..
 cmake --build . --config Release
 cmake --build . --config Release --target install
 cd $pwd
-#rm -r 3.3.9.tar.gz eigen-3.3.9
+#rm -r 3.4.0.tar.gz eigen-3.4.0
+fi
+
+# lz4 (required by flann)
+if [ ! -e $prefix/include/lz4.h ]
+then
+if [ ! -e lz4 ]
+then
+  echo "wget lz4..."
+  git clone https://github.com/lz4/lz4.git -b v1.10.0
+fi
+cd lz4
+if [ ! -e LZ4Config.cmake.in ]
+then
+  curl -L https://gist.githubusercontent.com/matlabbe/abd0242305c29495bbba26065269daf2/raw/ad0b1865c02e61449f58358fdc4ddbed3cb5fb87/LZ4Config.cmake.in -o LZ4Config.cmake.in
+fi
+if [ ! -e CMakeLists.txt ]
+then
+  curl -L https://gist.githubusercontent.com/matlabbe/abd0242305c29495bbba26065269daf2/raw/ad0b1865c02e61449f58358fdc4ddbed3cb5fb87/CMakeLists.txt -o CMakeLists.txt
+fi
+mkdir -p build
+cd build
+cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=12.0 -DCMAKE_INSTALL_PREFIX=$prefix ..
+cmake --build . --config Release -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED="NO" CODE_SIGN_ENTITLEMENTS=""  CODE_SIGNING_ALLOWED="NO"
+cmake --build . --config Release --target install -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED="NO" CODE_SIGN_ENTITLEMENTS=""  CODE_SIGNING_ALLOWED="NO"
+cd $pwd
+#rm -r lz4
 fi
 
 # FLANN
@@ -71,19 +95,17 @@ then
 if [ ! -e flann ]
 then
   echo "wget flann..."
-  git clone https://github.com/flann-lib/flann.git -b 1.8.4
+  git clone https://github.com/flann-lib/flann.git -b 1.9.2
 fi
 cd flann
-if [ ! -e flann_ios.patch ]
+if [ ! -e flann_ios_lz4.patch ]
 then
-  curl -L https://gist.githubusercontent.com/matlabbe/c858ba36fb85d5e44d8667dfb3543e12/raw/8fc40aa9bc3267604869444020476a49f14ab424/flann_ios.patch  -o flann_ios.patch
-  git apply flann_ios.patch
+  curl -L https://gist.githubusercontent.com/matlabbe/c858ba36fb85d5e44d8667dfb3543e12/raw/2586a356dec2b11440ec3c1bb113e709e1266d97/flann_ios_lz4.patch  -o flann_ios_lz4.patch
+  git apply flann_ios_lz4.patch
 fi
 mkdir -p build
 cd build
-# comment "add_subdirectory( test )" in top CMakeLists.txt
-# comment "add_subdirectory( doc )" in top CMakeLists.txt
-cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=12.0 -DCMAKE_INSTALL_PREFIX=$prefix -DBUILD_PYTHON_BINDINGS=OFF -DBUILD_MATLAB_BINDINGS=OFF -DBUILD_C_BINDINGS=OFF  ..
+cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=12.0 -DCMAKE_INSTALL_PREFIX=$prefix -DBUILD_PYTHON_BINDINGS=OFF -DBUILD_MATLAB_BINDINGS=OFF -DBUILD_C_BINDINGS=OFF -DBUILD_EXAMPLES=OFF -DBUILD_TESTS=OFF -DBUILD_DOC=OFF -DUSE_OPENMP=OFF -DLZ4_DIR=$prefix/lib/lz4  ..
 cmake --build . --config Release -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED="NO" CODE_SIGN_ENTITLEMENTS=""  CODE_SIGNING_ALLOWED="NO"
 cmake --build . --config Release --target install -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED="NO" CODE_SIGN_ENTITLEMENTS=""  CODE_SIGNING_ALLOWED="NO"
 cd $pwd
@@ -95,17 +117,14 @@ if [ ! -e $prefix/include/gtsam ]
 then
 if [ ! -e gtsam ]
 then
-  git clone https://bitbucket.org/gtborg/gtsam.git
-  cd gtsam
-  git checkout fbb9d3bdda8b88df51896bc401bfd170573e66f5
-else
-  cd gtsam
+  git clone https://github.com/borglab/gtsam.git -b 4.2
 fi
+cd gtsam
 # patch
-if [ ! -e gtsam_GKlib_ios_fix.patch ]
+if [ ! -e gtsam_4_2_ios.patch ]
 then
-  curl -L https://gist.github.com/matlabbe/76d658dddb841b3355ae3a6e32850cd8/raw/7033cba1c89097b0c830651d7277c04dc92cbdd9/gtsam_GKlib_ios_fix.patch -o gtsam_GKlib_ios_fix.patch
-  git apply gtsam_GKlib_ios_fix.patch
+  curl -L https://gist.githubusercontent.com/matlabbe/76d658dddb841b3355ae3a6e32850cd8/raw/e7355348c2d536ec50f41effa775ed251ae4e045/gtsam_4_2_ios.patch -o gtsam_4_2_ios.patch
+  git apply gtsam_4_2_ios.patch
 fi
 mkdir -p build
 cd build
@@ -116,22 +135,36 @@ cd $pwd
 #rm -rf gtsam
 fi
 
+# suitesparse (dependency of g2o)
+if [ ! -e $prefix/include/suitesparse/SuiteSparse_config.h ]
+then
+if [ ! -e SuiteSparse ]
+then
+  git clone https://github.com/DrTimothyAldenDavis/SuiteSparse.git -b v7.6.1
+fi
+cd SuiteSparse
+mkdir -p build
+cd build
+cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=12.0 -DCMAKE_INSTALL_PREFIX=$prefix -DCMAKE_FIND_ROOT_PATH=$prefix -DSUITESPARSE_USE_OPENMP=OFF -DSUITESPARSE_ENABLE_PROJECTS="cholmod;cxsparse;spqr"  ..
+cmake --build . --config Release -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED="NO" CODE_SIGN_ENTITLEMENTS=""  CODE_SIGNING_ALLOWED="NO"
+cmake --build . --config Release --target install -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED="NO" CODE_SIGN_ENTITLEMENTS=""  CODE_SIGNING_ALLOWED="NO"
+cd $pwd
+fi
+
 # g2o
 if [ ! -e $prefix/include/g2o ]
 then
 if [ ! -e g2o ]
 then
-  git clone https://github.com/RainerKuemmerle/g2o.git
-  cd g2o
-  git checkout a3f7706bdbb849b2808dc3e1b7aee189f63b498e
-else
-  cd g2o
+  git clone https://github.com/RainerKuemmerle/g2o.git -b 20241228_git
 fi
+cd g2o
 # patch
-if [ ! -e g2o_ios_fix.patch ]
+if [ ! -e g2o_20241228_ios.patch ]
 then
-  curl -L https://gist.github.com/matlabbe/b9ccfeae8f0744b275cab23510872680/raw/a58e06accba3976420d4b61341685c123193810e/g2o_ios_fix.patch -o g2o_ios_fix.patch
-  git apply g2o_ios_fix.patch
+ls
+  curl -L https://gist.githubusercontent.com/matlabbe/b9ccfeae8f0744b275cab23510872680/raw/6fe2ffe5ba8fba59171adbd2f38f9c3999c61f75/g2o_20241228_ios.patch -o g2o_20241228_ios.patch
+  git apply g2o_20241228_ios.patch
 fi
 mkdir -p build
 cd build
@@ -149,11 +182,10 @@ if [ ! -e VTK ]
 then
   git clone https://github.com/Kitware/VTK.git
   cd VTK
-  git checkout tags/v8.2.0
+  git checkout tags/v9.5.0.rc1
 else
   cd VTK
 fi
-git cherry-pick bf3ae8072df2393c7270509bae41be0776826346
 mkdir -p build
 cd build
 cmake -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_FRAMEWORK_INSTALL_PREFIX=$prefix/lib -DIOS_DEVICE_ARCHITECTURES="arm64" -DIOS_SIMULATOR_ARCHITECTURES="" -DBUILD_EXAMPLES=OFF -DBUILD_TESTING=OFF -DVTK_IOS_BUILD=ON -DModule_vtkFiltersModeling=ON ..
@@ -163,26 +195,28 @@ cd $pwd
 #rm -rf VTK
 fi
 
+
+
 # PCL
-if [ ! -e $prefix/include/pcl-1.11 ]
+if [ ! -e $prefix/include/pcl-1.15 ]
 then
 if [ ! -e pcl ]
 then
   git clone https://github.com/PointCloudLibrary/pcl.git
   cd pcl
-  git checkout tags/pcl-1.11.1
+  git checkout tags/pcl-1.15.0
 else
   cd pcl
 fi
 # patch
-if [ ! -e pcl_1_11_1_vtk_ios_support.patch ]
+if [ ! -e pcl_1_15_0_ios.patch ]
 then
-  curl -L https://gist.github.com/matlabbe/f3ba9366eb91e1b855dadd2ddce5746d/raw/6869cf26211ab15492599e557b0e729b23b2c119/pcl_1_11_1_vtk_ios_support.patch -o pcl_1_11_1_vtk_ios_support.patch
-  git apply pcl_1_11_1_vtk_ios_support.patch
+  curl -L https://gist.githubusercontent.com/matlabbe/f3ba9366eb91e1b855dadd2ddce5746d/raw/7231688d7fb9e86df72ca7c5f355d6b9727205d5/pcl_1_15_0_ios.patch -o pcl_1_15_0_ios.patch
+  git apply pcl_1_15_0_ios.patch
 fi
 mkdir -p build
 cd build
-cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=12.0 -DCMAKE_INSTALL_PREFIX=$prefix -DBUILD_apps=OFF -DBUILD_examples=OFF -DBUILD_tools=OFF -DBUILD_visualization=OFF -DBUILD_tracking=OFF -DBUILD_people=OFF -DBUILD_global_tests=OFF -DWITH_QT=OFF -DWITH_OPENGL=OFF -DWITH_VTK=ON -DPCL_SHARED_LIBS=OFF -DPCL_ENABLE_SSE=OFF -DCMAKE_FIND_ROOT_PATH=$prefix ..
+cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=12.0 -DCMAKE_INSTALL_PREFIX=$prefix -DBUILD_apps=OFF -DBUILD_examples=OFF -DBUILD_tools=OFF -DBUILD_visualization=OFF -DBUILD_tracking=OFF -DBUILD_people=OFF -DBUILD_recognition=OFF -DBUILD_global_tests=OFF -DWITH_QT=OFF -DWITH_OPENGL=OFF -DWITH_OPENMP=OFF -DWITH_VTK=ON -DPCL_FLANN_REQUIRED_TYPE=STATIC -DPCL_SHARED_LIBS=OFF -DPCL_ENABLE_SSE=OFF -DCMAKE_FIND_ROOT_PATH=$prefix ..
 cmake --build . --config Release
 cmake --build . --config Release --target install
 cd $pwd
@@ -190,38 +224,46 @@ cd $pwd
 fi
 
 # OpenCV
-if [ ! -e $prefix/include/opencv2 ]
+if [ ! -e $prefix/include/opencv4 ]
 then
 if [ ! -e opencv_contrib ]
 then
-  git clone https://github.com/opencv/opencv_contrib.git
-  cd opencv_contrib
-  git checkout tags/3.4.2
+  git clone https://github.com/opencv/opencv_contrib.git -b 4.11.0
 fi
 cd $pwd
 if [ ! -e opencv ]
 then
-  git clone https://github.com/opencv/opencv.git
-  cd opencv
-  git checkout tags/3.4.2
-else
-  cd opencv
-fi
-if [ ! -e opencv_ios.patch ]
-then
-  curl -L https://gist.githubusercontent.com/matlabbe/fdc3ab4854f3a68fbde7277f543b4e5b/raw/f340839c09165056d3845645df24b76507542fd2/opencv_ios.patch -o opencv_ios.patch
-  git apply opencv_ios.patch
+  git clone https://github.com/opencv/opencv.git -b 4.11.0
 fi
+cd opencv
 mkdir -p build
 cd build
-# add "add_definitions(-DPNG_ARM_NEON_OPT=0)" in 3rdparty/libpng/CMakeLists.txt
-cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=12.0 -DCMAKE_INSTALL_PREFIX=$prefix -DOPENCV_EXTRA_MODULES_PATH=$prefix/opencv_contrib/modules -DBUILD_TESTS=OFF -DBUILD_PERF_TESTS=OFF -DWITH_CUDA=OFF -DBUILD_opencv_apps=OFF -DBUILD_opencv_xobjdetect=OFF -DBUILD_opencv_stereo=OFF ..
+cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=12.0 -DCMAKE_INSTALL_PREFIX=$prefix -DOPENCV_EXTRA_MODULES_PATH=$prefix/opencv_contrib/modules -DBUILD_TESTS=OFF -DBUILD_PERF_TESTS=OFF -DWITH_CUDA=OFF -DWITH_WEBP=OFF -DWITH_OPENEXR=OFF -DBUILD_opencv_apps=OFF -DBUILD_opencv_xobjdetect=OFF -DBUILD_opencv_stereo=OFF -DOPENCV_ENABLE_NONFREE=ON ..
 cmake --build . --config Release
 cmake --build . --config Release --target install
 cd $pwd
 #rm -rf opencv opencv_contrib
 fi
 
+# LAZ (dependency for liblas)
+# LAS
+if [ ! -e $prefix/include/laszip ]
+then
+if [ ! -e LASzip ]
+then
+  git clone https://github.com/LASzip/LASzip.git -b 2.0.1
+fi
+cd LASzip
+sed -i '' 's/cmake_minimum_required(VERSION 2.6.0)/cmake_minimum_required(VERSION 3.5)/g' CMakeLists.txt
+sed -i '' 's/add_subdirectory(tools)/#add_subdirectory(tools)/g' CMakeLists.txt
+mkdir -p build
+cd build
+cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=12.0 -DCMAKE_INSTALL_PREFIX=$prefix -DCMAKE_FIND_ROOT_PATH=$prefix -DBUILD_STATIC=ON ..
+cmake --build . --config Release -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED="NO" CODE_SIGN_ENTITLEMENTS=""  CODE_SIGNING_ALLOWED="NO"
+cmake --build . --config Release --target install -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED="NO" CODE_SIGN_ENTITLEMENTS=""  CODE_SIGNING_ALLOWED="NO"
+cd $pwd
+fi
+
 # LAS
 if [ ! -e $prefix/include/liblas ]
 then
@@ -230,10 +272,11 @@ then
   git clone https://github.com/libLAS/libLAS.git
 fi
 cd libLAS
+sed -i '' 's/cmake_minimum_required(VERSION 2.8.11)/cmake_minimum_required(VERSION 3.5)/g' CMakeLists.txt
 sed -i '' 's/SHARED/STATIC/g' src/CMakeLists.txt
 mkdir -p build
 cd build
-cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=12.0 -DCMAKE_INSTALL_PREFIX=$prefix -DCMAKE_FIND_ROOT_PATH=$prefix -DWITH_UTILITIES=OFF -DWITH_TESTS=OFF -DWITH_GEOTIFF=OFF ..
+cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=12.0 -DCMAKE_INSTALL_PREFIX=$prefix -DCMAKE_FIND_ROOT_PATH=$prefix -DWITH_UTILITIES=OFF -DWITH_TESTS=OFF -DWITH_GEOTIFF=OFF -DWITH_LASZIP=ON -DWITH_STATIC_LASZIP=ON ..
 cmake --build . --config Release -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED="NO" CODE_SIGN_ENTITLEMENTS=""  CODE_SIGNING_ALLOWED="NO"
 cmake --build . --config Release --target install -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED="NO" CODE_SIGN_ENTITLEMENTS=""  CODE_SIGNING_ALLOWED="NO"
 cd $pwd
@@ -245,6 +288,6 @@ cmake -DANDROID_PREBUILD=ON ../../../../..
 cmake --build . --config Release
 mkdir -p ios
 cd ios
-cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=12.0 -DCMAKE_INSTALL_PREFIX=$prefix -DCMAKE_FIND_ROOT_PATH=$prefix -DWITH_QT=OFF -DBUILD_APP=OFF -DBUILD_TOOLS=OFF -DWITH_TORO=OFF -DWITH_VERTIGO=OFF -DWITH_MADGWICK=OFF -DWITH_ORB_OCTREE=OFF  -DBUILD_EXAMPLES=OFF -DWITH_LIBLAS=ON ../../../../../..
+cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=12.0 -DCMAKE_INSTALL_PREFIX=$prefix -DCMAKE_FIND_ROOT_PATH=$prefix -DWITH_QT=OFF -DBUILD_APP=OFF -DBUILD_TOOLS=OFF -DWITH_TORO=OFF -DWITH_VERTIGO=OFF -DWITH_MADGWICK=OFF -DWITH_ORB_OCTREE=ON  -DBUILD_EXAMPLES=OFF -DWITH_LIBLAS=ON ../../../../../..
 cmake --build . --config Release
 cmake --build . --config Release --target install

app/ios/RTABMapApp/text_atlas_png.h

@@ -0,0 +1,40 @@
+/*This is a generated file...*/
+
+#ifndef TEXT_ATLAS_PNG_H
+#define TEXT_ATLAS_PNG_H
+
+namespace rtabmap
+{
+
+static const char * TEXT_ATLAS_PNG = 
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
+   "128944229148241289442291482412894422914824128944229148241289442291482412894422914824128944229148241289442291482412FA07BC59000020004944415489610FB030306FD57A24AA01981F58A86A3D128944F9007303A3F33EAEF5A8CC3C926A37A3A7CDEC9DFC546A9075A8247F95AC9992BE6966CFE42CE74E49A35A747957D2DB925E92F4B0A40BCDECF13C7508F4F9BCA41D24AD29E9E39266CF364D96F4A4A4DB241D6F66F7E724AFD5F94F963441D2B392AE96F40F339B9687DC0EE5B7E35766767945F25F31B3F5FB91DD449FA524ED22F7F92F2EE903D9A6E992C6497A50D20566765A4EF2C2F37FD4CCBED3669FE5259D1E345F6D663FEF41FE5F25AD14347FA1C87B4BB7001749FAA4D7F4A699AD99D3B1BBBAFEC019923E13344F96B46D2FF7A55E3EFF3C69F1FBFB9D999DD7E131B690B44F64D334336BB9CC7244FE2449DF30B3D75BECB3A2A49383E67F9AD9BE9DE8DB469F8524FD44D23A929694F4C16CD35B72F7FF1BE5EEFF4FF52BAB13651606F603FE0DBC4C3D338137803B812381D573943B0A184B2387E425C393F56E444E2BA6019703CBE6ACC728E044606A073A4C058ECA496E37E73F06D82D0FB93DCA0FD9A642F92FE671FE812EBF03267528FF1E60B91C6486E77F7707FBAC1AD1E7FC1EE5DF1839D6DCBD1CAB28804702FDC6E778EC8EAF3F705093EF42D703AF5EE41741447E8D8E07F6C0154D8EF16E8FF24F6CB3CF9A917DCEE954DF16C7DD0B78BDC9B9F8BC01FCBA5F79AD1499073886CE6F460033808BC9E7A6B47513190FE7717E81AC5E1F00E3805573D2614EE0EA1E74B8A0A2F3BF829CCC523DCAAF316C0600C0F93DE8F002B0449F72D300A00D0C820100B031F1C9C1D965C82F8A88FC1A93E8C0F509CC0BBCD5E418BD0E00A6D262424B010300E0B826E7D08AB37A95374B0B45D6903333FF44D25C59F35449774A3A4BD231928E93748E9C39E24DEF98DF907413B071AF8A656CD1A47D19609D3E8F9D170B483A36A7631D2B29664E7E4BD2E3929E92FB0C423603F6CB49876EF8AAA4AB80392A903DEC000E93B47964D3643993FF5D9262038E85249D54A06A8941006E907782A4D9824D77CBB90B87237349DABA837EDB68C04D9617B349FA7DCEC76C4A760FDF39B269A69CDB6FACA41991EDDF05FE90A7221B0213BC11C65BC051C0622DF69903D81E78C2DBEF6DE0DB3DEAB020F04E8B514FE87BEC0B1A4780F7E25C1FB5D7F2C096C0354DF459A34FF95FC7594F7CA600BFC69B6503A37126E26941DFF1F41124D6E6FC9706BE059C064C8E9C7BE807CB5B7EBBD79C15CA5FB05FD999FC55689CD9CD044E20980101DF065E8D7C0EDFEA437EB200B4810A2D0038D7E0AD916B340E172F52A8FCA289C8F7B9B183FDFFDD62FF5E2D0035766FB24F6E16006025E2CFBBBFE359D3"
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
+   "0000000000000000000000000000000000000000000000000000000000000000000000000000000000000000D629FF0F4A0CD12A5E549F170000000049454E44AE426082";
+
+}
+
+#endif //TEXT_ATLAS_PNG_H
+

app/ios/Settings.bundle/Assembling.plist

@@ -320,6 +320,34 @@
 			<key>DefaultValue</key>
 			<true/>
 		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+			<key>FooterText</key>
+			<string>Standard OBJ format has colorless polygons where there is no texture. This policy can add color to vertex when exporting the OBJ.</string>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSMultiValueSpecifier</string>
+			<key>Title</key>
+			<string>Texture Vertex Color Policy</string>
+			<key>Key</key>
+			<string>TextureVertexColorPolicy</string>
+			<key>DefaultValue</key>
+			<integer>0</integer>
+			<key>Titles</key>
+			<array>
+				<string>Colorless (standard OBJ)</string>
+				<string>Keep color only on textureless polygons</string>
+				<string>Keep all color</string>
+			</array>
+			<key>Values</key>
+			<array>
+				<integer>0</integer>
+				<integer>1</integer>
+				<integer>2</integer>
+			</array>
+		</dict>
 		<dict>
 			<key>Type</key>
 			<string>PSGroupSpecifier</string>
@@ -365,11 +393,13 @@
 			<array>
 				<string>PLY</string>
 				<string>LAS</string>
+				<string>LAZ</string>
 			</array>
 			<key>Values</key>
 			<array>
 				<string>ply</string>
 				<string>las</string>
+				<string>laz</string>
 			</array>
 		</dict>
 	</array>

app/ios/Settings.bundle/Mapping.plist

@@ -58,7 +58,7 @@
 			<key>Type</key>
 			<string>PSGroupSpecifier</string>
 			<key>FooterText</key>
-			<string>Choosing between high and low depth confidence depends on the application. If precise geometry is critical, prioritize high depth confidence even if it means fewer points. If you need a dense point cloud for analysis, even in areas with weak LiDAR signal, a lower depth confidence might be suitable at the cost of highly interpolated points (that could give a wavy look of some surfaces). Important: the points with too low confidence are not saved in the db file, so we cannot recover or filter these points in post-processing.</string>
+			<string>Choosing between high and low depth confidence depends on the application. If precise geometry is critical, prioritize high depth confidence even if it means fewer points. If you need a dense point cloud for analysis, even in areas with weak LiDAR signal, a lower depth confidence might be suitable at the cost of highly interpolated points (that could give a wavy look of some surfaces). Note: it is possible to change confidence level in post-processing.</string>
 		</dict>
 		<dict>
 			<key>DefaultValue</key>
@@ -86,7 +86,7 @@
 			<key>Type</key>
 			<string>PSGroupSpecifier</string>
 			<key>FooterText</key>
-			<string>Smooth the point clouds.</string>
+			<string>Smooth the point clouds. Note that this can be changed in post-processing.</string>
 		</dict>
 		<dict>
 			<key>Type</key>
@@ -98,6 +98,44 @@
 			<key>DefaultValue</key>
 			<false/>
 		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+			<key>FooterText</key>
+			<string>Filter interpolated depth pixels between foreground and background objects. Note that values can be changed in post-processing.</string>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSMultiValueSpecifier</string>
+			<key>Title</key>
+			<string>Depth Bleeding Filter Error</string>
+			<key>Key</key>
+			<string>DepthBleedingError</string>
+			<key>DefaultValue</key>
+			<string>0</string>
+			<key>Titles</key>
+			<array>
+				<string>Disabled</string>
+				<string>2.5 cm</string>
+				<string>5 cm</string>
+				<string>10 cm</string>
+				<string>15 cm</string>
+				<string>20 cm</string>
+				<string>25 cm</string>
+				<string>30 cm</string>
+			</array>
+			<key>Values</key>
+			<array>
+				<string>0</string>
+				<string>0.025</string>
+				<string>0.05</string>
+				<string>0.10</string>
+				<string>0.15</string>
+				<string>0.20</string>
+				<string>0.25</string>
+				<string>0.30</string>
+			</array>
+		</dict>
 		<dict>
 			<key>Type</key>
 			<string>PSGroupSpecifier</string>
@@ -482,6 +520,14 @@
 			<key>Titles</key>
 			<array>
 				<string>No Limit</string>
+				<string>5000</string>
+				<string>4500</string>
+				<string>4000</string>
+				<string>3500</string>
+				<string>3000</string>
+				<string>2500</string>
+				<string>2000</string>
+				<string>1500</string>
 				<string>1000</string>
 				<string>900</string>
 				<string>800</string>
@@ -497,6 +543,14 @@
 			<key>Values</key>
 			<array>
 				<string>0</string>
+				<string>5000</string>
+				<string>4500</string>
+				<string>4000</string>
+				<string>3500</string>
+				<string>3000</string>
+				<string>2500</string>
+				<string>2000</string>
+				<string>1500</string>
 				<string>1000</string>
 				<string>900</string>
 				<string>800</string>
@@ -528,6 +582,14 @@
 			<key>Titles</key>
 			<array>
 				<string>No Limit</string>
+				<string>5000</string>
+				<string>4500</string>
+				<string>4000</string>
+				<string>3500</string>
+				<string>3000</string>
+				<string>2500</string>
+				<string>2000</string>
+				<string>1500</string>
 				<string>1000</string>
 				<string>900</string>
 				<string>800</string>
@@ -542,6 +604,14 @@
 			<key>Values</key>
 			<array>
 				<string>0</string>
+				<string>5000</string>
+				<string>4500</string>
+				<string>4000</string>
+				<string>3500</string>
+				<string>3000</string>
+				<string>2500</string>
+				<string>2000</string>
+				<string>1500</string>
 				<string>1000</string>
 				<string>900</string>
 				<string>800</string>
@@ -558,7 +628,7 @@
 			<key>Type</key>
 			<string>PSGroupSpecifier</string>
 			<key>FooterText</key>
-			<string>BRIEF features are fast to compute but are not rotation invariant like FREAK. Warning: Changing feature type will automatically reset the map!</string>
+			<string>BRIEF, FREAK and ORB are binary features, which are fast to compute and use less memory. SURF and SIFT are high dimensional float descriptors (rotation/scale/shear-invariant), so they use more memory and CPU but they are significantly better to detect loop closures in large environments. Warning: Changing feature type will automatically reset the map!</string>
 		</dict>
 		<dict>
 			<key>Type</key>
@@ -571,13 +641,21 @@
 			<string>6</string>
 			<key>Titles</key>
 			<array>
-				<string>BRIEF</string>
-				<string>FREAK</string>
+				<string>SURF</string>
+				<string>SIFT</string>
+				<string>GFTT-FREAK</string>
+				<string>GFTT-BRIEF</string>
+				<string>GFTT-ORB</string>
+				<string>ORB-OCTREE</string>
 			</array>
 			<key>Values</key>
 			<array>
-				<string>6</string>
+				<string>0</string>
+				<string>1</string>
 				<string>5</string>
+				<string>6</string>
+				<string>8</string>
+				<string>10</string>
 			</array>
 		</dict>
 		<dict>
@@ -804,6 +882,56 @@
 			<key>KeyboardType</key>
 			<string>NumberPad</string>
 		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+			<key>FooterText</key>
+			<string>Maximum range at which the markers are detected. Long range increases the estimated orientation error of the tag, which can then cause bad graph optimization. Using larger tags can improve orientation estimation at longer range.</string>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSMultiValueSpecifier</string>
+			<key>Title</key>
+			<string>Marker Maximum Range</string>
+			<key>Key</key>
+			<string>MarkerMaxRange</string>
+			<key>DefaultValue</key>
+			<real>2</real>
+			<key>Titles</key>
+			<array>
+				<string>No Limit</string>
+				<string>0.5 m</string>
+				<string>1 m</string>
+				<string>2 m</string>
+				<string>3 m</string>
+				<string>4 m</string>
+				<string>5 m</string>
+				<string>6 m</string>
+				<string>7 m</string>
+				<string>8 m</string>
+				<string>9 m</string>
+				<string>10 m</string>
+				<string>15 m</string>
+				<string>20 m</string>
+			</array>
+			<key>Values</key>
+			<array>
+				<string>0</string>
+				<real>0.5</real>
+				<real>1</real>
+				<real>2</real>
+				<real>3</real>
+				<real>4</real>
+				<real>5</real>
+				<real>6</real>
+				<real>7</real>
+				<real>8</real>
+				<real>9</real>
+				<integer>10</integer>
+				<real>15</real>
+				<real>20</real>
+			</array>
+		</dict>
 		<dict>
 			<key>Type</key>
 			<string>PSGroupSpecifier</string>

app/ios/Settings.bundle/Root.plist

@@ -430,6 +430,60 @@
 				<real>0.01</real>
 			</array>
 		</dict>
+		<dict>
+			<key>Title</key>
+			<string>Measuring</string>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+		</dict>
+		<dict>
+			<key>DefaultValue</key>
+			<integer>0</integer>
+			<key>Key</key>
+			<string>MeasuringUnits</string>
+			<key>Title</key>
+			<string>System of Units</string>
+			<key>Titles</key>
+			<array>
+				<string>Metric</string>
+				<string>Imperial</string>
+			</array>
+			<key>Type</key>
+			<string>PSMultiValueSpecifier</string>
+			<key>Values</key>
+			<array>
+				<integer>0</integer>
+				<integer>1</integer>
+			</array>
+		</dict>
+		<dict>
+			<key>DefaultValue</key>
+			<real>0.050000000000000003</real>
+			<key>Key</key>
+			<string>MeasuringTextSize</string>
+			<key>Title</key>
+			<string>Text Size</string>
+			<key>Titles</key>
+			<array>
+				<string>30 cm</string>
+				<string>25 cm</string>
+				<string>20 cm</string>
+				<string>15 cm</string>
+				<string>10 cm</string>
+				<string>5 cm</string>
+			</array>
+			<key>Type</key>
+			<string>PSMultiValueSpecifier</string>
+			<key>Values</key>
+			<array>
+				<real>0.29999999999999999</real>
+				<real>0.25</real>
+				<real>0.20000000000000001</real>
+				<real>0.14999999999999999</real>
+				<real>0.10000000000000001</real>
+				<real>0.050000000000000003</real>
+			</array>
+		</dict>
 		<dict>
 			<key>FooterText</key>
 			<string>Copyright (c) 2010-2024, Mathieu Labbe - IntRoLab - Université de Sherbrooke. All rights reserved.</string>
@@ -440,7 +494,7 @@
 		</dict>
 		<dict>
 			<key>DefaultValue</key>
-			<string>0.21.8</string>
+			<string>0.22.0</string>
 			<key>Key</key>
 			<string>Version</string>
 			<key>Title</key>

cmake_modules/android.toolchain.cmake

@@ -184,7 +184,7 @@
 #
 # ------------------------------------------------------------------------------
 
-cmake_minimum_required( VERSION 2.6.3 )
+cmake_minimum_required( VERSION 3.14 )
 
 if( DEFINED CMAKE_CROSSCOMPILING )
  # subsequent toolchain loading is not really needed

corelib/include/rtabmap/core/Compression.h

@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 
+#include <rtabmap/core/rvl_codec.h>
 #include <rtabmap/utilite/UThread.h>
 #include <opencv2/opencv.hpp>
 
@@ -86,5 +87,9 @@ cv::Mat RTABMAP_CORE_EXPORT uncompressData(const unsigned char * bytes, unsigned
 cv::Mat RTABMAP_CORE_EXPORT compressString(const std::string & str);
 std::string RTABMAP_CORE_EXPORT uncompressString(const cv::Mat & bytes);
 
+std::string RTABMAP_CORE_EXPORT compressedDepthFormat(const cv::Mat & bytes);
+std::string RTABMAP_CORE_EXPORT compressedDepthFormat(const std::vector<unsigned char> & bytes);
+std::string RTABMAP_CORE_EXPORT compressedDepthFormat(const unsigned char * bytes, size_t size);
+
 } /* namespace rtabmap */
 #endif /* COMPRESSION_H_ */

corelib/include/rtabmap/core/DBDriver.h

@@ -100,7 +100,7 @@ public:
 		int nodeId,
 		const std::vector<CameraModel> & models,
 		const std::vector<StereoCameraModel> & stereoModels);
-	void updateDepthImage(int nodeId, const cv::Mat & image);
+	void updateDepthImage(int nodeId, const cv::Mat & image, const std::string & format);
 	void updateLaserScan(int nodeId, const LaserScan & scan);
 
 public:
@@ -178,6 +178,7 @@ public:
 	void getWeight(int signatureId, int & weight) const;
 	void getLastNodeIds(std::set<int> & ids) const;
 	void getAllNodeIds(std::set<int> & ids, bool ignoreChildren = false, bool ignoreBadSignatures = false, bool ignoreIntermediateNodes = false) const;
+	void getAllOdomPoses(std::map<int, Transform> & poses, bool ignoreChildren = false, bool ignoreIntermediateNodes = false) const;
 	void getAllLinks(std::multimap<int, Link> & links, bool ignoreNullLinks = true, bool withLandmarks = false) const;
 	void getLastNodeId(int & id) const;
 	void getLastMapId(int & mapId) const;
@@ -242,7 +243,8 @@ protected:
 
 	virtual void updateDepthImageQuery(
 			int nodeId,
-			const cv::Mat & image) const = 0;
+			const cv::Mat & image,
+			const std::string & format) const = 0;
 
 	virtual void updateLaserScanQuery(
 			int nodeId,
@@ -286,6 +288,7 @@ protected:
 	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const = 0;
 	virtual void getLastNodeIdsQuery(std::set<int> & ids) const = 0;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures, bool ignoreIntermediateNodes) const = 0;
+	virtual void getAllOdomPosesQuery(std::map<int, Transform> & poses, bool ignoreChildren, bool ignoreIntermediateNodes) const = 0;
 	virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks, bool withLandmarks) const = 0;
 	virtual void getLastIdQuery(const std::string & tableName, int & id, const std::string & fieldName="id") const = 0;
 	virtual void getInvertedIndexNiQuery(int signatureId, int & ni) const = 0;

corelib/include/rtabmap/core/DBDriverSqlite3.h

@@ -103,7 +103,8 @@ protected:
 
 	virtual void updateDepthImageQuery(
 			int nodeId,
-			const cv::Mat & image) const;
+			const cv::Mat & image,
+			const std::string & format) const;
 
 	void updateLaserScanQuery(
 			int nodeId,
@@ -147,6 +148,7 @@ protected:
 	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const;
 	virtual void getLastNodeIdsQuery(std::set<int> & ids) const;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures, bool ignoreIntermediateNodes) const;
+	virtual void getAllOdomPosesQuery(std::map<int, Transform> & poses, bool ignoreChildren, bool ignoreIntermediateNodes) const;
 	virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks, bool withLandmarks) const;
 	virtual void getLastIdQuery(const std::string & tableName, int & id, const std::string & fieldName="id") const;
 	virtual void getInvertedIndexNiQuery(int signatureId, int & ni) const;
@@ -172,7 +174,7 @@ private:
 	void stepImage(sqlite3_stmt * ppStmt, int id, const cv::Mat & imageBytes) const;
 	void stepDepth(sqlite3_stmt * ppStmt, const SensorData & sensorData) const;
 	void stepCalibrationUpdate(sqlite3_stmt * ppStmt, int nodeId, const std::vector<CameraModel> & models, const std::vector<StereoCameraModel> & stereoModels) const;
-	void stepDepthUpdate(sqlite3_stmt * ppStmt, int nodeId, const cv::Mat & imageCompressed) const;
+	void stepDepthUpdate(sqlite3_stmt * ppStmt, int nodeId, const cv::Mat & image, const std::string & format) const;
 	void stepScanUpdate(sqlite3_stmt * ppStmt, int nodeId, const LaserScan & image) const;
 	void stepSensorData(sqlite3_stmt * ppStmt, const SensorData & sensorData) const;
 	void stepLink(sqlite3_stmt * ppStmt, const Link & link) const;

corelib/include/rtabmap/core/DBReader.h

@@ -51,28 +51,30 @@ public:
 			 bool ignoreGoalDelay = false,
 			 bool goalsIgnored = false,
 			 int startId = 0,
-			 int cameraIndex = -1,
+			 const std::vector<unsigned int> & cameraIndices = std::vector<unsigned int>(),
 			 int stopId = 0,
 			 bool intermediateNodesIgnored = false,
 			 bool landmarksIgnored = false,
 			 bool featuresIgnored = false,
 			 int startMapId = 0,
 			 int stopMapId = -1,
-			 bool priorsIgnored = false);
+			 bool priorsIgnored = false,
+			 const std::vector<Transform> & cameraLocalTransformOverrides = std::vector<Transform>());
 	DBReader(const std::list<std::string> & databasePaths,
 			 float frameRate = 0.0f, // -1 = use Database stamps, 0 = inf
 			 bool odometryIgnored = false,
 			 bool ignoreGoalDelay = false,
 			 bool goalsIgnored = false,
 			 int startId = 0,
-			 int cameraIndex = -1,
+			 const std::vector<unsigned int> & cameraIndices = std::vector<unsigned int>(),
 			 int stopId = 0,
 			 bool intermediateNodesIgnored = false,
 			 bool landmarksIgnored = false,
 			 bool featuresIgnored = false,
 			 int startMapId = 0,
 			 int stopMapId = -1,
-			 bool priorsIgnored = false);
+			 bool priorsIgnored = false,
+			 const std::vector<Transform> & cameraLocalTransformOverrides = std::vector<Transform>());
 	virtual ~DBReader();
 
 	virtual bool init(
@@ -91,6 +93,7 @@ protected:
 
 private:
 	SensorData getNextData(SensorCaptureInfo * info = 0);
+	void checkArguments();
 
 private:
 	std::list<std::string> _paths;
@@ -99,13 +102,14 @@ private:
 	bool _goalsIgnored;
 	int _startId;
 	int _stopId;
-	int _cameraIndex;
+	std::vector<unsigned int> _cameraIndices;
 	bool _intermediateNodesIgnored;
 	bool _landmarksIgnored;
 	bool _featuresIgnored;
 	bool _priorsIgnored;
 	int _startMapId;
 	int _stopMapId;
+	std::vector<Transform> _cameraLocalTransformOverrides;
 
 	DBDriver * _dbDriver;
 	UTimer _timer;

corelib/include/rtabmap/core/Graph.h

@@ -115,7 +115,8 @@ Transform RTABMAP_CORE_EXPORT calcRMSE(
 		float & rotational_median,
 		float & rotational_std,
 		float & rotational_min,
-		float & rotational_max);
+		float & rotational_max,
+		bool align2D = false);
 
 void RTABMAP_CORE_EXPORT computeMaxGraphErrors(
 		const std::map<int, Transform> & poses,

corelib/include/rtabmap/core/Memory.h

@@ -303,6 +303,7 @@ private:
 	bool _notLinkedNodesKeptInDb;
 	bool _saveIntermediateNodeData;
 	std::string _rgbCompressionFormat;
+	std::string _depthCompressionFormat;
 	bool _incrementalMemory;
 	bool _localizationDataSaved;
 	bool _reduceGraph;
@@ -314,6 +315,7 @@ private:
 	bool _badSignaturesIgnored;
 	bool _mapLabelsAdded;
 	bool _depthAsMask;
+	float _maskFloorThreshold;
 	bool _stereoFromMotion;
 	unsigned int _imagePreDecimation;
 	unsigned int _imagePostDecimation;
@@ -341,6 +343,7 @@ private:
 	bool _detectMarkers;
 	float _markerLinVariance;
 	float _markerAngVariance;
+	bool _markerOrientationIgnored;
 
 	int _idCount;
 	int _idMapCount;

corelib/include/rtabmap/core/OdometryInfo.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2025, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef ODOMETRYINFO_H_
 #define ODOMETRYINFO_H_
 
+#include <rtabmap/core/rtabmap_core_export.h>
 #include <map>
 #include "rtabmap/core/Transform.h"
 #include "rtabmap/core/RegistrationInfo.h"
@@ -37,63 +38,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
-class OdometryInfo
+class RTABMAP_CORE_EXPORT OdometryInfo
 {
 public:
-	OdometryInfo() :
-		lost(true),
-		features(0),
-		localMapSize(0),
-		localScanMapSize(0),
-		localKeyFrames(0),
-		localBundleOutliers(0),
-		localBundleConstraints(0),
-		localBundleTime(0),
-		keyFrameAdded(false),
-		timeDeskewing(0.0f),
-		timeEstimation(0.0f),
-		timeParticleFiltering(0.0f),
-		stamp(0),
-		interval(0),
-		distanceTravelled(0.0f),
-		memoryUsage(0),
-		gravityRollError(0.0),
-		gravityPitchError(0.0),
-		type(0)
-	{}
-
-	OdometryInfo copyWithoutData() const
-	{
-		OdometryInfo output;
-		output.lost = lost;
-		output.reg = reg.copyWithoutData();
-		output.features = features;
-		output.localMapSize = localMapSize;
-		output.localScanMapSize = localScanMapSize;
-		output.localKeyFrames = localKeyFrames;
-		output.localBundleOutliers = localBundleOutliers;
-		output.localBundleConstraints = localBundleConstraints;
-		output.localBundleTime = localBundleTime;
-		output.localBundlePoses = localBundlePoses;
-		output.localBundleModels = localBundleModels;
-		output.keyFrameAdded = keyFrameAdded;
-		output.timeDeskewing = timeDeskewing;
-		output.timeEstimation = timeEstimation;
-		output.timeParticleFiltering = timeParticleFiltering;
-		output.stamp = stamp;
-		output.interval = interval;
-		output.transform = transform;
-		output.transformFiltered = transformFiltered;
-		output.transformGroundTruth = transformGroundTruth;
-		output.guessVelocity = guessVelocity;
-		output.guess = guess;
-		output.distanceTravelled = distanceTravelled;
-		output.memoryUsage = memoryUsage;
-		output.gravityRollError = gravityRollError;
-		output.gravityPitchError = gravityPitchError;
-		output.type = type;
-		return output;
-	}
+	OdometryInfo();
+	OdometryInfo copyWithoutData() const;
+	std::map<std::string, float> statistics(const Transform & pose = Transform());
 
 	bool lost;
 	RegistrationInfo reg;
@@ -106,6 +56,9 @@ public:
 	float localBundleTime;
 	std::map<int, Transform> localBundlePoses;
 	std::map<int, std::vector<CameraModel> > localBundleModels;
+	float localBundleAvgInlierDistance;
+	int localBundleMaxKeyFramesForInlier;
+	std::vector<int> localBundleOutliersPerCam;
 	bool keyFrameAdded;
 	float timeDeskewing;
 	float timeEstimation;

corelib/include/rtabmap/core/Optimizer.h

@@ -149,14 +149,16 @@ public:
 			const std::map<int, Signature> & signatures,
 			std::map<int, cv::Point3f> & points3DMap,
 			std::map<int, std::map<int, FeatureBA> > & wordReferences, // <ID words, IDs frames + keypoint/depth/descriptor>
-			bool rematchFeatures = false);
+			bool rematchFeatures = false,
+			const ParametersMap & registrationParameters = ParametersMap());
 
 	std::map<int, Transform> optimizeBA(
 			int rootId,
 			const std::map<int, Transform> & poses,
 			const std::multimap<int, Link> & links,
 			const std::map<int, Signature> & signatures,
-			bool rematchFeatures = false);
+			bool rematchFeatures = false,
+			const ParametersMap & registrationParameters = ParametersMap());
 
 	Transform optimizeBA(
 			const Link & link,
@@ -171,7 +173,9 @@ public:
 			const std::map<int, Signature> & signatures,
 			std::map<int, cv::Point3f> & points3DMap,
 			std::map<int, std::map<int, FeatureBA > > & wordReferences, // <ID words, IDs frames + keypoint/depth/descriptor>
-			bool rematchFeatures = false);
+			bool rematchFeatures = false,
+			bool useLinkTransformAsGuess = false,
+			ParametersMap registrationParameters = ParametersMap());
 
 protected:
 	Optimizer(

corelib/include/rtabmap/core/Parameters.h

@@ -209,6 +209,7 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(Mem, NotLinkedNodesKept,          bool, true,     "Keep not linked nodes in db (rehearsed nodes and deleted nodes).");
     RTABMAP_PARAM(Mem, IntermediateNodeDataKept,    bool, false,    "Keep intermediate node data in db.");
     RTABMAP_PARAM_STR(Mem, ImageCompressionFormat,   ".jpg",        "RGB image compression format. It should be \".jpg\" or \".png\".");
+    RTABMAP_PARAM_STR(Mem, DepthCompressionFormat,   ".rvl",        "Depth image compression format for 16UC1 depth type. It should be \".png\" or \".rvl\". If depth type is 32FC1, \".png\" is used.");
     RTABMAP_PARAM(Mem, STMSize,                   unsigned int, 10, "Short-term memory size.");
     RTABMAP_PARAM(Mem, IncrementalMemory,           bool, true,     "SLAM mode, otherwise it is Localization mode.");
     RTABMAP_PARAM(Mem, LocalizationDataSaved,       bool, false,     uFormat("Save localization data during localization session (when %s=false). When enabled, the database will then also grow in localization mode. This mode would be used only for debugging purpose.", kMemIncrementalMemory().c_str()).c_str());
@@ -221,6 +222,7 @@ class RTABMAP_CORE_EXPORT 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, DepthMaskFloorThr,           float, 0.0,     uFormat("Filter floor from depth mask below specified threshold (m) before extracting features. 0 means disabled, negative means remove all objects above the floor threshold instead. Ignored if %s is false.", kMemDepthAsMask().c_str()));
     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,          unsigned int, 1, uFormat("Decimation of the RGB image before visual feature detection. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. If %s is true and if depth is smaller than decimated RGB, depth may be interpolated to match RGB size for feature detection.",kMemDepthAsMask().c_str()));
     RTABMAP_PARAM(Mem, ImagePostDecimation,         unsigned int, 1, uFormat("Decimation of the RGB image before saving it to database. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. Decimation is done from the original image. If set to same value than %s, data already decimated is saved (no need to re-decimate the image).", kMemImagePreDecimation().c_str()));
@@ -388,6 +390,7 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(RGBD, MaxOdomCacheSize,             int,  10,      uFormat("Maximum odometry cache size. Used only in localization mode (when %s=false). This is used to get smoother localizations and to verify localization transforms (when %s!=0) to make sure we don't teleport to a location very similar to one we previously localized on. Set 0 to disable caching.", kMemIncrementalMemory().c_str(), kRGBDOptimizeMaxError().c_str()));
     RTABMAP_PARAM(RGBD, LocalizationSmoothing,        bool, true,    uFormat("Adjust localization constraints based on optimized odometry cache poses (when %s>0).",  kRGBDMaxOdomCacheSize().c_str()));
     RTABMAP_PARAM(RGBD, LocalizationPriorError,       double, 0.001, uFormat("The corresponding variance (error x error) set to priors of the map's poses during localization (when %s>0).",  kRGBDMaxOdomCacheSize().c_str()));
+    RTABMAP_PARAM(RGBD, LocalizationSecondTryWithoutProximityLinks,  bool, true, uFormat("When localization is rejected by graph optimization validation, try a second time without proximity links if landmark or loop closure links are also present in odometry cache (see %s). If it succeeds, the proximity links are removed. This assumes that global loop closure and landmark links are more accurate than proximity links.", kRGBDMaxOdomCacheSize().c_str()));
 
     // Local/Proximity loop closure detection
     RTABMAP_PARAM(RGBD, ProximityByTime,              bool, false, "Detection over all locations in STM.");
@@ -475,6 +478,8 @@ class RTABMAP_CORE_EXPORT Parameters
     // Odometry Frame-to-Map
     RTABMAP_PARAM(OdomF2M, MaxSize,             int, 2000,    "[Visual] Local map size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words.");
     RTABMAP_PARAM(OdomF2M, MaxNewFeatures,      int, 0,       "[Visual] Maximum features (sorted by keypoint response) added to local map from a new key-frame. 0 means no limit.");
+    RTABMAP_PARAM(OdomF2M, InitDepthFactor,     float, 0.05,  "[Visual] Depth factor used to initialize depth of features without depth. Depth = Factor * fx.");
+    RTABMAP_PARAM(OdomF2M, FloorThreshold,      float, 0.0,   "[Visual] Only track features in 3D feature map that are over this threshold (height in base frame). Can be useful to ignore reflections on the floor. 0 means disabled.");
     RTABMAP_PARAM(OdomF2M, ScanMaxSize,         int, 2000,    "[Geometry] Maximum local scan map size.");
     RTABMAP_PARAM(OdomF2M, ScanSubtractRadius,  float, 0.05,  "[Geometry] Radius used to filter points of a new added scan to local map. This could match the voxel size of the scans.");
     RTABMAP_PARAM(OdomF2M, ScanSubtractAngle,   float, 45,    uFormat("[Geometry] Max angle (degrees) used to filter points of a new added scan to local map (when \"%s\">0). 0 means any angle.", kOdomF2MScanSubtractRadius().c_str()).c_str());
@@ -486,6 +491,9 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(OdomF2M, BundleAdjustment,          int, 0, "Local bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres.");
 #endif
     RTABMAP_PARAM(OdomF2M, BundleAdjustmentMaxFrames, int, 10, "Maximum frames used for bundle adjustment (0=inf or all current frames in the local map).");
+    RTABMAP_PARAM(OdomF2M, BundleAdjustmentMinMotion, float, 0.0, "To create a new keyframe with bundle adjustment, a minimum motion (in pixels) can be required. The motion is computed by the average distance between inliers of the previous keyframe and new frame.");
+    RTABMAP_PARAM(OdomF2M, BundleAdjustmentMaxKeyFramesPerFeature, int, 0, "Maximum keyframes per feature for bundle adjustment. 0 means not limit.");
+    RTABMAP_PARAM(OdomF2M, BundleUpdateFeatureMapOnAllFrames, bool, false, uFormat("Update 3D local feature map on every frame with bundle adjustment. Recommended if %s=false and %s=true so that features without depth are better triangulated on every frame (not only on keyframes). If disabled, the feature map is updated only when a new keyframe is added (legacy approach).", kVisDepthAsMask().c_str(), kMemUseOdomFeatures().c_str()));
 
     // Odometry Mono
     RTABMAP_PARAM(OdomMono, InitMinFlow,        float, 100,  "Minimum optical flow required for the initialization step.");
@@ -671,7 +679,6 @@ class RTABMAP_CORE_EXPORT Parameters
 
     // Visual registration parameters
     RTABMAP_PARAM(Vis, EstimationType,           int,    1,     "Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP), 2:2D->2D (Epipolar Geometry)");
-    RTABMAP_PARAM(Vis, ForwardEstOnly,           bool,   true,  "Forward estimation only (A->B). If false, a transformation is also computed in backward direction (B->A), then the two resulting transforms are merged (middle interpolation between the transforms).");
     RTABMAP_PARAM(Vis, InlierDistance,           float,  0.1,   uFormat("[%s = 0] Maximum distance for feature correspondences. Used by 3D->3D estimation approach.", kVisEstimationType().c_str()));
     RTABMAP_PARAM(Vis, RefineIterations,         int,    5,     uFormat("[%s = 0] Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.", kVisEstimationType().c_str()));
     RTABMAP_PARAM(Vis, PnPReprojError,           float,  2,     uFormat("[%s = 1] PnP reprojection error.", kVisEstimationType().c_str()));
@@ -703,6 +710,7 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(Vis, MaxDepth,                  float, 0,     "Max depth of the features (0 means no limit).");
     RTABMAP_PARAM(Vis, MinDepth,                  float, 0,     "Min depth of the features (0 means no limit).");
     RTABMAP_PARAM(Vis, DepthAsMask,               bool,  true,  "Use depth image as mask when extracting features.");
+    RTABMAP_PARAM(Vis, DepthMaskFloorThr,         float, 0.0,    uFormat("Filter floor from depth mask below specified threshold (m) before extracting features. 0 means disabled, negative means remove all objects above the floor threshold instead. Ignored if %s is false.", kVisDepthAsMask().c_str()));
     RTABMAP_PARAM_STR(Vis, RoiRatios,        "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
     RTABMAP_PARAM(Vis, SubPixWinSize,             int,   3,     "See cv::cornerSubPix().");
     RTABMAP_PARAM(Vis, SubPixIterations,          int,   0,     "See cv::cornerSubPix(). 0 disables sub pixel refining.");
@@ -875,8 +883,9 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(Marker, Dictionary,             int,   0,     "Dictionary to use: DICT_ARUCO_4X4_50=0, DICT_ARUCO_4X4_100=1, DICT_ARUCO_4X4_250=2, DICT_ARUCO_4X4_1000=3, DICT_ARUCO_5X5_50=4, DICT_ARUCO_5X5_100=5, DICT_ARUCO_5X5_250=6, DICT_ARUCO_5X5_1000=7, DICT_ARUCO_6X6_50=8, DICT_ARUCO_6X6_100=9, DICT_ARUCO_6X6_250=10, DICT_ARUCO_6X6_1000=11, DICT_ARUCO_7X7_50=12, DICT_ARUCO_7X7_100=13, DICT_ARUCO_7X7_250=14, DICT_ARUCO_7X7_1000=15, DICT_ARUCO_ORIGINAL = 16, DICT_APRILTAG_16h5=17, DICT_APRILTAG_25h9=18, DICT_APRILTAG_36h10=19, DICT_APRILTAG_36h11=20");
     RTABMAP_PARAM(Marker, Length,                 float, 0,     "The length (m) of the markers' side. 0 means automatic marker length estimation using the depth image (the camera should look at the marker perpendicularly for initialization).");
     RTABMAP_PARAM(Marker, MaxDepthError,          float, 0.01,  uFormat("Maximum depth error between all corners of a marker when estimating the marker length (when %s is 0). The smaller it is, the more perpendicular the camera should be toward the marker to initialize the length.", kMarkerLength().c_str()));
-    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, VarianceLinear,         float, 0.001, uFormat("Linear variance to set on marker detections. If %s is enabled and %s=2 (GTSAM): it is the variance of the range factor, with 9999 to disable range factor and to do only bearing.", kMarkerVarianceOrientationIgnored().c_str(), kOptimizerStrategy().c_str()));
+    RTABMAP_PARAM(Marker, VarianceAngular,        float, 0.01,  uFormat("Angular variance to set on marker detections. If %s is enabled, it is ignored with %s=1 (g2o) and it corresponds to bearing variance with %s=2 (GTSAM).", kMarkerVarianceOrientationIgnored().c_str(), kOptimizerStrategy().c_str(), kOptimizerStrategy().c_str()));
+    RTABMAP_PARAM(Marker, VarianceOrientationIgnored, bool, false, uFormat("When this setting is false, the landmark's orientation is optimized during graph optimization. When this setting is true, only the position of the landmark is optimized. This can be useful when the landmark's orientation estimation is not reliable. Note that for %s=1 (g2o), only %s needs be set if we ignore orientation. For %s=2 (GTSAM), instead of optimizing the landmark's position directly, a bearing/range factor is used, with %s as the variance of the range factor (with 9999 to optimize the position with only a bearing factor) and %s as the variance of the bearing factor (pitch/yaw).", kOptimizerStrategy().c_str(), kMarkerVarianceLinear().c_str(), kOptimizerStrategy().c_str(), kMarkerVarianceLinear().c_str(), kMarkerVarianceAngular().c_str()));
     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.");

corelib/include/rtabmap/core/RegistrationInfo.h

@@ -59,9 +59,11 @@ public:
 		output.covariance = covariance.clone();
 		output.rejectedMsg = rejectedMsg;
 		output.inliers = inliers;
+		output.inliersPerCam = inliersPerCam;
 		output.inliersMeanDistance = inliersMeanDistance;
 		output.inliersDistribution = inliersDistribution;
 		output.matches = matches;
+		output.matchesPerCam = matchesPerCam;
 		output.icpInliersRatio = icpInliersRatio;
 		output.icpTranslation = icpTranslation;
 		output.icpRotation = icpRotation;
@@ -85,6 +87,8 @@ public:
 	int matches;
 	std::vector<int> matchesIDs;
 	std::vector<int> projectedIDs; // "From" IDs
+	std::vector<int> inliersPerCam;
+	std::vector<int> matchesPerCam;
 
 	// RegistrationIcp
 	float icpInliersRatio;

corelib/include/rtabmap/core/RegistrationVis.h

@@ -78,7 +78,6 @@ private:
 	int _refineIterations;
 	float _epipolarGeometryVar;
 	int _estimationType;
-	bool _forwardEstimateOnly;
 	float _PnPReprojError;
 	int _PnPFlags;
 	int _PnPRefineIterations;
@@ -101,6 +100,7 @@ private:
 	bool _guessMatchToProjection;
 	int _bundleAdjustment;
 	bool _depthAsMask;
+	float _maskFloorThreshold;
 	float _minInliersDistributionThr;
 	float _maxInliersMeanDistance;
 

corelib/include/rtabmap/core/Rtabmap.h

@@ -333,6 +333,7 @@ private:
 	int _maxOdomCacheSize;
 	bool _localizationSmoothing;
 	double _localizationPriorInf;
+	bool _localizationSecondTryWithoutProximityLinks;
 	bool _createGlobalScanMap;
 	float _markerPriorsLinearVariance;
 	float _markerPriorsAngularVariance;

corelib/include/rtabmap/core/SensorData.h

@@ -78,6 +78,16 @@ public:
 			double stamp = 0.0,
 			const cv::Mat & userData = cv::Mat());
 
+	// RGB-D constructor + depth confidence
+	SensorData(
+		const cv::Mat & rgb,
+		const cv::Mat & depth,
+		const cv::Mat & depth_confidence,
+		const CameraModel & cameraModel,
+		int id = 0,
+		double stamp = 0.0,
+		const cv::Mat & userData = cv::Mat());
+
 	// RGB-D constructor + laser scan
 	SensorData(
 			const LaserScan & laserScan,
@@ -87,6 +97,17 @@ public:
 			int id = 0,
 			double stamp = 0.0,
 			const cv::Mat & userData = cv::Mat());
+	
+	// RGB-D constructor + confidence + laser scan
+	SensorData(
+			const LaserScan & laserScan,
+			const cv::Mat & rgb,
+			const cv::Mat & depth,
+			const cv::Mat & depthConfidence,
+			const CameraModel & cameraModel,
+			int id = 0,
+			double stamp = 0.0,
+			const cv::Mat & userData = cv::Mat());
 
 	// Multi-cameras RGB-D constructor
 	SensorData(
@@ -96,6 +117,16 @@ public:
 			int id = 0,
 			double stamp = 0.0,
 			const cv::Mat & userData = cv::Mat());
+	
+	// Multi-cameras RGB-D constructor + depth confidence
+	SensorData(
+			const cv::Mat & rgb,
+			const cv::Mat & depth,
+			const cv::Mat & depthConfidence,
+			const std::vector<CameraModel> & cameraModels,
+			int id = 0,
+			double stamp = 0.0,
+			const cv::Mat & userData = cv::Mat());
 
 	// Multi-cameras RGB-D constructor + laser scan
 	SensorData(
@@ -107,6 +138,17 @@ public:
 			double stamp = 0.0,
 			const cv::Mat & userData = cv::Mat());
 
+	// Multi-cameras RGB-D constructor + depth confidence + laser scan
+	SensorData(
+			const LaserScan & laserScan,
+			const cv::Mat & rgb,
+			const cv::Mat & depth,
+			const cv::Mat & depthConfidence,
+			const std::vector<CameraModel> & cameraModels,
+			int id = 0,
+			double stamp = 0.0,
+			const cv::Mat & userData = cv::Mat());
+
 	// Stereo constructor
 	SensorData(
 			const cv::Mat & left,
@@ -160,6 +202,8 @@ public:
 			_imageCompressed.empty() &&
 			_depthOrRightRaw.empty() &&
 			_depthOrRightCompressed.empty() &&
+			_depthConfidenceRaw.empty() &&
+			_depthConfidenceCompressed.empty() &&
 			_laserScanRaw.isEmpty() &&
 			_laserScanCompressed.isEmpty() &&
 			_cameraModels.empty() &&
@@ -178,10 +222,12 @@ public:
 
 	const cv::Mat & imageCompressed() const {return _imageCompressed;}
 	const cv::Mat & depthOrRightCompressed() const {return _depthOrRightCompressed;}
+	const cv::Mat & depthConfidenceCompressed() const {return _depthConfidenceCompressed;}
 	const LaserScan & laserScanCompressed() const {return _laserScanCompressed;}
 
 	const cv::Mat & imageRaw() const {return _imageRaw;}
 	const cv::Mat & depthOrRightRaw() const {return _depthOrRightRaw;}
+	const cv::Mat & depthConfidenceRaw() const {return _depthConfidenceRaw;}
 	const LaserScan & laserScanRaw() const {return _laserScanRaw;}
 
 	/**
@@ -190,7 +236,9 @@ public:
 	 * @param clearPreviousData, clear previous raw and compressed images before setting the new ones.
 	 */
 	void setRGBDImage(const cv::Mat & rgb, const cv::Mat & depth, const CameraModel & model, bool clearPreviousData = true);
+	void setRGBDImage(const cv::Mat & rgb, const cv::Mat & depth, const cv::Mat & depth_confidence, const CameraModel & model, bool clearPreviousData = true);
 	void setRGBDImage(const cv::Mat & rgb, const cv::Mat & depth, const std::vector<CameraModel> & models, bool clearPreviousData = true);
+	void setRGBDImage(const cv::Mat & rgb, const cv::Mat & depth, const cv::Mat & depth_confidence, const std::vector<CameraModel> & models, bool clearPreviousData = true);
 	void setStereoImage(const cv::Mat & left, const cv::Mat & right, const StereoCameraModel & stereoCameraModel, bool clearPreviousData = true);
 	void setStereoImage(const cv::Mat & left, const cv::Mat & right, const std::vector<StereoCameraModel> & stereoCameraModels, bool clearPreviousData = true);
 
@@ -207,8 +255,8 @@ public:
 	void setStereoCameraModels(const std::vector<StereoCameraModel> & stereoCameraModels) {_stereoCameraModels = stereoCameraModels;}
 
 	//for convenience
-	cv::Mat depthRaw() const {return _depthOrRightRaw.type()!=CV_8UC1?_depthOrRightRaw:cv::Mat();}
-	cv::Mat rightRaw() const {return _depthOrRightRaw.type()==CV_8UC1?_depthOrRightRaw:cv::Mat();}
+	cv::Mat depthRaw() const {return !(_depthOrRightRaw.type()==CV_8UC1 || _depthOrRightRaw.type()==CV_8UC3) ? _depthOrRightRaw : cv::Mat();}
+	cv::Mat rightRaw() const {return   _depthOrRightRaw.type()==CV_8UC1 || _depthOrRightRaw.type()==CV_8UC3  ? _depthOrRightRaw : cv::Mat();}
 
 	// Use setRGBDImage() or setStereoImage() with clearNotUpdated=false or removeRawData() instead. To be backward compatible, this function doesn't clear compressed data.
 	RTABMAP_DEPRECATED void setImageRaw(const cv::Mat & image);
@@ -227,7 +275,8 @@ public:
 			cv::Mat * userDataRaw = 0,
 			cv::Mat * groundCellsRaw = 0,
 			cv::Mat * obstacleCellsRaw = 0,
-			cv::Mat * emptyCellsRaw = 0);
+			cv::Mat * emptyCellsRaw = 0,
+			cv::Mat * depthConfidenceRaw = 0);
 	void uncompressDataConst(
 			cv::Mat * imageRaw,
 			cv::Mat * depthOrRightRaw,
@@ -235,7 +284,8 @@ public:
 			cv::Mat * userDataRaw = 0,
 			cv::Mat * groundCellsRaw = 0,
 			cv::Mat * obstacleCellsRaw = 0,
-			cv::Mat * emptyCellsRaw = 0) const;
+			cv::Mat * emptyCellsRaw = 0,
+			cv::Mat * depthConfidenceRaw = 0) const;
 
 	const std::vector<CameraModel> & cameraModels() const {return _cameraModels;}
 	const std::vector<StereoCameraModel> & stereoCameraModels() const {return _stereoCameraModels;}
@@ -327,10 +377,12 @@ private:
 
 	cv::Mat _imageCompressed;          // compressed image
 	cv::Mat _depthOrRightCompressed;   // compressed image
+	cv::Mat _depthConfidenceCompressed;   // compressed data
 	LaserScan _laserScanCompressed;      // compressed data
 
 	cv::Mat _imageRaw;          // CV_8UC1 or CV_8UC3
-	cv::Mat _depthOrRightRaw;   // depth CV_16UC1 or CV_32FC1, right image CV_8UC1
+	cv::Mat _depthOrRightRaw;   // depth CV_16UC1 or CV_32FC1, right image CV_8UC1 or CV_8UC3
+	cv::Mat _depthConfidenceRaw;   // CV_8UC1
 	LaserScan _laserScanRaw;
 
 	std::vector<CameraModel> _cameraModels;

corelib/include/rtabmap/core/Statistics.h

@@ -81,6 +81,7 @@ class RTABMAP_CORE_EXPORT 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, Proximity_links_cleared,);
 	//Odom correction
 	RTABMAP_STATS(Loop, Odom_correction_norm, m);
 	RTABMAP_STATS(Loop, Odom_correction_angle, deg);

corelib/include/rtabmap/core/Transform.h

@@ -113,7 +113,7 @@ public:
 	void getTranslationAndEulerAngles(float & x, float & y, float & z, float & roll, float & pitch, float & yaw) const;
 	void getEulerAngles(float & roll, float & pitch, float & yaw) const;
 	void getTranslation(float & x, float & y, float & z) const;
-	float getAngle(float x=1.0f, float y=0.0f, float z=0.0f) const;
+	float getAngle(const Transform & t) const;
 	float getNorm() const;
 	float getNormSquared() const;
 	float getDistance(const Transform & t) const;

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

@@ -50,21 +50,20 @@ public:
 public:
 	CameraDepthAI(
 			const std::string & mxidOrName = "",
-			int resolution = 1, // 0=720p, 1=800p, 2=400p
-			float imageRate=0.0f,
+			int imageWidth = 1280, // 640 or 1280
+			float imageRate = 0.0f,
 			const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraDepthAI();
 
 	void setOutputMode(int outputMode = 0);
 	void setDepthProfile(int confThreshold = 200, int lrcThreshold = 5);
-	void setExtendedDisparity(bool extendedDisparity);
-	void setSubpixelMode(bool enabled, int fractionalBits = 3);
-	void setCompanding(bool enabled, int width=96);
-	void setRectification(bool useSpecTranslation, float alphaScaling = 0.0f, bool enabled=true);
+	void setExtendedDisparity(bool extendedDisparity = false, bool enableCompanding = false);
+	void setSubpixelMode(bool enabled = false, int fractionalBits = 3);
+	void setDisparityWidthAndFilter(int disparityWidth = 96, int medianFilter = 5);
+	void setRectification(bool useSpecTranslation = false, float alphaScaling = 0.0f, bool enabled = true);
 	void setIMU(bool imuPublished, bool publishInterIMU);
 	void setIrIntensity(float dotIntensity = 0.0f, float floodIntensity = 0.0f);
-	void setDetectFeatures(int detectFeatures = 0);
-	void setBlobPath(const std::string & blobPath);
+	void setDetectFeatures(int detectFeatures = 0, const std::string & blobPath = "");
 	void setGFTTDetector(bool useHarrisDetector = false, float minDistance = 7.0f, int numTargetFeatures = 1000);
 	void setSuperPointDetector(float threshold = 0.01f, bool nms = true, int nmsRadius = 4);
 
@@ -84,10 +83,12 @@ private:
 	int outputMode_;
 	int confThreshold_;
 	int lrcThreshold_;
-	int resolution_;
+	int imageWidth_;
 	bool extendedDisparity_;
+	bool enableCompanding_;
 	int subpixelFractionalBits_;
-	int compandingWidth_;
+	int disparityWidth_;
+	int medianFilter_;
 	bool useSpecTranslation_;
 	float alphaScaling_;
 	bool imagesRectified_;
@@ -103,7 +104,7 @@ private:
 	bool nms_;
 	int nmsRadius_;
 	std::string blobPath_;
-	std::shared_ptr<dai::Device> device_;
+	std::unique_ptr<dai::Device> device_;
 	std::shared_ptr<dai::DataOutputQueue> cameraQueue_;
 	std::map<double, cv::Vec3f> accBuffer_;
 	std::map<double, cv::Vec3f> gyroBuffer_;

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

@@ -56,6 +56,8 @@ public:
 			const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraStereoImages();
 
+	void setRightGrayScale(bool enabled = true) {rightGrayScale_ = enabled;}
+
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
 	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
@@ -69,6 +71,7 @@ protected:
 private:
 	CameraImages * camera2_;
 	StereoCameraModel stereoModel_;
+	bool rightGrayScale_;
 };
 
 

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

@@ -70,6 +70,7 @@ public:
 
 	void setResolution(int width, int height) {_width=width, _height=height;}
 	void setFOURCC(const std::string & fourcc) { _fourcc = fourcc; }
+	void setRightGrayScale(bool enabled = true) {rightGrayScale_ = enabled;}
 
 protected:
 	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
@@ -88,6 +89,7 @@ private:
 	int _width;
 	int _height;
 	std::string _fourcc;
+	bool rightGrayScale_;
 };
 
 } // namespace rtabmap

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

@@ -79,6 +79,7 @@ public:
 	virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.0);
 
 	void postInterIMUPublic(const IMU & imu, double stamp);
+	void setRightGrayScale(bool enabled = true);
 
 protected:
 	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
@@ -100,6 +101,7 @@ private:
 	bool computeOdometry_;
 	bool lost_;
 	bool force3DoF_;
+	bool rightGrayScale_;
 	ZedIMUThread * imuPublishingThread_;
 #endif
 };

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

@@ -62,6 +62,8 @@ public:
 	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
 
+	void setRightGrayScale(bool enabled = true);
+
 protected:
 	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
 
@@ -74,6 +76,7 @@ private:
 	int usbDevice_;
 	int resolution_;
 	uint64_t lastStamp_;
+	bool rightGrayScale_;
 #endif
 };
 

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

@@ -151,7 +151,10 @@ void denseMeshPostProcessing(
 				coloredPts.at(i) = false;
 			}
 		}
-		pcl::toPCLPointCloud2(*coloredCloud, mesh->cloud);
+		if(coloredOutput) {
+			pcl::toPCLPointCloud2(*coloredCloud, mesh->cloud);
+			hasColors = true;
+		}
 
 		// remove polygons with no color
 		if(cleanMesh)
@@ -177,7 +180,6 @@ void denseMeshPostProcessing(
 			filteredPolygons.resize(oi);
 			mesh->polygons = filteredPolygons;
 		}
-		hasColors = true;
 	}
 
 	if(minClusterSize)
@@ -238,11 +240,11 @@ void denseMeshPostProcessing(
 		if(progressState) progressState->callback(uFormat("Filtered %d polygons.", before-(int)mesh->polygons.size()));
 	}
 
-	// compute normals for the mesh if not already here, add also white color if colored output is required
-	if(!hasNormals || (!hasColors && coloredOutput))
+	// compute normals for the mesh if not already here
+	if(!hasNormals)
 	{
 		// use polygons
-		if(hasColors || coloredOutput)
+		if(hasColors)
 		{
 			pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 			pcl::fromPCLPointCloud2(mesh->cloud, *cloud);
@@ -269,18 +271,9 @@ void denseMeshPostProcessing(
 				// flat normal (per face)
 				for(unsigned int j=0; j<v.vertices.size(); ++j)
 				{
-					if(!hasNormals)
-					{
-						cloud->at(v.vertices[j]).normal_x = normal[0];
-						cloud->at(v.vertices[j]).normal_y = normal[1];
-						cloud->at(v.vertices[j]).normal_z = normal[2];
-					}
-					if(!hasColors)
-					{
-						cloud->at(v.vertices[j]).r = 255;
-						cloud->at(v.vertices[j]).g = 255;
-						cloud->at(v.vertices[j]).b = 255;
-					}
+					cloud->at(v.vertices[j]).normal_x = normal[0];
+					cloud->at(v.vertices[j]).normal_y = normal[1];
+					cloud->at(v.vertices[j]).normal_z = normal[2];
 				}
 			}
 			pcl::toPCLPointCloud2 (*cloud, mesh->cloud);

corelib/include/rtabmap/core/lidar/LidarVLP16.h

@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 // Should be first on windows to avoid "WinSock.h has already been included" error
 #include <pcl/io/vlp_grabber.h>
+#include <boost/version.hpp>
 
 #include <rtabmap/core/Lidar.h>
 #include <rtabmap/utilite/USemaphore.h>

corelib/include/rtabmap/core/odometry/OdometryF2M.h

@@ -62,6 +62,8 @@ private:
 	float keyFrameThr_;
 	int visKeyFrameThr_;
 	int maxNewFeatures_;
+	float initDepthFactor_;
+	float floorThreshold_;
 	float scanKeyFrameThr_;
 	int scanMaximumMapSize_;
 	float scanSubtractRadius_;
@@ -69,6 +71,9 @@ private:
 	float scanMapMaxRange_;
 	int bundleAdjustment_;
 	int bundleMaxFrames_;
+	float bundleMinMotion_;
+	int bundleMaxKeyFramesPerFeature_;
+	bool bundleUpdateFeatureMapOnAllFrames_;
 	float validDepthRatio_;
 	int pointToPlaneK_;
 	float pointToPlaneRadius_;

corelib/include/rtabmap/core/rvl_codec.h

@@ -0,0 +1,37 @@
+// The following code is a C++ wrapper of the code presented by
+// Andrew D. Wilson in "Fast Lossless Depth Image Compression" at SIGCHI'17.
+// The original code is licensed under the MIT License.
+
+#ifndef RVL_CODEC_H_
+#define RVL_CODEC_H_
+
+#include <cstdint>
+#include "rtabmap/core/rtabmap_core_export.h"
+
+namespace rtabmap
+{
+
+class RTABMAP_CORE_EXPORT RvlCodec {
+public:
+    RvlCodec();
+    // Compress input data into output. The size of output can be equal to (1.5 * numPixels + 4) in the worst case.
+    int CompressRVL(const uint16_t * input, unsigned char * output, int numPixels);
+    // Decompress input data into output. The size of output must be equal to numPixels.
+    void DecompressRVL(const unsigned char * input, uint16_t * output, int numPixels);
+
+private:
+    RvlCodec(const RvlCodec &);
+    RvlCodec & operator=(const RvlCodec &);
+
+    void EncodeVLE(int value);
+    int DecodeVLE();
+
+    int *buffer_;
+    int *pBuffer_;
+    int word_;
+    int nibblesWritten_;
+};
+
+}  // namespace rtabmap
+
+#endif  // RVL_CODEC_H_

corelib/include/rtabmap/core/util2d.h

@@ -126,6 +126,14 @@ cv::Mat RTABMAP_CORE_EXPORT registerDepth(
 		const cv::Size & colorSize,
 		const cv::Mat & colorK,
 		const rtabmap::Transform & transform);
+cv::Mat RTABMAP_CORE_EXPORT registerDepth(
+	const cv::Mat & depth,
+	const cv::Mat & confidence,
+	const cv::Mat & depthK,
+	const cv::Size & colorSize,
+	const cv::Mat & colorK,
+	const rtabmap::Transform & transform,
+	cv::Mat & registeredConfidence);
 
 cv::Mat RTABMAP_CORE_EXPORT fillDepthHoles(
 		const cv::Mat & depth,
@@ -144,6 +152,10 @@ cv::Mat RTABMAP_CORE_EXPORT fastBilateralFiltering(
 		float sigmaR = 0.05f,
 		bool earlyDivision = false);
 
+void RTABMAP_CORE_EXPORT depthBleedingFiltering(
+	cv::Mat & depth,
+	float maxDepthError);
+
 cv::Mat RTABMAP_CORE_EXPORT brightnessAndContrastAuto(
 		const cv::Mat & src,
 		const cv::Mat & mask,

corelib/include/rtabmap/core/util3d.h

@@ -97,6 +97,15 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT cloudFromDepth(
 		float maxDepth = 0.0f,
 		float minDepth = 0.0f,
 		std::vector<int> * validIndices = 0);
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT cloudFromDepth(
+		const cv::Mat & imageDepth,
+		const cv::Mat & imageDepthConfidence,
+		const CameraModel & model,
+		int decimation = 1,
+		float maxDepth = 0.0f,
+		float minDepth = 0.0f,
+		unsigned char confidenceThr = 0,
+		std::vector<int> * validIndices = 0);
 
 // Use cloudFromDepthRGB with CameraModel interface.
 RTABMAP_DEPRECATED pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_CORE_EXPORT cloudFromDepthRGB(
@@ -116,6 +125,16 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_CORE_EXPORT cloudFromDepthRGB(
 		float maxDepth = 0.0f,
 		float minDepth = 0.0f,
 		std::vector<int> * validIndices = 0);
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_CORE_EXPORT cloudFromDepthRGB(
+	const cv::Mat & imageRgb,
+	const cv::Mat & imageDepth,
+	const cv::Mat & imageDepthConfidence,
+	const CameraModel & model,
+	int decimation = 1,
+	float maxDepth = 0.0f,
+	float minDepth = 0.0f,
+	unsigned char confidenceThr = 0, // 0=low, 100=high
+	std::vector<int> * validIndices = 0);
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT cloudFromDisparity(
 		const cv::Mat & imageDisparity,
@@ -164,7 +183,8 @@ std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> RTABMAP_CORE_EXPORT cloudsFromS
 		float minDepth = 0.0f,
 		std::vector<pcl::IndicesPtr> * validIndices = 0,
 		const ParametersMap & stereoParameters = ParametersMap(),
-		const std::vector<float> & roiRatios = std::vector<float>()); // ignored for stereo
+		const std::vector<float> & roiRatios = std::vector<float>(), // ignored for stereo
+		unsigned char confidenceThr = 0);                            // ignored for stereo
 
 /**
  * Create a XYZ cloud from the images contained in SensorData. If there is only one camera,
@@ -188,7 +208,8 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT cloudFromSensorData(
 		float minDepth = 0.0f,
 		std::vector<int> * validIndices = 0,
 		const ParametersMap & stereoParameters = ParametersMap(),
-		const std::vector<float> & roiRatios = std::vector<float>()); // ignored for stereo
+		const std::vector<float> & roiRatios = std::vector<float>(), // ignored for stereo
+		unsigned char confidenceThr = 0);                                // ignored for stereo
 
 /**
  * Create an RGB cloud from the images contained in SensorData, one for each camera
@@ -210,7 +231,8 @@ std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> RTABMAP_CORE_EXPORT cloudsRG
 		float minDepth = 0.0f,
 		std::vector<pcl::IndicesPtr > * validIndices = 0,
 		const ParametersMap & stereoParameters = ParametersMap(),
-		const std::vector<float> & roiRatios = std::vector<float>()); // ignored for stereo
+		const std::vector<float> & roiRatios = std::vector<float>(), // ignored for stereo
+		unsigned char confidenceThr = 0);                            // ignored for stereo
 
 /**
  * Create an RGB cloud from the images contained in SensorData. If there is only one camera,
@@ -234,7 +256,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_CORE_EXPORT cloudRGBFromSensorDat
 		float minDepth = 0.0f,
 		std::vector<int> * validIndices = 0,
 		const ParametersMap & stereoParameters = ParametersMap(),
-		const std::vector<float> & roiRatios = std::vector<float>()); // ignored for stereo
+		const std::vector<float> & roiRatios = std::vector<float>(), // ignored for stereo
+		unsigned char confidenceThr = 0);                            // ignored for stereo
 
 /**
  * Simulate a laser scan rotating counterclockwise, using middle line of the depth image.
@@ -364,6 +387,22 @@ void RTABMAP_CORE_EXPORT fillProjectedCloudHoles(
 		bool verticalDirection,
 		bool fillToBorder);
 
+/**
+ * @brief Remove values below a floor threshold in a depth image.
+ * 
+ * @param depth the depth image to filter (can be a multi-camera depth image).
+ * @param cameraModels corresponding camera model(s) to depth image, with valid 
+ *                     local transform between base frame to camera frame.
+ * @param threshold height from base frame at which pixels below it are set to 0.
+ * @param depthBelow depth image of the pixels below the floor theshold.
+ * @return cv::Mat depth image of the pixels above the floor theshold.
+ */
+cv::Mat RTABMAP_CORE_EXPORT filterFloor(
+	const cv::Mat & depth,
+	const std::vector<CameraModel> & cameraModels,
+	float threshold,
+	cv::Mat * depthBelow = 0);
+
 /**
  * For each point, return pixel of the best camera (NodeID->CameraIndex)
  * looking at it based on the policy and parameters
@@ -374,6 +413,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > RTABMAP_CORE_EXPORT
 		const std::map<int, std::vector<CameraModel> > & cameraModels,
 		float maxDistance = 0.0f,
 		float maxAngle = 0.0f,
+		float maxDepthError = 0.0f,
 		const std::vector<float> & roiRatios = std::vector<float>(),
 		const cv::Mat & projMask = cv::Mat(),
 		bool distanceToCamPolicy = false,
@@ -388,6 +428,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > RTABMAP_CORE_EXPORT
 		const std::map<int, std::vector<CameraModel> > & cameraModels,
 		float maxDistance = 0.0f,
 		float maxAngle = 0.0f,
+		float maxDepthError = 0.0f,
 		const std::vector<float> & roiRatios = std::vector<float>(),
 		const cv::Mat & projMask = cv::Mat(),
 		bool distanceToCamPolicy = false,

corelib/include/rtabmap/core/util3d_motion_estimation.h

@@ -76,6 +76,25 @@ Transform RTABMAP_CORE_EXPORT estimateMotion3DTo2D(
 			std::vector<int> * inliersOut = 0,
 			bool splitLinearCovarianceComponents = false);
 
+Transform estimateMotion3DTo2D(
+			const std::map<int, cv::Point3f> & words3A,
+			const std::map<int, cv::KeyPoint> & words2B,
+			const std::vector<CameraModel> & cameraModels,
+			unsigned int samplingPolicy,
+			int minInliers,
+			int iterations,
+			double reprojError,
+			int flagsPnP,
+			int refineIterations,
+			int varianceMedianRatio,
+			float maxVariance,
+			const Transform & guess,
+			const std::map<int, cv::Point3f> & words3B,
+			cv::Mat * covariance,
+			std::vector<std::vector<int> > * matchesOut,
+			std::vector<std::vector<int> > * inliersOut,
+			bool splitLinearCovarianceComponents);
+
 Transform RTABMAP_CORE_EXPORT estimateMotion3DTo3D(
 			const std::map<int, cv::Point3f> & words3A,
 			const std::map<int, cv::Point3f> & words3B,

corelib/include/rtabmap/core/util3d_surface.h

@@ -225,6 +225,7 @@ cv::Mat RTABMAP_CORE_EXPORT mergeTextures(
 		bool exposureFusion = false,         //Exposure fusion can be used only with OpenCV3
 		const ProgressState * state = 0,
 		unsigned char blankValue = 255,      //Gray value for blank polygons (without texture)
+		bool clearVertexColorUnderTexture = true,
 		std::map<int, std::map<int, cv::Vec4d> > * gains = 0, // <Camera ID, Camera Sub Index (multi-cameras), gains Gray-R-G-B>
 		std::map<int, std::map<int, cv::Mat> > * blendingGains = 0, // <Camera ID, Camera Sub Index (multi-cameras), gains>
 		std::pair<float, float> * contrastValues = 0); // Alpha/beta contrast values
@@ -247,6 +248,7 @@ cv::Mat RTABMAP_CORE_EXPORT mergeTextures(
 		bool exposureFusion = false,         //Exposure fusion can be used only with OpenCV3
 		const ProgressState * state = 0,
 		unsigned char blankValue = 255,      //Gray value for blank polygons (without texture)
+		bool clearVertexColorUnderTexture = true,
 		std::map<int, std::map<int, cv::Vec4d> > * gains = 0, // <Camera ID, Camera Sub Index (multi-cameras), gains Gray-R-G-B>
 		std::map<int, std::map<int, cv::Mat> > * blendingGains = 0, // <Camera ID, Camera Sub Index (multi-cameras), gains>
 		std::pair<float, float> * contrastValues = 0); // Alpha/beta contrast values
@@ -482,6 +484,22 @@ void RTABMAP_CORE_EXPORT adjustNormalsToViewPoint(
 		const Eigen::Vector3f & viewpoint = Eigen::Vector3f(0,0,0),
 		float groundNormalsUp = 0.0f);
 
+void RTABMAP_CORE_EXPORT adjustNormalsToViewPoints(
+		const std::map<int, Transform> & poses,
+		const std::vector<int> & cameraIndices,
+		pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		float groundNormalsUp = 0.0f);
+void RTABMAP_CORE_EXPORT adjustNormalsToViewPoints(
+		const std::map<int, Transform> & poses,
+		const std::vector<int> & cameraIndices,
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		float groundNormalsUp = 0.0f);
+void RTABMAP_CORE_EXPORT adjustNormalsToViewPoints(
+		const std::map<int, Transform> & poses,
+		const std::vector<int> & cameraIndices,
+		pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		float groundNormalsUp = 0.0f);
+
 void RTABMAP_CORE_EXPORT adjustNormalsToViewPoints(
 		const std::map<int, Transform> & poses,
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,
@@ -570,6 +588,15 @@ bool intersectRayMesh(
 		Eigen::Vector3f & normal,
 		int & index);
 
+int RTABMAP_CORE_EXPORT saveOBJFile(
+	const std::string &file_name,
+    const pcl::TextureMesh &tex_mesh,
+	unsigned precision = 5);
+
+int RTABMAP_CORE_EXPORT saveOBJFile(
+	const std::string &file_name,
+	const pcl::PolygonMesh &mesh,
+	unsigned precision = 5);
 
 } // namespace util3d
 } // namespace rtabmap

corelib/src/CMakeLists.txt

@@ -64,6 +64,8 @@ SET(SRC_FILES
     util3d_correspondences.cpp
     util3d_motion_estimation.cpp
     
+	rvl_codec.cpp
+	
     SensorData.cpp
     Graph.cpp
     Compression.cpp
@@ -83,6 +85,7 @@ SET(SRC_FILES
     
     Odometry.cpp
     OdometryThread.cpp
+	OdometryInfo.cpp
     odometry/OdometryF2M.cpp
     odometry/OdometryMono.cpp
     odometry/OdometryF2F.cpp
@@ -643,10 +646,10 @@ IF(octomap_FOUND)
 ENDIF(octomap_FOUND)
 
 IF(grid_map_core_FOUND)
-    IF(TARGET grid_map_core)
+    IF(TARGET grid_map_core::grid_map_core)
         SET(LIBRARIES
             ${LIBRARIES}
-            grid_map_core
+            grid_map_core::grid_map_core
         )
     ELSE()
         SET(INCLUDE_DIRS
@@ -813,6 +816,7 @@ CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/resources/DatabaseSchema.sql.in ${CMA
 
 SET(RESOURCES
 	${CMAKE_CURRENT_SOURCE_DIR}/resources/DatabaseSchema.sql
+	${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_20_0.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

corelib/src/Compression.cpp

@@ -34,14 +34,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
-// format : ".png" ".jpg" "" (empty is general)
+// format : ".jpg" ".png" ".rvl" "" (empty is general)
 CompressionThread::CompressionThread(const cv::Mat & mat, const std::string & format) :
 	uncompressedData_(mat),
 	format_(format),
 	image_(!format.empty()),
 	compressMode_(true)
 {
-	UASSERT(format.empty() || format.compare(".png") == 0 || format.compare(".jpg") == 0);
+	UASSERT(format.empty() || format.compare(".jpg") == 0 || format.compare(".png") == 0 || format.compare(".rvl") == 0);
 }
 // assume image
 CompressionThread::CompressionThread(const cv::Mat & bytes, bool isImage) :
@@ -96,7 +96,7 @@ void CompressionThread::mainLoop()
 	this->kill();
 }
 
-// ".png" or ".jpg"
+// ".jpg" or ".png" or ".rvl"
 std::vector<unsigned char> compressImage(const cv::Mat & image, const std::string & format)
 {
 	std::vector<unsigned char> bytes;
@@ -106,7 +106,21 @@ std::vector<unsigned char> compressImage(const cv::Mat & image, const std::strin
 		{
 			//save in 8bits-4channel
 			cv::Mat bgra(image.size(), CV_8UC4, image.data);
-			cv::imencode(format, bgra, bytes);
+			cv::imencode(".png", bgra, bytes);
+		}
+		else if(format == ".rvl")
+		{
+			bytes = {'D', 'E', 'P', 'T', 'H', 'R', 'V', 'L'};
+			int numPixels = image.rows * image.cols;
+        	// In the worst case, RVL compression results in ~1.5x larger data.
+        	bytes.resize(3 * numPixels + 20);
+        	uint32_t cols = image.cols;
+        	uint32_t rows = image.rows;
+        	memcpy(&bytes[8], &cols, 4);
+        	memcpy(&bytes[12], &rows, 4);
+        	RvlCodec rvl;
+        	int compressedSize = rvl.CompressRVL(image.ptr<uint16_t>(), &bytes[16], numPixels);
+        	bytes.resize(16 + compressedSize);
 		}
 		else
 		{
@@ -116,7 +130,7 @@ std::vector<unsigned char> compressImage(const cv::Mat & image, const std::strin
 	return bytes;
 }
 
-// ".png" or ".jpg"
+// ".jpg" or ".png" or ".rvl"
 cv::Mat compressImage2(const cv::Mat & image, const std::string & format)
 {
 	std::vector<unsigned char> bytes = compressImage(image, format);
@@ -129,21 +143,33 @@ cv::Mat compressImage2(const cv::Mat & image, const std::string & format)
 
 cv::Mat uncompressImage(const cv::Mat & bytes)
 {
-	 cv::Mat image;
+	cv::Mat image;
 	if(!bytes.empty())
 	{
-#if CV_MAJOR_VERSION>2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4)
-		image = cv::imdecode(bytes, cv::IMREAD_UNCHANGED);
-#else
-		image = cv::imdecode(bytes, -1);
-#endif
-		if(image.type() == CV_8UC4)
+		if (compressedDepthFormat(bytes) == ".rvl")
 		{
-			// Using clone() or copyTo() caused a memory leak !?!?
-			// image = cv::Mat(image.size(), CV_32FC1, image.data).clone();
-			cv::Mat depth(image.size(), CV_32FC1);
-			memcpy(depth.data, image.data, image.total()*image.elemSize());
-			image = depth;
+			uint32_t cols, rows;
+        	memcpy(&cols, &bytes.data[8], 4);
+        	memcpy(&rows, &bytes.data[12], 4);
+			image = cv::Mat(rows, cols, CV_16UC1);
+			RvlCodec rvl;
+        	rvl.DecompressRVL(&bytes.data[16], image.ptr<uint16_t>(), cols * rows);
+		}
+		else
+		{
+#if CV_MAJOR_VERSION>2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4)
+			image = cv::imdecode(bytes, cv::IMREAD_UNCHANGED);
+#else
+			image = cv::imdecode(bytes, -1);
+#endif
+			if(image.type() == CV_8UC4)
+			{
+				// Using clone() or copyTo() caused a memory leak !?!?
+				// image = cv::Mat(image.size(), CV_32FC1, image.data).clone();
+				cv::Mat depth(image.size(), CV_32FC1);
+				memcpy(depth.data, image.data, image.total()*image.elemSize());
+				image = depth;
+			}
 		}
 	}
 	return image;
@@ -151,17 +177,29 @@ cv::Mat uncompressImage(const cv::Mat & bytes)
 
 cv::Mat uncompressImage(const std::vector<unsigned char> & bytes)
 {
-	 cv::Mat image;
+	cv::Mat image;
 	if(bytes.size())
 	{
-#if CV_MAJOR_VERSION>2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4)
-		image = cv::imdecode(bytes, cv::IMREAD_UNCHANGED);
-#else
-		image = cv::imdecode(bytes, -1);
-#endif
-		if(image.type() == CV_8UC4)
+		if (compressedDepthFormat(bytes) == ".rvl")
 		{
-			image = cv::Mat(image.size(), CV_32FC1, image.data).clone();
+			uint32_t cols, rows;
+        	memcpy(&cols, &bytes[8], 4);
+        	memcpy(&rows, &bytes[12], 4);
+			image = cv::Mat(rows, cols, CV_16UC1);
+			RvlCodec rvl;
+        	rvl.DecompressRVL(&bytes[16], image.ptr<uint16_t>(), cols * rows);
+		}
+		else
+		{
+#if CV_MAJOR_VERSION>2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4)
+			image = cv::imdecode(bytes, cv::IMREAD_UNCHANGED);
+#else
+			image = cv::imdecode(bytes, -1);
+#endif
+			if(image.type() == CV_8UC4)
+			{
+				image = cv::Mat(image.size(), CV_32FC1, image.data).clone();
+			}
 		}
 	}
 	return image;
@@ -291,4 +329,33 @@ std::string uncompressString(const cv::Mat & bytes)
 	return "";
 }
 
+std::string compressedDepthFormat(const cv::Mat & bytes)
+{
+	return compressedDepthFormat(bytes.data, bytes.rows * bytes.cols * bytes.elemSize());
+}
+std::string compressedDepthFormat(const std::vector<unsigned char> & bytes)
+{
+	return compressedDepthFormat(bytes.data(), bytes.size());
+}
+std::string compressedDepthFormat(const unsigned char * bytes, size_t size)
+{
+	std::string format;
+	if(bytes && size)
+	{
+		size_t maxlen = std::min(size, size_t(8));
+		std::vector<unsigned char> signature(maxlen);
+		memcpy(&signature[0], bytes, maxlen);
+		if (std::string(signature.begin(), signature.end()) == "DEPTHRVL")
+		{
+			format = ".rvl";
+		}
+		else
+		{
+			// Assuming png by default
+			format = ".png";
+		}
+	}
+	return format;
+}
+
 } /* namespace rtabmap */

corelib/src/DBDriver.cpp

@@ -512,12 +512,13 @@ void DBDriver::updateCalibration(int nodeId, const std::vector<CameraModel> & mo
 	_dbSafeAccessMutex.unlock();
 }
 
-void DBDriver::updateDepthImage(int nodeId, const cv::Mat & image)
+void DBDriver::updateDepthImage(int nodeId, const cv::Mat & image, const std::string & format)
 {
 	_dbSafeAccessMutex.lock();
 	this->updateDepthImageQuery(
 			nodeId,
-			image);
+			image,
+			format);
 	_dbSafeAccessMutex.unlock();
 }
 
@@ -922,6 +923,45 @@ void DBDriver::getAllNodeIds(std::set<int> & ids, bool ignoreChildren, bool igno
 	_dbSafeAccessMutex.unlock();
 }
 
+void DBDriver::getAllOdomPoses(std::map<int, Transform> & poses, bool ignoreChildren, bool ignoreIntermediateNodes) const
+{
+	// look in the trash
+	_trashesMutex.lock();
+	if(_trashSignatures.size())
+	{
+		for(std::map<int, Signature*>::const_iterator sIter = _trashSignatures.begin(); sIter!=_trashSignatures.end(); ++sIter)
+		{
+			bool hasNeighbors = !ignoreChildren;
+			if(ignoreChildren)
+			{
+				for(std::map<int, Link>::const_iterator nIter = sIter->second->getLinks().begin();
+						nIter!=sIter->second->getLinks().end();
+						++nIter)
+				{
+					if(nIter->second.type() == Link::kNeighbor ||
+					   nIter->second.type() == Link::kNeighborMerged)
+					{
+						hasNeighbors = true;
+						break;
+					}
+				}
+			}
+			if(hasNeighbors && (!ignoreIntermediateNodes || sIter->second->getWeight() != -1))
+			{
+				poses.insert(std::make_pair(sIter->first, sIter->second->getPose()));
+			}
+		}
+
+		std::vector<int> keys = uKeys(_trashSignatures);
+
+	}
+	_trashesMutex.unlock();
+
+	_dbSafeAccessMutex.lock();
+	this->getAllOdomPosesQuery(poses, ignoreChildren, ignoreIntermediateNodes);
+	_dbSafeAccessMutex.unlock();
+}
+
 void DBDriver::getAllLinks(std::multimap<int, Link> & links, bool ignoreNullLinks, bool withLandmarks) const
 {
 	_dbSafeAccessMutex.lock();
@@ -1139,7 +1179,7 @@ void DBDriver::addInfoAfterRun(
 					  << processMemUsed << ","
 					  << databaseMemUsed << ","
 					  << dictionarySize << ","
-					  "\"" << param.c_str() << "\");";
+					  "'" << param.c_str() << "');";
 			}
 			else
 			{
@@ -1149,7 +1189,7 @@ void DBDriver::addInfoAfterRun(
 					  << processMemUsed << ","
 					  << databaseMemUsed << ","
 					  << dictionarySize << ","
-					  "\"" << param.c_str() << "\");";
+					  "'" << param.c_str() << "');";
 			}
 		}
 		else

corelib/src/DBDriverSqlite3.cpp

@@ -34,6 +34,7 @@ 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_20_0_sql.h"
 #include "DatabaseSchema_0_18_3_sql.h"
 #include "DatabaseSchema_0_18_0_sql.h"
 #include "DatabaseSchema_0_17_0_sql.h"
@@ -402,6 +403,7 @@ bool DBDriverSqlite3::connectDatabaseQuery(const std::string & url, bool overwri
 			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("0.20.0", DATABASESCHEMA_0_20_0_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)
 			{
@@ -1317,7 +1319,15 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 
 			if(images)
 			{
-				fields << "image, depth, calibration";
+				if(uStrNumCmp(_version, "0.22.0") >= 0)
+				{
+					fields << "image, depth, depth_confidence, calibration";
+				}
+				else
+				{
+					fields << "image, depth, calibration";
+				}
+				
 				if(scan || userData || occupancyGrid)
 				{
 					fields << ", ";
@@ -1448,6 +1458,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 
 				cv::Mat imageCompressed;
 				cv::Mat depthOrRightCompressed;
+				cv::Mat depthConfidenceCompressed;
 				std::vector<CameraModel> models;
 				std::vector<StereoCameraModel> stereoModels;
 				Transform localTransform = Transform::getIdentity();
@@ -1472,6 +1483,17 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 						depthOrRightCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
 					}
 
+					if(uStrNumCmp(_version, "0.22.0") >= 0)
+					{
+						//Create the depth image
+						data = sqlite3_column_blob(ppStmt, index);
+						dataSize = sqlite3_column_bytes(ppStmt, index++);
+						if(dataSize>4 && data)
+						{
+							depthConfidenceCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
+						}
+					}
+
 					if(uStrNumCmp(_version, "0.10.0") < 0)
 					{
 						data = sqlite3_column_blob(ppStmt, index); // local transform
@@ -1823,7 +1845,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 				{
 					if(models.size())
 					{
-						(*iter)->sensorData().setRGBDImage(imageCompressed, depthOrRightCompressed, models);
+						(*iter)->sensorData().setRGBDImage(imageCompressed, depthOrRightCompressed, depthConfidenceCompressed, models);
 					}
 					else
 					{
@@ -2427,9 +2449,13 @@ void DBDriverSqlite3::getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildre
 			  << "FROM Node ";
 		if(ignoreChildren)
 		{
-			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
+			// use to_id to ignore all children (which don't have link pointing on them)
+			// ignore self referring links
+			// keep nodes without link to other nodes (map has only a single node)
+			query << "WHERE ";
+			query << "(EXISTS (select 1 from Link where Node.id=to_id and from_id != to_id) OR ";
+			query << " NOT EXISTS (select 1 from Link where id=to_id and from_id != to_id)) ";
+			
 			query << "AND weight>-9 "; //ignore invalid nodes
 			if(ignoreIntermediateNodes)
 			{
@@ -2483,6 +2509,75 @@ void DBDriverSqlite3::getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildre
 	}
 }
 
+void DBDriverSqlite3::getAllOdomPosesQuery(std::map<int, Transform> & poses, bool ignoreChildren, bool ignoreIntermediateNodes) const
+{
+	if(_ppDb)
+	{
+		UTimer timer;
+		timer.start();
+		int rc = SQLITE_OK;
+		sqlite3_stmt * ppStmt = 0;
+		std::stringstream query;
+
+		query << "SELECT DISTINCT id, pose "
+			  << "FROM Node ";
+		if(ignoreChildren)
+		{
+			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(ignoreIntermediateNodes)
+			{
+				query << "AND weight!=-1 "; //ignore intermediate nodes
+			}
+		}
+		else if(ignoreIntermediateNodes)
+		{
+			query << "WHERE weight!=-1 "; //ignore intermediate nodes
+		}
+
+		query  << "ORDER BY id";
+
+		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		const void * data = 0;
+		int dataSize = 0;
+
+		// Process the result if one
+		rc = sqlite3_step(ppStmt);
+		while(rc == SQLITE_ROW)
+		{
+			int id = sqlite3_column_int(ppStmt, 0); // Signature Id
+			data = sqlite3_column_blob(ppStmt, 1);  // Pose
+			dataSize = sqlite3_column_bytes(ppStmt, 1);
+
+			Transform pose;
+			if((unsigned int)dataSize == pose.size()*sizeof(float) && data)
+			{
+				memcpy(pose.data(), data, dataSize);
+				if(uStrNumCmp(_version, "0.15.2") < 0)
+				{
+					pose.normalizeRotation();
+				}
+			}
+			else if(dataSize)
+			{
+				UERROR("Error while loading pose for node %d! Setting to null...", id);
+			}
+			poses.insert(std::make_pair(id, pose));
+			rc = sqlite3_step(ppStmt);
+		}
+		UASSERT_MSG(rc == SQLITE_DONE, 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=%f ids=%d", timer.ticks(), (int)poses.size());
+	}
+}
+
 void DBDriverSqlite3::getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks, bool withLandmarks) const
 {
 	links.clear();
@@ -4405,6 +4500,7 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures)
 			{
 				if(!(*i)->sensorData().imageCompressed().empty() ||
 				   !(*i)->sensorData().depthOrRightCompressed().empty() ||
+				   !(*i)->sensorData().depthConfidenceCompressed().empty() ||
 				   !(*i)->sensorData().laserScanCompressed().isEmpty() ||
 				   !(*i)->sensorData().userDataCompressed().empty() ||
 				   !(*i)->sensorData().cameraModels().empty() ||
@@ -4650,7 +4746,8 @@ void DBDriverSqlite3::updateCalibrationQuery(
 
 void DBDriverSqlite3::updateDepthImageQuery(
 		int nodeId,
-		const cv::Mat & image) const
+		const cv::Mat & image,
+		const std::string & format) const
 {
 	UDEBUG("");
 	if(_ppDb)
@@ -4669,7 +4766,8 @@ void DBDriverSqlite3::updateDepthImageQuery(
 		// Save depth
 		stepDepthUpdate(ppStmt,
 				nodeId,
-				image);
+				image,
+				format);
 
 		// Finalize (delete) the statement
 		rc = sqlite3_finalize(ppStmt);
@@ -5940,7 +6038,7 @@ std::string DBDriverSqlite3::queryStepDepthUpdate() const
 		return "UPDATE Data SET depth=? WHERE id=?;";
 	}
 }
-void DBDriverSqlite3::stepDepthUpdate(sqlite3_stmt * ppStmt, int nodeId, const cv::Mat & image) const
+void DBDriverSqlite3::stepDepthUpdate(sqlite3_stmt * ppStmt, int nodeId, const cv::Mat & image, const std::string & format) const
 {
 	if(!ppStmt)
 	{
@@ -5954,7 +6052,7 @@ void DBDriverSqlite3::stepDepthUpdate(sqlite3_stmt * ppStmt, int nodeId, const c
 	if(!image.empty() && (image.type()!=CV_8UC1 || image.rows > 1))
 	{
 		// compress
-		imageCompressed = compressImage2(image, ".png");
+		imageCompressed = compressImage2(image, format);
 	}
 	else
 	{
@@ -6111,7 +6209,11 @@ void DBDriverSqlite3::stepScanUpdate(sqlite3_stmt * ppStmt, int nodeId, const La
 std::string DBDriverSqlite3::queryStepSensorData() const
 {
 	UASSERT(uStrNumCmp(_version, "0.10.0") >= 0);
-	if(uStrNumCmp(_version, "0.16.0") >= 0)
+	if(uStrNumCmp(_version, "0.22.0") >= 0)
+	{
+		return "INSERT INTO Data(id, image, depth, depth_confidence, calibration, scan_info, scan, user_data, ground_cells, obstacle_cells, empty_cells, cell_size, view_point_x, view_point_y, view_point_z) VALUES(?,?,?,?,?,?,?,?,?,?,?,?,?,?,?);";
+	}
+	else if(uStrNumCmp(_version, "0.16.0") >= 0)
 	{
 		return "INSERT INTO Data(id, image, depth, calibration, scan_info, scan, user_data, ground_cells, obstacle_cells, empty_cells, cell_size, view_point_x, view_point_y, view_point_z) VALUES(?,?,?,?,?,?,?,?,?,?,?,?,?,?);";
 	}
@@ -6175,6 +6277,20 @@ void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt,
 	}
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 
+	//depth confidence
+	if(uStrNumCmp(_version, "0.22.0") >= 0)
+	{
+		if(!sensorData.depthConfidenceCompressed().empty())
+		{
+			rc = sqlite3_bind_blob(ppStmt, index++, sensorData.depthConfidenceCompressed().data, (int)sensorData.depthConfidenceCompressed().cols, SQLITE_STATIC);
+		}
+		else
+		{
+			rc = sqlite3_bind_null(ppStmt, index++);
+		}
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+	}
+
 	// calibration
 	std::vector<unsigned char> calibrationData;
 	std::vector<float> calibration;
@@ -6706,12 +6822,12 @@ void DBDriverSqlite3::stepGlobalDescriptor(sqlite3_stmt * ppStmt,
 	int rc = SQLITE_OK;
 	int index = 1;
 
-	//node_if
+	//node_id
 	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);
+	rc = sqlite3_bind_int(ppStmt, index++, descriptor.type());
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 
 	//info

corelib/src/DBReader.cpp

@@ -48,14 +48,15 @@ DBReader::DBReader(const std::string & databasePath,
 				   bool ignoreGoalDelay,
 				   bool goalsIgnored,
 				   int startId,
-				   int cameraIndex,
+				   const std::vector<unsigned int> & cameraIndices,
 				   int stopId,
 				   bool intermediateNodesIgnored,
 				   bool landmarksIgnored,
 				   bool featuresIgnored,
 				   int startMapId,
 				   int stopMapId,
-				   bool priorsIgnored) :
+				   bool priorsIgnored,
+				   const std::vector<Transform> & cameraLocalTransformOverrides) :
 	Camera(frameRate),
 	_paths(uSplit(databasePath, ';')),
 	_odometryIgnored(odometryIgnored),
@@ -63,13 +64,14 @@ DBReader::DBReader(const std::string & databasePath,
 	_goalsIgnored(goalsIgnored),
 	_startId(startId),
 	_stopId(stopId),
-	_cameraIndex(cameraIndex),
+	_cameraIndices(cameraIndices),
 	_intermediateNodesIgnored(intermediateNodesIgnored),
 	_landmarksIgnored(landmarksIgnored),
 	_featuresIgnored(featuresIgnored),
 	_priorsIgnored(priorsIgnored),
 	_startMapId(startMapId),
 	_stopMapId(stopMapId),
+	_cameraLocalTransformOverrides(cameraLocalTransformOverrides),
 	_dbDriver(0),
 	_currentId(_ids.end()),
 	_previousMapId(-1),
@@ -77,15 +79,7 @@ DBReader::DBReader(const std::string & databasePath,
 	_previousMapID(0),
 	_calibrated(false)
 {
-	if(_stopId>0 && _stopId<_startId)
-	{
-		_stopId = _startId;
-	}
-
-	if(_stopMapId>-1 && _stopMapId<_startMapId)
-	{
-		_stopMapId = _startMapId;
-	}
+	checkArguments();
 }
 
 DBReader::DBReader(const std::list<std::string> & databasePaths,
@@ -94,14 +88,15 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
 				   bool ignoreGoalDelay,
 				   bool goalsIgnored,
 				   int startId,
-				   int cameraIndex,
+				   const std::vector<unsigned int> & cameraIndices,
 				   int stopId,
 				   bool intermediateNodesIgnored,
 				   bool landmarksIgnored,
 				   bool featuresIgnored,
 				   int startMapId,
 				   int stopMapId,
-				   bool priorsIgnored) :
+				   bool priorsIgnored,
+				   const std::vector<Transform> & cameraLocalTransformOverrides) :
 	Camera(frameRate),
    _paths(databasePaths),
 	_odometryIgnored(odometryIgnored),
@@ -109,19 +104,25 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
 	_goalsIgnored(goalsIgnored),
 	_startId(startId),
 	_stopId(stopId),
-	_cameraIndex(cameraIndex),
+	_cameraIndices(cameraIndices),
 	_intermediateNodesIgnored(intermediateNodesIgnored),
 	_landmarksIgnored(landmarksIgnored),
 	_featuresIgnored(featuresIgnored),
 	_priorsIgnored(priorsIgnored),
 	_startMapId(startMapId),
 	_stopMapId(stopMapId),
+	_cameraLocalTransformOverrides(cameraLocalTransformOverrides),
 	_dbDriver(0),
 	_currentId(_ids.end()),
 	_previousMapId(-1),
 	_previousStamp(0),
 	_previousMapID(0),
 	_calibrated(false)
+{
+	checkArguments();
+}
+
+void DBReader::checkArguments()
 {
 	if(_stopId>0 && _stopId<_startId)
 	{
@@ -132,6 +133,28 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
 	{
 		_stopMapId = _startMapId;
 	}
+
+	if(!_cameraLocalTransformOverrides.empty())
+	{
+		if(!_cameraIndices.empty() &&
+			_cameraIndices.size() != _cameraLocalTransformOverrides.size())
+		{
+			UERROR("Camera local transform overrides (%d) are not the same size than the camera indices (%d). The overrides are ignored.",
+				_cameraLocalTransformOverrides.size(),
+				_cameraIndices.size()
+			);
+			_cameraLocalTransformOverrides.clear();
+		}
+		for(size_t i=0; i<_cameraLocalTransformOverrides.size(); ++i)
+		{
+			if(_cameraLocalTransformOverrides[i].isNull())
+			{
+				UERROR("Camera local transform overrides vector cannot contains null transforms! Clearing overrides.");
+				_cameraLocalTransformOverrides.clear();
+				break;
+			}
+		}
+	}
 }
 
 DBReader::~DBReader()
@@ -144,8 +167,8 @@ DBReader::~DBReader()
 }
 
 bool DBReader::init(
-		const std::string & calibrationFolder,
-		const std::string & cameraName)
+		const std::string &,
+		const std::string &)
 {
 	if(_dbDriver)
 	{
@@ -557,34 +580,147 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
 			}
 
 			data.uncompressData();
-			if(data.cameraModels().size() > 1 &&
-				_cameraIndex >= 0)
+			std::map<int, int> cameraOldNewIndices;
+			std::vector<CameraModel> dbModels = data.cameraModels();
+			if(dbModels.empty() && !data.stereoCameraModels().empty())
 			{
-				if(_cameraIndex < (int)data.cameraModels().size())
+				for(size_t i=0; i<data.stereoCameraModels().size(); ++i)
 				{
-					// select one camera
-					int subImageWidth = data.imageRaw().cols/data.cameraModels().size();
-					cv::Mat image;
+					dbModels.push_back(data.stereoCameraModels()[i].left());
+				}
+			}
+
+			if(!_cameraLocalTransformOverrides.empty() && 
+				!_cameraIndices.empty() &&
+				_cameraIndices.size() != _cameraLocalTransformOverrides.size())
+			{
+				UERROR("Camera local transform overrides (%d) are not the same size than the camera indices (%d). The overrides are ignored.",
+					_cameraLocalTransformOverrides.size(),
+					_cameraIndices.size()
+				);
+				_cameraLocalTransformOverrides.clear();
+			}
+
+			std::vector<Transform> combinedLocalTransforms;
+			if(dbModels.size() > 1 && !_cameraIndices.empty())
+			{
+				// update images and local transforms
+				cv::Mat combinedImages;
+				cv::Mat combinedDepthImages;
+				cv::Mat combinedDepthConfidenceImages;
+				std::vector<CameraModel> combinedModels;
+				std::vector<StereoCameraModel> combinedStereoModels;
+				for(size_t i=0; i<_cameraIndices.size(); ++i)
+				{
+					UASSERT_MSG(_cameraIndices[i] < dbModels.size(), uFormat("DBReader: camera index %ld is not valid (should be between 0 and %ld)",
+						_cameraIndices[i], dbModels.size()-1).c_str());
+
+					int addedCameras = std::max(combinedModels.size(), combinedStereoModels.size());
+
+					int subImageWidth = data.imageRaw().cols/dbModels.size();
 					UASSERT(!data.imageRaw().empty() &&
-							data.imageRaw().cols % data.cameraModels().size() == 0 &&
-							_cameraIndex*subImageWidth < data.imageRaw().cols);
-					image= cv::Mat(data.imageRaw(),
-						   cv::Rect(_cameraIndex*subImageWidth, 0, subImageWidth, data.imageRaw().rows)).clone();
+							data.imageRaw().cols % dbModels.size() == 0 &&
+							(int)_cameraIndices[i]*subImageWidth < data.imageRaw().cols);
+					if(combinedImages.empty())
+					{
+						// initialize with first camera
+						combinedImages = cv::Mat(data.imageRaw().rows, subImageWidth*(_cameraIndices.size()-i), data.imageRaw().type());
+					}
+
+					cv::Mat fromROI = cv::Mat(data.imageRaw(), cv::Rect(_cameraIndices[i]*subImageWidth, 0, subImageWidth, data.imageRaw().rows));
+					cv::Mat toROI = cv::Mat(combinedImages, cv::Rect(addedCameras*subImageWidth, 0, subImageWidth, combinedImages.rows));
+					fromROI.copyTo(toROI);
 
 					cv::Mat depth;
 					if(!data.depthOrRightRaw().empty())
 					{
-						UASSERT(data.depthOrRightRaw().cols % data.cameraModels().size() == 0 &&
-								subImageWidth == data.depthOrRightRaw().cols/(int)data.cameraModels().size() &&
-								_cameraIndex*subImageWidth < data.depthOrRightRaw().cols);
-						depth = cv::Mat(data.depthOrRightRaw(),
-							    cv::Rect(_cameraIndex*subImageWidth, 0, subImageWidth, data.depthOrRightRaw().rows)).clone();
+						subImageWidth = data.depthOrRightRaw().cols/dbModels.size();
+						UASSERT(data.depthOrRightRaw().cols % dbModels.size() == 0 &&
+								subImageWidth == data.depthOrRightRaw().cols/(int)dbModels.size() &&
+								(int)_cameraIndices[i]*subImageWidth < data.depthOrRightRaw().cols);
+						if(combinedDepthImages.empty())
+						{
+							// initialize with first camera
+							combinedDepthImages = cv::Mat(data.depthOrRightRaw().rows, subImageWidth*(_cameraIndices.size()-i), data.depthOrRightRaw().type());
+						}
+						fromROI = cv::Mat(data.depthOrRightRaw(), cv::Rect(_cameraIndices[i]*subImageWidth, 0, subImageWidth, data.depthOrRightRaw().rows));
+						toROI = cv::Mat(combinedDepthImages, cv::Rect(addedCameras*subImageWidth, 0, subImageWidth, combinedDepthImages.rows));
+						fromROI.copyTo(toROI);
+
+						if(!data.depthConfidenceRaw().empty())
+						{
+							UASSERT(data.depthConfidenceRaw().size() == data.depthOrRightRaw().size());
+							if(combinedDepthConfidenceImages.empty())
+							{
+								combinedDepthConfidenceImages = cv::Mat(data.depthConfidenceRaw().rows, subImageWidth*(_cameraIndices.size()-i), data.depthConfidenceRaw().type());
+							}
+							fromROI = cv::Mat(data.depthConfidenceRaw(), cv::Rect(_cameraIndices[i]*subImageWidth, 0, subImageWidth, data.depthConfidenceRaw().rows));
+							toROI = cv::Mat(combinedDepthConfidenceImages, cv::Rect(addedCameras*subImageWidth, 0, subImageWidth, combinedDepthConfidenceImages.rows));
+							fromROI.copyTo(toROI);
+						}
 					}
-					data.setRGBDImage(image, depth, data.cameraModels().at(_cameraIndex));
+
+					if(!data.cameraModels().empty())
+					{
+						CameraModel model = data.cameraModels()[_cameraIndices[i]];
+						if(!_cameraLocalTransformOverrides.empty())
+						{
+							model.setLocalTransform(_cameraLocalTransformOverrides[i] * CameraModel::opticalRotation());
+						}
+						combinedModels.push_back(model);
+						combinedLocalTransforms.push_back(model.localTransform());
+					}
+					else
+					{
+						StereoCameraModel stereoModel = data.stereoCameraModels()[_cameraIndices[i]];
+						if(!_cameraLocalTransformOverrides.empty())
+						{
+							stereoModel.setLocalTransform(_cameraLocalTransformOverrides[i] * CameraModel::opticalRotation());
+						}
+						combinedStereoModels.push_back(stereoModel);
+						combinedLocalTransforms.push_back(stereoModel.localTransform());
+					}
+					cameraOldNewIndices.insert(std::make_pair(_cameraIndices[i], i));
+				}
+				if(!combinedModels.empty())
+				{
+					data.setRGBDImage(combinedImages, combinedDepthImages, combinedDepthConfidenceImages, combinedModels);
 				}
 				else
 				{
-					UWARN("DBReader: Camera index %d doesn't exist! Camera models = %d.", _cameraIndex, (int)data.cameraModels().size());
+					data.setStereoImage(combinedImages, combinedDepthImages, combinedStereoModels);
+				}
+			}
+			else if(!_cameraLocalTransformOverrides.empty() &&
+					_cameraLocalTransformOverrides.size() == dbModels.size())
+			{
+				// just update local transforms
+				std::vector<CameraModel> combinedModels;
+				std::vector<StereoCameraModel> combinedStereoModels;
+				for(size_t i=0; i<dbModels.size(); ++i)
+				{
+					if(!data.cameraModels().empty())
+					{
+						CameraModel model = data.cameraModels()[i];
+						model.setLocalTransform(_cameraLocalTransformOverrides[i] * CameraModel::opticalRotation());
+						combinedModels.push_back(model);
+						combinedLocalTransforms.push_back(model.localTransform());
+					}
+					else
+					{
+						StereoCameraModel stereoModel = data.stereoCameraModels()[i];
+						stereoModel.setLocalTransform(_cameraLocalTransformOverrides[i] * CameraModel::opticalRotation());
+						combinedStereoModels.push_back(stereoModel);
+						combinedLocalTransforms.push_back(stereoModel.localTransform());
+					}
+				}
+				if(!combinedModels.empty())
+				{
+					data.setCameraModels(combinedModels);
+				}
+				else
+				{
+					data.setStereoCameraModels(combinedStereoModels);
 				}
 			}
 			data.setId(seq);
@@ -605,10 +741,11 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
 			}
 			data.setLandmarks(landmarks);
 
-			UDEBUG("Laser=%d RGB/Left=%d Depth/Right=%d, Grid=%d, UserData=%d, GlobalPose=%d, GPS=%d, IMU=%d",
+			UDEBUG("Laser=%d RGB/Left=%d Depth/Right=%d, Conf=%d, Grid=%d, UserData=%d, GlobalPose=%d, GPS=%d, IMU=%d",
 					data.laserScanRaw().isEmpty()?0:1,
 					data.imageRaw().empty()?0:1,
 					data.depthOrRightRaw().empty()?0:1,
+					data.depthConfidenceRaw().empty()?0:1,
 					data.gridCellSize()==0.0f?0:1,
 					data.userDataRaw().empty()?0:1,
 					globalPose.isNull()?0:1,
@@ -623,14 +760,68 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
 			   (keypoints3D.empty() || keypoints.size() == keypoints3D.size()) &&
 			   (descriptors.empty() || (int)keypoints.size() == descriptors.rows))
 			{
-				data.setFeatures(keypoints, keypoints3D, descriptors);
+				if(!cameraOldNewIndices.empty())
+				{
+					cv::Mat newDescriptors;
+					std::vector<cv::KeyPoint> newKeypoints;
+					std::vector<cv::Point3f> newKeypoints3D;
+					UASSERT(!dbModels.empty() && dbModels[0].imageWidth()>0);
+					int subImageWidth = dbModels[0].imageWidth();
+					for(size_t i = 0; i<keypoints.size(); ++i)
+					{
+						int cameraIndex = int(keypoints.at(i).pt.x / subImageWidth);
+						UASSERT_MSG(cameraIndex >= 0 && cameraIndex < (int)dbModels.size(),
+								uFormat("cameraIndex=%d, db models=%d, kpt.x=%f, image width=%d",
+										cameraIndex, (int)dbModels.size(), keypoints[i].pt.x, subImageWidth).c_str());
+						if(cameraOldNewIndices.find(cameraIndex) != cameraOldNewIndices.end())
+						{
+							int newCameraIndex = cameraOldNewIndices.at(cameraIndex);
+							newKeypoints.push_back(keypoints[i]);
+							newKeypoints.back().pt.x += (newCameraIndex-cameraIndex)*subImageWidth;
+							if(!keypoints3D.empty())
+							{
+								cv::Point3f pt = util3d::transformPoint(keypoints3D.at(i), dbModels[cameraIndex].localTransform().inverse());
+								pt = util3d::transformPoint(pt, combinedLocalTransforms[cameraIndex]);
+								newKeypoints3D.push_back(pt);
+							}
+							if(!descriptors.empty())
+							{
+								newDescriptors.push_back(descriptors.row(i));
+							}
+						}
+					}
+					data.setFeatures(newKeypoints, newKeypoints3D, newDescriptors);
+				}
+				else if(!combinedLocalTransforms.empty())
+				{
+					// We are overriding the camera local transforms, let's move 3D words accordingly
+					UASSERT(dbModels.size() == combinedLocalTransforms.size());
+					std::vector<cv::Point3f> newKeypoints3D;
+					UASSERT(dbModels[0].imageWidth()>0);
+					int subImageWidth = dbModels[0].imageWidth();
+					for(size_t i = 0; i<keypoints3D.size(); ++i)
+					{
+						int cameraIndex = int(keypoints.at(i).pt.x / subImageWidth);
+						UASSERT_MSG(cameraIndex >= 0 && cameraIndex < (int)dbModels.size(),
+								uFormat("cameraIndex=%d, db models=%d, kpt.x=%f, image width=%d",
+										cameraIndex, (int)dbModels.size(), keypoints[i].pt.x, subImageWidth).c_str());
+						cv::Point3f pt = util3d::transformPoint(keypoints3D.at(i), dbModels[cameraIndex].localTransform().inverse());
+						pt = util3d::transformPoint(pt, combinedLocalTransforms[cameraIndex]);
+						newKeypoints3D.push_back(pt);
+					}
+					data.setFeatures(keypoints, newKeypoints3D, descriptors);
+				}
+				else
+				{
+					data.setFeatures(keypoints, keypoints3D, descriptors);
+				}
 			}
 			else if(!_featuresIgnored && !keypoints.empty() && (!keypoints3D.empty() || !descriptors.empty()))
 			{
 				UERROR("Missing feature data, features won't be published.");
 			}
 
-			if(data.imageCompressed().empty() && s->getWeight()>=0 && keypoints.empty())
+			if(data.imageRaw().empty() && data.imageCompressed().empty() && s->getWeight()>=0 && keypoints.empty())
 			{
 				UWARN("No image loaded from the database for id=%d!", seq);
 			}

corelib/src/Features2d.cpp

@@ -908,25 +908,36 @@ std::vector<cv::Point3f> Feature2D::generateKeypoints3D(
 				if(d_imageLeft.empty()) {
 					d_imageLeft = cv::cuda::GpuMat(imageLeft);
 				}
-				// convert to grayscale
+				// convert to grayscale if not already
 				if(d_imageLeft.channels() > 1) {
 					cv::cuda::GpuMat tmp;
 					cv::cuda::cvtColor(d_imageLeft, tmp, cv::COLOR_BGR2GRAY);
 					d_imageLeft = tmp;
 				}
+
 				d_imageRight = data.depthOrRightRawGpu();
 				if(d_imageRight.empty()) {
 					d_imageRight = cv::cuda::GpuMat(imageRight);
 				}
+				// convert to grayscale if not already
+				if(d_imageRight.channels() > 1) {
+					cv::cuda::GpuMat tmp;
+					cv::cuda::cvtColor(d_imageRight, tmp, cv::COLOR_BGR2GRAY);
+					d_imageRight = tmp;
+				}
 			}
 			else
 #endif
 			{
-				// convert to grayscale (right image should be already grayscale)
+				// convert to grayscale
 				if(imageLeft.channels() > 1)
 				{
 					cv::cvtColor(data.imageRaw(), imageLeft, cv::COLOR_BGR2GRAY);
 				}
+				if(imageRight.channels() > 1)
+				{
+					cv::cvtColor(data.rightRaw(), imageRight, cv::COLOR_BGR2GRAY);
+				}
 			}
 
 			std::vector<cv::Point2f> leftCorners;
@@ -1315,6 +1326,11 @@ std::vector<cv::KeyPoint> SIFT::generateKeypointsImpl(const cv::Mat & image, con
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	std::vector<cv::KeyPoint> keypoints;
 	cv::Mat imgRoi(image, roi);
+	cv::Mat maskRoi;
+	if(!mask.empty())
+	{
+		maskRoi = cv::Mat(mask, roi);
+	}
 #ifdef RTABMAP_CUDASIFT
 	if(gpu_)
 	{
@@ -1383,6 +1399,12 @@ std::vector<cv::KeyPoint> SIFT::generateKeypointsImpl(const cv::Mat & image, con
 					//std::cout << cv::Mat(1, 128*4, CV_8UC1, desc) << std::endl;
 					continue;
 				}
+				// Ignore keypoints not in the mask
+				if(!maskRoi.empty() && maskRoi.at<unsigned char>(cudaSiftData_->h_data[i].ypos, cudaSiftData_->h_data[i].xpos) == 0)
+				{
+					continue;
+				}
+
 				//Keep track of the data, to be easier to manage the data in the next step
 				hessianMap.insert(std::pair<float, int>(cudaSiftData_->h_data[i].sharpness, i));
 			}
@@ -1412,12 +1434,6 @@ std::vector<cv::KeyPoint> SIFT::generateKeypointsImpl(const cv::Mat & image, con
 	else
 #endif
 	{
-		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

corelib/src/Graph.cpp

@@ -744,9 +744,8 @@ void calcRelativeErrors (
 		// compute rotational and translational errors
 		Transform pose_delta_gt     = poses_gt[i].inverse()*poses_gt[i+1];
 		Transform pose_delta_result = poses_result[i].inverse()*poses_result[i+1];
-		Transform pose_error        = pose_delta_result.inverse()*pose_delta_gt;
-		float r_err = pose_error.getAngle();
-		float t_err = pose_error.getNorm();
+		float r_err = pose_delta_result.getAngle(pose_delta_gt);
+		float t_err = pose_delta_result.getDistance(pose_delta_gt);
 
 		// write to file
 		err.push_back(errors(i,r_err,t_err,0,0));
@@ -783,7 +782,8 @@ Transform calcRMSE (
 		float & rotational_median,
 		float & rotational_std,
 		float & rotational_min,
-		float & rotational_max)
+		float & rotational_max,
+		bool align2D)
 {
 
 	translational_rmse = 0.0f;
@@ -815,8 +815,8 @@ Transform calcRMSE (
 			{
 				idFirst = iter->first;
 			}
-			cloud1[oi] = pcl::PointXYZ(jter->second.x(), jter->second.y(), jter->second.z());
-			cloud2[oi++] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
+			cloud1[oi] = pcl::PointXYZ(jter->second.x(), jter->second.y(), align2D?0:jter->second.z());
+			cloud2[oi++] = pcl::PointXYZ(iter->second.x(), iter->second.y(), align2D?0:iter->second.z());
 		}
 	}
 
@@ -957,12 +957,24 @@ void computeMaxGraphErrors(
 				return;
 			}
 
-			Transform t = t1.inverse()*t2;
+			Transform t;
+			Transform linkT;
+			if(iter->second.from() < 0)
+			{
+				// For landmarks, compare from node to landmark, in case we optimized only marker's position
+				t = t2.inverse()*t1;
+				linkT = iter->second.transform().inverse();
+			}
+			else
+			{
+				t = t1.inverse()*t2;
+				linkT = iter->second.transform();
+			}
 
 			float linearError = uMax3(
-					fabs(iter->second.transform().x() - t.x()),
-					fabs(iter->second.transform().y() - t.y()),
-					force3DoF?0:fabs(iter->second.transform().z() - t.z()));
+					fabs(linkT.x() - t.x()),
+					fabs(linkT.y() - t.y()),
+					force3DoF?0:fabs(linkT.z() - t.z()));
 			UASSERT(iter->second.transVariance(false)>0.0);
 			float stddevLinear = sqrt(iter->second.transVariance(false));
 			float linearErrorRatio = linearError/stddevLinear;
@@ -980,15 +992,21 @@ void computeMaxGraphErrors(
 			if(iter->second.type() != Link::kLandmark ||
 				1.0 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) < 9999.0)
 			{
-				float opt_roll,opt_pitch,opt_yaw;
-				float link_roll,link_pitch,link_yaw;
-				t.getEulerAngles(opt_roll, opt_pitch, opt_yaw);
-				iter->second.transform().getEulerAngles(link_roll, link_pitch, link_yaw);
-				float angularError = uMax3(
-						force3DoF?0:fabs(opt_roll - link_roll),
-						force3DoF?0:fabs(opt_pitch - link_pitch),
-						fabs(opt_yaw - link_yaw));
-				angularError = angularError>M_PI?2*M_PI-angularError:angularError;
+				float angularError = 0.0f;
+				if(force3DoF)
+				{
+					float opt_roll,opt_pitch,opt_yaw;
+					float link_roll,link_pitch,link_yaw;
+					t.getEulerAngles(opt_roll, opt_pitch, opt_yaw);
+					linkT.getEulerAngles(link_roll, link_pitch, link_yaw);
+					angularError = fabs(opt_yaw - link_yaw);
+					angularError = angularError>M_PI?2*M_PI-angularError:angularError;
+				}
+				else
+				{
+					angularError = t.getAngle(linkT);
+				}
+
 				UASSERT(iter->second.rotVariance(false)>0.0);
 				float stddevAngular = sqrt(iter->second.rotVariance(false));
 				float angularErrorRatio = angularError/stddevAngular;

corelib/src/MarkerDetector.cpp

@@ -226,8 +226,8 @@ std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
 		float rgbToDepthFactorY = 1.0f;
 		if(!depth.empty())
 		{
-			rgbToDepthFactorX = 1.0f/(model.imageWidth()>0?model.imageWidth()/depth.cols:1);
-			rgbToDepthFactorY = 1.0f/(model.imageHeight()>0?model.imageHeight()/depth.rows:1);
+			rgbToDepthFactorX = 1.0f/(model.imageWidth()>0?float(model.imageWidth())/float(depth.cols):1.0f);
+			rgbToDepthFactorY = 1.0f/(model.imageHeight()>0?float(model.imageHeight())/float(depth.rows):1.0f);
 		}
         else if(markerLength_ == 0)
         {

corelib/src/Memory.cpp

@@ -80,6 +80,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	_notLinkedNodesKeptInDb(Parameters::defaultMemNotLinkedNodesKept()),
 	_saveIntermediateNodeData(Parameters::defaultMemIntermediateNodeDataKept()),
 	_rgbCompressionFormat(Parameters::defaultMemImageCompressionFormat()),
+	_depthCompressionFormat(Parameters::defaultMemDepthCompressionFormat()),
 	_incrementalMemory(Parameters::defaultMemIncrementalMemory()),
 	_localizationDataSaved(Parameters::defaultMemLocalizationDataSaved()),
 	_reduceGraph(Parameters::defaultMemReduceGraph()),
@@ -91,6 +92,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	_badSignaturesIgnored(Parameters::defaultMemBadSignaturesIgnored()),
 	_mapLabelsAdded(Parameters::defaultMemMapLabelsAdded()),
 	_depthAsMask(Parameters::defaultMemDepthAsMask()),
+	_maskFloorThreshold(Parameters::defaultMemDepthMaskFloorThr()),
 	_stereoFromMotion(Parameters::defaultMemStereoFromMotion()),
     _imagePreDecimation(Parameters::defaultMemImagePreDecimation()),
 	_imagePostDecimation(Parameters::defaultMemImagePostDecimation()),
@@ -118,6 +120,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	_detectMarkers(Parameters::defaultRGBDMarkerDetection()),
 	_markerLinVariance(Parameters::defaultMarkerVarianceLinear()),
 	_markerAngVariance(Parameters::defaultMarkerVarianceAngular()),
+	_markerOrientationIgnored(Parameters::defaultMarkerVarianceOrientationIgnored()),
 	_idCount(kIdStart),
 	_idMapCount(kIdStart),
 	_lastSignature(0),
@@ -567,6 +570,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(params, Parameters::kMemNotLinkedNodesKept(), _notLinkedNodesKeptInDb);
 	Parameters::parse(params, Parameters::kMemIntermediateNodeDataKept(), _saveIntermediateNodeData);
 	Parameters::parse(params, Parameters::kMemImageCompressionFormat(), _rgbCompressionFormat);
+	Parameters::parse(params, Parameters::kMemDepthCompressionFormat(), _depthCompressionFormat);
 	Parameters::parse(params, Parameters::kMemRehearsalIdUpdatedToNewOne(), _idUpdatedToNewOneRehearsal);
 	Parameters::parse(params, Parameters::kMemGenerateIds(), _generateIds);
 	Parameters::parse(params, Parameters::kMemBadSignaturesIgnored(), _badSignaturesIgnored);
@@ -576,6 +580,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(params, Parameters::kMemTransferSortingByWeightId(), _transferSortingByWeightId);
 	Parameters::parse(params, Parameters::kMemSTMSize(), _maxStMemSize);
 	Parameters::parse(params, Parameters::kMemDepthAsMask(), _depthAsMask);
+	Parameters::parse(params, Parameters::kMemDepthMaskFloorThr(), _maskFloorThreshold);
 	Parameters::parse(params, Parameters::kMemStereoFromMotion(), _stereoFromMotion);
 	Parameters::parse(params, Parameters::kMemImagePreDecimation(), _imagePreDecimation);
 	Parameters::parse(params, Parameters::kMemImagePostDecimation(), _imagePostDecimation);
@@ -611,8 +616,24 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(params, Parameters::kRGBDMarkerDetection(), _detectMarkers);
 	Parameters::parse(params, Parameters::kMarkerVarianceLinear(), _markerLinVariance);
 	Parameters::parse(params, Parameters::kMarkerVarianceAngular(), _markerAngVariance);
+	Parameters::parse(params, Parameters::kMarkerVarianceOrientationIgnored(), _markerOrientationIgnored);
 	Parameters::parse(params, Parameters::kMemLocalizationDataSaved(), _localizationDataSaved);
 
+	if(_markerAngVariance>=9999)
+	{
+		UWARN("Using directly %s>=9999 to ignore marker orientation is deprecated. Use %s instead and "
+			  "read correctly the description of the new parameter. We will enable %s and set %s to "
+			  "same value than %s (%f) for backward compatibility.", 
+			Parameters::kMarkerVarianceAngular().c_str(),
+			Parameters::kMarkerVarianceOrientationIgnored().c_str(),
+			Parameters::kMarkerVarianceOrientationIgnored().c_str(),
+			Parameters::kMarkerVarianceAngular().c_str(),
+			Parameters::kMarkerVarianceLinear().c_str(),
+			_markerLinVariance);
+			_markerAngVariance = _markerLinVariance;
+			_markerOrientationIgnored = true;
+	}
+
 	UASSERT_MSG(_maxStMemSize >= 0, uFormat("value=%d", _maxStMemSize).c_str());
 	UASSERT_MSG(_similarityThreshold >= 0.0f && _similarityThreshold <= 1.0f, uFormat("value=%f", _similarityThreshold).c_str());
 	UASSERT_MSG(_recentWmRatio >= 0.0f && _recentWmRatio <= 1.0f, uFormat("value=%f", _recentWmRatio).c_str());
@@ -4515,17 +4536,55 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 {
 	UDEBUG("");
 	SensorData data = inputData;
+
+	bool isIntermediateNode = data.id() < 0;
+
+	// uncompress data if needed
+	
+	if(!isIntermediateNode)
+	{
+		// We need raw images if we need to extract features and/or do tag detection
+		bool needRawImages = _feature2D->getMaxFeatures() >= 0 && 
+			(!_useOdometryFeatures ||
+			 data.keypoints().empty() ||
+			 (int)data.keypoints().size() != data.descriptors().rows ||
+			 data.descriptors().empty() ||
+			 _detectMarkers ||
+			 _rotateImagesUpsideUp ||
+			 _imagePostDecimation > 1 ||
+			 (_createOccupancyGrid && _localMapMaker->isGridFromDepth()));
+
+		// Note: we could avoid uncompressing scan if we don't do any filtering
+		// and if we don't use it for local occupancy grid
+		bool needRawScan = true;
+
+		if( (needRawImages && data.imageRaw().empty() && !data.imageCompressed().empty()) ||
+			(needRawImages && data.depthOrRightRaw().empty() && !data.depthOrRightCompressed().empty()) ||
+			(needRawScan && data.laserScanRaw().empty() && !data.laserScanCompressed().empty()))
+		{
+			cv::Mat left, right;
+			LaserScan laserScan;
+			UDEBUG("Uncompressing data...");
+			data.uncompressData(
+				needRawImages && data.imageRaw().empty() && !data.imageCompressed().empty() ? &left : 0,
+				needRawImages && data.depthOrRightRaw().empty() && !data.depthOrRightCompressed().empty() ? &right : 0,
+				needRawScan && data.laserScanRaw().empty() && !data.laserScanCompressed().empty() ? &laserScan : 0);
+			UDEBUG("Uncompressing data...done!");
+		}
+	}
+
 	UASSERT(data.imageRaw().empty() ||
 			data.imageRaw().type() == CV_8UC1 ||
 			data.imageRaw().type() == CV_8UC3);
 	UASSERT_MSG(data.depthOrRightRaw().empty() ||
 			(  ( data.depthOrRightRaw().type() == CV_16UC1 ||
 				 data.depthOrRightRaw().type() == CV_32FC1 ||
-				 data.depthOrRightRaw().type() == CV_8UC1)
+				 data.depthOrRightRaw().type() == CV_8UC1 ||
+				 data.depthOrRightRaw().type() == CV_8UC3)
 			   &&
-				( (data.imageRaw().empty() && data.depthOrRightRaw().type() != CV_8UC1) ||
+				( (data.imageRaw().empty() && !(data.depthOrRightRaw().type() == CV_8UC1 || data.depthOrRightRaw().type() == CV_8UC3)) ||
 				  (data.depthOrRightRaw().rows <= data.imageRaw().rows && data.depthOrRightRaw().cols <= data.imageRaw().cols))),
-				uFormat("image=(%d/%d, type=%d, [accepted=%d,%d]) depth=(%d/%d, type=%d [accepted=%d(depth mm),%d(depth m),%d(stereo)]). "
+				uFormat("image=(%d/%d, type=%d, [accepted=%d,%d]) depth=(%d/%d, type=%d [accepted=%d(depth mm),%d(depth m),%d-%d(stereo)]). "
 						"For stereo, left and right images should be same size. "
 						"For RGB-D, depth can be X times smaller than RGB (where X is an integer).",
 						data.imageRaw().cols,
@@ -4536,7 +4595,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 						data.depthOrRightRaw().cols,
 						data.depthOrRightRaw().rows,
 						data.depthOrRightRaw().type(),
-						CV_16UC1, CV_32FC1, CV_8UC1).c_str());
+						CV_16UC1, CV_32FC1, CV_8UC1, CV_8UC3).c_str());
 
 	if(!data.depthOrRightRaw().empty() &&
 		data.cameraModels().empty() &&
@@ -4555,7 +4614,6 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 	float t;
 	std::vector<cv::KeyPoint> keypoints;
 	cv::Mat descriptors;
-	bool isIntermediateNode = data.id() < 0;
 	int id = data.id();
 	if(_generateIds)
 	{
@@ -4835,9 +4893,10 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 					else
 					{
 						decimationDepth = (int)ceil(float(data.depthRaw().rows) / float(targetSize));
+						UASSERT(data.depthConfidenceRaw().empty() || data.depthConfidenceRaw().size() == data.depthRaw().size());
 					}
 				}
-				UDEBUG("decimation rgbOrLeft(rows=%d)=%d, depthOrRight(rows=%d)=%d", data.imageRaw().rows, _imagePreDecimation, data.depthOrRightRaw().rows, decimationDepth);
+				UDEBUG("decimation rgbOrLeft(rows=%d)=%d, depthOrRight(rows=%d)=%d (conf? %d)", data.imageRaw().rows, _imagePreDecimation, data.depthOrRightRaw().rows, decimationDepth, data.depthConfidenceRaw().empty()?0:1);
 
 				std::vector<CameraModel> cameraModels = decimatedData.cameraModels();
 				for(unsigned int i=0; i<cameraModels.size(); ++i)
@@ -4849,6 +4908,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 					decimatedData.setRGBDImage(
 							util2d::decimate(decimatedData.imageRaw(), _imagePreDecimation),
 							util2d::decimate(decimatedData.depthOrRightRaw(), decimationDepth),
+							util2d::decimate(decimatedData.depthConfidenceRaw(), decimationDepth),
 							cameraModels);
 				}
 
@@ -4884,7 +4944,26 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 					imageMono.cols % decimatedData.depthRaw().cols == 0 &&
 					imageMono.rows/decimatedData.depthRaw().rows == imageMono.cols/decimatedData.depthRaw().cols)
 				{
-					depthMask = util2d::interpolate(decimatedData.depthRaw(), imageMono.rows/decimatedData.depthRaw().rows, 0.1f);
+					depthMask = decimatedData.depthRaw();
+
+					if(_maskFloorThreshold != 0.0f)
+					{
+						UASSERT(!decimatedData.cameraModels().empty());
+						UDEBUG("Masking floor (threshold=%f)", _maskFloorThreshold);
+						if(_maskFloorThreshold<0.0f)
+						{
+							cv::Mat depthBelow;
+							util3d::filterFloor(depthMask, decimatedData.cameraModels(), _maskFloorThreshold*-1.0f, &depthBelow);
+							depthMask = depthBelow;
+						}
+						else
+						{
+							depthMask = util3d::filterFloor(depthMask, decimatedData.cameraModels(), _maskFloorThreshold);
+						}
+						UDEBUG("Masking floor done.");
+					}
+
+					depthMask = util2d::interpolate(depthMask, imageMono.rows/depthMask.rows, 0.1f);
 				}
 				else
 				{
@@ -5533,8 +5612,32 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 						continue;
 					}
 					cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
-					covariance(cv::Range(0,3), cv::Range(0,3)) *= _markerLinVariance;
-					covariance(cv::Range(3,6), cv::Range(3,6)) *= _markerAngVariance;
+					if(_markerOrientationIgnored)
+					{
+						covariance(cv::Range(3,6), cv::Range(3,6)) *= 9999; // disable orientation estimation
+						bool isGTSAM = uStr2Int(uValue(parameters_, Parameters::kOptimizerStrategy(), uNumber2Str(Parameters::defaultOptimizerStrategy()))) == Optimizer::kTypeGTSAM;
+						if(!isGTSAM)
+						{
+							covariance(cv::Range(0,3), cv::Range(0,3)) *= _markerLinVariance;
+						}
+						else if(_registrationPipeline->force3DoF())
+						{
+							// Bearing/Range in 2D, set X as bearing and Y as range (see OptimizerGTSAM)
+							covariance(cv::Range(0,1), cv::Range(0,1)) *= _markerAngVariance;
+							covariance(cv::Range(1,3), cv::Range(1,3)) *= _markerLinVariance;
+						}
+						else
+						{
+							// Bearing/Range in 3D, set X and Y as bearing and Z as range (see OptimizerGTSAM)
+							covariance(cv::Range(0,2), cv::Range(0,2)) *= _markerAngVariance;
+							covariance(cv::Range(2,3), cv::Range(2,3)) *= _markerLinVariance;
+						}
+					}
+					else
+					{
+						covariance(cv::Range(0,3), cv::Range(0,3)) *= _markerLinVariance;
+						covariance(cv::Range(3,6), cv::Range(3,6)) *= _markerAngVariance;
+					}
 					landmarks.insert(std::make_pair(iter->first, Landmark(iter->first, iter->second.length(), iter->second.pose(), covariance)));
 				}
 				UDEBUG("Markers detected = %d", (int)markers.size());
@@ -5555,6 +5658,8 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 
 	cv::Mat image = data.imageRaw();
 	cv::Mat depthOrRightImage = data.depthOrRightRaw();
+	cv::Mat depthConfidence = data.depthConfidenceRaw();
+
 	std::vector<CameraModel> cameraModels = data.cameraModels();
 	std::vector<StereoCameraModel> stereoCameraModels = data.stereoCameraModels();
 
@@ -5565,6 +5670,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		{
 			image = decimatedData.imageRaw();
 			depthOrRightImage = decimatedData.depthOrRightRaw();
+			depthConfidence = decimatedData.depthConfidenceRaw();
 			cameraModels = decimatedData.cameraModels();
 			stereoCameraModels = decimatedData.stereoCameraModels();
 		}
@@ -5731,8 +5837,8 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		}
 	}
 
-	// Filter the laser scan?
 	LaserScan laserScan = data.laserScanRaw();
+	// Filter the laser scan?
 	if(!isIntermediateNode && laserScan.size())
 	{
 		if(laserScan.rangeMax() == 0.0f)
@@ -5778,29 +5884,64 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 	Signature * s;
 	if(this->isBinDataKept() && (!isIntermediateNode || _saveIntermediateNodeData))
 	{
-		UDEBUG("Bin data kept: rgb=%d, depth=%d, scan=%d, userData=%d",
+		UDEBUG("Bin data kept: rgb=%d, depth=%d, conf=%d, scan=%d, userData=%d",
 				image.empty()?0:1,
 				depthOrRightImage.empty()?0:1,
+				depthConfidence.empty()?0:1,
 				laserScan.isEmpty()?0:1,
 				data.userDataRaw().empty()?0:1);
 
 		std::vector<unsigned char> imageBytes;
 		std::vector<unsigned char> depthBytes;
 
-		if(_saveDepth16Format && !depthOrRightImage.empty() && depthOrRightImage.type() == CV_32FC1)
+		if(!depthOrRightImage.empty() && depthOrRightImage.type() == CV_32FC1)
 		{
-			UWARN("Save depth data to 16 bits format: depth type detected is 32FC1, use 16UC1 depth format to avoid this conversion (or set parameter \"Mem/SaveDepth16Format\"=false to use 32bits format).");
-			depthOrRightImage = util2d::cvtDepthFromFloat(depthOrRightImage);
+			if(_saveDepth16Format)
+			{
+				static bool warned = false;
+				if(!warned)
+				{
+					UWARN("Converting depth data to 16 bits format because depth type detected is 32FC1, "
+					      "feed 16UC1 depth format directly to avoid this conversion (or set parameter %s=false "
+						  "to save 32bits format). This warning is only printed once.",
+						Parameters::kMemSaveDepth16Format().c_str());
+					warned = true;
+				}
+				depthOrRightImage = util2d::cvtDepthFromFloat(depthOrRightImage);
+			}
+			else if(_depthCompressionFormat == ".rvl")
+			{
+				static bool warned = false;
+				if(!warned)
+				{
+					UWARN("%s is set to false to use 32bits format but this is not "
+					 	  "compatible with the compressed depth format chosen (%s=\"%s\"), depth "
+						  "images will be compressed in \".png\" format instead. Explicitly "
+						  "set %s to true to keep using \"%s\" format and images will be "
+						  "converted to 16bits for convenience (warning: that would "
+						  "remove all depth values over 65 meters). Explicitly set %s=\".png\" "
+						  "to suppress this warning. This warning is only printed once.",
+						Parameters::kMemSaveDepth16Format().c_str(),
+						Parameters::kMemDepthCompressionFormat().c_str(),
+						_depthCompressionFormat.c_str(),
+						Parameters::kMemSaveDepth16Format().c_str(),
+						_depthCompressionFormat.c_str(),
+						Parameters::kMemDepthCompressionFormat().c_str());
+					warned = true;
+				}
+			}
 		}
 
 		cv::Mat compressedImage;
 		cv::Mat compressedDepth;
+		cv::Mat compressedDepthConfidence;
 		cv::Mat compressedScan;
 		cv::Mat compressedUserData;
 		if(_compressionParallelized)
 		{
 			rtabmap::CompressionThread ctImage(image, _rgbCompressionFormat);
-			rtabmap::CompressionThread ctDepth(depthOrRightImage, depthOrRightImage.type() == CV_32FC1 || depthOrRightImage.type() == CV_16UC1?std::string(".png"):_rgbCompressionFormat);
+			rtabmap::CompressionThread ctDepth(depthOrRightImage, depthOrRightImage.type() == CV_32FC1 || depthOrRightImage.type() == CV_16UC1?_depthCompressionFormat:_rgbCompressionFormat);
+			rtabmap::CompressionThread ctDepthConfidence(depthConfidence);
 			rtabmap::CompressionThread ctLaserScan(laserScan.data());
 			rtabmap::CompressionThread ctUserData(data.userDataRaw());
 			if(!image.empty())
@@ -5811,6 +5952,10 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			{
 				ctDepth.start();
 			}
+			if(!depthConfidence.empty())
+			{
+				ctDepthConfidence.start();
+			}
 			if(!laserScan.isEmpty())
 			{
 				ctLaserScan.start();
@@ -5821,18 +5966,21 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			}
 			ctImage.join();
 			ctDepth.join();
+			ctDepthConfidence.join();
 			ctLaserScan.join();
 			ctUserData.join();
 
 			compressedImage = ctImage.getCompressedData();
 			compressedDepth = ctDepth.getCompressedData();
+			compressedDepthConfidence = ctDepthConfidence.getCompressedData();
 			compressedScan = ctLaserScan.getCompressedData();
 			compressedUserData = ctUserData.getCompressedData();
 		}
 		else
 		{
 			compressedImage = compressImage2(image, _rgbCompressionFormat);
-			compressedDepth = compressImage2(depthOrRightImage, depthOrRightImage.type() == CV_32FC1 || depthOrRightImage.type() == CV_16UC1?std::string(".png"):_rgbCompressionFormat);
+			compressedDepth = compressImage2(depthOrRightImage, depthOrRightImage.type() == CV_32FC1 || depthOrRightImage.type() == CV_16UC1?_depthCompressionFormat:_rgbCompressionFormat);
+			compressedDepthConfidence = compressData2(depthConfidence);
 			compressedScan = compressData2(laserScan.data());
 			compressedUserData = compressData2(data.userDataRaw());
 		}
@@ -5847,12 +5995,12 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			!stereoCameraModels.empty()?
 				SensorData(
 						laserScan.angleIncrement() == 0.0f?
-							LaserScan(compressedScan,
+							LaserScan(compressedScan.empty()?data.laserScanCompressed().data():compressedScan,
 								laserScan.maxPoints(),
 								laserScan.rangeMax(),
 								laserScan.format(),
 								laserScan.localTransform()):
-							LaserScan(compressedScan,
+							LaserScan(compressedScan.empty()?data.laserScanCompressed().data():compressedScan,
 								laserScan.format(),
 								laserScan.rangeMin(),
 								laserScan.rangeMax(),
@@ -5860,20 +6008,20 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 								laserScan.angleMax(),
 								laserScan.angleIncrement(),
 								laserScan.localTransform()),
-						compressedImage,
-						compressedDepth,
+						compressedImage.empty()?data.imageCompressed():compressedImage,
+						compressedDepth.empty()?data.depthOrRightCompressed():compressedDepth,
 						stereoCameraModels,
 						id,
 						0,
 						compressedUserData):
 				SensorData(
 						laserScan.angleIncrement() == 0.0f?
-							LaserScan(compressedScan,
+							LaserScan(compressedScan.empty()?data.laserScanCompressed().data():compressedScan,
 								laserScan.maxPoints(),
 								laserScan.rangeMax(),
 								laserScan.format(),
 								laserScan.localTransform()):
-							LaserScan(compressedScan,
+							LaserScan(compressedScan.empty()?data.laserScanCompressed().data():compressedScan,
 								laserScan.format(),
 								laserScan.rangeMin(),
 								laserScan.rangeMax(),
@@ -5881,8 +6029,9 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 								laserScan.angleMax(),
 								laserScan.angleIncrement(),
 								laserScan.localTransform()),
-						compressedImage,
-						compressedDepth,
+						compressedImage.empty()?data.imageCompressed():compressedImage,
+						compressedDepth.empty()?data.depthOrRightCompressed():compressedDepth,
+						compressedDepthConfidence.empty()?data.depthConfidenceCompressed():compressedDepthConfidence,
 						cameraModels,
 						id,
 						0,
@@ -5931,12 +6080,12 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			!stereoCameraModels.empty()?
 				SensorData(
 						laserScan.angleIncrement() == 0.0f?
-								LaserScan(compressedScan,
+								LaserScan(compressedScan.empty()?data.laserScanCompressed().data():compressedScan,
 									laserScan.maxPoints(),
 									laserScan.rangeMax(),
 									laserScan.format(),
 									laserScan.localTransform()):
-								LaserScan(compressedScan,
+								LaserScan(compressedScan.empty()?data.laserScanCompressed().data():compressedScan,
 									laserScan.format(),
 									laserScan.rangeMin(),
 									laserScan.rangeMax(),
@@ -5952,12 +6101,12 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 						compressedUserData):
 				SensorData(
 						laserScan.angleIncrement() == 0.0f?
-								LaserScan(compressedScan,
+								LaserScan(compressedScan.empty()?data.laserScanCompressed().data():compressedScan,
 									laserScan.maxPoints(),
 									laserScan.rangeMax(),
 									laserScan.format(),
 									laserScan.localTransform()):
-								LaserScan(compressedScan,
+								LaserScan(compressedScan.empty()?data.laserScanCompressed().data():compressedScan,
 									laserScan.format(),
 									laserScan.rangeMin(),
 									laserScan.rangeMax(),
@@ -5980,7 +6129,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 	// set raw data
 	if(!cameraModels.empty())
 	{
-		s->sensorData().setRGBDImage(image, depthOrRightImage, cameraModels, false);
+		s->sensorData().setRGBDImage(image, depthOrRightImage, depthConfidence, cameraModels, false);
 	}
 	else
 	{
@@ -6077,13 +6226,13 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 				UINFO("Added GPS origin: long=%f lat=%f alt=%f bearing=%f error=%f", data.gps().longitude(), data.gps().latitude(), data.gps().altitude(), data.gps().bearing(), data.gps().error());
 			}
 			cv::Point3f pt = data.gps().toGeodeticCoords().toENU_WGS84(_gpsOrigin.toGeodeticCoords());
-			Transform gpsPose(pt.x, pt.y, pose.z(), 0, 0, -(data.gps().bearing()-90.0)*M_PI/180.0);
+			Transform gpsPose(pt.x, pt.y, data.gps().altitude(), 0, 0, -(data.gps().bearing()-90.0)*M_PI/180.0);
 			cv::Mat gpsInfMatrix = cv::Mat::eye(6,6,CV_64FC1)/9999.0; // variance not used >= 9999
 
 			UDEBUG("Added GPS prior: x=%f y=%f z=%f yaw=%f", gpsPose.x(), gpsPose.y(), gpsPose.z(), gpsPose.theta());
-			// only set x, y as we don't know variance for other degrees of freedom.
+			// only set x, y, z as we don't know variance for other degrees of freedom.
 			gpsInfMatrix.at<double>(0,0) = gpsInfMatrix.at<double>(1,1) = 1.0/data.gps().error();
-			gpsInfMatrix.at<double>(2,2) = 1; // z variance is set to avoid issues with g2o and gtsam requiring a prior on Z
+			gpsInfMatrix.at<double>(2,2) = data.gps().error()>1.0?1.0/data.gps().error():1.0;
 			s->addLink(Link(s->id(), s->id(), Link::kPosePrior, gpsPose, gpsInfMatrix));
 		}
 		else

corelib/src/Odometry.cpp

@@ -337,6 +337,15 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 		}
 	}
 
+	if((data.imageRaw().empty() && !data.imageCompressed().empty()) ||
+	   (data.depthOrRightRaw().empty() && !data.depthOrRightCompressed().empty()) ||
+	   (data.laserScanRaw().empty() && !data.laserScanCompressed().empty()))
+	{
+		UDEBUG("Received compressed data, uncompressing...");
+		data.uncompressData();
+		UDEBUG("Received compressed data, uncompressing...done!");
+	}
+
 	if(!data.imageRaw().empty())
 	{
 		UDEBUG("Processing image data %dx%d: rgbd models=%ld, stereo models=%ld",

corelib/src/OdometryInfo.cpp

@@ -0,0 +1,219 @@
+/*
+Copyright (c) 2010-2025, 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.
+*/
+
+#include "rtabmap/core/OdometryInfo.h"
+#include <rtabmap/utilite/UConversion.h>
+
+namespace rtabmap {
+
+OdometryInfo::OdometryInfo() :
+	lost(true),
+	features(0),
+	localMapSize(0),
+	localScanMapSize(0),
+	localKeyFrames(0),
+	localBundleOutliers(0),
+	localBundleConstraints(0),
+	localBundleTime(0),
+	localBundleAvgInlierDistance(0.0f),
+	localBundleMaxKeyFramesForInlier(0),
+	keyFrameAdded(false),
+	timeDeskewing(0.0f),
+	timeEstimation(0.0f),
+	timeParticleFiltering(0.0f),
+	stamp(0),
+	interval(0),
+	distanceTravelled(0.0f),
+	memoryUsage(0),
+	gravityRollError(0.0),
+	gravityPitchError(0.0),
+	type(0)
+{}
+
+OdometryInfo OdometryInfo::copyWithoutData() const
+{
+	OdometryInfo output;
+	output.lost = lost;
+	output.reg = reg.copyWithoutData();
+	output.features = features;
+	output.localMapSize = localMapSize;
+	output.localScanMapSize = localScanMapSize;
+	output.localKeyFrames = localKeyFrames;
+	output.localBundleOutliers = localBundleOutliers;
+	output.localBundleConstraints = localBundleConstraints;
+	output.localBundleTime = localBundleTime;
+	output.localBundlePoses = localBundlePoses;
+	output.localBundleModels = localBundleModels;
+	output.localBundleAvgInlierDistance = localBundleAvgInlierDistance;
+	output.localBundleMaxKeyFramesForInlier = localBundleMaxKeyFramesForInlier;
+	output.localBundleOutliersPerCam = localBundleOutliersPerCam;
+	output.keyFrameAdded = keyFrameAdded;
+	output.timeDeskewing = timeDeskewing;
+	output.timeEstimation = timeEstimation;
+	output.timeParticleFiltering = timeParticleFiltering;
+	output.stamp = stamp;
+	output.interval = interval;
+	output.transform = transform;
+	output.transformFiltered = transformFiltered;
+	output.transformGroundTruth = transformGroundTruth;
+	output.guessVelocity = guessVelocity;
+	output.guess = guess;
+	output.distanceTravelled = distanceTravelled;
+	output.memoryUsage = memoryUsage;
+	output.gravityRollError = gravityRollError;
+	output.gravityPitchError = gravityPitchError;
+	output.type = type;
+	return output;
+}
+
+std::map<std::string, float> OdometryInfo::statistics(const Transform & pose)
+{
+	std::map<std::string, float> stats;
+
+	stats.insert(std::make_pair("Odometry/TimeRegistration/ms", reg.totalTime*1000.0f));
+	stats.insert(std::make_pair("Odometry/RAM_usage/MB", memoryUsage));
+
+	// Based on rtabmap/MainWindow.cpp
+	stats.insert(std::make_pair("Odometry/Features/", features));
+	stats.insert(std::make_pair("Odometry/Matches/", reg.matches));
+	stats.insert(std::make_pair("Odometry/MatchesRatio/", features<=0?0.0f:float(reg.inliers)/float(features)));
+	stats.insert(std::make_pair("Odometry/Inliers/", reg.inliers));
+	stats.insert(std::make_pair("Odometry/InliersMeanDistance/m", reg.inliersMeanDistance));
+	stats.insert(std::make_pair("Odometry/InliersDistribution/", reg.inliersDistribution));
+	stats.insert(std::make_pair("Odometry/InliersRatio/", reg.inliers));
+	for(size_t i=0; i<reg.matchesPerCam.size(); ++i)
+	{
+		stats.insert(std::make_pair(uFormat("Odometry/matchesCam%ld/", i), reg.matchesPerCam[i]));
+	}
+	for(size_t i=0; i<reg.inliersPerCam.size(); ++i)
+	{
+		stats.insert(std::make_pair(uFormat("Odometry/inliersCam%ld/", i), reg.inliersPerCam[i]));
+	}
+	if(reg.matchesPerCam.size() == reg.inliersPerCam.size())
+	{
+		for(size_t i=0; i<reg.matchesPerCam.size(); ++i)
+		{
+			stats.insert(std::make_pair(uFormat("Odometry/inliersRatioCam%ld/", i), reg.matchesPerCam[i]>0 ? (float)reg.inliersPerCam[i] / (float)reg.matchesPerCam[i] : 0.0f));
+		}
+	}
+	stats.insert(std::make_pair("Odometry/ICPInliersRatio/", reg.icpInliersRatio));
+	stats.insert(std::make_pair("Odometry/ICPRotation/rad", reg.icpRotation));
+	stats.insert(std::make_pair("Odometry/ICPTranslation/m", reg.icpTranslation));
+	stats.insert(std::make_pair("Odometry/ICPStructuralComplexity/", reg.icpStructuralComplexity));
+	stats.insert(std::make_pair("Odometry/ICPStructuralDistribution/", reg.icpStructuralDistribution));
+	stats.insert(std::make_pair("Odometry/ICPCorrespondences/", reg.icpCorrespondences));
+	stats.insert(std::make_pair("Odometry/StdDevLin/", sqrt((float)reg.covariance.at<double>(0,0))));
+	stats.insert(std::make_pair("Odometry/StdDevAng/", sqrt((float)reg.covariance.at<double>(5,5))));
+	stats.insert(std::make_pair("Odometry/VarianceLin/", (float)reg.covariance.at<double>(0,0)));
+	stats.insert(std::make_pair("Odometry/VarianceAng/", (float)reg.covariance.at<double>(5,5)));
+	stats.insert(std::make_pair("Odometry/TimeEstimation/ms", timeEstimation*1000.0f));
+	stats.insert(std::make_pair("Odometry/TimeFiltering/ms", timeParticleFiltering*1000.0f));
+	stats.insert(std::make_pair("Odometry/LocalMapSize/", localMapSize));
+	stats.insert(std::make_pair("Odometry/LocalScanMapSize/", localScanMapSize));
+	stats.insert(std::make_pair("Odometry/LocalKeyFrames/", localKeyFrames));
+	stats.insert(std::make_pair("Odometry/LocalBundleOutliers/", localBundleOutliers));
+	stats.insert(std::make_pair("Odometry/LocalBundleConstraints/", localBundleConstraints));
+	stats.insert(std::make_pair("Odometry/LocalBundleTime/ms", localBundleTime*1000.0f));
+	stats.insert(std::make_pair("Odometry/localBundleAvgInlierDistance/pix", localBundleAvgInlierDistance));
+	stats.insert(std::make_pair("Odometry/localBundleMaxKeyFramesForInlier/", localBundleMaxKeyFramesForInlier));
+	for(size_t i=0; i<localBundleOutliersPerCam.size(); ++i)
+	{
+		stats.insert(std::make_pair(uFormat("Odometry/localBundleOutliersCam%ld/", i), localBundleOutliersPerCam[i]));
+	}
+	stats.insert(std::make_pair("Odometry/KeyFrameAdded/", keyFrameAdded?1.0f:0.0f));
+	stats.insert(std::make_pair("Odometry/Interval/ms", (float)interval));
+	stats.insert(std::make_pair("Odometry/Distance/m", distanceTravelled));
+
+	float x,y,z,roll,pitch,yaw;
+	if(!pose.isNull())
+	{
+		pose.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+		stats.insert(std::make_pair("Odometry/Px/m", x));
+		stats.insert(std::make_pair("Odometry/Py/m", y));
+		stats.insert(std::make_pair("Odometry/Pz/m", z));
+		stats.insert(std::make_pair("Odometry/Proll/deg", roll*180.0/CV_PI));
+		stats.insert(std::make_pair("Odometry/Ppitch/deg", pitch*180.0/CV_PI));
+		stats.insert(std::make_pair("Odometry/Pyaw/deg", yaw*180.0/CV_PI));
+	}
+
+	float dist = 0.0f, speed=0.0f;
+	if(!transform.isNull())
+	{
+		transform.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+		dist = transform.getNorm();
+		stats.insert(std::make_pair("Odometry/T/m", dist));
+		stats.insert(std::make_pair("Odometry/Tx/m", x));
+		stats.insert(std::make_pair("Odometry/Ty/m", y));
+		stats.insert(std::make_pair("Odometry/Tz/m", z));
+		stats.insert(std::make_pair("Odometry/Troll/deg", roll*180.0/CV_PI));
+		stats.insert(std::make_pair("Odometry/Tpitch/deg", pitch*180.0/CV_PI));
+		stats.insert(std::make_pair("Odometry/Tyaw/deg", yaw*180.0/CV_PI));
+
+		if(interval>0.0)
+		{
+			speed = dist/interval;
+			stats.insert(std::make_pair("Odometry/Speed/kph", speed*3.6));
+			stats.insert(std::make_pair("Odometry/Speed/mph", speed*2.237));
+			stats.insert(std::make_pair("Odometry/Speed/mps", speed));
+			stats.insert(std::make_pair("Odometry/Vx/mps", x/interval));
+			stats.insert(std::make_pair("Odometry/Vy/mps", y/interval));
+			stats.insert(std::make_pair("Odometry/Vz/mps", z/interval));
+			stats.insert(std::make_pair("Odometry/Vroll/degps", (roll*180.0/CV_PI)/interval));
+			stats.insert(std::make_pair("Odometry/Vpitch/degps", (pitch*180.0/CV_PI)/interval));
+			stats.insert(std::make_pair("Odometry/Vyaw/degps", (yaw*180.0/CV_PI)/interval));
+		}
+	}
+	if(!transformGroundTruth.isNull())
+	{
+		if(!transform.isNull())
+		{
+			stats.insert(std::make_pair("Odometry/TG_error_lin/m", transformGroundTruth.getDistance(transform)));
+			stats.insert(std::make_pair("Odometry/TG_error_ang/deg", transformGroundTruth.getAngle(transform)*180.0/CV_PI));
+		}
+
+		transformGroundTruth.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+		dist = transformGroundTruth.getNorm();
+		stats.insert(std::make_pair("Odometry/TG/m", dist));
+		stats.insert(std::make_pair("Odometry/TGx/m", x));
+		stats.insert(std::make_pair("Odometry/TGy/m", y));
+		stats.insert(std::make_pair("Odometry/TGz/m", z));
+		stats.insert(std::make_pair("Odometry/TGroll/deg", roll*180.0/CV_PI));
+		stats.insert(std::make_pair("Odometry/TGpitch/deg", pitch*180.0/CV_PI));
+		stats.insert(std::make_pair("Odometry/TGyaw/deg", yaw*180.0/CV_PI));
+
+		if(interval>0.0)
+		{
+			speed = dist/interval;
+			stats.insert(std::make_pair("Odometry/SpeedG/kph", speed*3.6));
+			stats.insert(std::make_pair("Odometry/SpeedG/mph", speed*2.237));
+			stats.insert(std::make_pair("Odometry/SpeedG/mps", speed));
+		}
+	}
+	return stats;
+}
+
+} // namespace rtabmap

corelib/src/OdometryThread.cpp

@@ -155,7 +155,11 @@ void OdometryThread::addData(const SensorData & data)
 	bool notify = true;
 	_dataMutex.lock();
 	{
-		if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty() || data.imu().empty())
+		if( !data.imageRaw().empty() ||
+			!data.imageCompressed().empty() ||
+			!data.laserScanRaw().isEmpty() ||
+			!data.laserScanCompressed().empty() || 
+			data.imu().empty())
 		{
 			_dataBuffer.push_back(data);
 			while(_dataBufferMaxSize > 0 && _dataBuffer.size() > _dataBufferMaxSize)

corelib/src/Optimizer.cpp

@@ -447,7 +447,8 @@ std::map<int, Transform> Optimizer::optimizeBA(
 		const std::map<int, Signature> & signatures,
 		std::map<int, cv::Point3f> & points3DMap,
 		std::map<int, std::map<int, FeatureBA> > & wordReferences,
-		bool rematchFeatures)
+		bool rematchFeatures,
+		const ParametersMap & registrationParameters)
 {
 	UDEBUG("");
 	std::map<int, std::vector<CameraModel> > multiModels;
@@ -497,7 +498,7 @@ std::map<int, Transform> Optimizer::optimizeBA(
 	}
 
 	// compute correspondences
-	this->computeBACorrespondences(poses, links, signatures, points3DMap, wordReferences, rematchFeatures);
+	this->computeBACorrespondences(poses, links, signatures, points3DMap, wordReferences, rematchFeatures, false, registrationParameters);
 
 	return optimizeBA(rootId, poses, links, multiModels, points3DMap, wordReferences);
 }
@@ -507,11 +508,12 @@ std::map<int, Transform> Optimizer::optimizeBA(
 		const std::map<int, Transform> & poses,
 		const std::multimap<int, Link> & links,
 		const std::map<int, Signature> & signatures,
-		bool rematchFeatures)
+		bool rematchFeatures,
+		const ParametersMap & registrationParameters)
 {
 	std::map<int, cv::Point3f> points3DMap;
 	std::map<int, std::map<int, FeatureBA> > wordReferences;
-	return optimizeBA(rootId, poses, links, signatures, points3DMap, wordReferences, rematchFeatures);
+	return optimizeBA(rootId, poses, links, signatures, points3DMap, wordReferences, rematchFeatures, registrationParameters);
 }
 
 Transform Optimizer::optimizeBA(
@@ -556,11 +558,21 @@ void Optimizer::computeBACorrespondences(
 		const std::map<int, Signature> & signatures,
 		std::map<int, cv::Point3f> & points3DMap,
 		std::map<int, std::map<int, FeatureBA> > & wordReferences,
-		bool rematchFeatures)
+		bool rematchFeatures,
+		bool useLinkTransformAsGuess,
+		ParametersMap registrationParameters)
 {
 	UDEBUG("rematchFeatures=%d", rematchFeatures?1:0);
 	int wordCount = 0;
 	int edgeWithWordsAdded = 0;
+
+	// Some defaults if not provided
+	registrationParameters.insert(ParametersPair(Parameters::kVisEstimationType(), "1"));
+	registrationParameters.insert(ParametersPair(Parameters::kVisPnPReprojError(), "5"));
+	registrationParameters.insert(ParametersPair(Parameters::kVisMinInliers(), "6"));
+	registrationParameters.insert(ParametersPair(Parameters::kVisCorNNDR(), "0.6"));
+	RegistrationVis reg(registrationParameters);
+
 	std::map<int, std::map<cv::KeyPoint, int, KeyPointCompare> > frameToWordMap; // <FrameId, <Keypoint, wordId> >
 	for(std::multimap<int, Link>::const_iterator iter=links.lower_bound(1); iter!=links.end(); ++iter)
 	{
@@ -600,22 +612,21 @@ void Optimizer::computeBACorrespondences(
 					sTo.getWords().size() &&
 					sFrom.getWords3().size())
 				{
-					ParametersMap regParam;
-					regParam.insert(ParametersPair(Parameters::kVisEstimationType(), "1"));
-					regParam.insert(ParametersPair(Parameters::kVisPnPReprojError(), "5"));
-					regParam.insert(ParametersPair(Parameters::kVisMinInliers(), "6"));
-					regParam.insert(ParametersPair(Parameters::kVisCorNNDR(), "0.6"));
-					RegistrationVis reg(regParam);
-
 					if(!rematchFeatures)
 					{
 						sFrom.setWordsDescriptors(cv::Mat());
 						sTo.setWordsDescriptors(cv::Mat());
 					}
+					else if(sFrom.getWordsDescriptors().empty() && sTo.getWordsDescriptors().empty())
+					{
+						UWARN("Rematching features is enabled but signatures (%d and %d) don't have word descriptors!? "
+							  "Features won't be rematched. If it is an old database, do rtabmap-reprocess "
+							  "so that signatures contain word desriptors.",
+							  sFrom.id(), sTo.id());
+					}
 
 					RegistrationInfo info;
-					Transform t = reg.computeTransformationMod(sFrom, sTo, Transform(), &info);
-					//Transform t = reg.computeTransformationMod(sFrom, sTo, iter->second.transform(), &info);
+					Transform t = reg.computeTransformationMod(sFrom, sTo, useLinkTransformAsGuess?iter->second.transform():Transform(), &info);
 					UDEBUG("%d->%d, inliers=%d",sFrom.id(), sTo.id(), (int)info.inliersIDs.size());
 
 					if(!t.isNull())

corelib/src/PDALWriter.cpp

@@ -115,13 +115,14 @@ int savePDALFile(const std::string & filePath,
 
 	pdal::StageFactory factory;
 	std::string ext = UFile::getExtension(filePath);
-	pdal::Stage *writer = factory.createStage("writers." + ext);
+	pdal::Stage *writer = factory.createStage("writers." + (ext.compare("laz")==0?"las":ext));
 	if(writer)
 	{
 		pdal::Options writerOps;
 		writerOps.add("filename", filePath);
 		if(ext.compare("ply")==0) writerOps.add("storage_mode", binary?"little endian":"ascii"); // PLY
 		if(ext.compare("pcd")==0) writerOps.add("compression", binary?"binary":"ascii"); // PCD
+		if(ext.compare("laz")==0) writerOps.add("compression", "lazperf");
 
 		writer->setOptions(writerOps);
 		writer->setInput(bufferReader);
@@ -221,13 +222,14 @@ int savePDALFile(const std::string & filePath,
 
 	pdal::StageFactory factory;
 	std::string ext = UFile::getExtension(filePath);
-	pdal::Stage *writer = factory.createStage("writers." + ext);
+	pdal::Stage *writer = factory.createStage("writers." + (ext.compare("laz")==0?"las":ext));
 	if(writer)
 	{
 		pdal::Options writerOps;
 		writerOps.add("filename", filePath);
 		if(ext.compare("ply")==0) writerOps.add("storage_mode", binary?"little endian":"ascii"); // PLY
 		if(ext.compare("pcd")==0) writerOps.add("compression", binary?"binary":"ascii"); // PCD
+		if(ext.compare("laz")==0) writerOps.add("compression", "lazperf");
 
 		writer->setOptions(writerOps);
 		writer->setInput(bufferReader);
@@ -342,13 +344,14 @@ int savePDALFile(const std::string & filePath,
 
 	pdal::StageFactory factory;
 	std::string ext = UFile::getExtension(filePath);
-	pdal::Stage *writer = factory.createStage("writers." + ext);
+	pdal::Stage *writer = factory.createStage("writers." + (ext.compare("laz")==0?"las":ext));
 	if(writer)
 	{
 		pdal::Options writerOps;
 		writerOps.add("filename", filePath);
 		if(ext.compare("ply")==0) writerOps.add("storage_mode", binary?"little endian":"ascii"); // PLY
 		if(ext.compare("pcd")==0) writerOps.add("compression", binary?"binary":"ascii"); // PCD
+		if(ext.compare("laz")==0) writerOps.add("compression", "lazperf");
 
 		writer->setOptions(writerOps);
 		writer->setInput(bufferReader);
@@ -412,13 +415,14 @@ int savePDALFile(const std::string & filePath,
 
 	pdal::StageFactory factory;
 	std::string ext = UFile::getExtension(filePath);
-	pdal::Stage *writer = factory.createStage("writers." + ext);
+	pdal::Stage *writer = factory.createStage("writers." + (ext.compare("laz")==0?"las":ext));
 	if(writer)
 	{
 		pdal::Options writerOps;
 		writerOps.add("filename", filePath);
 		if(ext.compare("ply")==0) writerOps.add("storage_mode", binary?"little endian":"ascii"); // PLY
 		if(ext.compare("pcd")==0) writerOps.add("compression", binary?"binary":"ascii"); // PCD
+		if(ext.compare("laz")==0) writerOps.add("compression", "lazperf");
 
 		writer->setOptions(writerOps);
 		writer->setInput(bufferReader);
@@ -491,13 +495,14 @@ int savePDALFile(const std::string & filePath,
 
 	pdal::StageFactory factory;
 	std::string ext = UFile::getExtension(filePath);
-	pdal::Stage *writer = factory.createStage("writers." + ext);
+	pdal::Stage *writer = factory.createStage("writers." + (ext.compare("laz")==0?"las":ext));
 	if(writer)
 	{
 		pdal::Options writerOps;
 		writerOps.add("filename", filePath);
 		if(ext.compare("ply")==0) writerOps.add("storage_mode", binary?"little endian":"ascii"); // PLY
 		if(ext.compare("pcd")==0) writerOps.add("compression", binary?"binary":"ascii"); // PCD
+		if(ext.compare("laz")==0) writerOps.add("compression", "lazperf");
 
 		writer->setOptions(writerOps);
 		writer->setInput(bufferReader);

corelib/src/Parameters.cpp

@@ -236,6 +236,9 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 	{
 		// removed parameters
 
+		// 0.21.13
+		removedParameters_.insert(std::make_pair("Vis/ForwardEstOnly",    std::make_pair(false, "")));
+
 		// 0.21.7
 		removedParameters_.insert(std::make_pair("SIFT/NFeatures",    std::make_pair(false, "")));
 

corelib/src/RegistrationVis.cpp

@@ -71,7 +71,6 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
 		_refineIterations(Parameters::defaultVisRefineIterations()),
 		_epipolarGeometryVar(Parameters::defaultVisEpipolarGeometryVar()),
 		_estimationType(Parameters::defaultVisEstimationType()),
-		_forwardEstimateOnly(Parameters::defaultVisForwardEstOnly()),
 		_PnPReprojError(Parameters::defaultVisPnPReprojError()),
 		_PnPFlags(Parameters::defaultVisPnPFlags()),
 		_PnPRefineIterations(Parameters::defaultVisPnPRefineIterations()),
@@ -94,6 +93,7 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
 		_guessMatchToProjection(Parameters::defaultVisCorGuessMatchToProjection()),
 		_bundleAdjustment(Parameters::defaultVisBundleAdjustment()),
 		_depthAsMask(Parameters::defaultVisDepthAsMask()),
+		_maskFloorThreshold(Parameters::defaultVisDepthMaskFloorThr()),
 		_minInliersDistributionThr(Parameters::defaultVisMinInliersDistribution()),
 		_maxInliersMeanDistance(Parameters::defaultVisMeanInliersDistance()),
 		_detectorFrom(0),
@@ -131,7 +131,6 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kVisIterations(), _iterations);
 	Parameters::parse(parameters, Parameters::kVisRefineIterations(), _refineIterations);
 	Parameters::parse(parameters, Parameters::kVisEstimationType(), _estimationType);
-	Parameters::parse(parameters, Parameters::kVisForwardEstOnly(), _forwardEstimateOnly);
 	Parameters::parse(parameters, Parameters::kVisEpipolarGeometryVar(), _epipolarGeometryVar);
 	Parameters::parse(parameters, Parameters::kVisPnPReprojError(), _PnPReprojError);
 	Parameters::parse(parameters, Parameters::kVisPnPFlags(), _PnPFlags);
@@ -155,6 +154,7 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kVisCorGuessMatchToProjection(), _guessMatchToProjection);
 	Parameters::parse(parameters, Parameters::kVisBundleAdjustment(), _bundleAdjustment);
 	Parameters::parse(parameters, Parameters::kVisDepthAsMask(), _depthAsMask);
+	Parameters::parse(parameters, Parameters::kVisDepthMaskFloorThr(), _maskFloorThreshold);
 	Parameters::parse(parameters, Parameters::kVisMinInliersDistribution(), _minInliersDistributionThr);
 	Parameters::parse(parameters, Parameters::kVisMeanInliersDistance(), _maxInliersMeanDistance);
 	uInsert(_bundleParameters, parameters);
@@ -312,7 +312,6 @@ Transform RegistrationVis::computeTransformationImpl(
 	UDEBUG("%s=%f", Parameters::kVisInlierDistance().c_str(), _inlierDistance);
 	UDEBUG("%s=%d", Parameters::kVisIterations().c_str(), _iterations);
 	UDEBUG("%s=%d", Parameters::kVisEstimationType().c_str(), _estimationType);
-	UDEBUG("%s=%d", Parameters::kVisForwardEstOnly().c_str(), _forwardEstimateOnly);
 	UDEBUG("%s=%f", Parameters::kVisEpipolarGeometryVar().c_str(), _epipolarGeometryVar);
 	UDEBUG("%s=%f", Parameters::kVisPnPReprojError().c_str(), _PnPReprojError);
 	UDEBUG("%s=%d", Parameters::kVisPnPFlags().c_str(), _PnPFlags);
@@ -370,7 +369,7 @@ Transform RegistrationVis::computeTransformationImpl(
 	   (_estimationType==1 || toSignature.getWords3().size())) // required only for 3D->3D and 2D->2D
 	{
 		// no need to extract new features, we have all the data we need
-		UDEBUG("Bypassing feature matching as descriptors and images are empty. We assume features are already matched.");
+		UDEBUG("Bypassing feature matching as descriptors are empty. We assume features are already matched.");
 	}
 	else
 	{
@@ -423,13 +422,32 @@ Transform RegistrationVis::computeTransformationImpl(
 						   imageFrom.cols % fromSignature.sensorData().depthRaw().cols == 0 &&
 						   imageFrom.rows/fromSignature.sensorData().depthRaw().rows == fromSignature.sensorData().imageRaw().cols/fromSignature.sensorData().depthRaw().cols)
 						{
-							depthMask = util2d::interpolate(fromSignature.sensorData().depthRaw(), fromSignature.sensorData().imageRaw().rows/fromSignature.sensorData().depthRaw().rows, 0.1f);
+							depthMask = fromSignature.sensorData().depthRaw();
+
+							if(_maskFloorThreshold != 0.0f)
+							{
+								UASSERT(!fromSignature.sensorData().cameraModels().empty());
+								UDEBUG("Masking floor (threshold=%f)", _maskFloorThreshold);
+								if(_maskFloorThreshold<0.0f)
+								{
+									cv::Mat depthBelow;
+									util3d::filterFloor(depthMask, fromSignature.sensorData().cameraModels(), _maskFloorThreshold*-1.0f, &depthBelow);
+									depthMask = depthBelow;
+								}
+								else
+								{
+									depthMask = util3d::filterFloor(depthMask, fromSignature.sensorData().cameraModels(), _maskFloorThreshold);
+								}
+								UDEBUG("Masking floor done.");
+							}
+
+							depthMask = util2d::interpolate(depthMask, imageFrom.rows/depthMask.rows, 0.1f);
 						}
 						else
 						{
 							UWARN("%s is true, but RGB size (%dx%d) modulo depth size (%dx%d) is not 0. Ignoring depth mask for feature detection.",
 									Parameters::kVisDepthAsMask().c_str(),
-									fromSignature.sensorData().imageRaw().rows, fromSignature.sensorData().imageRaw().cols,
+									imageFrom.rows, imageFrom.cols,
 									fromSignature.sensorData().depthRaw().rows, fromSignature.sensorData().depthRaw().cols);
 						}
 					}
@@ -700,7 +718,7 @@ Transform RegistrationVis::computeTransformationImpl(
 				kptsFrom3D = kptsFrom3DKept;
 
 				std::vector<cv::Point3f> kptsTo3D;
-				if(_estimationType == 0 || _estimationType == 1 || !_forwardEstimateOnly)
+				if(_estimationType == 0 || _estimationType == 1)
 				{
 					kptsTo3D = _detectorTo->generateKeypoints3D(toSignature.sensorData(), kptsTo);
 				}
@@ -770,13 +788,32 @@ Transform RegistrationVis::computeTransformationImpl(
 						   imageTo.cols % toSignature.sensorData().depthRaw().cols == 0 &&
 						   imageTo.rows/toSignature.sensorData().depthRaw().rows == imageTo.cols/toSignature.sensorData().depthRaw().cols)
 						{
-							depthMask = util2d::interpolate(toSignature.sensorData().depthRaw(), imageTo.rows/toSignature.sensorData().depthRaw().rows, 0.1f);
+							depthMask = toSignature.sensorData().depthRaw();
+
+							if(_maskFloorThreshold != 0.0f)
+							{
+								UASSERT(!toSignature.sensorData().cameraModels().empty());
+								UDEBUG("Masking floor (threshold=%f)", _maskFloorThreshold);
+								if(_maskFloorThreshold<0.0f)
+								{
+									cv::Mat depthBelow;
+									util3d::filterFloor(depthMask, toSignature.sensorData().cameraModels(), _maskFloorThreshold*-1.0f, &depthBelow);
+									depthMask = depthBelow;
+								}
+								else
+								{
+									depthMask = util3d::filterFloor(depthMask, toSignature.sensorData().cameraModels(), _maskFloorThreshold);
+								}
+								UDEBUG("Masking floor done.");
+							}
+
+							depthMask = util2d::interpolate(depthMask, imageTo.rows/depthMask.rows, 0.1f);
 						}
 						else
 						{
 							UWARN("%s is true, but RGB size (%dx%d) modulo depth size (%dx%d) is not 0. Ignoring depth mask for feature detection.",
 									Parameters::kVisDepthAsMask().c_str(),
-									toSignature.sensorData().imageRaw().rows, toSignature.sensorData().imageRaw().cols,
+									imageTo.rows, imageTo.cols,
 									toSignature.sensorData().depthRaw().rows, toSignature.sensorData().depthRaw().cols);
 						}
 					}
@@ -1536,347 +1573,305 @@ Transform RegistrationVis::computeTransformationImpl(
 	info.matchesIDs.clear();
 	if(toSignature.getWords().size())
 	{
-		Transform transforms[2];
-		std::vector<int> inliers[2];
-		std::vector<int> matches[2];
-		cv::Mat covariances[2];
-		covariances[0] = cv::Mat::eye(6,6,CV_64FC1);
-		covariances[1] = cv::Mat::eye(6,6,CV_64FC1);
-		for(int dir=0; dir<(!_forwardEstimateOnly?2:1); ++dir)
+		std::vector<int> inliers;
+		std::vector<int> matches;
+
+		if(_estimationType == 2) // Epipolar Geometry
 		{
-			// A to B
-			Signature * signatureA;
-			Signature * signatureB;
-			if(dir == 0)
+			UDEBUG("");
+			if((toSignature.sensorData().stereoCameraModels().size() != 1 ||
+				!toSignature.sensorData().stereoCameraModels()[0].isValidForProjection()) &&
+				(toSignature.sensorData().cameraModels().size() != 1 ||
+				!toSignature.sensorData().cameraModels()[0].isValidForProjection()))
 			{
-				signatureA = &fromSignature;
-				signatureB = &toSignature;
+				UERROR("Calibrated camera required (multi-cameras not supported).");
 			}
-			else
+			else if((int)fromSignature.getWords().size() >= _minInliers &&
+					(int)toSignature.getWords().size() >= _minInliers)
 			{
-				signatureA = &toSignature;
-				signatureB = &fromSignature;
-			}
-			if(_estimationType == 2) // Epipolar Geometry
-			{
-				UDEBUG("");
-				if((signatureB->sensorData().stereoCameraModels().size() != 1 ||
-					!signatureB->sensorData().stereoCameraModels()[0].isValidForProjection()) &&
-				   (signatureB->sensorData().cameraModels().size() != 1 ||
-					!signatureB->sensorData().cameraModels()[0].isValidForProjection()))
+				UASSERT((fromSignature.sensorData().stereoCameraModels().size() == 1 && fromSignature.sensorData().stereoCameraModels()[0].isValidForProjection()) || (fromSignature.sensorData().cameraModels().size() == 1 && fromSignature.sensorData().cameraModels()[0].isValidForProjection()));
+				const CameraModel & cameraModel = fromSignature.sensorData().stereoCameraModels().size()?fromSignature.sensorData().stereoCameraModels()[0].left():fromSignature.sensorData().cameraModels()[0];
+
+				// we only need the camera transform, send guess words3 for scale estimation
+				Transform cameraTransform;
+				double variance = 1.0f;
+				std::vector<int> matchesV;
+				std::map<int, int> uniqueWordsA = uMultimapToMapUnique(fromSignature.getWords());
+				std::map<int, int> uniqueWordsB = uMultimapToMapUnique(toSignature.getWords());
+				std::map<int, cv::KeyPoint> wordsA;
+				std::map<int, cv::Point3f> words3A;
+				std::map<int, cv::KeyPoint> wordsB;
+				for(std::map<int, int>::iterator iter=uniqueWordsA.begin(); iter!=uniqueWordsA.end(); ++iter)
 				{
-					UERROR("Calibrated camera required (multi-cameras not supported).");
+					wordsA.insert(std::make_pair(iter->first, fromSignature.getWordsKpts()[iter->second]));
+					if(!fromSignature.getWords3().empty())
+					{
+						words3A.insert(std::make_pair(iter->first, fromSignature.getWords3()[iter->second]));
+					}
 				}
-				else if((int)signatureA->getWords().size() >= _minInliers &&
-						(int)signatureB->getWords().size() >= _minInliers)
+				for(std::map<int, int>::iterator iter=uniqueWordsB.begin(); iter!=uniqueWordsB.end(); ++iter)
 				{
-					UASSERT((signatureA->sensorData().stereoCameraModels().size() == 1 && signatureA->sensorData().stereoCameraModels()[0].isValidForProjection()) || (signatureA->sensorData().cameraModels().size() == 1 && signatureA->sensorData().cameraModels()[0].isValidForProjection()));
-					const CameraModel & cameraModel = signatureA->sensorData().stereoCameraModels().size()?signatureA->sensorData().stereoCameraModels()[0].left():signatureA->sensorData().cameraModels()[0];
+					wordsB.insert(std::make_pair(iter->first, toSignature.getWordsKpts()[iter->second]));
+				}
+				std::map<int, cv::Point3f> inliers3D = util3d::generateWords3DMono(
+						wordsA,
+						wordsB,
+						cameraModel,
+						cameraTransform,
+						_PnPReprojError,
+						0.99f,
+						words3A, // for scale estimation
+						&variance,
+						&matchesV);
+				covariance *= variance;
+				inliers = uKeys(inliers3D);
+				matches = matchesV;
 
-					// we only need the camera transform, send guess words3 for scale estimation
-					Transform cameraTransform;
-					double variance = 1.0f;
-					std::vector<int> matchesV;
-					std::map<int, int> uniqueWordsA = uMultimapToMapUnique(signatureA->getWords());
-					std::map<int, int> uniqueWordsB = uMultimapToMapUnique(signatureB->getWords());
-					std::map<int, cv::KeyPoint> wordsA;
-					std::map<int, cv::Point3f> words3A;
-					std::map<int, cv::KeyPoint> wordsB;
-					for(std::map<int, int>::iterator iter=uniqueWordsA.begin(); iter!=uniqueWordsA.end(); ++iter)
+				if(!cameraTransform.isNull())
+				{
+					if((int)inliers3D.size() >= _minInliers)
 					{
-						wordsA.insert(std::make_pair(iter->first, signatureA->getWordsKpts()[iter->second]));
-						if(!signatureA->getWords3().empty())
+						if(variance <= _epipolarGeometryVar)
 						{
-							words3A.insert(std::make_pair(iter->first, signatureA->getWords3()[iter->second]));
-						}
-					}
-					for(std::map<int, int>::iterator iter=uniqueWordsB.begin(); iter!=uniqueWordsB.end(); ++iter)
-					{
-						wordsB.insert(std::make_pair(iter->first, signatureB->getWordsKpts()[iter->second]));
-					}
-					std::map<int, cv::Point3f> inliers3D = util3d::generateWords3DMono(
-							wordsA,
-							wordsB,
-							cameraModel,
-							cameraTransform,
-							_PnPReprojError,
-							0.99f,
-							words3A, // for scale estimation
-							&variance,
-							&matchesV);
-					covariances[dir] *= variance;
-					inliers[dir] = uKeys(inliers3D);
-					matches[dir] = matchesV;
-
-					if(!cameraTransform.isNull())
-					{
-						if((int)inliers3D.size() >= _minInliers)
-						{
-							if(variance <= _epipolarGeometryVar)
+							if(this->force3DoF())
 							{
-								if(this->force3DoF())
-								{
-									transforms[dir] = cameraTransform.to3DoF();
-								}
-								else
-								{
-									transforms[dir] = cameraTransform;
-								}
+								transform = cameraTransform.to3DoF();
 							}
 							else
 							{
-								msg = uFormat("Variance is too high! (Max %s=%f, variance=%f)", Parameters::kVisEpipolarGeometryVar().c_str(), _epipolarGeometryVar, variance);
-								UINFO(msg.c_str());
+								transform = cameraTransform;
 							}
 						}
 						else
 						{
-							msg = uFormat("Not enough inliers %d < %d", (int)inliers3D.size(), _minInliers);
+							msg = uFormat("Variance is too high! (Max %s=%f, variance=%f)", Parameters::kVisEpipolarGeometryVar().c_str(), _epipolarGeometryVar, variance);
 							UINFO(msg.c_str());
 						}
 					}
 					else
 					{
-						msg = uFormat("No camera transform found");
+						msg = uFormat("Not enough inliers %d < %d", (int)inliers3D.size(), _minInliers);
 						UINFO(msg.c_str());
 					}
 				}
-				else if(signatureA->getWords().size() == 0)
-				{
-					msg = uFormat("No enough features (%d)", (int)signatureA->getWords().size());
-					UWARN(msg.c_str());
-				}
 				else
 				{
-					msg = uFormat("No camera model");
-					UWARN(msg.c_str());
+					msg = uFormat("No camera transform found");
+					UINFO(msg.c_str());
 				}
 			}
-			else if(_estimationType == 1) // PnP
+			else if(fromSignature.getWords().size() == 0)
 			{
-				UDEBUG("");
-				if((signatureB->sensorData().stereoCameraModels().empty() || !signatureB->sensorData().stereoCameraModels()[0].isValidForProjection()) &&
-				   (signatureB->sensorData().cameraModels().empty() || !signatureB->sensorData().cameraModels()[0].isValidForProjection()))
-				{
-					UERROR("Calibrated camera required. Id=%d Models=%d StereoModels=%d weight=%d",
-							signatureB->id(),
-							(int)signatureB->sensorData().cameraModels().size(),
-							signatureB->sensorData().stereoCameraModels().size(),
-							signatureB->getWeight());
-				}
-#ifndef RTABMAP_OPENGV
-				else if(signatureB->sensorData().cameraModels().size() > 1)
-				{
-					UERROR("Multi-camera 2D-3D PnP registration is only available if rtabmap is built "
-							"with OpenGV dependency. Use 3D-3D registration approach instead for multi-camera.");
-				}
-#endif
-				else
-				{
-					UDEBUG("words from3D=%d to2D=%d", (int)signatureA->getWords3().size(), (int)signatureB->getWords().size());
-					// 3D to 2D
-					if((int)signatureA->getWords3().size() >= _minInliers &&
-					   (int)signatureB->getWords().size() >= _minInliers)
-					{
-						std::vector<int> inliersV;
-						std::vector<int> matchesV;
-						std::map<int, int> uniqueWordsA = uMultimapToMapUnique(signatureA->getWords());
-						std::map<int, int> uniqueWordsB = uMultimapToMapUnique(signatureB->getWords());
-						std::map<int, cv::Point3f> words3A;
-						std::map<int, cv::Point3f> words3B;
-						std::map<int, cv::KeyPoint> wordsB;
-						for(std::map<int, int>::iterator iter=uniqueWordsA.begin(); iter!=uniqueWordsA.end(); ++iter)
-						{
-							words3A.insert(std::make_pair(iter->first, signatureA->getWords3()[iter->second]));
-						}
-						for(std::map<int, int>::iterator iter=uniqueWordsB.begin(); iter!=uniqueWordsB.end(); ++iter)
-						{
-							wordsB.insert(std::make_pair(iter->first, signatureB->getWordsKpts()[iter->second]));
-							if(!signatureB->getWords3().empty())
-							{
-								words3B.insert(std::make_pair(iter->first, signatureB->getWords3()[iter->second]));
-							}
-						}
-
-						std::vector<CameraModel> models;
-						if(signatureB->sensorData().stereoCameraModels().size())
-						{
-							for(size_t i=0; i<signatureB->sensorData().stereoCameraModels().size(); ++i)
-							{
-								models.push_back(signatureB->sensorData().stereoCameraModels()[i].left());
-							}
-						}
-						else
-						{
-							models = signatureB->sensorData().cameraModels();
-						}
-
-						if(models.size()>1)
-						{
-							// Multi-Camera
-							UASSERT(models[0].isValidForProjection());
-
-							transforms[dir] = util3d::estimateMotion3DTo2D(
-									words3A,
-									wordsB,
-									models,
-									_multiSamplingPolicy,
-									_minInliers,
-									_iterations,
-									_PnPReprojError,
-									_PnPFlags,
-									_PnPRefineIterations,
-									_PnPVarMedianRatio,
-									_PnPMaxVar,
-									dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()),
-									words3B,
-									&covariances[dir],
-									&matchesV,
-									&inliersV,
-									_PnPSplitLinearCovarianceComponents);
-							inliers[dir] = inliersV;
-							matches[dir] = matchesV;
-						}
-						else
-						{
-							UASSERT(models.size() == 1 && models[0].isValidForProjection());
-
-							transforms[dir] = util3d::estimateMotion3DTo2D(
-									words3A,
-									wordsB,
-									models[0],
-									_minInliers,
-									_iterations,
-									_PnPReprojError,
-									_PnPFlags,
-									_PnPRefineIterations,
-									_PnPVarMedianRatio,
-									_PnPMaxVar,
-									dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()),
-									words3B,
-									&covariances[dir],
-									&matchesV,
-									&inliersV,
-									_PnPSplitLinearCovarianceComponents);
-							inliers[dir] = inliersV;
-							matches[dir] = matchesV;
-						}
-						UDEBUG("inliers: %d/%d", (int)inliersV.size(), (int)matchesV.size());
-						if(transforms[dir].isNull())
-						{
-							msg = uFormat("Not enough inliers %d/%d (matches=%d) between %d and %d",
-									(int)inliers[dir].size(), _minInliers, (int)matches[dir].size(), signatureA->id(), signatureB->id());
-							UINFO(msg.c_str());
-						}
-						else if(this->force3DoF())
-						{
-							transforms[dir] = transforms[dir].to3DoF();
-						}
-					}
-					else
-					{
-						msg = uFormat("Not enough features in images (old=%d, new=%d, min=%d)",
-								(int)signatureA->getWords3().size(), (int)signatureB->getWords().size(), _minInliers);
-						UINFO(msg.c_str());
-					}
-				}
-
+				msg = uFormat("No enough features (%d)", (int)fromSignature.getWords().size());
+				UWARN(msg.c_str());
 			}
 			else
 			{
-				UDEBUG("");
-				// 3D -> 3D
-				if((int)signatureA->getWords3().size() >= _minInliers &&
-				   (int)signatureB->getWords3().size() >= _minInliers)
+				msg = uFormat("No camera model");
+				UWARN(msg.c_str());
+			}
+		}
+		else if(_estimationType == 1) // PnP
+		{
+			UDEBUG("");
+			if((toSignature.sensorData().stereoCameraModels().empty() || !toSignature.sensorData().stereoCameraModels()[0].isValidForProjection()) &&
+				(toSignature.sensorData().cameraModels().empty() || !toSignature.sensorData().cameraModels()[0].isValidForProjection()))
+			{
+				UERROR("Calibrated camera required. Id=%d Models=%d StereoModels=%d weight=%d",
+						toSignature.id(),
+						(int)toSignature.sensorData().cameraModels().size(),
+						toSignature.sensorData().stereoCameraModels().size(),
+						toSignature.getWeight());
+			}
+#ifndef RTABMAP_OPENGV
+			else if(toSignature.sensorData().cameraModels().size() > 1)
+			{
+				UERROR("Multi-camera 2D-3D PnP registration is only available if rtabmap is built "
+						"with OpenGV dependency. Use 3D-3D registration approach instead for multi-camera.");
+			}
+#endif
+			else
+			{
+				UDEBUG("words from3D=%d to2D=%d", (int)fromSignature.getWords3().size(), (int)toSignature.getWords().size());
+				// 3D to 2D
+				if((int)fromSignature.getWords3().size() >= _minInliers &&
+					(int)toSignature.getWords().size() >= _minInliers)
 				{
 					std::vector<int> inliersV;
 					std::vector<int> matchesV;
-					std::map<int, int> uniqueWordsA = uMultimapToMapUnique(signatureA->getWords());
-					std::map<int, int> uniqueWordsB = uMultimapToMapUnique(signatureB->getWords());
+					std::map<int, int> uniqueWordsA = uMultimapToMapUnique(fromSignature.getWords());
+					std::map<int, int> uniqueWordsB = uMultimapToMapUnique(toSignature.getWords());
 					std::map<int, cv::Point3f> words3A;
 					std::map<int, cv::Point3f> words3B;
+					std::map<int, cv::KeyPoint> wordsB;
 					for(std::map<int, int>::iterator iter=uniqueWordsA.begin(); iter!=uniqueWordsA.end(); ++iter)
 					{
-						words3A.insert(std::make_pair(iter->first, signatureA->getWords3()[iter->second]));
+						words3A.insert(std::make_pair(iter->first, fromSignature.getWords3()[iter->second]));
 					}
 					for(std::map<int, int>::iterator iter=uniqueWordsB.begin(); iter!=uniqueWordsB.end(); ++iter)
 					{
-						words3B.insert(std::make_pair(iter->first, signatureB->getWords3()[iter->second]));
+						wordsB.insert(std::make_pair(iter->first, toSignature.getWordsKpts()[iter->second]));
+						if(!toSignature.getWords3().empty())
+						{
+							words3B.insert(std::make_pair(iter->first, toSignature.getWords3()[iter->second]));
+						}
+					}
+
+					std::vector<CameraModel> models;
+					if(toSignature.sensorData().stereoCameraModels().size())
+					{
+						for(size_t i=0; i<toSignature.sensorData().stereoCameraModels().size(); ++i)
+						{
+							models.push_back(toSignature.sensorData().stereoCameraModels()[i].left());
+						}
+					}
+					else
+					{
+						models = toSignature.sensorData().cameraModels();
+					}
+
+					if(models.size()>1)
+					{
+						// Multi-Camera
+						UASSERT(models[0].isValidForProjection());
+
+						std::vector<std::vector<int> > matchesPerCam;
+						std::vector<std::vector<int> > inliersPerCam;
+
+						transform = util3d::estimateMotion3DTo2D(
+								words3A,
+								wordsB,
+								models,
+								_multiSamplingPolicy,
+								_minInliers,
+								_iterations,
+								_PnPReprojError,
+								_PnPFlags,
+								_PnPRefineIterations,
+								_PnPVarMedianRatio,
+								_PnPMaxVar,
+								!guess.isNull()?guess:Transform::getIdentity(),
+								words3B,
+								&covariance,
+								&matchesPerCam,
+								&inliersPerCam,
+								_PnPSplitLinearCovarianceComponents);
+
+						info.matchesPerCam.resize(matchesPerCam.size());
+						for(size_t i=0; i<matchesPerCam.size(); ++i)
+						{
+							matches.insert(matches.end(), matchesPerCam[i].begin(), matchesPerCam[i].end());
+							info.matchesPerCam[i] = matchesPerCam[i].size();
+						}
+						info.inliersPerCam.resize(inliersPerCam.size());
+						for(size_t i=0; i<inliersPerCam.size(); ++i)
+						{
+							inliers.insert(inliers.end(), inliersPerCam[i].begin(), inliersPerCam[i].end());
+							info.inliersPerCam[i] = inliersPerCam[i].size();
+						}
+					}
+					else
+					{
+						UASSERT(models.size() == 1 && models[0].isValidForProjection());
+
+						transform = util3d::estimateMotion3DTo2D(
+								words3A,
+								wordsB,
+								models[0],
+								_minInliers,
+								_iterations,
+								_PnPReprojError,
+								_PnPFlags,
+								_PnPRefineIterations,
+								_PnPVarMedianRatio,
+								_PnPMaxVar,
+								!guess.isNull()?guess:Transform::getIdentity(),
+								words3B,
+								&covariance,
+								&matchesV,
+								&inliersV,
+								_PnPSplitLinearCovarianceComponents);
+						inliers = inliersV;
+						matches = matchesV;
 					}
-					transforms[dir] = util3d::estimateMotion3DTo3D(
-							words3A,
-							words3B,
-							_minInliers,
-							_inlierDistance,
-							_iterations,
-							_refineIterations,
-							&covariances[dir],
-							&matchesV,
-							&inliersV);
-					inliers[dir] = inliersV;
-					matches[dir] = matchesV;
 					UDEBUG("inliers: %d/%d", (int)inliersV.size(), (int)matchesV.size());
-					if(transforms[dir].isNull())
+					if(transform.isNull())
 					{
 						msg = uFormat("Not enough inliers %d/%d (matches=%d) between %d and %d",
-								(int)inliers[dir].size(), _minInliers, (int)matches[dir].size(), signatureA->id(), signatureB->id());
+								(int)inliers.size(), _minInliers, (int)matches.size(), fromSignature.id(), toSignature.id());
 						UINFO(msg.c_str());
 					}
 					else if(this->force3DoF())
 					{
-						transforms[dir] = transforms[dir].to3DoF();
+						transform = transform.to3DoF();
 					}
 				}
 				else
 				{
-					msg = uFormat("Not enough 3D features in images (old=%d, new=%d, min=%d)",
-							(int)signatureA->getWords3().size(), (int)signatureB->getWords3().size(), _minInliers);
+					msg = uFormat("Not enough features in images (old=%d, new=%d, min=%d)",
+							(int)fromSignature.getWords3().size(), (int)toSignature.getWords().size(), _minInliers);
 					UINFO(msg.c_str());
 				}
 			}
-		}
 
-		if(!_forwardEstimateOnly)
-		{
-			UDEBUG("from->to=%s", transforms[0].prettyPrint().c_str());
-			UDEBUG("to->from=%s", transforms[1].prettyPrint().c_str());
 		}
-
-		std::vector<int> allInliers = inliers[0];
-		if(inliers[1].size())
+		else
 		{
-			std::set<int> allInliersSet(allInliers.begin(), allInliers.end());
-			unsigned int oi = allInliers.size();
-			allInliers.resize(allInliers.size() + inliers[1].size());
-			for(unsigned int i=0; i<inliers[1].size(); ++i)
+			UDEBUG("");
+			// 3D -> 3D
+			if((int)fromSignature.getWords3().size() >= _minInliers &&
+				(int)toSignature.getWords3().size() >= _minInliers)
 			{
-				if(allInliersSet.find(inliers[1][i]) == allInliersSet.end())
+				std::vector<int> inliersV;
+				std::vector<int> matchesV;
+				std::map<int, int> uniqueWordsA = uMultimapToMapUnique(fromSignature.getWords());
+				std::map<int, int> uniqueWordsB = uMultimapToMapUnique(toSignature.getWords());
+				std::map<int, cv::Point3f> words3A;
+				std::map<int, cv::Point3f> words3B;
+				for(std::map<int, int>::iterator iter=uniqueWordsA.begin(); iter!=uniqueWordsA.end(); ++iter)
 				{
-					allInliers[oi++] = inliers[1][i];
+					words3A.insert(std::make_pair(iter->first, fromSignature.getWords3()[iter->second]));
+				}
+				for(std::map<int, int>::iterator iter=uniqueWordsB.begin(); iter!=uniqueWordsB.end(); ++iter)
+				{
+					words3B.insert(std::make_pair(iter->first, toSignature.getWords3()[iter->second]));
+				}
+				transform = util3d::estimateMotion3DTo3D(
+						words3A,
+						words3B,
+						_minInliers,
+						_inlierDistance,
+						_iterations,
+						_refineIterations,
+						&covariance,
+						&matchesV,
+						&inliersV);
+				inliers = inliersV;
+				matches = matchesV;
+				UDEBUG("inliers: %d/%d", (int)inliersV.size(), (int)matchesV.size());
+				if(transform.isNull())
+				{
+					msg = uFormat("Not enough inliers %d/%d (matches=%d) between %d and %d",
+							(int)inliers.size(), _minInliers, (int)matches.size(), fromSignature.id(), toSignature.id());
+					UINFO(msg.c_str());
+				}
+				else if(this->force3DoF())
+				{
+					transform = transform.to3DoF();
 				}
 			}
-			allInliers.resize(oi);
-		}
-		std::vector<int> allMatches = matches[0];
-		if(matches[1].size())
-		{
-			std::set<int> allMatchesSet(allMatches.begin(), allMatches.end());
-			unsigned int oi = allMatches.size();
-			allMatches.resize(allMatches.size() + matches[1].size());
-			for(unsigned int i=0; i<matches[1].size(); ++i)
+			else
 			{
-				if(allMatchesSet.find(matches[1][i]) == allMatchesSet.end())
-				{
-					allMatches[oi++] = matches[1][i];
-				}
+				msg = uFormat("Not enough 3D features in images (old=%d, new=%d, min=%d)",
+						(int)fromSignature.getWords3().size(), (int)toSignature.getWords3().size(), _minInliers);
+				UINFO(msg.c_str());
 			}
-			allMatches.resize(oi);
 		}
 
 		if(_bundleAdjustment > 0 &&
 			_estimationType < 2 &&
-			!transforms[0].isNull() &&
-			allInliers.size() &&
+			!transform.isNull() &&
+			inliers.size() &&
 			fromSignature.getWords3().size() &&
 			toSignature.getWords().size() &&
 			(fromSignature.sensorData().stereoCameraModels().size() >= 1 || fromSignature.sensorData().cameraModels().size() >= 1) &&
@@ -1890,34 +1885,23 @@ Transform RegistrationVis::computeTransformationImpl(
 			std::map<int, cv::Point3f> points3DMap;
 
 			poses.insert(std::make_pair(1, Transform::getIdentity()));
-			poses.insert(std::make_pair(2, transforms[0]));
+			poses.insert(std::make_pair(2, transform));
 
-			for(int i=0;i<2;++i)
-			{
-				UASSERT(covariances[i].cols==6 && covariances[i].rows == 6 && covariances[i].type() == CV_64FC1);
-				if(covariances[i].at<double>(0,0)<=COVARIANCE_LINEAR_EPSILON)
-					covariances[i].at<double>(0,0) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
-				if(covariances[i].at<double>(1,1)<=COVARIANCE_LINEAR_EPSILON)
-					covariances[i].at<double>(1,1) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
-				if(covariances[i].at<double>(2,2)<=COVARIANCE_LINEAR_EPSILON)
-					covariances[i].at<double>(2,2) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
-				if(covariances[i].at<double>(3,3)<=COVARIANCE_ANGULAR_EPSILON)
-					covariances[i].at<double>(3,3) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
-				if(covariances[i].at<double>(4,4)<=COVARIANCE_ANGULAR_EPSILON)
-					covariances[i].at<double>(4,4) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
-				if(covariances[i].at<double>(5,5)<=COVARIANCE_ANGULAR_EPSILON)
-					covariances[i].at<double>(5,5) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
-			}
-
-			cv::Mat cov = covariances[0].clone();
-
-			links.insert(std::make_pair(1, Link(1, 2, Link::kNeighbor, transforms[0], cov.inv())));
-			if(!transforms[1].isNull() && inliers[1].size())
-			{
-				cov = covariances[1].clone();
-				links.insert(std::make_pair(2, Link(2, 1, Link::kNeighbor, transforms[1], cov.inv())));
-			}
+			UASSERT(covariance.cols==6 && covariance.rows == 6 && covariance.type() == CV_64FC1);
+			if(covariance.at<double>(0,0)<=COVARIANCE_LINEAR_EPSILON)
+				covariance.at<double>(0,0) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
+			if(covariance.at<double>(1,1)<=COVARIANCE_LINEAR_EPSILON)
+				covariance.at<double>(1,1) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
+			if(covariance.at<double>(2,2)<=COVARIANCE_LINEAR_EPSILON)
+				covariance.at<double>(2,2) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
+			if(covariance.at<double>(3,3)<=COVARIANCE_ANGULAR_EPSILON)
+				covariance.at<double>(3,3) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
+			if(covariance.at<double>(4,4)<=COVARIANCE_ANGULAR_EPSILON)
+				covariance.at<double>(4,4) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
+			if(covariance.at<double>(5,5)<=COVARIANCE_ANGULAR_EPSILON)
+				covariance.at<double>(5,5) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
 
+			links.insert(std::make_pair(1, Link(1, 2, Link::kNeighbor, transform, covariance.inv())));
 			std::map<int, Transform> optimizedPoses;
 
 			UASSERT((toSignature.sensorData().stereoCameraModels().size()  >= 1 && toSignature.sensorData().stereoCameraModels()[0].isValidForProjection()) ||
@@ -1975,17 +1959,12 @@ Transform RegistrationVis::computeTransformationImpl(
 			std::map<int, std::map<int, FeatureBA> > wordReferences;
 			std::set<int> sbaOutliers;
 			UDEBUG("");
-			for(unsigned int i=0; i<allInliers.size(); ++i)
+			for(unsigned int i=0; i<inliers.size(); ++i)
 			{
-				int wordId = allInliers[i];
+				int wordId = inliers[i];
 				int indexFrom = fromSignature.getWords().find(wordId)->second;
 				const cv::Point3f & pt3D = fromSignature.getWords3()[indexFrom];
-				if(!util3d::isFinite(pt3D))
-				{
-					UASSERT_MSG(!_forwardEstimateOnly, uFormat("3D point %d is not finite!?", wordId).c_str());
-					sbaOutliers.insert(wordId);
-					continue;
-				}
+				UASSERT_MSG(util3d::isFinite(pt3D), uFormat("3D point %d is not finite!?", wordId).c_str());
 
 				points3DMap.insert(std::make_pair(wordId, pt3D));
 
@@ -2053,32 +2032,32 @@ Transform RegistrationVis::computeTransformationImpl(
 				!optimizedPoses.begin()->second.isNull() &&
 				!optimizedPoses.rbegin()->second.isNull())
 			{
-				UDEBUG("Pose optimization: %s -> %s", transforms[0].prettyPrint().c_str(), optimizedPoses.rbegin()->second.prettyPrint().c_str());
+				UDEBUG("Pose optimization: %s -> %s", transform.prettyPrint().c_str(), optimizedPoses.rbegin()->second.prettyPrint().c_str());
 
 				if(sbaOutliers.size())
 				{
-					std::vector<int> newInliers(allInliers.size());
+					std::vector<int> newInliers(inliers.size());
 					int oi=0;
-					for(unsigned int i=0; i<allInliers.size(); ++i)
+					for(unsigned int i=0; i<inliers.size(); ++i)
 					{
-						if(sbaOutliers.find(allInliers[i]) == sbaOutliers.end())
+						if(sbaOutliers.find(inliers[i]) == sbaOutliers.end())
 						{
-							newInliers[oi++] = allInliers[i];
+							newInliers[oi++] = inliers[i];
 						}
 					}
 					newInliers.resize(oi);
-					UDEBUG("BA outliers ratio %f", float(sbaOutliers.size())/float(allInliers.size()));
-					allInliers = newInliers;
+					UDEBUG("BA outliers ratio %f", float(sbaOutliers.size())/float(inliers.size()));
+					inliers = newInliers;
 				}
-				if((int)allInliers.size() < _minInliers)
+				if((int)inliers.size() < _minInliers)
 				{
 					msg = uFormat("Not enough inliers after bundle adjustment %d/%d (matches=%d) between %d and %d",
-							(int)allInliers.size(), _minInliers, (int)allInliers.size()+sbaOutliers.size(), fromSignature.id(), toSignature.id());
-					transforms[0].setNull();
+							(int)inliers.size(), _minInliers, (int)inliers.size()+sbaOutliers.size(), fromSignature.id(), toSignature.id());
+					transform.setNull();
 				}
 				else
 				{
-					transforms[0] = optimizedPoses.rbegin()->second;
+					transform = optimizedPoses.rbegin()->second;
 				}
 				// update 3D points, both from and to signatures
 				/*std::multimap<int, cv::Point3f> cpyWordsFrom3 = fromSignature.getWords3();
@@ -2097,36 +2076,16 @@ Transform RegistrationVis::computeTransformationImpl(
 			}
 			else
 			{
-				transforms[0].setNull();
+				transform.setNull();
 			}
-			transforms[1].setNull();
 		}
 
-		info.inliersIDs = allInliers;
-		info.matchesIDs = allMatches;
-		inliersCount = (int)allInliers.size();
-		matchesCount = (int)allMatches.size();
-		if(!transforms[1].isNull())
-		{
-			transforms[1] = transforms[1].inverse();
-			if(transforms[0].isNull())
-			{
-				transform = transforms[1];
-				covariance = covariances[1];
-			}
-			else
-			{
-				transform = transforms[0].interpolate(0.5f, transforms[1]);
-				covariance = (covariances[0]+covariances[1])/2.0f;
-			}
-		}
-		else
-		{
-			transform = transforms[0];
-			covariance = covariances[0];
-		}
+		info.inliersIDs = inliers;
+		info.matchesIDs = matches;
+		inliersCount = (int)inliers.size();
+		matchesCount = (int)matches.size();
 
-		if(!transform.isNull() && !allInliers.empty() && (_minInliersDistributionThr>0.0f || _maxInliersMeanDistance>0.0f))
+		if(!transform.isNull() && !inliers.empty() && (_minInliersDistributionThr>0.0f || _maxInliersMeanDistance>0.0f))
 		{
 			cv::Mat pcaData;
 			std::vector<CameraModel> cameraModelsTo;
@@ -2147,7 +2106,7 @@ Transform RegistrationVis::computeTransformationImpl(
 				{
 					if(cameraModelsTo[0].imageWidth()>0 && cameraModelsTo[0].imageHeight()>0)
 					{
-						pcaData = cv::Mat(allInliers.size(), 2, CV_32FC1);
+						pcaData = cv::Mat(inliers.size(), 2, CV_32FC1);
 					}
 					else
 					{
@@ -2164,11 +2123,11 @@ Transform RegistrationVis::computeTransformationImpl(
 			std::vector<float> distances;
 			if(_maxInliersMeanDistance>0.0f)
 			{
-				distances.reserve(allInliers.size());
+				distances.reserve(inliers.size());
 			}
-			for(unsigned int i=0; i<allInliers.size(); ++i)
+			for(unsigned int i=0; i<inliers.size(); ++i)
 			{
-				std::multimap<int, int>::const_iterator wordsIter = toSignature.getWords().find(allInliers[i]);
+				std::multimap<int, int>::const_iterator wordsIter = toSignature.getWords().find(inliers[i]);
 				if(wordsIter != toSignature.getWords().end() && !toSignature.getWordsKpts().empty())
 				{
 					const cv::KeyPoint & kpt = toSignature.getWordsKpts()[wordsIter->second];

corelib/src/Rtabmap.cpp

@@ -152,6 +152,7 @@ Rtabmap::Rtabmap() :
 	_maxOdomCacheSize(Parameters::defaultRGBDMaxOdomCacheSize()),
 	_localizationSmoothing(Parameters::defaultRGBDLocalizationSmoothing()),
 	_localizationPriorInf(1.0/(Parameters::defaultRGBDLocalizationPriorError()*Parameters::defaultRGBDLocalizationPriorError())),
+	_localizationSecondTryWithoutProximityLinks(Parameters::defaultRGBDLocalizationSecondTryWithoutProximityLinks()),
 	_createGlobalScanMap(Parameters::defaultRGBDProximityGlobalScanMap()),
 	_markerPriorsLinearVariance(Parameters::defaultMarkerPriorsVarianceLinear()),
 	_markerPriorsAngularVariance(Parameters::defaultMarkerPriorsVarianceAngular()),
@@ -632,6 +633,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRGBDLocalizationPriorError(), localizationPriorError);
 	UASSERT(localizationPriorError>0.0);
 	_localizationPriorInf = 1.0/(localizationPriorError*localizationPriorError);
+	Parameters::parse(parameters, Parameters::kRGBDLocalizationSecondTryWithoutProximityLinks(), _localizationSecondTryWithoutProximityLinks);
 	Parameters::parse(parameters, Parameters::kRGBDProximityGlobalScanMap(), _createGlobalScanMap);
 
 	Parameters::parse(parameters, Parameters::kMarkerPriorsVarianceLinear(), _markerPriorsLinearVariance);
@@ -716,6 +718,36 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 
 	if(_memory)
 	{
+		bool isMemIncremental = _memory->isIncremental();
+		if(Parameters::parse(parameters, Parameters::kMemIncrementalMemory(), isMemIncremental) &&
+			isMemIncremental != _memory->isIncremental())
+		{
+			// Mode has changed from Mapping to Localization, cleanup the local graph
+			if(_memory->isGraphReduced() && _memory->isIncremental())
+			{
+				// Force reducing graph, then remove filtered nodes from the optimized poses
+				std::map<int, int> reducedIds;
+				_memory->incrementMapId(&reducedIds);
+				for(std::map<int, int>::iterator iter=reducedIds.begin(); iter!=reducedIds.end(); ++iter)
+				{
+					_optimizedPoses.erase(iter->first);
+				}
+			}
+
+			// In both cases, we save the latest optimized graph and latest localization pose
+			_memory->saveOptimizedPoses(_optimizedPoses, _lastLocalizationPose);
+
+			// Mode changed from Localization to Mapping, clear local graph
+			if(!_memory->isIncremental()) {
+				_optimizedPoses.clear();
+				_lastLocalizationPose.setNull();
+				_mapCorrection.setIdentity();
+				_mapCorrectionBackup.setNull();
+				_localizationCovariance = cv::Mat();
+				_lastLocalizationNodeId = 0;
+			}
+		}
+
 		_memory->parseParameters(parameters);
 		if(_memory->isIncremental() && !_globalScanMap.empty())
 		{
@@ -1211,7 +1243,7 @@ bool Rtabmap::process(
 	double timeStatsCreation = 0;
 
 	float hypothesisRatio = 0.0f; // Only used for statistics
-	bool rejectedGlobalLoopClosure = false;
+	bool rejectedLoopClosure = false;
 
 	std::map<int, float> rawLikelihood;
 	std::map<int, float> adjustedLikelihood;
@@ -1669,7 +1701,7 @@ bool Rtabmap::process(
 			{
 				distanceToClosestNodeInTheGraph = sqrt(sqrdDistance);
 				UDEBUG("Last localization pose = %s, closest node=%d (%f m)", newPose.prettyPrint().c_str(), closestNode, distanceToClosestNodeInTheGraph);
-				angleToClosestNodeInTheGraph = (newPose.inverse() * _optimizedPoses.at(closestNode)).getAngle();
+				angleToClosestNodeInTheGraph = newPose.getAngle(_optimizedPoses.at(closestNode));
 			}
 		}
 
@@ -1720,6 +1752,7 @@ bool Rtabmap::process(
 					_constraints.erase(--_constraints.end());
 				}
 			}
+
 			_constraints.insert(std::make_pair(tmp.from(), tmp));
 		}
 		// Localization mode stuff
@@ -2128,7 +2161,7 @@ bool Rtabmap::process(
 				// Loop closure Threshold
 				if(_highestHypothesis.second >= loopThr)
 				{
-					rejectedGlobalLoopClosure = true;
+					rejectedLoopClosure = true;
 					if(posterior.size() <= 2 && loopThr>0.0f)
 					{
 						// Ignore loop closure if there is only one loop closure hypothesis
@@ -2150,7 +2183,7 @@ bool Rtabmap::process(
 					else
 					{
 						_loopClosureHypothesis = _highestHypothesis;
-						rejectedGlobalLoopClosure = false;
+						rejectedLoopClosure = false;
 					}
 
 					timeHypothesesValidation = timer.ticks();
@@ -2161,7 +2194,7 @@ bool Rtabmap::process(
 					// Used for Precision-Recall computation.
 					// When analyzing logs, it's convenient to know
 					// if the hypothesis would be rejected if T_loop would be lower.
-					rejectedGlobalLoopClosure = true;
+					rejectedLoopClosure = true;
 					UDEBUG("rejected hypothesis: under loop ratio %f < %f", _highestHypothesis.second, _loopRatio*lastHighestHypothesis.second);
 				}
 
@@ -3030,15 +3063,15 @@ bool Rtabmap::process(
 				loopClosureVisualInliers = info.inliers;
 				loopClosureVisualInliersRatio = info.inliersRatio;
 				loopClosureVisualMatches = info.matches;
-				rejectedGlobalLoopClosure = transform.isNull();
-				if(rejectedGlobalLoopClosure)
+				rejectedLoopClosure = transform.isNull();
+				if(rejectedLoopClosure)
 				{
 					UWARN("Rejected loop closure %d -> %d: %s",
 							_loopClosureHypothesis.first, signature->id(), info.rejectedMsg.c_str());
 				}
 				else if(_maxLoopClosureDistance>0.0f && transform.getNorm() > _maxLoopClosureDistance)
 				{
-					rejectedGlobalLoopClosure = true;
+					rejectedLoopClosure = true;
 					UWARN("Rejected localization %d -> %d because distance to map (%fm) is over %s=%fm.",
 							_loopClosureHypothesis.first, signature->id(), transform.getNorm(), Parameters::kRGBDMaxLoopClosureDistance().c_str(), _maxLoopClosureDistance);
 				}
@@ -3047,7 +3080,7 @@ bool Rtabmap::process(
 					transform = transform.inverse();
 				}
 			}
-			if(!rejectedGlobalLoopClosure)
+			if(!rejectedLoopClosure)
 			{
 				// Make the new one the parent of the old one
 				UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
@@ -3055,14 +3088,14 @@ bool Rtabmap::process(
 				loopClosureLinearVariance = uMax3(info.covariance.at<double>(0,0), info.covariance.at<double>(1,1)>=9999?0:info.covariance.at<double>(1,1), info.covariance.at<double>(2,2)>=9999?0:info.covariance.at<double>(2,2));
 				loopClosureAngularVariance = uMax3(info.covariance.at<double>(3,3)>=9999?0:info.covariance.at<double>(3,3), info.covariance.at<double>(4,4)>=9999?0:info.covariance.at<double>(4,4), info.covariance.at<double>(5,5));
 				cv::Mat information = getInformation(info.covariance);
-				rejectedGlobalLoopClosure = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, information));
-				if(!rejectedGlobalLoopClosure)
+				rejectedLoopClosure = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, information));
+				if(!rejectedLoopClosure)
 				{
 					loopClosureLinksAdded.push_back(std::make_pair(signature->id(), _loopClosureHypothesis.first));
 				}
 			}
 
-			if(rejectedGlobalLoopClosure)
+			if(rejectedLoopClosure)
 			{
 				_loopClosureHypothesis.first = 0;
 			}
@@ -3106,7 +3139,7 @@ bool Rtabmap::process(
 				{
 					UINFO("Landmark %d observed again! Seen the first time by node %d.", -iter->first, *_memory->getLandmarksIndex().find(iter->first)->second.begin());
 					landmarksDetected.insert(std::make_pair(iter->first, _memory->getLandmarksIndex().find(iter->first)->second));
-					rejectedGlobalLoopClosure = false; // If it was true, it will be set back to false if landmarks are rejected on graph optimization
+					rejectedLoopClosure = false; // If it was true, it will be set back to false if landmarks are rejected on graph optimization
 					loopClosureLinksAdded.push_back(std::make_pair(signature->id(), iter->first));
 				}
 			}
@@ -3149,8 +3182,8 @@ bool Rtabmap::process(
 	double optimizationError = 0.0;
 	int optimizationIterations = 0;
 	Transform previousMapCorrection;
-	bool rejectedLandmark = false;
 	bool delayedLocalization = false;
+	int odomCacheProximityLinksCleared = 0;
 	UDEBUG("RGB-D SLAM mode: %d", _rgbdSlamMode?1:0);
 	UDEBUG("Incremental: %d", _memory->isIncremental());
 	UDEBUG("Loop hyp: %d", _loopClosureHypothesis.first);
@@ -3269,7 +3302,7 @@ bool Rtabmap::process(
 				if(!posesOut.empty() &&
 				   posesOut.begin()->first < _odomCachePoses.begin()->first)
 				{
-					optPoses = _graphOptimizer->optimize(posesOut.begin()->first, posesOut, edgeConstraintsOut, locOptCovariance);
+					optPoses = _graphOptimizer->optimize(posesOut.begin()->first, posesOut, edgeConstraintsOut, locOptCovariance, 0, &optimizationError, &optimizationIterations);
 				}
 				else
 				{
@@ -3394,7 +3427,8 @@ bool Rtabmap::process(
 				}
 
 				bool hasGlobalLoopClosuresOrLandmarks = false;
-				if(rejectLocalization && !graph::filterLinks(constraints, Link::kLocalSpaceClosure, true).empty())
+				if(rejectLocalization && 
+					(_localizationSecondTryWithoutProximityLinks && !graph::filterLinks(constraints, Link::kLocalSpaceClosure, true).empty()))
 				{
 					// Let's try again without local loop closures
 					localizationLinks = graph::filterLinks(localizationLinks, Link::kLocalSpaceClosure);
@@ -3418,7 +3452,7 @@ bool Rtabmap::process(
 						if(!posesOut.empty() &&
 						   posesOut.begin()->first < _odomCachePoses.begin()->first)
 						{
-							optPoses = _graphOptimizer->optimize(posesOut.begin()->first, posesOut, edgeConstraintsOut, locOptCovariance);
+							optPoses = _graphOptimizer->optimize(posesOut.begin()->first, posesOut, edgeConstraintsOut, locOptCovariance, 0, &optimizationError, &optimizationIterations);
 						}
 						else
 						{
@@ -3554,13 +3588,14 @@ bool Rtabmap::process(
 						_odomCacheConstraints = graph::filterLinks(_odomCacheConstraints, Link::kLocalSpaceClosure);
 						if(before != _odomCacheConstraints.size())
 						{
-							UWARN("Successfully optimized without local loop closures! Clear them from local odometry cache. %ld/%ld have been removed.",
+							UWARN("Successfully optimized without local loop closures! Clearing them from local odometry cache. %ld/%ld have been removed.",
 									before - _odomCacheConstraints.size(), before);
 						}
 						else
 						{
 							UWARN("Successfully optimized without local loop closures!");
 						}
+						odomCacheProximityLinksCleared = before - _odomCacheConstraints.size();
 					}
 
 					// Count how many localization links are in the constraints
@@ -3615,6 +3650,14 @@ bool Rtabmap::process(
 					if(hadAlreadyLocalizationLinks || _maxOdomCacheSize == 0)
 					{
 						UINFO("Update localization");
+
+						// update odomCachePoses with optimized poses (but make sure to put them back in odom frame)
+						Transform mapToOdomCache = signature->getPose() * newOptPoseInv;
+						for(std::map<int, Transform>::iterator iter = _odomCachePoses.begin(); iter!=_odomCachePoses.end(); ++iter)
+						{
+							iter->second = mapToOdomCache * optPoses.at(iter->first);
+						}
+
 						if(_optimizeFromGraphEnd)
 						{
 							// update all previous nodes
@@ -3737,8 +3780,7 @@ bool Rtabmap::process(
 			{
 				_loopClosureHypothesis.first = 0;
 				lastProximitySpaceClosureId = 0;
-				rejectedGlobalLoopClosure = true;
-				rejectedLandmark = true;
+				rejectedLoopClosure = true;
 			}
 		}
 		else
@@ -3773,8 +3815,7 @@ bool Rtabmap::process(
 				updateConstraints = false;
 				_loopClosureHypothesis.first = 0;
 				lastProximitySpaceClosureId = 0;
-				rejectedGlobalLoopClosure = true;
-				rejectedLandmark = true;
+				rejectedLoopClosure = true;
 			}
 			else if(_memory->isIncremental() &&
 			  loopClosureLinksAdded.size() &&
@@ -3884,8 +3925,7 @@ bool Rtabmap::process(
 					updateConstraints = false;
 					_loopClosureHypothesis.first = 0;
 					lastProximitySpaceClosureId = 0;
-					rejectedGlobalLoopClosure = true;
-					rejectedLandmark = true;
+					rejectedLoopClosure = true;
 				}
 			}
 
@@ -3914,7 +3954,7 @@ bool Rtabmap::process(
 			{
 				distanceToClosestNodeInTheGraph = _lastLocalizationPose.getDistance(_optimizedPoses.at(closestNode));
 				UDEBUG("Last localization pose = %s, updated closest node=%d (%f m)", _lastLocalizationPose.prettyPrint().c_str(), closestNode, distanceToClosestNodeInTheGraph);
-				angleToClosestNodeInTheGraph = (_lastLocalizationPose.inverse() * _optimizedPoses.at(closestNode)).getAngle();
+				angleToClosestNodeInTheGraph = _lastLocalizationPose.getAngle(_optimizedPoses.at(closestNode));
 			}
 		}
 		_lastLocalizationPose = _optimizedPoses.at(signature->id()); // update
@@ -4046,7 +4086,7 @@ bool Rtabmap::process(
 			statistics_.addStatistic(Statistics::kLoopDistance_since_last_loc(), _distanceTravelledSinceLastLocalization);
 
 			float x,y,z,roll,pitch,yaw;
-			if(_loopClosureHypothesis.first || lastProximitySpaceClosureId || (!rejectedLandmark && !landmarksDetected.empty()))
+			if(_loopClosureHypothesis.first || lastProximitySpaceClosureId || (!rejectedLoopClosure && !landmarksDetected.empty()))
 			{
 				if(_loopClosureHypothesis.first || lastProximitySpaceClosureId)
 				{
@@ -4063,16 +4103,15 @@ bool Rtabmap::process(
 					if(!sLoop->getGroundTruthPose().isNull() && !signature->getGroundTruthPose().isNull())
 					{
 						Transform transformGT = sLoop->getGroundTruthPose().inverse() * signature->getGroundTruthPose();
-						Transform error = loopIter->second.transform().inverse() * transformGT;
-						statistics_.addStatistic(Statistics::kGtLocalization_linear_error(), error.getNorm());
-						statistics_.addStatistic(Statistics::kGtLocalization_angular_error(), error.getAngle(1,0,0)*180/M_PI);
+						statistics_.addStatistic(Statistics::kGtLocalization_linear_error(), loopIter->second.transform().getDistance(transformGT));
+						statistics_.addStatistic(Statistics::kGtLocalization_angular_error(), loopIter->second.transform().getAngle(transformGT)*180/M_PI);
 					}
 				}
 
 				_distanceTravelledSinceLastLocalization = 0.0f;
 
 				statistics_.addStatistic(Statistics::kLoopMapToOdom_norm(), _mapCorrection.getNorm());
-				statistics_.addStatistic(Statistics::kLoopMapToOdom_angle(), _mapCorrection.getAngle()*180.0f/M_PI);
+				statistics_.addStatistic(Statistics::kLoopMapToOdom_angle(), _mapCorrection.getAngle(Transform::getIdentity())*180.0f/M_PI);
 				_mapCorrection.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
 				statistics_.addStatistic(Statistics::kLoopMapToOdom_x(), x);
 				statistics_.addStatistic(Statistics::kLoopMapToOdom_y(), y);
@@ -4086,7 +4125,7 @@ bool Rtabmap::process(
 				{
 					Transform odomCorrection = (previousMapCorrection*odomPose).inverse()*_mapCorrection*odomPose;
 					statistics_.addStatistic(Statistics::kLoopOdom_correction_norm(), odomCorrection.getNorm());
-					statistics_.addStatistic(Statistics::kLoopOdom_correction_angle(), odomCorrection.getAngle()*180.0f/M_PI);
+					statistics_.addStatistic(Statistics::kLoopOdom_correction_angle(), odomCorrection.getAngle(Transform::getIdentity())*180.0f/M_PI);
 					odomCorrection.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
 					statistics_.addStatistic(Statistics::kLoopOdom_correction_x(), x);
 					statistics_.addStatistic(Statistics::kLoopOdom_correction_y(), y);
@@ -4107,6 +4146,10 @@ bool Rtabmap::process(
 				statistics_.addStatistic(Statistics::kLoopMapToBase_yaw(), yaw*180.0f/M_PI);
 				UINFO("Localization pose = %s", _lastLocalizationPose.prettyPrint().c_str());
 
+				if(_localizationSecondTryWithoutProximityLinks) {
+					statistics_.addStatistic(Statistics::kLoopProximity_links_cleared(), (float)odomCacheProximityLinksCleared);
+				}
+
 				if(_localizationCovariance.total()==36)
 				{
 					double varLin = _graphOptimizer->isSlam2d()?
@@ -4148,7 +4191,7 @@ bool Rtabmap::process(
 			statistics_.addStatistic(Statistics::kKeypointIndex_memory_usage(), _memory->getVWDictionary()->getIndexMemoryUsed());
 
 			//Epipolar geometry constraint
-			statistics_.addStatistic(Statistics::kLoopRejectedHypothesis(), rejectedGlobalLoopClosure?1.0f:0);
+			statistics_.addStatistic(Statistics::kLoopRejectedHypothesis(), rejectedLoopClosure?1.0f:0);
 
 			statistics_.addStatistic(Statistics::kMemorySmall_movement(), smallDisplacement?1.0f:0);
 			statistics_.addStatistic(Statistics::kMemoryDistance_travelled(), _distanceTravelled);
@@ -4243,7 +4286,7 @@ bool Rtabmap::process(
 		if(_startNewMapOnLoopClosure &&
 			_memory->isIncremental() &&              // only in mapping mode
 			graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size() == 0 &&      // alone in the current map
-			(landmarksDetected.empty() || rejectedLandmark) &&      // if we re not seeing a landmark from a previous map
+			(landmarksDetected.empty() || rejectedLoopClosure) &&      // if we re not seeing a landmark from a previous map
 			_memory->getWorkingMem().size()>=2)       // The working memory should not be empty (beside virtual signature)
 		{
 			UWARN("Ignoring location %d because a global loop closure is required before starting a new map!",
@@ -4263,7 +4306,7 @@ bool Rtabmap::process(
 		else if((smallDisplacement || tooFastMovement) &&
 				_loopClosureHypothesis.first == 0 &&
 				lastProximitySpaceClosureId == 0 &&
-				(rejectedLandmark || landmarksDetected.empty()) &&
+				(rejectedLoopClosure || landmarksDetected.empty()) &&
 				!addedNewLandmark)
 		{
 			// Don't delete the location if a loop closure is detected
@@ -4283,7 +4326,7 @@ bool Rtabmap::process(
 			_loopClosureHypothesis.first == 0 &&
 			lastProximitySpaceClosureId == 0 &&
 			!delayedLocalization &&
-			(rejectedLandmark || landmarksDetected.empty()))
+			(rejectedLoopClosure || landmarksDetected.empty()))
 	{
 		_odomCachePoses.erase(signatureRemoved);
 		for(std::multimap<int, Link>::iterator iter=_odomCacheConstraints.begin(); iter!=_odomCacheConstraints.end();)
@@ -4691,7 +4734,7 @@ bool Rtabmap::process(
 									refWordsCount,
 									dictionarySize,
 									int(_memory->getWorkingMem().size()),
-									rejectedGlobalLoopClosure?1:0,
+									rejectedLoopClosure?1:0,
 									0,
 									0,
 									int(signaturesRetrieved.size()),

corelib/src/SensorCaptureThread.cpp

@@ -530,7 +530,7 @@ void SensorCaptureThread::mainLoop()
 		info.odomPose.setNull();
 	}
 
-	if(!data.imageRaw().empty() || !data.laserScanRaw().empty() || (dynamic_cast<DBReader*>(_camera) != 0 && data.id()>0)) // intermediate nodes could not have image set
+	if(!data.imageCompressed().empty() || !data.imageRaw().empty() || !data.laserScanRaw().empty() || (dynamic_cast<DBReader*>(_camera) != 0 && data.id()>0)) // intermediate nodes could not have image set
 	{
 		postUpdate(&data, &info);
 		info.cameraName = _lidar?_lidar->getSerial():_camera->getSerial();

corelib/src/SensorData.cpp

@@ -107,6 +107,25 @@ SensorData::SensorData(
 	setUserData(userData);
 }
 
+// RGB-D constructor + confidence + laser scan
+SensorData::SensorData(
+		const LaserScan & laserScan,
+		const cv::Mat & rgb,
+		const cv::Mat & depth,
+		const cv::Mat & depthConfidence,
+		const CameraModel & cameraModel,
+		int id,
+		double stamp,
+		const cv::Mat & userData) :
+		_id(id),
+		_stamp(stamp),
+		_cellSize(0.0f)
+{
+	setRGBDImage(rgb, depth, depthConfidence, cameraModel);
+	setLaserScan(laserScan);
+	setUserData(userData);
+}
+
 // Multi-cameras RGB-D constructor
 SensorData::SensorData(
 		const cv::Mat & rgb,
@@ -123,6 +142,23 @@ SensorData::SensorData(
 	setUserData(userData);
 }
 
+// Multi-cameras RGB-D constructor + confidence
+SensorData::SensorData(
+		const cv::Mat & rgb,
+		const cv::Mat & depth,
+		const cv::Mat & depthConfidence,
+		const std::vector<CameraModel> & cameraModels,
+		int id,
+		double stamp,
+		const cv::Mat & userData) :
+		_id(id),
+		_stamp(stamp),
+		_cellSize(0.0f)
+{
+	setRGBDImage(rgb, depth, depthConfidence, cameraModels);
+	setUserData(userData);
+}
+
 // Multi-cameras RGB-D constructor + laser scan
 SensorData::SensorData(
 		const LaserScan & laserScan,
@@ -141,6 +177,25 @@ SensorData::SensorData(
 	setUserData(userData);
 }
 
+// Multi-cameras RGB-D constructor + confidence + laser scan
+SensorData::SensorData(
+		const LaserScan & laserScan,
+		const cv::Mat & rgb,
+		const cv::Mat & depth,
+		const cv::Mat & depthConfidence,
+		const std::vector<CameraModel> & cameraModels,
+		int id,
+		double stamp,
+		const cv::Mat & userData) :
+		_id(id),
+		_stamp(stamp),
+		_cellSize(0.0f)
+{
+	setRGBDImage(rgb, depth, depthConfidence, cameraModels);
+	setLaserScan(laserScan);
+	setUserData(userData);
+}
+
 // Stereo constructor
 SensorData::SensorData(
 		const cv::Mat & left,
@@ -234,9 +289,29 @@ void SensorData::setRGBDImage(
 	models.push_back(model);
 	setRGBDImage(rgb, depth, models, clearPreviousData);
 }
+void SensorData::setRGBDImage(
+	const cv::Mat & rgb,
+	const cv::Mat & depth,
+	const cv::Mat & depthConfidence,
+	const CameraModel & model,
+	bool clearPreviousData)
+{
+	std::vector<CameraModel> models;
+	models.push_back(model);
+	setRGBDImage(rgb, depth, depthConfidence, models, clearPreviousData);
+}
+void SensorData::setRGBDImage(
+	const cv::Mat & rgb,
+	const cv::Mat & depth,
+	const std::vector<CameraModel> & models,
+	bool clearPreviousData)
+{
+	setRGBDImage(rgb, depth, cv::Mat(), models, clearPreviousData);
+}
 void SensorData::setRGBDImage(
 		const cv::Mat & rgb,
 		const cv::Mat & depth,
+		const cv::Mat & depthConfidence,
 		const std::vector<CameraModel> & models,
 		bool clearPreviousData)
 {
@@ -300,6 +375,30 @@ void SensorData::setRGBDImage(
 		_depthOrRightRaw = cv::Mat();
 		_depthOrRightCompressed = cv::Mat();
 	}
+
+	if(depthConfidence.rows == 1)
+	{
+		UASSERT(depthConfidence.type() == CV_8UC1); // Bytes
+		_depthConfidenceCompressed = depthConfidence;
+		if(clearData)
+		{
+			_depthConfidenceRaw = cv::Mat();
+		}
+	}
+	else if(!depthConfidence.empty())
+	{
+		UASSERT(depthConfidence.type() == CV_8UC1);
+		_depthConfidenceRaw = depthConfidence;
+		if(clearData)
+		{
+			_depthConfidenceCompressed = cv::Mat();
+		}
+	}
+	else if(clearData)
+	{
+		_depthConfidenceRaw = cv::Mat();
+		_depthConfidenceCompressed = cv::Mat();
+	}
 }
 void SensorData::setStereoImage(
 		const cv::Mat & left,
@@ -365,7 +464,8 @@ void SensorData::setStereoImage(
 	}
 	else if(!right.empty())
 	{
-		UASSERT(right.type() == CV_8UC1); // Mono
+		UASSERT(right.type() == CV_8UC1 || // Mono
+				right.type() == CV_8UC3);  // RGB
 		_depthOrRightRaw = right;
 		if(clearData)
 		{
@@ -527,7 +627,7 @@ void SensorData::setOccupancyGrid(
 
 void SensorData::uncompressData()
 {
-	cv::Mat tmpA, tmpB, tmpD, tmpE, tmpF, tmpG;
+	cv::Mat tmpA, tmpB, tmpD, tmpE, tmpF, tmpG, tmpH;
 	LaserScan tmpC;
 	uncompressData(_imageCompressed.empty()?0:&tmpA,
 				_depthOrRightCompressed.empty()?0:&tmpB,
@@ -535,7 +635,8 @@ void SensorData::uncompressData()
 				_userDataCompressed.empty()?0:&tmpD,
 				_groundCellsCompressed.empty()?0:&tmpE,
 				_obstacleCellsCompressed.empty()?0:&tmpF,
-				_emptyCellsCompressed.empty()?0:&tmpG);
+				_emptyCellsCompressed.empty()?0:&tmpG,
+				_depthConfidenceCompressed.empty()?0:&tmpH);
 }
 
 void SensorData::uncompressData(
@@ -545,16 +646,27 @@ void SensorData::uncompressData(
 		cv::Mat * userDataRaw,
 		cv::Mat * groundCellsRaw,
 		cv::Mat * obstacleCellsRaw,
-		cv::Mat * emptyCellsRaw)
+		cv::Mat * emptyCellsRaw,
+		cv::Mat * depthConfidenceRaw)
 {
-	UDEBUG("%d data(%d,%d,%d,%d,%d,%d,%d)", this->id(), imageRaw?1:0, depthRaw?1:0, laserScanRaw?1:0, userDataRaw?1:0, groundCellsRaw?1:0, obstacleCellsRaw?1:0, emptyCellsRaw?1:0);
+	UDEBUG("%d data(%d,%d,%d,%d,%d,%d,%d,%d)", 
+		this->id(),
+		imageRaw?1:0,
+		depthRaw?1:0,
+		laserScanRaw?1:0,
+		userDataRaw?1:0,
+		groundCellsRaw?1:0,
+		obstacleCellsRaw?1:0, 
+		emptyCellsRaw?1:0,
+		depthConfidenceRaw?1:0);
 	if(imageRaw == 0 &&
 		depthRaw == 0 &&
 		laserScanRaw == 0 &&
 		userDataRaw == 0 &&
 		groundCellsRaw == 0 &&
 		obstacleCellsRaw == 0 &&
-		emptyCellsRaw == 0)
+		emptyCellsRaw == 0 &&
+		depthConfidenceRaw == 0)
 	{
 		return;
 	}
@@ -565,7 +677,8 @@ void SensorData::uncompressData(
 			userDataRaw,
 			groundCellsRaw,
 			obstacleCellsRaw,
-			emptyCellsRaw);
+			emptyCellsRaw,
+			depthConfidenceRaw);
 
 	if(imageRaw && !imageRaw->empty() && _imageRaw.empty())
 	{
@@ -587,6 +700,10 @@ void SensorData::uncompressData(
 	{
 		_depthOrRightRaw = *depthRaw;
 	}
+	if(depthConfidenceRaw && !depthConfidenceRaw->empty() && _depthConfidenceRaw.empty())
+	{
+		_depthConfidenceRaw = *depthConfidenceRaw;
+	}
 	if(laserScanRaw && !laserScanRaw->isEmpty() && _laserScanRaw.isEmpty())
 	{
 		_laserScanRaw = *laserScanRaw;
@@ -627,7 +744,8 @@ void SensorData::uncompressDataConst(
 		cv::Mat * userDataRaw,
 		cv::Mat * groundCellsRaw,
 		cv::Mat * obstacleCellsRaw,
-		cv::Mat * emptyCellsRaw) const
+		cv::Mat * emptyCellsRaw,
+		cv::Mat * depthConfidenceRaw) const
 {
 	if(imageRaw)
 	{
@@ -637,6 +755,10 @@ void SensorData::uncompressDataConst(
 	{
 		*depthRaw = _depthOrRightRaw;
 	}
+	if(depthConfidenceRaw)
+	{
+		*depthConfidenceRaw = _depthConfidenceRaw;
+	}
 	if(laserScanRaw)
 	{
 		*laserScanRaw = _laserScanRaw;
@@ -659,6 +781,7 @@ void SensorData::uncompressDataConst(
 	}
 	if( (imageRaw && imageRaw->empty()) ||
 		(depthRaw && depthRaw->empty()) ||
+		(depthConfidenceRaw && depthConfidenceRaw->empty()) ||
 		(laserScanRaw && laserScanRaw->isEmpty()) ||
 		(userDataRaw && userDataRaw->empty()) ||
 		(groundCellsRaw && groundCellsRaw->empty()) ||
@@ -667,6 +790,7 @@ void SensorData::uncompressDataConst(
 	{
 		rtabmap::CompressionThread ctImage(_imageCompressed, true);
 		rtabmap::CompressionThread ctDepth(_depthOrRightCompressed, true);
+		rtabmap::CompressionThread ctDepthConfidence(_depthConfidenceCompressed, false);
 		rtabmap::CompressionThread ctLaserScan(_laserScanCompressed.data(), false);
 		rtabmap::CompressionThread ctUserData(_userDataCompressed, false);
 		rtabmap::CompressionThread ctGroundCells(_groundCellsCompressed, false);
@@ -682,6 +806,11 @@ void SensorData::uncompressDataConst(
 			UASSERT(_depthOrRightCompressed.type() == CV_8UC1);
 			ctDepth.start();
 		}
+		if(depthConfidenceRaw && depthConfidenceRaw->empty() && !_depthConfidenceCompressed.empty())
+		{
+			UASSERT(_depthConfidenceCompressed.type() == CV_8UC1);
+			ctDepthConfidence.start();
+		}
 		if(laserScanRaw && laserScanRaw->isEmpty() && !_laserScanCompressed.isEmpty())
 		{
 			UASSERT(_laserScanCompressed.isCompressed());
@@ -709,6 +838,7 @@ void SensorData::uncompressDataConst(
 		}
 		ctImage.join();
 		ctDepth.join();
+		ctDepthConfidence.join();
 		ctLaserScan.join();
 		ctUserData.join();
 		ctGroundCells.join();
@@ -745,6 +875,21 @@ void SensorData::uncompressDataConst(
 				}
 			}
 		}
+		if(depthConfidenceRaw && depthConfidenceRaw->empty())
+		{
+			*depthConfidenceRaw = ctDepthConfidence.getUncompressedData();
+			if(depthConfidenceRaw->empty())
+			{
+				if(_depthConfidenceCompressed.empty())
+				{
+					UWARN("Requested depth confidence data, but the sensor data (%d) doesn't have depth confidence.", this->id());
+				}
+				else
+				{
+					UERROR("Requested depth confidence data, but failed to uncompress (%d).", this->id());
+				}
+			}
+		}
 		if(laserScanRaw && laserScanRaw->isEmpty())
 		{
 			if(_laserScanCompressed.angleIncrement() > 0.0f)
@@ -814,6 +959,8 @@ unsigned long SensorData::getMemoryUsed() const // Return memory usage in Bytes
 			(_imageRaw.empty()?0:_imageRaw.total()*_imageRaw.elemSize()) +
 			(_depthOrRightCompressed.empty()?0:_depthOrRightCompressed.total()*_depthOrRightCompressed.elemSize()) +
 			(_depthOrRightRaw.empty()?0:_depthOrRightRaw.total()*_depthOrRightRaw.elemSize()) +
+			(_depthConfidenceCompressed.empty()?0:_depthConfidenceCompressed.total()*_depthConfidenceCompressed.elemSize()) +
+			(_depthConfidenceRaw.empty()?0:_depthConfidenceRaw.total()*_depthConfidenceRaw.elemSize()) +
 			(_userDataCompressed.empty()?0:_userDataCompressed.total()*_userDataCompressed.elemSize()) +
 			(_userDataRaw.empty()?0:_userDataRaw.total()*_userDataRaw.elemSize()) +
 			(_laserScanCompressed.empty()?0:_laserScanCompressed.data().total()*_laserScanCompressed.data().elemSize()) +
@@ -835,6 +982,7 @@ void SensorData::clearCompressedData(bool images, bool scan, bool userData)
 	{
 		_imageCompressed=cv::Mat();
 		_depthOrRightCompressed=cv::Mat();
+		_depthConfidenceCompressed=cv::Mat();
 	}
 	if(scan)
 	{
@@ -851,6 +999,7 @@ void SensorData::clearRawData(bool images, bool scan, bool userData)
 	{
 		_imageRaw=cv::Mat();
 		_depthOrRightRaw=cv::Mat();
+		_depthConfidenceRaw=cv::Mat();
 	}
 	if(scan)
 	{

corelib/src/Stereo.cpp

@@ -79,6 +79,8 @@ std::vector<cv::Point2f> Stereo::computeCorrespondences(
 		const std::vector<cv::Point2f> & leftCorners,
 		std::vector<unsigned char> & status) const
 {
+	UASSERT(leftImage.type() == CV_8UC1);
+	UASSERT(rightImage.type() == CV_8UC1);
 	std::vector<cv::Point2f> rightCorners;
 	UDEBUG("util2d::calcStereoCorrespondences() begin");
 	rightCorners = util2d::calcStereoCorrespondences(
@@ -145,6 +147,8 @@ std::vector<cv::Point2f> StereoOpticalFlow::computeCorrespondences(
 		const std::vector<cv::Point2f> & leftCorners,
 		std::vector<unsigned char> & status) const
 {
+	UASSERT(leftImage.type() == CV_8UC1);
+	UASSERT(rightImage.type() == CV_8UC1);
 	std::vector<cv::Point2f> rightCorners;
 	std::vector<float> err;
 #ifdef HAVE_OPENCV_CUDAOPTFLOW
@@ -184,6 +188,8 @@ std::vector<cv::Point2f> StereoOpticalFlow::computeCorrespondences(
 {
 	std::vector<cv::Point2f> rightCorners;
 #ifdef HAVE_OPENCV_CUDAOPTFLOW
+	UASSERT(leftImage.type() == CV_8UC1);
+	UASSERT(rightImage.type() == CV_8UC1);
 	UDEBUG("cv::cuda::SparsePyrLKOpticalFlow transfer host to device begin");
 	cv::cuda::GpuMat d_leftImage(leftImage);
 	cv::cuda::GpuMat d_rightImage(rightImage);

corelib/src/Transform.cpp

@@ -273,11 +273,9 @@ void Transform::getTranslation(float & x, float & y, float & z) const
 	z = this->z();
 }
 
-float Transform::getAngle(float x, float y, float z) const
+float Transform::getAngle(const Transform & t) const
 {
-	Eigen::Vector3f vA(x,y,z);
-	Eigen::Vector3f vB = this->toEigen3f().linear()*Eigen::Vector3f(1,0,0);
-	return pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
+	return getQuaternionf().angularDistance(t.getQuaternionf());
 }
 
 float Transform::getNorm() const

corelib/src/camera/CameraDepthAI.cpp

@@ -47,7 +47,7 @@ bool CameraDepthAI::available()
 
 CameraDepthAI::CameraDepthAI(
 		const std::string & mxidOrName,
-		int resolution,
+		int imageWidth,
 		float imageRate,
 		const Transform & localTransform) :
 	Camera(imageRate, localTransform)
@@ -57,12 +57,15 @@ CameraDepthAI::CameraDepthAI(
 	outputMode_(0),
 	confThreshold_(200),
 	lrcThreshold_(5),
-	resolution_(resolution),
+	imageWidth_(imageWidth),
 	extendedDisparity_(false),
-	subpixelFractionalBits_(0),
-	compandingWidth_(0),
+	enableCompanding_(false),
+	subpixelFractionalBits_(3),
+	disparityWidth_(1),
+	medianFilter_(5),
 	useSpecTranslation_(false),
 	alphaScaling_(0.0),
+	imagesRectified_(true),
 	imuPublished_(true),
 	publishInterIMU_(false),
 	dotIntensity_(0.0),
@@ -70,14 +73,16 @@ CameraDepthAI::CameraDepthAI(
 	detectFeatures_(0),
 	useHarrisDetector_(false),
 	minDistance_(7.0),
-	numTargetFeatures_(1000),
+	numTargetFeatures_(320),
 	threshold_(0.01),
 	nms_(true),
 	nmsRadius_(4)
 #endif
 {
 #ifdef RTABMAP_DEPTHAI
-	UASSERT(resolution_>=(int)dai::MonoCameraProperties::SensorResolution::THE_720_P && resolution_<=(int)dai::MonoCameraProperties::SensorResolution::THE_1200_P);
+	UASSERT(imageWidth_ == 640 || imageWidth_ == 1280);
+	if(this->getImageRate() <= 0)
+		this->setImageRate(30);
 #endif
 }
 
@@ -85,9 +90,7 @@ CameraDepthAI::~CameraDepthAI()
 {
 #ifdef RTABMAP_DEPTHAI
 	if(device_.get())
-	{
 		device_->close();
-	}
 #endif
 }
 
@@ -110,22 +113,15 @@ void CameraDepthAI::setDepthProfile(int confThreshold, int lrcThreshold)
 #endif
 }
 
-void CameraDepthAI::setExtendedDisparity(bool extendedDisparity)
+void CameraDepthAI::setExtendedDisparity(bool extendedDisparity, bool enableCompanding)
 {
 #ifdef RTABMAP_DEPTHAI
 	extendedDisparity_ = extendedDisparity;
-	if(extendedDisparity_)
+	enableCompanding_ = enableCompanding;
+	if(extendedDisparity_ && enableCompanding_)
 	{
-		if(subpixelFractionalBits_>0)
-		{
-			UWARN("Extended disparity has been enabled while subpixel being also enabled, disabling subpixel...");
-			subpixelFractionalBits_ = 0;
-		}
-		if(compandingWidth_>0)
-		{
-			UWARN("Extended disparity has been enabled while companding being also enabled, disabling companding...");
-			compandingWidth_ = 0;
-		}
+		UWARN("Extended disparity has been enabled while companding being also enabled, disabling companding...");
+		enableCompanding_ = false;
 	}
 #else
 	UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
@@ -137,25 +133,22 @@ void CameraDepthAI::setSubpixelMode(bool enabled, int fractionalBits)
 #ifdef RTABMAP_DEPTHAI
 	UASSERT(fractionalBits>=3 && fractionalBits<=5);
 	subpixelFractionalBits_ = enabled?fractionalBits:0;
-	if(subpixelFractionalBits_ != 0 && extendedDisparity_)
-	{
-		UWARN("Subpixel has been enabled while extended disparity being also enabled, disabling extended disparity...");
-		extendedDisparity_ = false;
-	}
 #else
 	UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
 #endif
 }
 
-void CameraDepthAI::setCompanding(bool enabled, int width)
+void CameraDepthAI::setDisparityWidthAndFilter(int disparityWidth, int medianFilter)
 {
 #ifdef RTABMAP_DEPTHAI
-	UASSERT(width == 64 || width == 96);
-	compandingWidth_ = enabled?width:0;
-	if(compandingWidth_ != 0 && extendedDisparity_)
+	UASSERT(disparityWidth == 64 || disparityWidth == 96);
+	disparityWidth_ = disparityWidth;
+	medianFilter_ = medianFilter;
+	int maxDisp = (extendedDisparity_?2:1) * std::pow(2,subpixelFractionalBits_) * (disparityWidth_-1);
+	if(medianFilter_ && maxDisp > 1024)
 	{
-		UWARN("Companding has been enabled while extended disparity being also enabled, disabling extended disparity...");
-		extendedDisparity_ = false;
+		UWARN("Maximum disparity value '%d' exceeds the maximum supported '1024' by median filter, disabling median filter...", maxDisp);
+		medianFilter_ = 0;
 	}
 #else
 	UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
@@ -193,19 +186,21 @@ void CameraDepthAI::setIrIntensity(float dotIntensity, float floodIntensity)
 #endif
 }
 
-void CameraDepthAI::setDetectFeatures(int detectFeatures)
+void CameraDepthAI::setDetectFeatures(int detectFeatures, const std::string & blobPath)
 {
 #ifdef RTABMAP_DEPTHAI
 	detectFeatures_ = detectFeatures;
-#else
-	UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
-#endif
-}
-
-void CameraDepthAI::setBlobPath(const std::string & blobPath)
-{
-#ifdef RTABMAP_DEPTHAI
 	blobPath_ = blobPath;
+	if(detectFeatures_>=2 && blobPath_.empty())
+	{
+		UWARN("Missing MyriadX blob file, disabling on-device feature detector");
+		detectFeatures_ = 0;
+	}
+	if(detectFeatures_>=2 && this->getImageRate()>15)
+	{
+		UWARN("On-device SuperPoint or HF-Net enabled, image rate is limited to 15 FPS!");
+		this->setImageRate(15);
+	}
 #else
 	UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
 #endif
@@ -245,9 +240,6 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 		return false;
 	}
 
-	if(device_.get())
-		device_->close();
-
 	accBuffer_.clear();
 	gyroBuffer_.clear();
 
@@ -268,259 +260,22 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 		return false;
 	}
 
-	// look for calibration files
-	stereoModel_ = StereoCameraModel();
-	targetSize_ = cv::Size(resolution_<2?1280:resolution_==4?1920:640, resolution_==0?720:resolution_==1?800:resolution_==2?400:resolution_==3?480:1200);
-
-	dai::Pipeline p;
-	auto monoLeft  = p.create<dai::node::MonoCamera>();
-	auto monoRight = p.create<dai::node::MonoCamera>();
-	std::shared_ptr<dai::node::StereoDepth> stereo;
-	if(imagesRectified_)
-		stereo = p.create<dai::node::StereoDepth>();
-	std::shared_ptr<dai::node::Camera> colorCam;
-	if(outputMode_==2)
-	{
-		colorCam = p.create<dai::node::Camera>();
-		if(!imagesRectified_)
-			colorCam->setMeshSource(dai::CameraProperties::WarpMeshSource::NONE);
-		if(detectFeatures_)
-		{
-			UWARN("On-device feature detectors cannot be enabled on color camera input!");
-			detectFeatures_ = 0;
-		}
-	}
-	std::shared_ptr<dai::node::IMU> imu;
-	if(imuPublished_)
-		imu = p.create<dai::node::IMU>();
-	std::shared_ptr<dai::node::FeatureTracker> gfttDetector;
-	std::shared_ptr<dai::node::ImageManip> manip;
-	std::shared_ptr<dai::node::NeuralNetwork> neuralNetwork;
-	if(detectFeatures_ == 1)
-	{
-		gfttDetector = p.create<dai::node::FeatureTracker>();
-	}
-	else if(detectFeatures_ >= 2)
-	{
-		if(!blobPath_.empty())
-		{
-			manip = p.create<dai::node::ImageManip>();
-			neuralNetwork = p.create<dai::node::NeuralNetwork>();
-		}
-		else
-		{
-			UWARN("Missing MyriadX blob file!");
-			detectFeatures_ = 0;
-		}
-	}
-
-	auto sync = p.create<dai::node::Sync>();
-	auto xoutCamera = p.create<dai::node::XLinkOut>();
-	std::shared_ptr<dai::node::XLinkOut> xoutIMU;
-	if(imuPublished_)
-		xoutIMU = p.create<dai::node::XLinkOut>();
-
-	// XLinkOut
-	xoutCamera->setStreamName("camera");
-	if(imuPublished_)
-		xoutIMU->setStreamName("imu");
-
-	monoLeft->setResolution((dai::MonoCameraProperties::SensorResolution)resolution_);
-	monoRight->setResolution((dai::MonoCameraProperties::SensorResolution)resolution_);
-	monoLeft->setCamera("left");
-	monoRight->setCamera("right");
-	if(detectFeatures_ >= 2)
-	{
-		if(this->getImageRate() <= 0 || this->getImageRate() > 15)
-		{
-			UWARN("On-device SuperPoint or HF-Net enabled, image rate is limited to 15 FPS!");
-			monoLeft->setFps(15);
-			monoRight->setFps(15);
-		}
-	}
-	else if(this->getImageRate() > 0)
-	{
-		monoLeft->setFps(this->getImageRate());
-		monoRight->setFps(this->getImageRate());
-	}
-
-	// StereoDepth
-	if(stereo.get())
-	{
-		if(outputMode_ == 2)
-			stereo->setDepthAlign(dai::CameraBoardSocket::CAM_A);
-		else
-			stereo->setDepthAlign(dai::StereoDepthProperties::DepthAlign::RECTIFIED_LEFT);
-		stereo->setExtendedDisparity(extendedDisparity_);
-		stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout
-		stereo->enableDistortionCorrection(true);
-		stereo->setDisparityToDepthUseSpecTranslation(useSpecTranslation_);
-		stereo->setDepthAlignmentUseSpecTranslation(useSpecTranslation_);
-		if(alphaScaling_ > -1.0f)
-			stereo->setAlphaScaling(alphaScaling_);
-		stereo->initialConfig.setConfidenceThreshold(confThreshold_);
-		stereo->initialConfig.setLeftRightCheck(lrcThreshold_>=0);
-		if(lrcThreshold_>=0)
-			stereo->initialConfig.setLeftRightCheckThreshold(lrcThreshold_);
-		stereo->initialConfig.setMedianFilter(dai::MedianFilter::KERNEL_7x7);
-		auto config = stereo->initialConfig.get();
-		config.censusTransform.kernelSize = dai::StereoDepthConfig::CensusTransform::KernelSize::KERNEL_7x9;
-		config.censusTransform.kernelMask = 0X2AA00AA805540155;
-		config.postProcessing.brightnessFilter.maxBrightness = 255;
-		stereo->initialConfig.set(config);
-
-		// Link plugins CAM -> STEREO -> XLINK
-		monoLeft->out.link(stereo->left);
-		monoRight->out.link(stereo->right);
-	}
-
-	if(outputMode_ == 2)
-	{
-		colorCam->setBoardSocket(dai::CameraBoardSocket::CAM_A);
-		colorCam->setSize(targetSize_.width, targetSize_.height);
-		if(this->getImageRate() > 0)
-			colorCam->setFps(this->getImageRate());
-		if(alphaScaling_ > -1.0f)
-			colorCam->setCalibrationAlpha(alphaScaling_);
-	}
-	this->setImageRate(0);
-
-	// Using VideoEncoder on PoE devices, Subpixel is not supported
-	if(deviceToUse.protocol == X_LINK_TCP_IP || mxidOrName_.find(".") != std::string::npos)
-	{
-		auto leftOrColorEnc  = p.create<dai::node::VideoEncoder>();
-		auto depthOrRightEnc  = p.create<dai::node::VideoEncoder>();
-		leftOrColorEnc->setDefaultProfilePreset(monoLeft->getFps(), dai::VideoEncoderProperties::Profile::MJPEG);
-		depthOrRightEnc->setDefaultProfilePreset(monoRight->getFps(), dai::VideoEncoderProperties::Profile::MJPEG);
-		if(outputMode_ < 2)
-		{
-			if(imagesRectified_) {
-				stereo->rectifiedLeft.link(leftOrColorEnc->input);
-			}
-			else {
-				monoLeft->out.link(leftOrColorEnc->input);
-			}
-			leftOrColorEnc->bitstream.link(sync->inputs["left"]);
-		}
-		else
-		{
-			colorCam->video.link(leftOrColorEnc->input);
-			leftOrColorEnc->bitstream.link(sync->inputs["color"]);
-		}
-		if(imagesRectified_ && outputMode_)
-		{
-			depthOrRightEnc->setQuality(100);
-			stereo->disparity.link(depthOrRightEnc->input);
-			depthOrRightEnc->bitstream.link(sync->inputs["depth"]);
-		}
-		else
-		{
-			if(imagesRectified_) {
-				stereo->rectifiedRight.link(depthOrRightEnc->input);
-			}
-			else {
-				monoRight->out.link(depthOrRightEnc->input);
-			}
-			depthOrRightEnc->bitstream.link(sync->inputs["right"]);
-		}
-	}
-	else
-	{
-		if(stereo.get()) {
-			stereo->setSubpixel(subpixelFractionalBits_>=3 && subpixelFractionalBits_<=5);
-			if(subpixelFractionalBits_>=3 && subpixelFractionalBits_<=5)
-				stereo->setSubpixelFractionalBits(subpixelFractionalBits_);
-			auto config = stereo->initialConfig.get();
-			config.costMatching.enableCompanding = compandingWidth_>0;
-			if(compandingWidth_>0)
-				config.costMatching.disparityWidth = compandingWidth_==64?dai::StereoDepthConfig::CostMatching::DisparityWidth::DISPARITY_64:dai::StereoDepthConfig::CostMatching::DisparityWidth::DISPARITY_96;
-			stereo->initialConfig.set(config);
-		}
-		if(outputMode_ < 2)
-		{
-			if(imagesRectified_)
-				stereo->rectifiedLeft.link(sync->inputs["left"]);
-			else
-				monoLeft->out.link(sync->inputs["left"]);
-		}
-		else
-		{
-			monoLeft->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P);
-			monoRight->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P);
-			colorCam->video.link(sync->inputs["color"]);
-		}
-		if(imagesRectified_) {
-			if(outputMode_)
-				stereo->depth.link(sync->inputs["depth"]);
-			else
-				stereo->rectifiedRight.link(sync->inputs["right"]);
-		}
-		else {
-			monoRight->out.link(sync->inputs["right"]);
-		}
-	}
-
-	sync->setSyncThreshold(std::chrono::milliseconds(int(500 / monoLeft->getFps())));
-	sync->out.link(xoutCamera->input);
-
-	if(imuPublished_)
-	{
-		// enable ACCELEROMETER_RAW and GYROSCOPE_RAW at 200 hz rate
-		imu->enableIMUSensor({dai::IMUSensor::ACCELEROMETER_RAW, dai::IMUSensor::GYROSCOPE_RAW}, 200);
-		// above this threshold packets will be sent in batch of X, if the host is not blocked and USB bandwidth is available
-		imu->setBatchReportThreshold(1);
-		// maximum number of IMU packets in a batch, if it's reached device will block sending until host can receive it
-		// if lower or equal to batchReportThreshold then the sending is always blocking on device
-		// useful to reduce device's CPU load  and number of lost packets, if CPU load is high on device side due to multiple nodes
-		imu->setMaxBatchReports(10);
-
-		// Link plugins IMU -> XLINK
-		imu->out.link(xoutIMU->input);
-	}
-
-	if(detectFeatures_ == 1)
-	{
-		gfttDetector->setHardwareResources(1, 2);
-		gfttDetector->initialConfig.setCornerDetector(
-			useHarrisDetector_?dai::FeatureTrackerConfig::CornerDetector::Type::HARRIS:dai::FeatureTrackerConfig::CornerDetector::Type::SHI_THOMASI);
-		gfttDetector->initialConfig.setNumTargetFeatures(numTargetFeatures_);
-		gfttDetector->initialConfig.setMotionEstimator(false);
-		auto cfg = gfttDetector->initialConfig.get();
-		cfg.featureMaintainer.minimumDistanceBetweenFeatures = minDistance_ * minDistance_;
-		gfttDetector->initialConfig.set(cfg);
-		if(stereo.get())
-			stereo->rectifiedLeft.link(gfttDetector->inputImage);
-		else
-			monoLeft->out.link(gfttDetector->inputImage);
-		gfttDetector->outputFeatures.link(sync->inputs["feat"]);
-	}
-	else if(detectFeatures_ >= 2)
-	{
-		manip->setKeepAspectRatio(false);
-		manip->setMaxOutputFrameSize(320 * 200);
-		manip->initialConfig.setResize(320, 200);
-		neuralNetwork->setBlobPath(blobPath_);
-		neuralNetwork->setNumInferenceThreads(2);
-		neuralNetwork->setNumNCEPerInferenceThread(1);
-		neuralNetwork->input.setBlocking(false);
-		if(stereo.get())
-			stereo->rectifiedLeft.link(manip->inputImage);
-		else
-			monoLeft->out.link(manip->inputImage);
-		manip->out.link(neuralNetwork->input);
-		neuralNetwork->out.link(sync->inputs["feat"]);
-	}
-
-	device_.reset(new dai::Device(p, deviceToUse));
-
-	UINFO("Device serial: %s", device_->getMxId().c_str());
-	UINFO("Available camera sensors: ");
+	device_ = std::make_unique<dai::Device>(deviceToUse);
+	auto deviceName = device_->getDeviceName();
+	auto imuType = device_->getConnectedIMU();
+	UINFO("Device Name: %s, Device Serial: %s", deviceName.c_str(), device_->getMxId().c_str());
+	UINFO("Available Camera Sensors: ");
 	for(auto& sensor : device_->getCameraSensorNames()) {
 		UINFO("Socket: CAM_%c - %s", 'A'+(unsigned char)sensor.first, sensor.second.c_str());
 	}
+	UINFO("IMU Type: %s", imuType.c_str());
 
 	UINFO("Loading eeprom calibration data");
-	dai::CalibrationHandler calibHandler = device_->readCalibration();
+	auto calibHandler = device_->readCalibration();
+	auto boardName = calibHandler.getEepromData().boardName;
+
+	stereoModel_ = StereoCameraModel();
+	targetSize_ = cv::Size(imageWidth_, imageWidth_/640*((outputMode_==2&&boardName!="BC2087")?360:400));
 
 	if(!calibrationFolder.empty() && !cameraName.empty() && imagesRectified_)
 	{
@@ -591,10 +346,6 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 					std::cout << "Expected K with rectification_alpha=0: " << stereoModel_.left().K()*(double(targetSize_.width)/double(stereoModel_.left().imageWidth())) << std::endl;
 				}
 				device_->flashCalibration2(calibHandler);
-				UINFO("Closing device...");
-				device_->close();
-				UINFO("Restarting pipeline...");
-				device_.reset(new dai::Device(p, deviceToUse));
 			}
 			catch(const std::runtime_error & e) {
 				UERROR("Failed flashing calibration: %s", e.what());
@@ -609,10 +360,7 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 		calibHandler = device_->readCalibration();
 	}
 
-	auto eeprom = calibHandler.getEepromData();
-	UINFO("Product name: %s, board name: %s", eeprom.productName.c_str(), eeprom.boardName.c_str());
-
-	auto cameraId = outputMode_<2?dai::CameraBoardSocket::CAM_B:dai::CameraBoardSocket::CAM_A;
+	auto cameraId = outputMode_==2?dai::CameraBoardSocket::CAM_A:dai::CameraBoardSocket::CAM_B;
 	cv::Mat cameraMatrix, distCoeffs, newCameraMatrix;
 
 	std::vector<std::vector<float> > matrix = calibHandler.getCameraIntrinsics(cameraId, targetSize_.width, targetSize_.height);
@@ -636,15 +384,15 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 	double cy = newCameraMatrix.at<double>(1, 2);
 	UINFO("fx=%f fy=%f cx=%f cy=%f (target size = %dx%d)", fx, fy, cx, cy, targetSize_.width, targetSize_.height);
 	if(outputMode_ == 2) {
-		stereoModel_ = StereoCameraModel(device_->getDeviceName(), fx, fy, cx, cy, 0, this->getLocalTransform(), targetSize_);
+		stereoModel_ = StereoCameraModel(deviceName, fx, fy, cx, cy, 0, this->getLocalTransform(), targetSize_);
 	}
 	else {
 		double baseline = calibHandler.getBaselineDistance(dai::CameraBoardSocket::CAM_C, dai::CameraBoardSocket::CAM_B, false)/100.0;
 		UINFO("baseline=%f", baseline);
-		stereoModel_ = StereoCameraModel(device_->getDeviceName(), fx, fy, cx, cy, outputMode_==0?baseline:0, this->getLocalTransform()*Transform(-calibHandler.getBaselineDistance(dai::CameraBoardSocket::CAM_A)/100.0, 0, 0), targetSize_);
+		stereoModel_ = StereoCameraModel(deviceName, fx, fy, cx, cy, outputMode_==0?baseline:0, this->getLocalTransform()*Transform(-calibHandler.getBaselineDistance(dai::CameraBoardSocket::CAM_A)/100.0, 0, 0), targetSize_);
 	}
 
-	if(imuPublished_)
+	if(imuPublished_ || imuType.empty())
 	{
 		// Cannot test the following, I get "IMU calibration data is not available on device yet." with my camera
 		// Update: now (as March 6, 2022) it crashes in "dai::CalibrationHandler::getImuToCameraExtrinsics(dai::CameraBoardSocket, bool)"
@@ -653,44 +401,271 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 		//		matrix[0][0], matrix[0][1], matrix[0][2], matrix[0][3],
 		//		matrix[1][0], matrix[1][1], matrix[1][2], matrix[1][3],
 		//		matrix[2][0], matrix[2][1], matrix[2][2], matrix[2][3]);
-		if(eeprom.boardName == "OAK-D" ||
-		   eeprom.boardName == "BW1098OBC")
+		if(deviceName == "OAK-D")
 		{
 			imuLocalTransform_ = Transform(
 				 0, -1,  0,  0.0525,
 				 1,  0,  0,  0.013662,
 				 0,  0,  1,  0);
 		}
-		else if(eeprom.boardName == "DM9098")
+		else if(boardName == "BC2087") // OAK-D LR
+		{
+			imuLocalTransform_ = Transform(
+				1,  0,  0,  0.021425,
+				0,  1,  0,  0.009925,
+				0,  0,  1,  0);
+		}
+		else if(boardName == "DM2080") // OAK-D SR
+		{
+			imuLocalTransform_ = Transform(
+			   	-1,  0,  0,  0,
+				 0, -1,  0, -0.0024,
+				 0,  0,  1,  0);
+		}
+		else if(boardName == "DM9098") // OAK-D S2, OAK-D W, OAK-D Pro, OAK-D Pro W
 		{
 			imuLocalTransform_ = Transform(
 				 0,  1,  0,  0.037945,
 				 1,  0,  0,  0.00079,
 				 0,  0, -1,  0);
 		}
-		else if(eeprom.boardName == "NG2094")
+		else if(boardName == "NG2094") // OAK-D Pro W Dev
 		{
 			imuLocalTransform_ = Transform(
 				 0,  1,  0,  0.0374,
 				 1,  0,  0,  0.00176,
 				 0,  0, -1,  0);
 		}
-		else if(eeprom.boardName == "NG9097")
+		else if(boardName == "NG9097") // OAK-D S2 PoE, OAK-D W PoE, OAK-D Pro PoE, OAK-D Pro W PoE
 		{
-			imuLocalTransform_ = Transform(
-				 0,  1,  0,  0.04,
-				 1,  0,  0,  0.020265,
-				 0,  0, -1,  0);
+			if(imuType == "BMI270")
+			{
+				imuLocalTransform_ = Transform(
+				 	 0,  1,  0,  0.04,
+					 1,  0,  0,  0.020265,
+					 0,  0, -1,  0);
+			}
+			else // BNO085/086
+			{
+				imuLocalTransform_ = Transform(
+				 	 0, -1,  0,  0.04,
+					-1,  0,  0,  0.020265,
+					 0,  0, -1,  0);
+			}
 		}
 		else
 		{
-			UWARN("Unknown boardName (%s)! Disabling IMU!", eeprom.boardName.c_str());
+			UWARN("Unsupported boardName (%s)! Disabling IMU!", boardName.c_str());
 			imuPublished_ = false;
 		}
 	}
 	else
 	{
 		UINFO("IMU disabled");
+		imuPublished_ = false;
+	}
+
+	dai::Pipeline pipeline;
+
+	auto sync = pipeline.create<dai::node::Sync>();
+	sync->setSyncThreshold(std::chrono::milliseconds(int(500 / this->getImageRate())));
+
+	std::shared_ptr<dai::node::Camera> rgbCamera;
+	if(outputMode_ == 2)
+	{
+		rgbCamera = pipeline.create<dai::node::Camera>();
+		rgbCamera->setCamera("color");
+		if(boardName == "BC2087")
+			rgbCamera->setSize(1920, 1200);
+		else if(boardName == "NG2094")
+			rgbCamera->setSize(1280, 720);
+		else
+			rgbCamera->setSize(1920, 1080);
+		rgbCamera->setVideoSize(targetSize_.width, targetSize_.height);
+		rgbCamera->setPreviewSize(targetSize_.width, targetSize_.height);
+		rgbCamera->setFps(this->getImageRate());
+		rgbCamera->setMeshSource(imagesRectified_?dai::CameraProperties::WarpMeshSource::CALIBRATION:dai::CameraProperties::WarpMeshSource::NONE);
+		if(imagesRectified_ && alphaScaling_>-1.0f)
+			rgbCamera->setCalibrationAlpha(alphaScaling_);
+		rgbCamera->properties.ispScale.horizNumerator = rgbCamera->properties.ispScale.vertNumerator = imageWidth_/640;
+		rgbCamera->properties.ispScale.horizDenominator = rgbCamera->properties.ispScale.vertDenominator = boardName=="NG2094"?2:3;
+
+		auto rgbEncoder  = pipeline.create<dai::node::VideoEncoder>();
+		rgbEncoder->setDefaultProfilePreset(this->getImageRate(), dai::VideoEncoderProperties::Profile::MJPEG);
+
+		rgbCamera->video.link(rgbEncoder->input);
+		rgbEncoder->bitstream.link(sync->inputs["rgb"]);
+	}
+
+	auto stereoDepth = pipeline.create<dai::node::StereoDepth>();
+	if(outputMode_ == 2)
+		stereoDepth->setDepthAlign(dai::CameraBoardSocket::CAM_A);
+	else
+		stereoDepth->setDepthAlign(dai::StereoDepthProperties::DepthAlign::RECTIFIED_LEFT);
+	if(subpixelFractionalBits_>=3 && subpixelFractionalBits_<=5)
+	{
+		stereoDepth->setSubpixel(true);
+		stereoDepth->setSubpixelFractionalBits(subpixelFractionalBits_);
+	}
+	stereoDepth->setExtendedDisparity(extendedDisparity_);
+	stereoDepth->enableDistortionCorrection(true);
+	stereoDepth->setDisparityToDepthUseSpecTranslation(useSpecTranslation_);
+	stereoDepth->setDepthAlignmentUseSpecTranslation(useSpecTranslation_);
+	if(alphaScaling_ > -1.0f)
+		stereoDepth->setAlphaScaling(alphaScaling_);
+	stereoDepth->initialConfig.setConfidenceThreshold(confThreshold_);
+	stereoDepth->initialConfig.setLeftRightCheck(lrcThreshold_>=0);
+	if(lrcThreshold_>=0)
+		stereoDepth->initialConfig.setLeftRightCheckThreshold(lrcThreshold_);
+	stereoDepth->initialConfig.setMedianFilter(dai::MedianFilter(medianFilter_));
+	auto config = stereoDepth->initialConfig.get();
+	config.censusTransform.kernelSize = dai::StereoDepthConfig::CensusTransform::KernelSize::KERNEL_7x9;
+	config.censusTransform.kernelMask = 0X5092A28C5152428;
+	config.costMatching.disparityWidth = disparityWidth_==64?dai::StereoDepthConfig::CostMatching::DisparityWidth::DISPARITY_64:dai::StereoDepthConfig::CostMatching::DisparityWidth::DISPARITY_96;
+	config.costMatching.enableCompanding = enableCompanding_;
+	config.costMatching.linearEquationParameters.alpha = 2;
+	config.costMatching.linearEquationParameters.beta = 4;
+	config.costAggregation.horizontalPenaltyCostP1 = 100;
+	config.costAggregation.horizontalPenaltyCostP2 = 500;
+	config.costAggregation.verticalPenaltyCostP1 = 100;
+	config.costAggregation.verticalPenaltyCostP2 = 500;
+	config.postProcessing.brightnessFilter.maxBrightness = 255;
+	stereoDepth->initialConfig.set(config);
+
+	stereoDepth->depth.link(sync->inputs["depth"]);
+
+	if(outputMode_ < 2)
+	{
+		auto leftEncoder  = pipeline.create<dai::node::VideoEncoder>();
+		leftEncoder->setDefaultProfilePreset(this->getImageRate(), dai::VideoEncoderProperties::Profile::MJPEG);
+
+		if(imagesRectified_)
+			stereoDepth->rectifiedLeft.link(leftEncoder->input);
+		else
+			stereoDepth->syncedLeft.link(leftEncoder->input);
+		leftEncoder->bitstream.link(sync->inputs["left"]);
+	}
+
+	if(!outputMode_)
+	{
+		auto rightEncoder  = pipeline.create<dai::node::VideoEncoder>();
+		rightEncoder->setDefaultProfilePreset(this->getImageRate(), dai::VideoEncoderProperties::Profile::MJPEG);
+
+		if(imagesRectified_)
+			stereoDepth->rectifiedRight.link(rightEncoder->input);
+		else
+			stereoDepth->syncedRight.link(rightEncoder->input);
+		rightEncoder->bitstream.link(sync->inputs["right"]);
+	}
+
+	if(boardName == "BC2087")
+	{
+		auto leftCamera = pipeline.create<dai::node::ColorCamera>();
+		leftCamera->setCamera("left");
+		leftCamera->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1200_P);
+		leftCamera->setIspScale(imageWidth_/640, 3);
+		leftCamera->setFps(this->getImageRate());
+	
+		auto rightCamera = pipeline.create<dai::node::ColorCamera>();
+		rightCamera->setCamera("right");
+		rightCamera->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1200_P);
+		rightCamera->setIspScale(imageWidth_/640, 3);
+		rightCamera->setFps(this->getImageRate());
+	
+		leftCamera->isp.link(stereoDepth->left);
+		rightCamera->isp.link(stereoDepth->right);
+	}
+	else
+	{
+		auto leftCamera = pipeline.create<dai::node::MonoCamera>();
+		leftCamera->setCamera("left");
+		leftCamera->setResolution(imageWidth_==640?dai::MonoCameraProperties::SensorResolution::THE_400_P:dai::MonoCameraProperties::SensorResolution::THE_800_P);
+		leftCamera->setFps(this->getImageRate());
+	
+		auto rightCamera = pipeline.create<dai::node::MonoCamera>();
+		rightCamera->setCamera("right");
+		rightCamera->setResolution(imageWidth_==640?dai::MonoCameraProperties::SensorResolution::THE_400_P:dai::MonoCameraProperties::SensorResolution::THE_800_P);
+		rightCamera->setFps(this->getImageRate());
+	
+		leftCamera->out.link(stereoDepth->left);
+		rightCamera->out.link(stereoDepth->right);
+	}
+
+	if(detectFeatures_ == 1)
+	{
+		auto gfttDetector = pipeline.create<dai::node::FeatureTracker>();
+		gfttDetector->setHardwareResources(2, 2);
+		gfttDetector->initialConfig.setCornerDetector(
+			useHarrisDetector_?dai::FeatureTrackerConfig::CornerDetector::Type::HARRIS:dai::FeatureTrackerConfig::CornerDetector::Type::SHI_THOMASI);
+		gfttDetector->initialConfig.setNumTargetFeatures(numTargetFeatures_);
+		gfttDetector->initialConfig.setMotionEstimator(false);
+		auto cfg = gfttDetector->initialConfig.get();
+		cfg.featureMaintainer.minimumDistanceBetweenFeatures = minDistance_ * minDistance_;
+		gfttDetector->initialConfig.set(cfg);
+
+		if(outputMode_ == 2)
+			rgbCamera->video.link(gfttDetector->inputImage);
+		else if(imagesRectified_)
+			stereoDepth->rectifiedLeft.link(gfttDetector->inputImage);
+		else
+			stereoDepth->syncedLeft.link(gfttDetector->inputImage);
+		gfttDetector->outputFeatures.link(sync->inputs["feat"]);
+	}
+	else if(detectFeatures_ >= 2)
+	{
+		auto imageManip = pipeline.create<dai::node::ImageManip>();
+		imageManip->setKeepAspectRatio(false);
+		imageManip->setMaxOutputFrameSize(320 * 200);
+		imageManip->initialConfig.setResize(320, 200);
+		imageManip->initialConfig.setFrameType(dai::ImgFrame::Type::GRAY8);
+
+		auto neuralNetwork = pipeline.create<dai::node::NeuralNetwork>();
+		neuralNetwork->setBlobPath(blobPath_);
+		neuralNetwork->setNumInferenceThreads(2);
+		neuralNetwork->setNumNCEPerInferenceThread(1);
+		neuralNetwork->input.setBlocking(false);
+
+		if(outputMode_ == 2)
+			rgbCamera->video.link(imageManip->inputImage);
+		else if(imagesRectified_)
+			stereoDepth->rectifiedLeft.link(imageManip->inputImage);
+		else
+			stereoDepth->syncedLeft.link(imageManip->inputImage);
+		imageManip->out.link(neuralNetwork->input);
+		neuralNetwork->out.link(sync->inputs["feat"]);
+	}
+
+	auto xoutCamera = pipeline.create<dai::node::XLinkOut>();
+	xoutCamera->setStreamName("camera");
+
+	sync->out.link(xoutCamera->input);
+
+	if(imuPublished_)
+	{
+		auto imu = pipeline.create<dai::node::IMU>();
+		if(imuType == "BMI270")
+			imu->enableIMUSensor({dai::IMUSensor::ACCELEROMETER_RAW, dai::IMUSensor::GYROSCOPE_RAW}, 200);
+		else // BNO085/086
+			imu->enableIMUSensor({dai::IMUSensor::ACCELEROMETER, dai::IMUSensor::GYROSCOPE_UNCALIBRATED}, 200);
+		imu->setBatchReportThreshold(boardName=="NG9097"?4:1);
+		imu->setMaxBatchReports(10);
+
+		auto xoutIMU = pipeline.create<dai::node::XLinkOut>();
+		xoutIMU->setStreamName("imu");
+
+		imu->out.link(xoutIMU->input);
+	}
+
+	device_->startPipeline(pipeline);
+	if(!device_->getIrDrivers().empty())
+	{
+		UINFO("Setting IR intensity");
+		device_->setIrLaserDotProjectorIntensity(dotIntensity_);
+		device_->setIrFloodLightIntensity(floodIntensity_);
+	}
+	else if(dotIntensity_ > 0 || floodIntensity_ > 0)
+	{
+		UWARN("No IR drivers were detected! IR intensity cannot be set.");
 	}
 
 	cameraQueue_ = device_->getOutputQueue("camera", 8, false);
@@ -726,17 +701,7 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 		});
 	}
 
-	if(!device_->getIrDrivers().empty())
-	{
-		UINFO("Setting IR intensity");
-		device_->setIrLaserDotProjectorIntensity(dotIntensity_);
-		device_->setIrFloodLightIntensity(floodIntensity_);
-	}
-	else if(dotIntensity_ > 0 || floodIntensity_ > 0)
-	{
-		UWARN("No IR drivers were detected! IR intensity cannot be set.");
-	}
-
+	this->setImageRate(0);
 	uSleep(2000); // avoid bad frames on start
 
 	return true;
@@ -769,32 +734,14 @@ SensorData CameraDepthAI::captureImage(SensorCaptureInfo * info)
 #ifdef RTABMAP_DEPTHAI
 
 	auto messageGroup = cameraQueue_->get<dai::MessageGroup>();
-	auto rectifLeftOrColor = messageGroup->get<dai::ImgFrame>(outputMode_<2?"left":"color");
-	auto rectifRightOrDepth = messageGroup->get<dai::ImgFrame>(imagesRectified_ && outputMode_?"depth":"right");
+	auto rgbOrLeft = messageGroup->get<dai::ImgFrame>(outputMode_==2?"rgb":"left");
+	auto depthOrRight = messageGroup->get<dai::ImgFrame>(outputMode_?"depth":"right");
 
-	cv::Mat leftOrColor, depthOrRight;
-	if(device_->getDeviceInfo().protocol == X_LINK_TCP_IP || mxidOrName_.find(".") != std::string::npos)
-	{
-		leftOrColor = cv::imdecode(rectifLeftOrColor->getData(), cv::IMREAD_ANYCOLOR);
-		depthOrRight = cv::imdecode(rectifRightOrDepth->getData(), cv::IMREAD_GRAYSCALE);
-		if(imagesRectified_ && outputMode_)
-		{
-			cv::Mat disp;
-			depthOrRight.convertTo(disp, CV_16UC1);
-			cv::divide(-stereoModel_.right().Tx() * 1000, disp, depthOrRight);
-		}
-	}
+	double stamp = std::chrono::duration<double>(depthOrRight->getTimestampDevice(dai::CameraExposureOffset::MIDDLE).time_since_epoch()).count();
+	if(outputMode_)
+		data = SensorData(cv::imdecode(rgbOrLeft->getData(), cv::IMREAD_ANYCOLOR), depthOrRight->getCvFrame(), stereoModel_.left(), this->getNextSeqID(), stamp);
 	else
-	{
-		leftOrColor = rectifLeftOrColor->getCvFrame();
-		depthOrRight = rectifRightOrDepth->getCvFrame();
-	}
-
-	double stamp = std::chrono::duration<double>(rectifLeftOrColor->getTimestampDevice(dai::CameraExposureOffset::MIDDLE).time_since_epoch()).count();
-	if(imagesRectified_ && outputMode_)
-		data = SensorData(leftOrColor, depthOrRight, stereoModel_.left(), this->getNextSeqID(), stamp);
-	else
-		data = SensorData(leftOrColor, depthOrRight, stereoModel_, this->getNextSeqID(), stamp);
+		data = SensorData(cv::imdecode(rgbOrLeft->getData(), cv::IMREAD_GRAYSCALE), cv::imdecode(depthOrRight->getData(), cv::IMREAD_GRAYSCALE), stereoModel_, this->getNextSeqID(), stamp);
 
 	if(imuPublished_ && !publishInterIMU_)
 	{
@@ -896,34 +843,38 @@ SensorData CameraDepthAI::captureImage(SensorCaptureInfo * info)
 
 		std::vector<cv::Point> kpts;
 		cv::findNonZero(scores > threshold_, kpts);
-		std::vector<cv::KeyPoint> keypoints;
-		for(auto& kpt : kpts)
-		{
-			float response = scores.at<float>(kpt);
-			keypoints.emplace_back(cv::KeyPoint(kpt, 8, -1, response));
-		}
 
-		cv::Mat coarse_desc(25, 40, CV_32FC(256), local_descriptor_map.data());
-		if(detectFeatures_ == 2)
-			coarse_desc.forEach<cv::Vec<float, 256>>([&](cv::Vec<float, 256>& descriptor, const int position[]) -> void {
+		if(!kpts.empty()){
+			std::vector<cv::KeyPoint> keypoints;
+			for(auto& kpt : kpts)
+			{
+				float response = scores.at<float>(kpt);
+				keypoints.emplace_back(cv::KeyPoint(kpt, 8, -1, response));
+			}
+
+			cv::Mat coarse_desc(25, 40, CV_32FC(256), local_descriptor_map.data());
+			if(detectFeatures_ == 2)
+				coarse_desc.forEach<cv::Vec<float, 256>>([&](cv::Vec<float, 256>& descriptor, const int position[]) -> void {
+					cv::normalize(descriptor, descriptor);
+				});
+			cv::Mat mapX(keypoints.size(), 1, CV_32FC1);
+			cv::Mat mapY(keypoints.size(), 1, CV_32FC1);
+			for(size_t i=0; i<keypoints.size(); ++i)
+			{
+				mapX.at<float>(i) = (keypoints[i].pt.x - (targetSize_.width-1)/2) * 40/targetSize_.width + (40-1)/2;
+				mapY.at<float>(i) = (keypoints[i].pt.y - (targetSize_.height-1)/2) * 25/targetSize_.height + (25-1)/2;
+			}
+			cv::Mat map1, map2, descriptors;
+			cv::convertMaps(mapX, mapY, map1, map2, CV_16SC2);
+			cv::remap(coarse_desc, descriptors, map1, map2, cv::INTER_LINEAR);
+			descriptors.forEach<cv::Vec<float, 256>>([&](cv::Vec<float, 256>& descriptor, const int position[]) -> void {
 				cv::normalize(descriptor, descriptor);
 			});
-		cv::Mat mapX(keypoints.size(), 1, CV_32FC1);
-		cv::Mat mapY(keypoints.size(), 1, CV_32FC1);
-		for(size_t i=0; i<keypoints.size(); ++i)
-		{
-			mapX.at<float>(i) = (keypoints[i].pt.x - (targetSize_.width-1)/2) * 40/targetSize_.width + (40-1)/2;
-			mapY.at<float>(i) = (keypoints[i].pt.y - (targetSize_.height-1)/2) * 25/targetSize_.height + (25-1)/2;
+			descriptors = descriptors.reshape(1);
+
+			data.setFeatures(keypoints, std::vector<cv::Point3f>(), descriptors);
 		}
-		cv::Mat map1, map2, descriptors;
-		cv::convertMaps(mapX, mapY, map1, map2, CV_16SC2);
-		cv::remap(coarse_desc, descriptors, map1, map2, cv::INTER_LINEAR);
-		descriptors.forEach<cv::Vec<float, 256>>([&](cv::Vec<float, 256>& descriptor, const int position[]) -> void {
-			cv::normalize(descriptor, descriptor);
-		});
-		descriptors = descriptors.reshape(1);
 
-		data.setFeatures(keypoints, std::vector<cv::Point3f>(), descriptors);
 		if(detectFeatures_ == 3)
 			data.addGlobalDescriptor(GlobalDescriptor(1, cv::Mat(1, global_descriptor.size(), CV_32FC1, global_descriptor.data()).clone()));
 	}

corelib/src/camera/CameraRealSense2.cpp

@@ -632,7 +632,10 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 		if ("Stereo Module" == module_name)
 		{
 			sensors[1] = elem;
-			sensors[1].set_option(rs2_option::RS2_OPTION_EMITTER_ENABLED, emitterEnabled_);
+			if(sensors[1].supports(rs2_option::RS2_OPTION_EMITTER_ENABLED))
+			{
+				sensors[1].set_option(rs2_option::RS2_OPTION_EMITTER_ENABLED, emitterEnabled_);
+			}
 		}
 		else if ("Coded-Light Depth Sensor" == module_name)
 		{

corelib/src/camera/CameraStereoImages.cpp

@@ -44,7 +44,8 @@ CameraStereoImages::CameraStereoImages(
 		float imageRate,
 		const Transform & localTransform) :
 		CameraImages(pathLeftImages, imageRate, localTransform),
-		camera2_(new CameraImages(pathRightImages))
+		camera2_(new CameraImages(pathRightImages)),
+		rightGrayScale_(true)
 {
 	this->setImagesRectified(rectifyImages);
 }
@@ -55,7 +56,8 @@ CameraStereoImages::CameraStereoImages(
 		float imageRate,
 		const Transform & localTransform) :
 		CameraImages("", imageRate, localTransform),
-		camera2_(0)
+		camera2_(0),
+		rightGrayScale_(true)
 {
 	std::vector<std::string> paths = uListToVector(uSplit(pathLeftRightImages, uStrContains(pathLeftRightImages, ":")?':':';'));
 	if(paths.size() >= 1)
@@ -179,7 +181,7 @@ SensorData CameraStereoImages::captureImage(SensorCaptureInfo * info)
 			// Rectification
 			cv::Mat leftImage = left.imageRaw();
 			cv::Mat rightImage = right.imageRaw();
-			if(rightImage.type() != CV_8UC1)
+			if(rightImage.type() != CV_8UC1 && rightGrayScale_)
 			{
 				cv::Mat tmp;
 				cv::cvtColor(rightImage, tmp, CV_BGR2GRAY);