merged master to noetic

* remove an extra line

* fix capture delay

.github/workflows/cmake-ros.yml

@@ -3,7 +3,7 @@ name: CMake-ROS
 on:
   push:
     branches: 
-      - jazzy-devel
+      - noetic-devel
   pull_request:
     branches: 
       - '**'
@@ -23,10 +23,10 @@ jobs:
     strategy:
       fail-fast: false
       matrix:
-        ros_distribution: [jazzy]
+        ros_distribution: [ noetic ]
         include:
-        - ros_distribution: 'jazzy'
+        - ros_distribution: 'noetic'
-          os: ubuntu-24.04
+          os: ubuntu-20.04
 
     steps:   
     - uses: ros-tooling/setup-ros@v0.7

.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,6 +7,16 @@ 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:
@@ -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
@@ -74,7 +84,7 @@ jobs:
           cache-to: type=inline
   
   docker:
-    needs: docker_deps
+    #needs: docker_deps
     runs-on: ubuntu-latest
     
     strategy:
@@ -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

@@ -20,7 +20,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 21)
-SET(RTABMAP_PATCH_VERSION 10)
+SET(RTABMAP_PATCH_VERSION 13)
 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>Jazzy</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><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_BINARY_DIR}/corelib/include
-	${PROJECT_SOURCE_DIR}/corelib/include
+	  ${PROJECT_SOURCE_DIR}/corelib/include
-	${PROJECT_SOURCE_DIR}/utilite/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/CameraMobile.cpp

@@ -108,8 +108,9 @@ void CameraMobile::close()
     dataReady_.release();
 }
 
-void CameraMobile::resetOrigin()
+void CameraMobile::resetOrigin(const rtabmap::Transform & offset)
 {
+    manualOriginOffset_ = offset;
 	originUpdate_ = true;
 }
 
@@ -193,7 +194,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;
 		}
         

app/android/jni/CameraMobile.h

@@ -99,7 +99,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; }
@@ -150,6 +150,7 @@ private:
 	EnvSensors lastEnvSensors_;
 	Transform originOffset_;
 	bool originUpdate_;
+    rtabmap::Transform manualOriginOffset_;
 	float upstreamRelocalizationAccThr_;
 	rtabmap::Transform previousAnchorPose_;
 	std::vector<float> previousAnchorLinearVelocity_;

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.cpp

@@ -132,6 +132,7 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
 
 	parameters.insert(mappingParameters_.begin(), mappingParameters_.end());
 
+    parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpDetectorStrategy(), "5")); // GFTT/FREAK
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpMaxFeatures(), std::string("200")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kGFTTQualityLevel(), std::string("0.0001")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemImagePreDecimation(), std::string(cameraColor_&&fullResolution_?"2":"1")));
@@ -150,6 +151,7 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpParallelized(), std::string("false")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDOptimizeFromGraphEnd(), std::string("true")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisMinInliers(), std::string("25")));
+    parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisPnPVarianceMedianRatio(), std::string("2")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDProximityPathMaxNeighbors(), std::string("0"))); // disable scan matching to merged nodes
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDProximityBySpace(), std::string("false"))); // just keep loop closure detection
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDLinearUpdate(), std::string("0.05")));
@@ -264,13 +266,43 @@ RTABMapApp::RTABMapApp() :
 		lastPoseEventTime_(0.0),
 		visualizingMesh_(false),
 		exportedMeshUpdated_(false),
-		optMesh_(new pcl::TextureMesh),
+		measuresUpdated_(false),
+        targetPoint_(new pcl::PointCloud<pcl::PointXYZRGB>),
+        quadSample_(new pcl::PointCloud<pcl::PointXYZ>),
+        quadSamplePolygons_(2),
+        metricSystem_(true),
+        measuringTextSize_(0.05f),
+		snapAxisThr_(0.95),
+        measuringMode_(0),
+        addMeasureClicked_(false),
+        teleportClicked_(false),
+        removeMeasureClicked_(false),
+		optTextureMesh_(new pcl::TextureMesh),
 		optRefId_(0),
 		optRefPose_(0),
 		mapToOdom_(rtabmap::Transform::getIdentity())
 
 {
-	mappingParameters_.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpDetectorStrategy(), "5")); // GFTT/FREAK
+    pcl::PointXYZRGB ptWhite;
+    ptWhite.r = ptWhite.g = ptWhite.b = 255;
+    targetPoint_->push_back(ptWhite);
+    snapAxes_.push_back(cv::Vec3f(1,0,0));
+    snapAxes_.push_back(cv::Vec3f(0,1,0));
+    snapAxes_.push_back(cv::Vec3f(0,0,1));
+
+    float quadSize = 0.05f;
+    quadSample_->push_back(pcl::PointXYZ(-quadSize, -quadSize, 0.0f));
+    quadSample_->push_back(pcl::PointXYZ(quadSize, -quadSize, 0.0f));
+    quadSample_->push_back(pcl::PointXYZ(quadSize, quadSize, 0.0f));
+    quadSample_->push_back(pcl::PointXYZ(-quadSize, quadSize, 0.0f));
+    quadSamplePolygons_[0].vertices.resize(3);
+    quadSamplePolygons_[0].vertices[0] = 0;
+    quadSamplePolygons_[0].vertices[1] = 1;
+    quadSamplePolygons_[0].vertices[2] = 2;
+    quadSamplePolygons_[1].vertices.resize(3);
+    quadSamplePolygons_[1].vertices[0] = 0;
+    quadSamplePolygons_[1].vertices[1] = 2;
+    quadSamplePolygons_[1].vertices[2] = 3;
 
 #ifdef __ANDROID__
 	env->GetJavaVM(&jvm);
@@ -278,19 +310,6 @@ RTABMapApp::RTABMapApp() :
 #endif
     
 	LOGI("RTABMapApp::RTABMapApp()");
-	createdMeshes_.clear();
-	rawPoses_.clear();
-	clearSceneOnNextRender_ = true;
-	openingDatabase_ = false;
-	exporting_ = false;
-	postProcessing_=false;
-	totalPoints_ = 0;
-	totalPolygons_ = 0;
-	lastDrawnCloudsCount_ = 0;
-	renderingTime_ = 0.0f;
-	lastPostRenderEventTime_ = 0.0;
-	lastPoseEventTime_ = 0.0;
-	bufferedStatsData_.clear();
 #ifdef __ANDROID__
 	progressionStatus_.setJavaObjects(jvm, RTABMapActivity);
 #endif
@@ -404,13 +423,16 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
     lastPoseEventTime_ = 0.0;
     bufferedStatsData_.clear();
     graphOptimization_ = true;
+    measuresUpdated_ = !measures_.empty();
+    measures_.clear();
 
 	this->registerToEventsManager();
 
 	int status = 0;
 
 	// Open visualization while we load (if there is an optimized mesh saved in database)
-	optMesh_.reset(new pcl::TextureMesh);
+	optTextureMesh_.reset(new pcl::TextureMesh);
+    optMesh_ = rtabmap::Mesh();
 	optTexture_ = cv::Mat();
 	optRefId_ = 0;
 	if(optRefPose_)
@@ -418,6 +440,7 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 		delete optRefPose_;
 		optRefPose_ = 0;
 	}
+    visualizingMesh_ = false;
 	cv::Mat cloudMat;
 	std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons;
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
@@ -436,19 +459,20 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 			if(!cloudMat.empty())
 			{
 				LOGI("Open: Found optimized mesh! Visualizing it.");
-				optMesh_ = rtabmap::util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, true);
+				optTextureMesh_ = rtabmap::util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, true);
+                optMesh_ = rtabmap::Mesh();
 				optTexture_ = textures;
-				if(!optTexture_.empty())
+                if(!optTexture_.empty())
 				{
 					LOGI("Open: Texture mesh: %dx%d.", optTexture_.cols, optTexture_.rows);
 					status=3;
 				}
-				else if(optMesh_->tex_polygons.size())
+				else if(optTextureMesh_->tex_polygons.size())
 				{
 					LOGI("Open: Polygon mesh");
 					status=2;
 				}
-				else if(!optMesh_->cloud.data.empty())
+				else if(!optTextureMesh_->cloud.data.empty())
 				{
 					LOGI("Open: Point cloud");
 					status=1;
@@ -1330,7 +1354,8 @@ int RTABMapApp::Render()
 #ifdef DEBUG_RENDERING_PERFORMANCE
 				LOGD("Camera updateOnRender %fs", time.ticks());
 #endif
-				if(main_scene_.background_renderer_ == 0 && camera_->getTextureId() != 0)
+                // We detect if we are in measuring mode if rtabmap is not running
+				if(main_scene_.background_renderer_ == 0 && camera_->getTextureId() != 0 && !(rtabmapThread_ == 0 || !rtabmapThread_->isRunning()))
 				{
 					main_scene_.background_renderer_ = new BackgroundRenderer();
 					main_scene_.background_renderer_->InitializeGlContent(((rtabmap::CameraMobile*)camera_)->getTextureId(), cameraDriver_ <= 2);
@@ -1424,40 +1449,189 @@ int RTABMapApp::Render()
 			{
 				main_scene_.clear();
 				exportedMeshUpdated_ = false;
+                measuresUpdated_ = measures_.size()>0;
 			}
 			if(!main_scene_.hasCloud(g_optMeshId))
 			{
 				LOGI("Adding optimized mesh to opengl (%d points, %d polygons, %d tex_coords, materials=%d texture=%dx%d)...",
-						optMesh_->cloud.point_step==0?0:(int)optMesh_->cloud.data.size()/optMesh_->cloud.point_step,
+						optTextureMesh_->cloud.point_step==0?0:(int)optTextureMesh_->cloud.data.size()/optTextureMesh_->cloud.point_step,
-						optMesh_->tex_polygons.size()!=1?0:(int)optMesh_->tex_polygons[0].size(),
+						optTextureMesh_->tex_polygons.size()!=1?0:(int)optTextureMesh_->tex_polygons[0].size(),
-						optMesh_->tex_coordinates.size()!=1?0:(int)optMesh_->tex_coordinates[0].size(),
+						optTextureMesh_->tex_coordinates.size()!=1?0:(int)optTextureMesh_->tex_coordinates[0].size(),
-						(int)optMesh_->tex_materials.size(),
+						(int)optTextureMesh_->tex_materials.size(),
 						optTexture_.cols, optTexture_.rows);
-				if(optMesh_->tex_polygons.size() && optMesh_->tex_polygons[0].size())
+				if(optTextureMesh_->tex_polygons.size() && optTextureMesh_->tex_polygons[0].size())
 				{
-					rtabmap::Mesh mesh;
+                    optMesh_ = rtabmap::Mesh();
-					mesh.gains[0] = mesh.gains[1] = mesh.gains[2] = 1.0;
+                    optMesh_.gains[0] = optMesh_.gains[1] = optMesh_.gains[2] = 1.0;
-					mesh.cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
+                    optMesh_.cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
-					mesh.normals.reset(new pcl::PointCloud<pcl::Normal>);
+                    optMesh_.normals.reset(new pcl::PointCloud<pcl::Normal>);
-					pcl::fromPCLPointCloud2(optMesh_->cloud, *mesh.cloud);
+					pcl::fromPCLPointCloud2(optTextureMesh_->cloud, *optMesh_.cloud);
-					pcl::fromPCLPointCloud2(optMesh_->cloud, *mesh.normals);
+					pcl::fromPCLPointCloud2(optTextureMesh_->cloud, *optMesh_.normals);
-					mesh.polygons = optMesh_->tex_polygons[0];
+                    bool hasColors = false;
-					mesh.pose.setIdentity();
+                    for(unsigned int i=0; i<optTextureMesh_->cloud.fields.size(); ++i)
-					if(optMesh_->tex_coordinates.size())
+                    {
+                        if(optTextureMesh_->cloud.fields[i].name.compare("rgb") == 0)
+                        {
+                            hasColors = true;
+                            break;
+                        }
+                    }
+                    if(!hasColors)
+                    {
+                        std::uint8_t r = 255, g = 255, b = 255;    // White
+                        std::uint32_t rgb = ((std::uint32_t)r << 16 | (std::uint32_t)g << 8 | (std::uint32_t)b);
+                        for(size_t i=0; i<optMesh_.cloud->size(); ++i)
+                        {
+                            optMesh_.cloud->at(i).rgb = *reinterpret_cast<float*>(&rgb);
+                        }
+                    }
+                    optMesh_.polygons = optTextureMesh_->tex_polygons[0];
+					if(optTextureMesh_->tex_coordinates.size())
 					{
-						mesh.texCoords = optMesh_->tex_coordinates[0];
+                        optMesh_.texCoords = optTextureMesh_->tex_coordinates[0];
-						mesh.texture = optTexture_;
+                        optMesh_.texture = optTexture_;
 					}
-					main_scene_.addMesh(g_optMeshId, mesh, rtabmap::opengl_world_T_rtabmap_world, true);
+					main_scene_.addMesh(g_optMeshId, optMesh_, rtabmap::opengl_world_T_rtabmap_world, true);
 				}
 				else
 				{
 					pcl::IndicesPtr indices(new std::vector<int>); // null
 					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
-					pcl::fromPCLPointCloud2(optMesh_->cloud, *cloud);
+					pcl::fromPCLPointCloud2(optTextureMesh_->cloud, *cloud);
 					main_scene_.addCloud(g_optMeshId, cloud, indices, rtabmap::opengl_world_T_rtabmap_world);
 				}
+                
+                if(!measures_.empty())
+                {
+                    measuresUpdated_ = true;
+                }
 			}
+            
+            if(camera_ != 0 && (rtabmapThread_ == 0 || !rtabmapThread_->isRunning()))
+            {
+                updateMeasuringState();
+            }
+            else
+            {
+                main_scene_.removeLine(55555);
+                main_scene_.removeQuad(55555);
+                main_scene_.removeQuad(55556);
+                main_scene_.removeCircle(55555);
+                main_scene_.removeCircle(55556);
+                main_scene_.removeText(55555);
+                main_scene_.removeCloudOrMesh(-99999);
+                measuringTmpPts_.clear();
+                measuringTmpNormals_.clear();
+            }
+
+            if(measuresUpdated_)
+            {
+                std::list<Measure> measures = measures_;
+                measuresUpdated_ = false;
+                main_scene_.clearLines();
+                main_scene_.clearTexts();
+                main_scene_.clearQuads();
+                main_scene_.clearCircles();
+                int lineId = 0;
+                int textId = 0;
+                int quadId = 0;
+                int circleId = 0;
+                float sphereRadius = 0.02f;
+                float quadSize=0.05f;
+                float quadAlpha = 0.3f;
+
+                tango_gl::Color color(1.0f, 0.0f, 1.0f);
+                tango_gl::Color xColor(1.0f, 0.0f, 0.0f);
+                tango_gl::Color yColor(0.0f, 1.0f, 0.0f);
+                tango_gl::Color zColor(0.0f, 0.0f, 1.0f);
+
+                float restrictiveSnapThr = 0.9999;
+                for(std::list<Measure>::iterator iter=measures.begin(); iter!=measures.end(); ++iter)
+                {
+                    // Determinate color based on current snap axes
+                    tango_gl::Color quadColor = color;
+                    bool sameNormal = false;
+                    if(iter->n1().dot(iter->n2()) > 0.99) // Same normal, plane to plane
+                    {
+                        sameNormal = true;
+                        Eigen::Vector3f n(iter->n1()[0], iter->n1()[1], iter->n1()[2]);
+                        float n1ProdX = n.dot(Eigen::Vector3f(snapAxes_[0][0], snapAxes_[0][1], snapAxes_[0][2]));
+                        float n1ProdY = n.dot(Eigen::Vector3f(snapAxes_[1][0], snapAxes_[1][1], snapAxes_[1][2]));
+                        float n1ProdZ = n.dot(Eigen::Vector3f(snapAxes_[2][0], snapAxes_[2][1], snapAxes_[2][2]));
+                        if(fabs(n1ProdX) > restrictiveSnapThr)
+                        {
+                            quadColor = xColor;
+                        }
+                        else if(fabs(n1ProdY) > restrictiveSnapThr)
+                        {
+                            quadColor = yColor;
+                        }
+                        else if(fabs(n1ProdZ) > restrictiveSnapThr)
+                        {
+                            quadColor = zColor;
+                        }
+                    }
+                    
+                    const Measure & m = *iter;
+                    LOGI("dist=%f, %f,%f,%f -> %f,%f,%f", m.length(),
+                            m.pt1().x, m.pt1().y, m.pt1().z,
+                            m.pt2().x, m.pt2().y, m.pt2().z);
+                    cv::Point3f pt1 = rtabmap::util3d::transformPoint(m.pt1(), rtabmap::opengl_world_T_rtabmap_world);
+                    cv::Point3f pt2 = rtabmap::util3d::transformPoint(m.pt2(), rtabmap::opengl_world_T_rtabmap_world);
+                    main_scene_.addLine(++lineId, pt1, pt2, quadColor);
+
+                    if (fabs(iter->n1()[2]) < 0.00001 && sameNormal)
+                    {
+                        // Add a line so that in orthogonal view, we can see better where the lines are starting/finishing
+                        cv::Point3f n = cv::Vec3f(0,0,1).cross(iter->n1());
+
+                        n = rtabmap::util3d::transformPoint(n, rtabmap::opengl_world_T_rtabmap_world) * (quadSize/2);
+                        cv::Point3f pa = pt1 + n;
+                        cv::Point3f pb = pt1 - n;
+                        main_scene_.addLine(++lineId, pa, pb, quadColor);
+                        cv::Point3f pc = pt2 + n;
+                        cv::Point3f pd = pt2 - n;
+                        main_scene_.addLine(++lineId, pc, pd, quadColor);
+                    }
+
+                    float diff = m.length();
+                    std::string text = uFormat("%0.2f m", diff);
+                    if(!metricSystem_)
+                    {
+                        static const double METERS_PER_FOOT = 0.3048;
+                        static double INCHES_PER_FOOT = 12.0;
+                        double lengthInFeet = diff / METERS_PER_FOOT;
+                        int feet = (int)lengthInFeet;
+                        float inches = (lengthInFeet - feet) * INCHES_PER_FOOT;
+                        if(feet > 0)
+                        {
+                            text = uFormat("%d' %0.1f\"", feet, inches);
+                        }
+                        else
+                        {
+                            text = uFormat("%0.1f\"", inches);
+                        }
+                    }
+                    main_scene_.addText(++textId, text, rtabmap::Transform((pt1.x+pt2.x)/2.0f, (pt1.y+pt2.y)/2.0f, (pt1.z+pt2.z)/2.0f, 0, 0,0), measuringTextSize_, quadColor);
+                    
+                    cv::Vec3f n1 = rtabmap::util3d::transformPoint(m.n1(), rtabmap::opengl_world_T_rtabmap_world);
+                    cv::Vec3f n2 = rtabmap::util3d::transformPoint(m.n2(), rtabmap::opengl_world_T_rtabmap_world);
+                    Eigen::Quaternionf q1, q2;
+                    q1.setFromTwoVectors(Eigen::Vector3f(0,0,1), Eigen::Vector3f(n1[0],n1[1],n1[2]));
+                    q2.setFromTwoVectors(Eigen::Vector3f(0,0,1), Eigen::Vector3f(n2[0],n2[1],n2[2]));
+                    
+                    if(sameNormal)
+                    {
+                        main_scene_.addQuad(++quadId, quadSize, rtabmap::Transform(pt1.x, pt1.y, pt1.z, q1.x(), q1.y(), q1.z(), q1.w()), quadColor, quadAlpha);
+                        main_scene_.addQuad(++quadId, quadSize, rtabmap::Transform(pt2.x, pt2.y, pt2.z, q2.x(), q2.y(), q2.z(), q2.w()), quadColor, quadAlpha);
+                    }
+                    else
+                    {
+                        main_scene_.addCircle(++circleId, quadSize/2, rtabmap::Transform(pt1.x, pt1.y, pt1.z, q1.x(), q1.y(), q1.z(), q1.w()), quadColor, quadAlpha);
+                        main_scene_.addCircle(++circleId, quadSize/2, rtabmap::Transform(pt2.x, pt2.y, pt2.z, q2.x(), q2.y(), q2.z(), q2.w()), quadColor, quadAlpha);
+                    }
+                }
+            }
 
 			if(!openingDatabase_)
 			{
@@ -1576,7 +1750,8 @@ int RTABMapApp::Render()
 			if(main_scene_.hasCloud(g_optMeshId))
 			{
 				main_scene_.clear();
-				optMesh_.reset(new pcl::TextureMesh);
+				optTextureMesh_.reset(new pcl::TextureMesh);
+                optMesh_ = rtabmap::Mesh();
 				optTexture_ = cv::Mat();
 			}
 
@@ -1639,6 +1814,8 @@ int RTABMapApp::Render()
 				lastPostRenderEventTime_ = 0.0;
 				lastPoseEventTime_ = 0.0;
 				bufferedStatsData_.clear();
+                measuresUpdated_ = !measures_.empty();
+                measures_.clear();
 			}
 
 			// Did we lose OpenGL context? If so, recreate the context;
@@ -2270,6 +2447,395 @@ int RTABMapApp::Render()
 	}
 }
 
+void RTABMapApp::updateMeasuringState()
+{
+    rtabmap::Transform openglCam = main_scene_.GetOpenGLCameraPose();
+    rtabmap::Transform rtabmapCam = rtabmap::rtabmap_world_T_opengl_world * openglCam * rtabmap::opengl_world_T_rtabmap_world;
+    Eigen::Vector3f origin(openglCam.x(), openglCam.y(), openglCam.z());
+    Eigen::Vector3f rtabmapOrigin(rtabmapCam.x(), rtabmapCam.y(), rtabmapCam.z());
+    rtabmap::Transform v = openglCam.rotation() * rtabmap::Transform(0,0,-1,0,0,0);
+    Eigen::Vector3f dir(v.x(), v.y(), v.z());
+    v = rtabmapCam.rotation() * rtabmap::Transform(1,0,0,0,0,0);
+    Eigen::Vector3f rtabmapDir(v.x(), v.y(), v.z());
+    tango_gl::Color color(1.0f, 0.0f, 1.0f);
+    tango_gl::Color xColor(1.0f, 0.0f, 0.0f); // in rtabmap world
+    tango_gl::Color yColor(0.0f, 1.0f, 0.0f); // in rtabmap world
+    tango_gl::Color zColor(0.0f, 0.0f, 1.0f); // in rtabmap world
+    float circleRadius = 0.025;
+    float quadSize=0.05f;
+    float quadAlpha = 0.3f;
+    
+    main_scene_.removeQuad(55555);
+    main_scene_.removeQuad(55556);
+    main_scene_.removeCircle(55555);
+    main_scene_.removeCircle(55556);
+    main_scene_.removeLine(55555);
+    main_scene_.removeText(55555);
+
+    if(removeMeasureClicked_)
+    {
+        if(!measuringTmpPts_.empty())
+        {
+            measuringTmpPts_.clear();
+            measuringTmpNormals_.clear();
+        }
+        else
+        {
+            bool removed = false;
+            for(std::list<Measure>::iterator iter=measures_.begin(); iter!=measures_.end() && !removed; ++iter)
+            {
+                const Measure & m = *iter;
+
+                for(int i=0; i<2;++i)
+                {
+                    // intersecting the quad?
+                    cv::Point3f pt = i==0?m.pt1():m.pt2();
+                    cv::Point3f n = i==0?m.n1():m.n2();
+                    pt = rtabmap::util3d::transformPoint(pt, rtabmap::opengl_world_T_rtabmap_world);
+                    n = rtabmap::util3d::transformPoint(n, rtabmap::opengl_world_T_rtabmap_world);
+                    Eigen::Quaternionf q;
+                    q.setFromTwoVectors(Eigen::Vector3f(0,0,-1), Eigen::Vector3f(n.x,n.y,n.z));
+                    float dTmp;
+                    Eigen::Vector3f nTmp;
+                    int indexTmp;
+                    if(rtabmap::util3d::intersectRayMesh(
+                                            origin,
+                                            dir,
+                                            *rtabmap::util3d::transformPointCloud(quadSample_, rtabmap::Transform(pt.x, pt.y, pt.z, q.x(), q.y(), q.z(), q.w())),
+                                            quadSamplePolygons_,
+                                            false,
+                                            dTmp,
+                                            nTmp,
+                                            indexTmp))
+                    {
+                        measures_.erase(iter);
+                        removed = true;
+                        break;
+                    }
+                }
+            }
+
+            measuresUpdated_ |= removed;
+        }
+    }
+
+    float distance =0.0f;
+    Eigen::Vector3f n;
+    int index;
+    if(rtabmap::util3d::intersectRayMesh(
+                    rtabmapOrigin,
+                    rtabmapDir,
+                    *optMesh_.cloud,
+                    optMesh_.polygons,
+                    true,
+                    distance,
+                    n,
+                    index))
+    {
+        
+        Eigen::Vector3f intersectionPt = origin + dir*distance;
+        cv::Point3f pt(intersectionPt[0], intersectionPt[1], intersectionPt[2]);
+        cv::Point3f normal(n[0], n[1], n[2]); // rtabmap world
+        cv::Point3f normalGl = rtabmap::util3d::transformPoint(normal, rtabmap::opengl_world_T_rtabmap_world); // opengl world
+        tango_gl::Color quadColor = color;
+        float normalProdX = n.dot(Eigen::Vector3f(snapAxes_[0][0], snapAxes_[0][1], snapAxes_[0][2]));
+        float normalProdY = n.dot(Eigen::Vector3f(snapAxes_[1][0], snapAxes_[1][1], snapAxes_[1][2]));
+        float normalProdZ = n.dot(Eigen::Vector3f(snapAxes_[2][0], snapAxes_[2][1], snapAxes_[2][2]));
+        if(fabs(normal.x) > fabs(normal.z) && fabs(normal.y) > fabs(normal.z))
+        {
+            if(fabs(normalProdX) > snapAxisThr_)
+            {
+                normal.x = snapAxes_[0][0] * (normalProdX>0?1:-1);
+                normal.y = snapAxes_[0][1] * (normalProdX>0?1:-1);
+                normal.z = snapAxes_[0][2] * (normalProdX>0?1:-1);
+                quadColor = xColor;
+            }
+            else if(fabs(normalProdY) > snapAxisThr_)
+            {
+                normal.x = snapAxes_[1][0] * (normalProdY>0?1:-1);
+                normal.y = snapAxes_[1][1] * (normalProdY>0?1:-1);
+                normal.z = snapAxes_[1][2] * (normalProdY>0?1:-1);
+                quadColor = yColor;
+            }
+            else if(measuringMode_ == 0)
+            {
+                // We force to be aligned with xy plane
+                normal.z = 0;
+                float n = cv::norm(normal);
+                normal.x/=n;
+                normal.y/=n;
+                normal.z/=n;
+            }
+        }
+        else if(fabs(normalProdZ) > snapAxisThr_)
+        {
+            normal.x = snapAxes_[2][0] * (normalProdZ>0?1:-1);
+            normal.y = snapAxes_[2][1] * (normalProdZ>0?1:-1);
+            normal.z = snapAxes_[2][2] * (normalProdZ>0?1:-1);
+            quadColor = zColor;
+        }
+        normalGl = rtabmap::util3d::transformPoint(normal, rtabmap::opengl_world_T_rtabmap_world);
+
+        if(teleportClicked_)
+        {
+            camera_->resetOrigin(rtabmap::Transform(-pt.z, -pt.x, pt.y-Scene::kHeightOffset.y,0,0,0));
+        }
+        else if(addMeasureClicked_) // Add measure
+        {
+            if((measuringMode_ == 1 || measuringTmpPts_.size()==1))
+            {
+                if(measuringMode_ == 1) // Height single click
+                {
+                    measuringTmpPts_.clear();
+                    measuringTmpNormals_.clear();
+
+                    normal.x=0;
+                    normal.y=0;
+                    normal.z=1;
+                    normalGl = rtabmap::util3d::transformPoint(normal, rtabmap::opengl_world_T_rtabmap_world);
+
+                    measuringTmpPts_.push_back(pt);
+                    measuringTmpNormals_.push_back(normalGl);
+                    pt.y = 0;
+                    measuringTmpPts_.push_back(pt);
+                    measuringTmpNormals_.push_back(normalGl);
+                }
+                else if(measuringMode_ == 0 || measuringMode_ == 2) // Plane to Plane or point to point
+                {
+                    if(measuringMode_ == 0)
+                    {
+                        normalGl = measuringTmpNormals_.front();
+
+                        // project point on line
+                        float n = (pt-measuringTmpPts_.front()).dot(measuringTmpNormals_.front());
+                        pt = measuringTmpPts_.front() + (measuringTmpNormals_.front() * n);
+                    }
+                    measuringTmpPts_.push_back(pt);
+                    measuringTmpNormals_.push_back(normalGl);
+                }
+
+                // Add measure!
+                const cv::Point3f & pt1 = measuringTmpPts_.at(0);
+                const cv::Point3f & pt2 = measuringTmpPts_.at(1);
+                const cv::Vec3f & n1 = measuringTmpNormals_.at(0);
+                const cv::Vec3f & n2 = measuringTmpNormals_.at(1);
+
+                Measure measure(
+                        rtabmap::util3d::transformPoint(pt1, rtabmap::rtabmap_world_T_opengl_world),
+                        rtabmap::util3d::transformPoint(pt2, rtabmap::rtabmap_world_T_opengl_world),
+                        rtabmap::util3d::transformPoint(n1, rtabmap::rtabmap_world_T_opengl_world),
+                        rtabmap::util3d::transformPoint(n2, rtabmap::rtabmap_world_T_opengl_world));
+
+                if(measure.length()>=0.01f)
+                {
+                    measures_.push_back(measure);
+
+                    measuringTmpPts_.clear();
+                    measuringTmpNormals_.clear();
+
+                    measuresUpdated_ = true;
+                }
+            }
+            else
+            {
+                measuringTmpPts_.clear();
+                measuringTmpNormals_.clear();
+
+                // init the first point
+                measuringTmpPts_.push_back(pt);
+                measuringTmpNormals_.push_back(normalGl);
+            }
+        }
+        else if(measuringMode_ >= 0)
+        {
+            // move action
+            if(measuringMode_ == 0 && measuringTmpNormals_.size()==1)
+            {
+                normalGl = measuringTmpNormals_.front();
+                normal = rtabmap::util3d::transformPoint(normalGl, rtabmap::rtabmap_world_T_opengl_world);
+                n = Eigen::Vector3f(normal.x, normal.y, normal.z);
+                float normalProdX = n.dot(Eigen::Vector3f(snapAxes_[0][0], snapAxes_[0][1], snapAxes_[0][2]));
+                float normalProdY = n.dot(Eigen::Vector3f(snapAxes_[1][0], snapAxes_[1][1], snapAxes_[1][2]));
+                float normalProdZ = n.dot(Eigen::Vector3f(snapAxes_[2][0], snapAxes_[2][1], snapAxes_[2][2]));
+                if(fabs(normalProdX) > snapAxisThr_)
+                {
+                    quadColor = xColor;
+                }
+                else if(fabs(normalProdY) > snapAxisThr_)
+                {
+                    quadColor = yColor;
+                }
+                else if(fabs(normalProdZ) > snapAxisThr_)
+                {
+                    quadColor = zColor;
+                }
+                else
+                {
+                    quadColor = color;
+                }
+            }
+
+            float quadWidthLeft = 0.05;
+            float quadWidthRight = quadWidthLeft;
+            float quadHeightBottom = quadWidthLeft;
+            float quadHeightTop = quadWidthLeft;
+            cv::Point3f pt2 = pt;
+            float lineLength = 0.0f;
+            // project point on line
+            if(measuringTmpPts_.size() == 1)
+            {
+                cv::Point3f v = pt2-measuringTmpPts_.front();
+                if(measuringMode_ == 0)
+                {
+                    float n = v.dot(measuringTmpNormals_.front());
+                    lineLength = fabs(n);
+                    pt2 = measuringTmpPts_.front() + (measuringTmpNormals_.front() * n);
+                    v = rtabmap::util3d::transformPoint(v, rtabmap::rtabmap_world_T_opengl_world);
+                    if(!(fabs(normal.z) > fabs(normal.x) && fabs(normal.z) > fabs(normal.y)))
+                    {
+                        quadHeightTop = v.z>quadSize?v.z:quadSize;
+                        quadHeightBottom = -v.z>quadSize?-v.z:quadSize;
+                        if(fabs(normal.x) > fabs(normal.y))
+                        {
+                            cv::Point3f y = cv::Point3f(0,0,1).cross(normal);
+                            float n = v.dot(y);
+                            quadWidthRight = n>quadSize?n:quadSize;
+                            quadWidthLeft = n<-quadSize?fabs(n):quadSize;
+                            if(normal.x>0)
+                            {
+                                quadWidthLeft = n>quadSize?n:quadSize;
+                                quadWidthRight = n<-quadSize?fabs(n):quadSize;
+                                float tmp  =quadHeightTop;
+                                quadHeightTop= quadHeightBottom;
+                                quadHeightBottom = tmp;
+                            }
+                        }
+                        else
+                        {
+                            cv::Point3f x = normal.cross(cv::Point3f(0,0,1));
+                            float n = v.dot(x);
+                            quadWidthRight = n<-quadSize?fabs(n):quadSize;
+                            quadWidthLeft = n>quadSize?n:quadSize;
+                        }
+                    }
+                    else
+                    {
+                        if(normal.z>0)
+                        {
+                            quadHeightBottom = v.x<-quadSize?fabs(v.x):quadSize;
+                            quadHeightTop = v.x>quadSize?v.x:quadSize;
+                            quadWidthRight = v.y<-quadSize?fabs(v.y):quadSize;
+                            quadWidthLeft = v.y>quadSize?v.y:quadSize;
+                        }
+                        else
+                        {
+                            quadHeightTop = v.x<-quadSize?fabs(v.x):quadSize;
+                            quadHeightBottom = v.x>quadSize?v.x:quadSize;
+                            quadWidthRight = v.y<-quadSize?fabs(v.y):quadSize;
+                            quadWidthLeft = v.y>quadSize?v.y:quadSize;
+                        }
+                    }
+                }
+                else
+                {
+                    lineLength = cv::norm(v);
+                }
+                
+                std::string text = uFormat("%0.2f m", lineLength);
+                if(!metricSystem_)
+                {
+                    static const double METERS_PER_FOOT = 0.3048;
+                    static double INCHES_PER_FOOT = 12.0;
+                    double lengthInFeet = lineLength / METERS_PER_FOOT;
+                    int feet = (int)lengthInFeet;
+                    float inches = (lengthInFeet - feet) * INCHES_PER_FOOT;
+                    if(feet > 0)
+                    {
+                        text = uFormat("%d' %0.1f\"", feet, inches);
+                    }
+                    else
+                    {
+                        text = uFormat("%0.1f\"", inches);
+                    }
+                }
+                main_scene_.addText(55555, text, rtabmap::Transform(pt.x, pt.y, pt.z, 0,0,0), 0.05f, measuringMode_ == 0?quadColor:color);
+            }
+
+            Eigen::Quaternionf q;
+            q.setFromTwoVectors(Eigen::Vector3f(0,0,1), Eigen::Vector3f(normalGl.x,normalGl.y,normalGl.z));
+
+            if(measuringTmpPts_.size() == 1 && measuringMode_ != 1)
+            {
+                const cv::Point3f & pt1 = measuringTmpPts_.at(0);
+                main_scene_.addLine(55555, pt1, pt2, measuringMode_ == 0?quadColor:color);
+
+                if(measuringMode_ == 2)
+                {
+                    // Use respective orientation for each circle
+                    Eigen::Quaternionf q1;
+                	cv::Vec3f normalGL1 = measuringTmpNormals_.front();
+                    q1.setFromTwoVectors(Eigen::Vector3f(0,0,1), Eigen::Vector3f(normalGL1[0],normalGL1[0],normalGL1[0]));
+                    main_scene_.addCircle(55555,
+                                        circleRadius,
+                                        rtabmap::Transform(
+                                                           measuringTmpPts_.front().x,
+                                                           measuringTmpPts_.front().y,
+                                                           measuringTmpPts_.front().z,
+                                                           q1.x(),q1.y(), q1.z(), q1.w()), color, quadAlpha);
+                    main_scene_.addCircle(55556,
+                                        circleRadius,
+                                        rtabmap::Transform(pt2.x, pt2.y, pt2.z, q.x(),q.y(), q.z(), q.w()), color, quadAlpha);
+                }
+                else
+                {
+                    // Use same orientation for both quads
+                    main_scene_.addQuad(55555,
+                                        quadSize,
+                                        rtabmap::Transform(
+                                                           measuringTmpPts_.front().x,
+                                                           measuringTmpPts_.front().y,
+                                                           measuringTmpPts_.front().z,
+                                                           q.x(),q.y(), q.z(), q.w()), quadColor, quadAlpha);
+                    main_scene_.addQuad(55556,
+                                quadWidthLeft,
+                                quadWidthRight,
+                                quadHeightBottom,
+                                quadHeightTop,
+                                rtabmap::Transform(pt2.x, pt2.y, pt2.z, q.x(),q.y(), q.z(), q.w()), quadColor, quadAlpha);
+                }
+            }
+            else if(measuringMode_ == 2)
+            {
+                main_scene_.addCircle(55555,
+                            circleRadius,
+                            rtabmap::Transform(pt2.x, pt2.y, pt2.z, q.x(),q.y(), q.z(), q.w()), color, quadAlpha);
+            }
+            else
+            {
+                main_scene_.addQuad(55555,
+                            quadSize,
+                            rtabmap::Transform(pt2.x, pt2.y, pt2.z, q.x(),q.y(), q.z(), q.w()), quadColor, quadAlpha);
+            }
+        }
+    }
+
+    Eigen::Vector3f target = origin+dir*(distance<100.0f?distance:0.5f);
+    rtabmap::Transform pose(target[0], target[1], target[2], 0,0,0);
+    if(main_scene_.hasCloud(-99999))
+    {
+        main_scene_.setCloudPose(-99999, pose);
+    }
+    else
+    {
+        main_scene_.addCloud(-99999, targetPoint_, pcl::IndicesPtr(), pose);
+    }
+
+    // reset states
+    removeMeasureClicked_ = false;
+    addMeasureClicked_ = false;
+    teleportClicked_ = false;
+}
+
 void RTABMapApp::SetCameraType(
     tango_gl::GestureCamera::CameraType camera_type) {
   main_scene_.SetCameraType(camera_type);
@@ -2337,6 +2903,15 @@ void RTABMapApp::setOrthoCropFactor(float value)
 }
 void RTABMapApp::setGridRotation(float value)
 {
+    // Update measuring snap axes
+    rtabmap::Transform rotation(0,0, value * DEGREE_2_RADIANS);
+    renderingMutex_.lock();
+    snapAxes_[0] = rtabmap::util3d::transformPoint(cv::Vec3f(1,0,0), rotation);
+    snapAxes_[1] = rtabmap::util3d::transformPoint(cv::Vec3f(0,1,0), rotation);
+    measuresUpdated_ = true;
+    renderingMutex_.unlock();
+    
+    // Update grid
 	main_scene_.setGridRotation(value);
 }
 void RTABMapApp::setLighting(bool enabled)
@@ -2351,6 +2926,10 @@ void RTABMapApp::setWireframe(bool enabled)
 {
 	main_scene_.setWireframe(enabled);
 }
+void RTABMapApp::setTextureColorSeamsHidden(bool hidden)
+{
+    main_scene_.setTextureColorSeamsHidden(hidden);
+}
 
 void RTABMapApp::setLocalizationMode(bool enabled)
 {
@@ -2538,7 +3117,7 @@ void RTABMapApp::setDepthConfidence(int value)
 void RTABMapApp::setExportPointCloudFormat(const std::string & format)
 {
 #if defined(RTABMAP_PDAL) || defined(RTABMAP_LIBLAS)
-    if(format == "las") {
+    if(format == "las" || format == "laz") {
         exportPointCloudFormat_ = format;
     }
     else
@@ -2710,6 +3289,7 @@ bool RTABMapApp::exportMesh(
 		int optimizedMinClusterSize,
 		float optimizedMaxTextureDistance,
 		int optimizedMinTextureClusterSize,
+        int textureVertexColorPolicy,
 		bool blockRendering)
 {
 	// make sure createdMeshes_ is not modified while exporting! We don't
@@ -2998,7 +3578,7 @@ bool RTABMapApp::exportMesh(
 									0,
 									mergedClouds,
 									optimizedColorRadius,
-									textureSize == 0,
+                                    !(textureSize > 0 && textureVertexColorPolicy == 0),
 									optimizedCleanWhitePolygons,
 									optimizedMinClusterSize);
 
@@ -3240,7 +3820,9 @@ bool RTABMapApp::exportMesh(
 							textureCount,
 							vertexToPixels,
 							true, 10.0f, true ,true, 0, 0, 0, false,
-							&progressionStatus_);
+							&progressionStatus_,
+                        	255,
+                            textureVertexColorPolicy == 1);
                     LOGI("Merging %d textures... globalTextures=%dx%d", (int)textureMesh->tex_materials.size(),
                          globalTextures.cols, globalTextures.rows);
 				}
@@ -3281,9 +3863,28 @@ bool RTABMapApp::exportMesh(
 				}
 				else if(textureMesh->tex_materials.size())
 				{
-					pcl::PointCloud<pcl::PointNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointNormal>);
+                    bool hasColors = false;
-					pcl::fromPCLPointCloud2(textureMesh->cloud, *cloud);
+                    for(unsigned int i=0; i<textureMesh->cloud.fields.size(); ++i)
-					cv::Mat cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*cloud, rtabmap::Transform(), false).data()); // for database
+                    {
+                        if(textureMesh->cloud.fields[i].name.compare("rgb") == 0)
+                        {
+                            hasColors = true;
+                            break;
+                        }
+                    }
+                    cv::Mat cloudMat;
+                    if(hasColors)
+                    {
+                        pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+                        pcl::fromPCLPointCloud2(textureMesh->cloud, *cloud);
+                        cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*cloud, rtabmap::Transform(), false).data()); // for database
+                    }
+                    else
+                    {
+                        pcl::PointCloud<pcl::PointNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointNormal>);
+                        pcl::fromPCLPointCloud2(textureMesh->cloud, *cloud);
+                        cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*cloud, rtabmap::Transform(), false).data()); // for database
+                    }
 
 					// save in database
 					std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons(textureMesh->tex_polygons.size());
@@ -3492,7 +4093,8 @@ bool RTABMapApp::exportMesh(
 bool RTABMapApp::postExportation(bool visualize)
 {
 	LOGI("postExportation(visualize=%d)", visualize?1:0);
-	optMesh_.reset(new pcl::TextureMesh);
+	optTextureMesh_.reset(new pcl::TextureMesh);
+    optMesh_= rtabmap::Mesh();
 	optTexture_ = cv::Mat();
 	exportedMeshUpdated_ = false;
 
@@ -3513,8 +4115,9 @@ bool RTABMapApp::postExportation(bool visualize)
 			if(!cloudMat.empty())
 			{
 				LOGI("postExportation: Found optimized mesh! Visualizing it.");
-				optMesh_ = rtabmap::util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, true);
+				optTextureMesh_ = rtabmap::util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, true);
-				optTexture_ = textures;
+                optMesh_ = rtabmap::Mesh();
+                optTexture_ = textures;
 
 				boost::mutex::scoped_lock  lock(renderingMutex_);
 				visualizingMesh_ = true;
@@ -3606,9 +4209,9 @@ bool RTABMapApp::writeExportedMesh(const std::string & directory, const std::str
 	if(polygonMesh->cloud.data.size())
 	{
 #if defined(RTABMAP_PDAL) || defined(RTABMAP_LIBLAS)
-        if(polygonMesh->polygons.empty() && exportPointCloudFormat_ == "las") {
+        if(polygonMesh->polygons.empty() && (exportPointCloudFormat_ == "las" || exportPointCloudFormat_ == "laz")) {
             // Point cloud LAS
-            std::string filePath = directory + UDirectory::separator() + name + ".las";
+            std::string filePath = directory + UDirectory::separator() + name + (exportPointCloudFormat_ == "las"? ".las" : ".laz");
             LOGI("Saving las (%d vertices) to %s.", (int)polygonMesh->cloud.data.size()/polygonMesh->cloud.point_step, filePath.c_str());
             pcl::PointCloud<pcl::PointXYZRGB> output;
             pcl::fromPCLPointCloud2(polygonMesh->cloud, output);
@@ -3684,7 +4287,7 @@ bool RTABMapApp::writeExportedMesh(const std::string & directory, const std::str
 				totalPolygons += textureMesh->tex_polygons[i].size();
 			}
 			LOGI("Saving obj (%d vertices, %d polygons) to %s.", (int)textureMesh->cloud.data.size()/textureMesh->cloud.point_step, totalPolygons, filePath.c_str());
-			success = pcl::io::saveOBJFile(filePath, *textureMesh) == 0;
+			success = rtabmap::util3d::saveOBJFile(filePath, *textureMesh) == 0;
 
 			if(success)
 			{
@@ -3799,6 +4402,66 @@ int RTABMapApp::postProcessing(int approach)
 	return returnedValue;
 }
 
+void RTABMapApp::clearMeasures()
+{
+    boost::mutex::scoped_lock  lockRender(renderingMutex_);
+    measures_.clear();
+    measuresUpdated_ = true;
+}
+
+void RTABMapApp::setMeasuringMode(int mode)
+{
+    if(visualizingMesh_)
+    {
+        measuringMode_ = mode;
+    }
+}
+
+void RTABMapApp::addMeasureButtonClicked()
+{
+    if(visualizingMesh_)
+    {
+        boost::mutex::scoped_lock lock(renderingMutex_);
+        addMeasureClicked_ = true;
+    }
+}
+
+void RTABMapApp::teleportButtonClicked()
+{
+    if(visualizingMesh_)
+    {
+        boost::mutex::scoped_lock lock(renderingMutex_);
+        teleportClicked_ = true;
+    }
+}
+
+void RTABMapApp::removeMeasure()
+{
+    if(visualizingMesh_)
+    {
+        boost::mutex::scoped_lock lock(renderingMutex_);
+        removeMeasureClicked_ = true;
+    }
+}
+
+void RTABMapApp::setMetricSystem(bool enabled)
+{
+    metricSystem_ = enabled;
+    if(measures_.size())
+    {
+        measuresUpdated_ = true;
+    }
+}
+
+void RTABMapApp::setMeasuringTextSize(float size)
+{
+    measuringTextSize_ = size;
+    if(measures_.size())
+    {
+        measuresUpdated_ = true;
+    }
+}
+
 void RTABMapApp::postOdometryEvent(
 		rtabmap::Transform pose,
 		float rgb_fx, float rgb_fy, float rgb_cx, float rgb_cy,

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);
@@ -176,10 +178,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 +214,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;
@@ -264,11 +276,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

@@ -821,6 +821,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");
-				attribute_color = glGetAttribLocation(program, "aColor");
+            glEnableVertexAttribArray(attribute_color);
-				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);
+    removeCloudOrMesh(id);
-	if(iter != pointClouds_.end())
-	{
-		delete iter->second;
-		pointClouds_.erase(iter);
-	}
 
 	//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);
+    removeCloudOrMesh(id);
-	if(iter != pointClouds_.end())
-	{
-		delete iter->second;
-		pointClouds_.erase(iter);
-	}
 
 	//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/ios/RTABMapApp.xcodeproj/project.pbxproj

@@ -8,12 +8,15 @@
 
 /* Begin PBXBuildFile section */
 		4E0D83832621F52C00C879AC /* Settings.bundle in Resources */ = {isa = PBXBuildFile; fileRef = 4E0D83822621F52C00C879AC /* Settings.bundle */; };
+		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 */; };
 		4E8B155426273A580037FC53 /* Zip in Frameworks */ = {isa = PBXBuildFile; productRef = 4E8B155326273A580037FC53 /* Zip */; };
 		4EB1A66625A0D90B0043C7BF /* RTABMap.swift in Sources */ = {isa = PBXBuildFile; fileRef = 4EB1A66525A0D90B0043C7BF /* RTABMap.swift */; };
+		4ED73AD12D7FECF3007899C2 /* liblaszip.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4ED73AD02D7FECF3007899C2 /* liblaszip.a */; };
 		4EE016B3259BE441008CCE65 /* ViewController.swift in Sources */ = {isa = PBXBuildFile; fileRef = 4EE016AF259BE441008CCE65 /* ViewController.swift */; };
 		4EE016B4259BE441008CCE65 /* SceneDelegate.swift in Sources */ = {isa = PBXBuildFile; fileRef = 4EE016B0259BE441008CCE65 /* SceneDelegate.swift */; };
 		4EE016B9259BE449008CCE65 /* AppDelegate.swift in Sources */ = {isa = PBXBuildFile; fileRef = 4EE016B8259BE449008CCE65 /* AppDelegate.swift */; };
@@ -155,13 +158,21 @@
 /* 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>"; };
+		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>"; };
 		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>"; };
@@ -346,6 +357,7 @@
 			buildActionMask = 2147483647;
 			files = (
 				4EFAA9432CAE4E960055DA51 /* liblas.a in Frameworks */,
+				4ED73AD12D7FECF3007899C2 /* liblaszip.a in Frameworks */,
 				4EFD0F50259D67D900575D88 /* liblibwebp.a in Frameworks */,
 				4EFD0F51259D67D900575D88 /* liblibpng.a in Frameworks */,
 				4EFD0F52259D67D900575D88 /* liblibjpeg-turbo.a in Frameworks */,
@@ -475,6 +487,7 @@
 		4EE01634259BDCC7008CCE65 /* RTABMapApp */ = {
 			isa = PBXGroup;
 			children = (
+				44D4681D2D538A4100B094BA /* RTABMapApp.entitlements */,
 				4E20B24F266AB95600316EE6 /* Images.xcassets */,
 				4EFD0BB0259D503200575D88 /* NativeWrapper */,
 				4EFD0B62259D501E00575D88 /* tango-gl */,
@@ -496,6 +509,7 @@
 		4EFD0B2A259D4DE900575D88 /* Frameworks */ = {
 			isa = PBXGroup;
 			children = (
+				4ED73AD02D7FECF3007899C2 /* liblaszip.a */,
 				4EFAA9422CAE4E960055DA51 /* liblas.a */,
 				4EFD0F4D259D67D900575D88 /* liblibjpeg-turbo.a */,
 				4EFD0F4C259D67D900575D88 /* liblibpng.a */,
@@ -679,6 +693,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 +811,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 +820,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 */,
@@ -986,6 +1008,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;
@@ -1013,7 +1036,7 @@
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/lib",
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/share/OpenCV/3rdparty/lib",
 				);
-				MARKETING_VERSION = 0.21.8;
+				MARKETING_VERSION = 0.21.11;
 				OTHER_CFLAGS = "";
 				PRODUCT_BUNDLE_IDENTIFIER = com.introlab.rtabmap;
 				PRODUCT_NAME = "$(TARGET_NAME)";
@@ -1043,6 +1066,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;
@@ -1070,7 +1094,7 @@
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/lib",
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/share/OpenCV/3rdparty/lib",
 				);
-				MARKETING_VERSION = 0.21.8;
+				MARKETING_VERSION = 0.21.11;
 				ONLY_ACTIVE_ARCH = YES;
 				OTHER_CFLAGS = "";
 				PRODUCT_BUNDLE_IDENTIFIER = com.introlab.rtabmap;

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">
+                            <stackView opaque="NO" contentMode="scaleToFill" axis="vertical" spacing="30" translatesAutoresizingMaskIntoConstraints="NO" id="kPv-DO-hpd">
-                                <rect key="frame" x="323" y="366" width="60" height="120"/>
+                                <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"/>
+                                <rect key="frame" x="51" y="637.33333333333337" width="291" height="20.333333333333371"/>
-                                <color key="backgroundColor" white="0.0" alpha="0.3002232142857143" colorSpace="custom" customColorSpace="genericGamma22GrayColorSpace"/>
                                 <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="qAR-pI-uT2" firstAttribute="bottom" secondItem="773-n4-lob" secondAttribute="bottom" constant="20" id="h09-nI-8yH"/>
-                            <constraint firstItem="Z1x-Af-hdf" firstAttribute="top" secondItem="JMv-UE-cvK" secondAttribute="bottom" constant="38.5" id="icg-DR-ZpX"/>
+                            <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"/>
+                                <rect key="frame" x="21" y="394" width="351.33333333333331" height="64.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>
+                                <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 {
+        if let image = ViewController.previewImages[databaseURL.path]
-            let downloadedImage = getPreviewImage(databasePath: databaseURL.path)
+        {
-          DispatchQueue.main.async {
+            self.coverImageView.image = image
-            self.coverImageView.image = downloadedImage
+        }
-          }
+        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)
@@ -622,3 +630,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);
@@ -115,6 +117,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)
     }

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_CAMERA,     // Camera/Motion on  - Showing optimized mesh and localizing
-        STATE_VISUALIZING_WHILE_LOADING // Camera/Motion off - Loading data while showing optimized mesh
+        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()
     }
     
@@ -175,13 +191,46 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         }
     }
     
+    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 +285,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 +325,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 +346,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 +383,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 +407,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 +494,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 +544,22 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         }
     }
     
-    func getMemoryUsage() -> UInt64 {
+    func getAvailableMemory() -> Int {
-        var taskInfo = mach_task_basic_info()
+        return os_proc_available_memory()/(1024*1024)
-        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
-        }
     }
     
     @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 +603,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"))
+            	if(mState != .STATE_VISUALIZING_AND_MEASURING)
-                {
+            	{
-                    message = "LiDAR is disabled (Settings->Mapping->LiDAR Mode = OFF), only tracked features will be mapped."
+                	if(!UserDefaults.standard.bool(forKey: "LidarMode"))
-                    self.setMeshRendering(viewMode: 0)
+                	{
-                }
+       		        	message = "LiDAR is disabled (Settings->Mapping->LiDAR Mode = OFF), only tracked features will be mapped."
-                else if !depthSupported
+       		        	self.setMeshRendering(viewMode: 0)
-                {
+        	    	}
-                    message = "The device does not have a LiDAR, only tracked features will be mapped. A LiDAR is required for accurate 3D reconstruction."
+                	else if !depthSupported
-                    self.setMeshRendering(viewMode: 0)
+                	{
-                }
+                    	message = "The device does not have a LiDAR, only tracked features will be mapped. A LiDAR is required for accurate 3D reconstruction."
-                else
+                    	self.setMeshRendering(viewMode: 0)
-                {
+                	}
-                    configuration.frameSemantics = .sceneDepth
+                	else
-                }
+                	{
+                    	configuration.frameSemantics = .sceneDepth
+                	}
+            	}
                 
                 session.run(configuration, options: [.resetSceneReconstruction, .resetTracking, .removeExistingAnchors])
                 
                 switch mState {
-                case .STATE_VISUALIZING:
+                case .STATE_VISUALIZING_AND_MEASURING,
-                    updateState(state: .STATE_VISUALIZING_CAMERA)
+                    .STATE_VISUALIZING_CAMERA:
+                    break // State should be already set
                 default:
                     locationManager?.startUpdatingLocation()
                     updateState(state: .STATE_CAMERA)
@@ -599,6 +696,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 +709,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 +724,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 +735,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 +750,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 +764,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
+            orthoDistanceSlider.isHidden = cameraMode != 3 || mState == .STATE_PROCESSING
-            orthoGridSlider.isHidden = cameraMode != 3 || mState != .STATE_VISUALIZING_WHILE_LOADING
+            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 +779,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 +790,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 +805,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 +816,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,10 +831,11 @@ 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
@@ -717,7 +846,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         {
             view?.enableSetNeedsDisplay = false
             self.isPaused = false
-            print("diaableSetNeedsDisplay")
+            print("disableSetNeedsDisplay")
         }
         
         if !self.isPaused {
@@ -726,6 +855,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 +932,25 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
             	self.newScan(dataRecordingMode: true)
         	})
         ])
+        
+        // Measuring menu
+        print("measuringMode = \(measuringMode)")
+        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 +976,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 +998,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 +1037,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 +1053,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 +1076,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 +1104,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)
@@ -1378,6 +1555,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 +1577,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 +1896,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?
@@ -1750,6 +1930,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 +1951,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
                     optimizedMinClusterSize: optimizedMinClusterSize,
                     optimizedMaxTextureDistance: maxTextureDistance,
                     optimizedMinTextureClusterSize: minTextureClusterSize,
+                    textureVertexColorPolicy: textureVertexColorPolicy,
                     blockRendering: blockRendering)
                 
             }, completion:{
@@ -1789,14 +1972,17 @@ 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)
                             }
 
@@ -1856,6 +2042,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 +2086,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         })
     }
     
-    func stopMapping(ignoreSaving: Bool)
+    func stopMapping(ignoreSaving: Bool, offerPostProcessing: Bool = true)
     {
         session.pause()
         locationManager?.stopUpdatingLocation()
@@ -1913,11 +2101,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 +2201,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 +2297,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 +2347,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 +2396,34 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
                         let fileURLs = try FileManager.default.contentsOfDirectory(at: exportDir, includingPropertiesForKeys: nil)
                         if(!fileURLs.isEmpty)
                         {
-                            do {
+                            if(isLAZ)
-                                zipFileUrl = try Zip.quickZipFiles(fileURLs, fileName: fileName) // Zip
+                            {
-                                print("Zip file \(zipFileUrl.path) created (size=\(zipFileUrl.fileSizeString)")
+                                zipFileUrl = self.getDocumentDirectory().appendingPathComponent(fileName+".laz")
-                                success = true
+                                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 {
+                            else
-                              print("Something went wrong while zipping")
+                            {
+                                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 +2439,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 +2525,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
 
     @IBAction func recordAction(_ sender: UIButton) {
         rtabmap?.setPausedMapping(paused: false);
+        lowMemoryWarningShown = false
         updateState(state: .STATE_MAPPING)
     }
     
@@ -2344,6 +2557,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 +2605,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

@@ -222,6 +222,27 @@ 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
+  cd LASzip
+  git checkout 2.0.1
+else
+  cd LASzip
+fi
+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
@@ -233,7 +254,7 @@ cd libLAS
 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

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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
+   "FB721A581AB28819F73FE51E829FF0DABE28E9119C997DAC5C46402C47BE8805517EA7FA9CFF5BBC6D7EBDFCF72AF2E152033FEE6DEB666072B9EA17C2915C7DF8B525DD2DB748D367E402B3627CA80B5983954755BF1CF2E7248D295B89EC73F417987AD3CC4A5B1974A4825B7DF5BDDFBF998D3413F47992FC8A932BCAAD0B51065B6920EE66AA4642ED7F1F606E5C6EBB4FD5C5484A075779AC866FF6F6F3642F2E48F6B691CFA01D4F67B3F5BC75F911CE24FD5ECEBFB76D3B4FFE0CE04C606FEAD792184717CBB5E2AC4FF77679EE2127E67D1DCA04574BC1E7F7EDF72A448F6F057A8CC815F780DD70F136B557AC38569EF2FC65984BC9BA182C64BFFFA9C1F76E2639A757B790EF57BEBCA1FD1EC38CEC0398147C00E3AAD6AB6C80DDBDF33FCB6B3FD26BFF5D81F27F44FDBAE7AD788602170AC215E689D6DCA7BE646B8C4E5D06FE3197001EEEF0DCA7E16A0E84ECDBFF99574376BD277AE77273FBBD0AD1E3F8E09AC652E4123943FD006046D5FA940D6ED2E0534AD601B018F06E3FF7AE61016E35BADA85980CEC56B54E65039CE87D117EE9B5FFC36BDFAAD53172D06129E074E0B5E8A3CF55233C960A57C30316C0653F844179E37175037A3DEEDCB8E0ABB00E7B8D89C045C05AD9757A3ED8FE2EB0659EE75A26C005DEB94CC3A58796AD839F913181022B5E2606A03E10F69DAAF5291B604706B288DEA6A4E70FF02BFFBA57795FAD9CECA6BA29B078D5BA8C74705699B5191819DF4D494B03770A6EF4BC35F063E01B04EE823E8FBD32CEDDB02B2E03606DBA702B0C45B273AC655E009C50B2FC1D8201D59076AB0C2580D7BDEB5E4866CF6007188D5B0CAC14D37F26D35F01F59AB2E426121DC1407AD888F4C58E34806BBC1BD25BC052EDF7CA45EE28EA6B604CA4DA4C841103B07C30F02B24CE28514F36819984B3784F0676A95AA744A28E3400185964960F7F4192AB4A927B6830FB3FB00CB9090997F6E6B379D53A2512894100B0332E3E63C8FAB613DD01FCBACC8731B0116EBDFB1A29EDAF2470F134E3D3B54F24128984A4862579A703852CF804AC437DE6C913C9F45F1EC0260C647F8C0756AE5AA7442291485408CE27EF0F028A1A007C1657CF015C0C40E99907231D6051E02460C3F6BD138944223122000EC0A54516968E07FC17389712CBAF2612894422916803C19A1043EDF88944229148241289442291482412894422914824128944229148241289442291482412894422914824128944229148241289442291482412894422914824128944229148241289C41000987324D7E8CE96CB5CA822D9A3B2853AE6AD427E2201CC0B2C52A1FC05D2F73F3112C9BEFB731729C39A08DE48D28E925696345FD6EF6D490F4BBA4CD2116636A548C5223A7D47D2E6921E31B3FD0A94335AD25E92BE2C691949EFCF364D97"
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
+   "FA48A903301D57F86B9B3CCE73D04263AEF18B3406829D1BF49949C166F1C80752C80000F81A8D5FF877806F17212F901D9A9CA60161DDF76641897DE7C2E306804BB7795D19C87DD3DBD6D72C10B7CA5D68EE7E98C0B44FDCFC7E4E7F67FFDEB12F8F7CF6EB057D36A5F121745B1EF20339C746CEB3C8551F7789C8BB89A0D012715749A1A9B0B800D5336974C9E5B20CB22767C82CC65316C00238F770587D11F21F002C0D2CDFC3ABEF6260C40700E381BF02E1FA208583CBC8F915EE79BB3F39E6FDB7121A5BE9EE39DC43673FE2E508EF2C41AF902256035C93C6E548C1959EBDA683D7727DCA8FAD853E0D17797F20CE2F158B429D414EABA175A063D185806229684F017F047E817B084C899CFFEAED8FDE91FC7568CCB2780B57A5EE97B86A8CB1A582B7CE43BEA7C73CB878139FFBF3941191B900F1D9DE23B892C807E28A92C4D625CFA51AA0A7CB1AC005C055B88C9B7095528089C08A39CB0D57042D7341985A76C973B55799B23B01B712E90DDEE791EB00A04AA81F003C805B7532D712EB5DEA7371F05DBCAC0CA1A380BB223FB6664C01D62A41AF90220600477471DE31D6EC53FE68E0C11EE49E96D325E844C7A20700CBD3BEFC68C8C939EBD0EDF720DF929C4E87032372727DC836917B7097E70E907B043EB04D1B99E3812FE72C73148D6EB0999458918FFA14C89965C9ED165C49F4D3C8B3167DC5009FC5059C9652DABB1D344E34A690D3B2DFED042F815B7CA51D6F01DB15AE9046C60020D36161E08E0EE5CD20E7875F07FA955108680D1A6BCE37E39F14601A03FE42F3A233A1FCBC17621A45A38F7D5C11E7D944FE31C467DC31EEA180FA07B41E00DC44418B3051FD6240FE00604699B213830B5CC6854F792BA3E266A387110FCA7807E70BCEAD025807FA840CCB0180A7CBEEB855FF6235B9DFC19946BF9697BC2EF42AA5101003DFBF30D80FDC83F9095CDA5A6123625C30E89DC41F868F033F2B486ECC175F48A9D1163A7C39FB8EC5DC1D3319A8855EC8F5A77100F002CE255068453AAA5F0CC81FF8E65BEA3531A4C8BE8B3597DC5B14B0E85B8D56F5BE85F3AF2E2E57A77D9C5C4182094529931800B7D8C6E7E4D26FDE95F482DCF52F747DEAC1042E006739B92245E3253D64660F95287F71B9CF603EB954D087CDAC507FFC600167755853D2C2726B318C97748799F59D75D146EE629256944B777DDACCC616292F903D5AD2B292FE676645A476B692FDBA060AE18C31B3C55AF54F0C6F7041A89F96BBE7549EAA9D4824128902C0C5BFF86EA78BABD629313208CBFA26128944A25CBEA77A6BECB95529924824128944A2047029987E0A72A9E9878944229148242A00D80457D7806C20505A80752291482412890AC1957E3E09D8B06A5D12894422914824128944229148241289442291482412894422914824"
+   "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

@@ -482,6 +482,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 +505,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 +544,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 +566,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>
@@ -804,6 +836,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.21.11</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/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/Memory.h

@@ -343,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() :
+	OdometryInfo();
-		lost(true),
+	OdometryInfo copyWithoutData() const;
-		features(0),
+	std::map<std::string, float> statistics(const Transform & pose = Transform());
-		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;
-	}
 
 	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

@@ -171,7 +171,8 @@ 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);
 
 protected:
 	Optimizer(

corelib/include/rtabmap/core/Parameters.h

@@ -477,6 +477,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());
@@ -488,6 +490,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.");
@@ -673,7 +678,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()));
@@ -878,8 +882,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, 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,  "Angular variance to set on marker detections. Set to >=9999 to use only position (xyz) constraint in graph optimization.");
+    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;

corelib/include/rtabmap/core/SensorData.h

@@ -207,8 +207,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 depthRaw() const {return !(_depthOrRightRaw.type()==CV_8UC1 || _depthOrRightRaw.type()==CV_8UC3) ? _depthOrRightRaw : cv::Mat();}
-	cv::Mat rightRaw() const {return _depthOrRightRaw.type()==CV_8UC1?_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);
@@ -330,7 +330,7 @@ private:
 	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
 	LaserScan _laserScanRaw;
 
 	std::vector<CameraModel> _cameraModels;

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
+			int imageWidth = 1280, // 640 or 1280
-			float imageRate=0.0f,
+			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 setExtendedDisparity(bool extendedDisparity = false, bool enableCompanding = false);
-	void setSubpixelMode(bool enabled, int fractionalBits = 3);
+	void setSubpixelMode(bool enabled = false, int fractionalBits = 3);
-	void setCompanding(bool enabled, int width=96);
+	void setDisparityWidthAndFilter(int disparityWidth = 96, int medianFilter = 5);
-	void setRectification(bool useSpecTranslation, float alphaScaling = 0.0f, bool enabled=true);
+	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 setDetectFeatures(int detectFeatures = 0, const std::string & blobPath = "");
-	void setBlobPath(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
+	// compute normals for the mesh if not already here
-	if(!hasNormals || (!hasColors && coloredOutput))
+	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_x = normal[0];
+					cloud->at(v.vertices[j]).normal_z = normal[2];
-						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;
-					}
 				}
 			}
 			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/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
@@ -586,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

@@ -85,6 +85,7 @@ SET(SRC_FILES
     
     Odometry.cpp
     OdometryThread.cpp
+	OdometryInfo.cpp
     odometry/OdometryF2M.cpp
     odometry/OdometryMono.cpp
     odometry/OdometryF2F.cpp

corelib/src/DBDriverSqlite3.cpp

@@ -6781,12 +6781,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)
+	checkArguments();
-	{
-		_stopId = _startId;
-	}
-
-	if(_stopMapId>-1 && _stopMapId<_startMapId)
-	{
-		_stopMapId = _startMapId;
-	}
 }
 
 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 &,
-		const std::string & cameraName)
+		const std::string &)
 {
 	if(_dbDriver)
 	{
@@ -557,34 +580,134 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
 			}
 
 			data.uncompressData();
-			if(data.cameraModels().size() > 1 &&
+			std::map<int, int> cameraOldNewIndices;
-				_cameraIndex >= 0)
+			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
+					dbModels.push_back(data.stereoCameraModels()[i].left());
-					int subImageWidth = data.imageRaw().cols/data.cameraModels().size();
+				}
-					cv::Mat image;
+			}
+
+			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;
+				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 &&
+							data.imageRaw().cols % dbModels.size() == 0 &&
-							_cameraIndex*subImageWidth < data.imageRaw().cols);
+							(int)_cameraIndices[i]*subImageWidth < data.imageRaw().cols);
-					image= cv::Mat(data.imageRaw(),
+					if(combinedImages.empty())
-						   cv::Rect(_cameraIndex*subImageWidth, 0, subImageWidth, data.imageRaw().rows)).clone();
+					{
+						// 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/dbModels.size();
-								subImageWidth == data.depthOrRightRaw().cols/(int)data.cameraModels().size() &&
+						UASSERT(data.depthOrRightRaw().cols % dbModels.size() == 0 &&
-								_cameraIndex*subImageWidth < data.depthOrRightRaw().cols);
+								subImageWidth == data.depthOrRightRaw().cols/(int)dbModels.size() &&
-						depth = cv::Mat(data.depthOrRightRaw(),
+								(int)_cameraIndices[i]*subImageWidth < data.depthOrRightRaw().cols);
-							    cv::Rect(_cameraIndex*subImageWidth, 0, subImageWidth, data.depthOrRightRaw().rows)).clone();
+						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);
 					}
-					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, 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);
@@ -623,14 +746,62 @@ 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 camra local transforms, let's move 3D words accordingly
+					UASSERT(dbModels.size() == combinedLocalTransforms.size());
+					std::vector<cv::Point3f> newKeypoints3D;
+					for(size_t i = 0; i<keypoints3D.size(); ++i)
+					{
+						cv::Point3f pt = util3d::transformPoint(keypoints3D.at(i), dbModels[i].localTransform().inverse());
+						pt = util3d::transformPoint(pt, combinedLocalTransforms[i]);
+						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;

corelib/src/Graph.cpp

@@ -958,12 +958,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(linkT.x() - t.x()),
-					fabs(iter->second.transform().y() - t.y()),
+					fabs(linkT.y() - t.y()),
-					force3DoF?0:fabs(iter->second.transform().z() - t.z()));
+					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;
@@ -984,7 +996,7 @@ void computeMaxGraphErrors(
 				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);
+				linkT.getEulerAngles(link_roll, link_pitch, link_yaw);
 				float angularError = uMax3(
 						force3DoF?0:fabs(opt_roll - link_roll),
 						force3DoF?0:fabs(opt_pitch - link_pitch),

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);
+			rgbToDepthFactorX = 1.0f/(model.imageWidth()>0?float(model.imageWidth())/float(depth.cols):1.0f);
-			rgbToDepthFactorY = 1.0f/(model.imageHeight()>0?model.imageHeight()/depth.rows:1);
+			rgbToDepthFactorY = 1.0f/(model.imageHeight()>0?float(model.imageHeight())/float(depth.rows):1.0f);
 		}
         else if(markerLength_ == 0)
         {

corelib/src/Memory.cpp

@@ -120,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),
@@ -615,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());
@@ -4519,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,
@@ -4540,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() &&
@@ -4559,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)
 	{
@@ -5556,8 +5610,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;
+					if(_markerOrientationIgnored)
-					covariance(cv::Range(3,6), cv::Range(3,6)) *= _markerAngVariance;
+					{
+						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());
@@ -5754,8 +5832,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)
@@ -5902,12 +5980,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(),
@@ -5915,20 +5993,20 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 								laserScan.angleMax(),
 								laserScan.angleIncrement(),
 								laserScan.localTransform()),
-						compressedImage,
+						compressedImage.empty()?data.imageCompressed():compressedImage,
-						compressedDepth,
+						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(),
@@ -5936,8 +6014,8 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 								laserScan.angleMax(),
 								laserScan.angleIncrement(),
 								laserScan.localTransform()),
-						compressedImage,
+						compressedImage.empty()?data.imageCompressed():compressedImage,
-						compressedDepth,
+						compressedDepth.empty()?data.depthOrRightCompressed():compressedDepth,
 						cameraModels,
 						id,
 						0,
@@ -5986,12 +6064,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(),
@@ -6007,12 +6085,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(),
@@ -6132,13 +6210,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,220 @@
+/*
+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())
+		{
+			rtabmap::Transform diff = transformGroundTruth.inverse()*transform;
+			stats.insert(std::make_pair("Odometry/TG_error_lin/m", diff.getNorm()));
+			stats.insert(std::make_pair("Odometry/TG_error_ang/deg", diff.getAngle()*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

@@ -556,7 +556,8 @@ 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)
 {
 	UDEBUG("rematchFeatures=%d", rematchFeatures?1:0);
 	int wordCount = 0;
@@ -612,10 +613,16 @@ void Optimizer::computeBACorrespondences(
 						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, useLinkTransformAsGuess?iter->second.transform():Transform(), &info);
-					//Transform t = reg.computeTransformationMod(sFrom, sTo, iter->second.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()),
@@ -132,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);
@@ -314,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);
@@ -372,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
 	{
@@ -721,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);
 				}
@@ -1576,347 +1573,305 @@ Transform RegistrationVis::computeTransformationImpl(
 	info.matchesIDs.clear();
 	if(toSignature.getWords().size())
 	{
-		Transform transforms[2];
+		std::vector<int> inliers;
-		std::vector<int> inliers[2];
+		std::vector<int> matches;
-		std::vector<int> matches[2];
+
-		cv::Mat covariances[2];
+		if(_estimationType == 2) // Epipolar Geometry
-		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)
 		{
-			// A to B
+			UDEBUG("");
-			Signature * signatureA;
+			if((toSignature.sensorData().stereoCameraModels().size() != 1 ||
-			Signature * signatureB;
+				!toSignature.sensorData().stereoCameraModels()[0].isValidForProjection()) &&
-			if(dir == 0)
+				(toSignature.sensorData().cameraModels().size() != 1 ||
+				!toSignature.sensorData().cameraModels()[0].isValidForProjection()))
 			{
-				signatureA = &fromSignature;
+				UERROR("Calibrated camera required (multi-cameras not supported).");
-				signatureB = &toSignature;
 			}
-			else
+			else if((int)fromSignature.getWords().size() >= _minInliers &&
+					(int)toSignature.getWords().size() >= _minInliers)
 			{
-				signatureA = &toSignature;
+				UASSERT((fromSignature.sensorData().stereoCameraModels().size() == 1 && fromSignature.sensorData().stereoCameraModels()[0].isValidForProjection()) || (fromSignature.sensorData().cameraModels().size() == 1 && fromSignature.sensorData().cameraModels()[0].isValidForProjection()));
-				signatureB = &fromSignature;
+				const CameraModel & cameraModel = fromSignature.sensorData().stereoCameraModels().size()?fromSignature.sensorData().stereoCameraModels()[0].left():fromSignature.sensorData().cameraModels()[0];
-			}
+
-			if(_estimationType == 2) // Epipolar Geometry
+				// we only need the camera transform, send guess words3 for scale estimation
-			{
+				Transform cameraTransform;
-				UDEBUG("");
+				double variance = 1.0f;
-				if((signatureB->sensorData().stereoCameraModels().size() != 1 ||
+				std::vector<int> matchesV;
-					!signatureB->sensorData().stereoCameraModels()[0].isValidForProjection()) &&
+				std::map<int, int> uniqueWordsA = uMultimapToMapUnique(fromSignature.getWords());
-				   (signatureB->sensorData().cameraModels().size() != 1 ||
+				std::map<int, int> uniqueWordsB = uMultimapToMapUnique(toSignature.getWords());
-					!signatureB->sensorData().cameraModels()[0].isValidForProjection()))
+				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 &&
+				for(std::map<int, int>::iterator iter=uniqueWordsB.begin(); iter!=uniqueWordsB.end(); ++iter)
-						(int)signatureB->getWords().size() >= _minInliers)
 				{
-					UASSERT((signatureA->sensorData().stereoCameraModels().size() == 1 && signatureA->sensorData().stereoCameraModels()[0].isValidForProjection()) || (signatureA->sensorData().cameraModels().size() == 1 && signatureA->sensorData().cameraModels()[0].isValidForProjection()));
+					wordsB.insert(std::make_pair(iter->first, toSignature.getWordsKpts()[iter->second]));
-					const CameraModel & cameraModel = signatureA->sensorData().stereoCameraModels().size()?signatureA->sensorData().stereoCameraModels()[0].left():signatureA->sensorData().cameraModels()[0];
+				}
+				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
+				if(!cameraTransform.isNull())
-					Transform cameraTransform;
+				{
-					double variance = 1.0f;
+					if((int)inliers3D.size() >= _minInliers)
-					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)
 					{
-						wordsA.insert(std::make_pair(iter->first, signatureA->getWordsKpts()[iter->second]));
+						if(variance <= _epipolarGeometryVar)
-						if(!signatureA->getWords3().empty())
 						{
-							words3A.insert(std::make_pair(iter->first, signatureA->getWords3()[iter->second]));
+							if(this->force3DoF())
-						}
-					}
-					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())
+								transform = cameraTransform.to3DoF();
-								{
-									transforms[dir] = cameraTransform.to3DoF();
-								}
-								else
-								{
-									transforms[dir] = cameraTransform;
-								}
 							}
 							else
 							{
-								msg = uFormat("Variance is too high! (Max %s=%f, variance=%f)", Parameters::kVisEpipolarGeometryVar().c_str(), _epipolarGeometryVar, variance);
+								transform = cameraTransform;
-								UINFO(msg.c_str());
 							}
 						}
 						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");
+					msg = uFormat("No camera transform found");
-					UWARN(msg.c_str());
+					UINFO(msg.c_str());
 				}
 			}
-			else if(_estimationType == 1) // PnP
+			else if(fromSignature.getWords().size() == 0)
 			{
-				UDEBUG("");
+				msg = uFormat("No enough features (%d)", (int)fromSignature.getWords().size());
-				if((signatureB->sensorData().stereoCameraModels().empty() || !signatureB->sensorData().stereoCameraModels()[0].isValidForProjection()) &&
+				UWARN(msg.c_str());
-				   (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());
-					}
-				}
-
 			}
 			else
 			{
-				UDEBUG("");
+				msg = uFormat("No camera model");
-				// 3D -> 3D
+				UWARN(msg.c_str());
-				if((int)signatureA->getWords3().size() >= _minInliers &&
+			}
-				   (int)signatureB->getWords3().size() >= _minInliers)
+		}
+		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> uniqueWordsA = uMultimapToMapUnique(fromSignature.getWords());
-					std::map<int, int> uniqueWordsB = uMultimapToMapUnique(signatureB->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)",
+					msg = uFormat("Not enough features in images (old=%d, new=%d, min=%d)",
-							(int)signatureA->getWords3().size(), (int)signatureB->getWords3().size(), _minInliers);
+							(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());
 		}
-
+		else
-		std::vector<int> allInliers = inliers[0];
-		if(inliers[1].size())
 		{
-			std::set<int> allInliersSet(allInliers.begin(), allInliers.end());
+			UDEBUG("");
-			unsigned int oi = allInliers.size();
+			// 3D -> 3D
-			allInliers.resize(allInliers.size() + inliers[1].size());
+			if((int)fromSignature.getWords3().size() >= _minInliers &&
-			for(unsigned int i=0; i<inliers[1].size(); ++i)
+				(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);
+			else
-		}
-		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)
 			{
-				if(allMatchesSet.find(matches[1][i]) == allMatchesSet.end())
+				msg = uFormat("Not enough 3D features in images (old=%d, new=%d, min=%d)",
-				{
+						(int)fromSignature.getWords3().size(), (int)toSignature.getWords3().size(), _minInliers);
-					allMatches[oi++] = matches[1][i];
+				UINFO(msg.c_str());
-				}
 			}
-			allMatches.resize(oi);
 		}
 
 		if(_bundleAdjustment > 0 &&
 			_estimationType < 2 &&
-			!transforms[0].isNull() &&
+			!transform.isNull() &&
-			allInliers.size() &&
+			inliers.size() &&
 			fromSignature.getWords3().size() &&
 			toSignature.getWords().size() &&
 			(fromSignature.sensorData().stereoCameraModels().size() >= 1 || fromSignature.sensorData().cameraModels().size() >= 1) &&
@@ -1930,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(covariance.cols==6 && covariance.rows == 6 && covariance.type() == CV_64FC1);
-			{
+			if(covariance.at<double>(0,0)<=COVARIANCE_LINEAR_EPSILON)
-				UASSERT(covariances[i].cols==6 && covariances[i].rows == 6 && covariances[i].type() == CV_64FC1);
+				covariance.at<double>(0,0) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
-				if(covariances[i].at<double>(0,0)<=COVARIANCE_LINEAR_EPSILON)
+			if(covariance.at<double>(1,1)<=COVARIANCE_LINEAR_EPSILON)
-					covariances[i].at<double>(0,0) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
+				covariance.at<double>(1,1) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
-				if(covariances[i].at<double>(1,1)<=COVARIANCE_LINEAR_EPSILON)
+			if(covariance.at<double>(2,2)<=COVARIANCE_LINEAR_EPSILON)
-					covariances[i].at<double>(1,1) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
+				covariance.at<double>(2,2) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
-				if(covariances[i].at<double>(2,2)<=COVARIANCE_LINEAR_EPSILON)
+			if(covariance.at<double>(3,3)<=COVARIANCE_ANGULAR_EPSILON)
-					covariances[i].at<double>(2,2) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
+				covariance.at<double>(3,3) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
-				if(covariances[i].at<double>(3,3)<=COVARIANCE_ANGULAR_EPSILON)
+			if(covariance.at<double>(4,4)<=COVARIANCE_ANGULAR_EPSILON)
-					covariances[i].at<double>(3,3) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
+				covariance.at<double>(4,4) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
-				if(covariances[i].at<double>(4,4)<=COVARIANCE_ANGULAR_EPSILON)
+			if(covariance.at<double>(5,5)<=COVARIANCE_ANGULAR_EPSILON)
-					covariances[i].at<double>(4,4) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
+				covariance.at<double>(5,5) = 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())));
-			}
 
+			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()) ||
@@ -2015,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(util3d::isFinite(pt3D), uFormat("3D point %d is not finite!?", wordId).c_str());
-				{
-					UASSERT_MSG(!_forwardEstimateOnly, uFormat("3D point %d is not finite!?", wordId).c_str());
-					sbaOutliers.insert(wordId);
-					continue;
-				}
 
 				points3DMap.insert(std::make_pair(wordId, pt3D));
 
@@ -2093,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()));
+					UDEBUG("BA outliers ratio %f", float(sbaOutliers.size())/float(inliers.size()));
-					allInliers = newInliers;
+					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());
+							(int)inliers.size(), _minInliers, (int)inliers.size()+sbaOutliers.size(), fromSignature.id(), toSignature.id());
-					transforms[0].setNull();
+					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();
@@ -2137,36 +2076,16 @@ Transform RegistrationVis::computeTransformationImpl(
 			}
 			else
 			{
-				transforms[0].setNull();
+				transform.setNull();
 			}
-			transforms[1].setNull();
 		}
 
-		info.inliersIDs = allInliers;
+		info.inliersIDs = inliers;
-		info.matchesIDs = allMatches;
+		info.matchesIDs = matches;
-		inliersCount = (int)allInliers.size();
+		inliersCount = (int)inliers.size();
-		matchesCount = (int)allMatches.size();
+		matchesCount = (int)matches.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];
-		}
 
-		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;
@@ -2187,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
 					{
@@ -2204,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

@@ -1241,7 +1241,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;
@@ -2159,7 +2159,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
@@ -2181,7 +2181,7 @@ bool Rtabmap::process(
 					else
 					{
 						_loopClosureHypothesis = _highestHypothesis;
-						rejectedGlobalLoopClosure = false;
+						rejectedLoopClosure = false;
 					}
 
 					timeHypothesesValidation = timer.ticks();
@@ -2192,7 +2192,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);
 				}
 
@@ -3061,15 +3061,15 @@ bool Rtabmap::process(
 				loopClosureVisualInliers = info.inliers;
 				loopClosureVisualInliersRatio = info.inliersRatio;
 				loopClosureVisualMatches = info.matches;
-				rejectedGlobalLoopClosure = transform.isNull();
+				rejectedLoopClosure = transform.isNull();
-				if(rejectedGlobalLoopClosure)
+				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);
 				}
@@ -3078,7 +3078,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);
@@ -3086,14 +3086,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));
+				rejectedLoopClosure = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, information));
-				if(!rejectedGlobalLoopClosure)
+				if(!rejectedLoopClosure)
 				{
 					loopClosureLinksAdded.push_back(std::make_pair(signature->id(), _loopClosureHypothesis.first));
 				}
 			}
 
-			if(rejectedGlobalLoopClosure)
+			if(rejectedLoopClosure)
 			{
 				_loopClosureHypothesis.first = 0;
 			}
@@ -3137,7 +3137,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));
 				}
 			}
@@ -3180,7 +3180,6 @@ bool Rtabmap::process(
 	double optimizationError = 0.0;
 	int optimizationIterations = 0;
 	Transform previousMapCorrection;
-	bool rejectedLandmark = false;
 	bool delayedLocalization = false;
 	UDEBUG("RGB-D SLAM mode: %d", _rgbdSlamMode?1:0);
 	UDEBUG("Incremental: %d", _memory->isIncremental());
@@ -3768,8 +3767,7 @@ bool Rtabmap::process(
 			{
 				_loopClosureHypothesis.first = 0;
 				lastProximitySpaceClosureId = 0;
-				rejectedGlobalLoopClosure = true;
+				rejectedLoopClosure = true;
-				rejectedLandmark = true;
 			}
 		}
 		else
@@ -3804,8 +3802,7 @@ bool Rtabmap::process(
 				updateConstraints = false;
 				_loopClosureHypothesis.first = 0;
 				lastProximitySpaceClosureId = 0;
-				rejectedGlobalLoopClosure = true;
+				rejectedLoopClosure = true;
-				rejectedLandmark = true;
 			}
 			else if(_memory->isIncremental() &&
 			  loopClosureLinksAdded.size() &&
@@ -3915,8 +3912,7 @@ bool Rtabmap::process(
 					updateConstraints = false;
 					_loopClosureHypothesis.first = 0;
 					lastProximitySpaceClosureId = 0;
-					rejectedGlobalLoopClosure = true;
+					rejectedLoopClosure = true;
-					rejectedLandmark = true;
 				}
 			}
 
@@ -4077,7 +4073,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)
 				{
@@ -4179,7 +4175,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);
@@ -4274,7 +4270,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!",
@@ -4294,7 +4290,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
@@ -4314,7 +4310,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();)
@@ -4722,7 +4718,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

@@ -365,7 +365,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)
 		{

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/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),
+	enableCompanding_(false),
-	compandingWidth_(0),
+	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 companding being also enabled, disabling companding...");
-		{
+		enableCompanding_ = false;
-			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;
-		}
 	}
 #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);
+	UASSERT(disparityWidth == 64 || disparityWidth == 96);
-	compandingWidth_ = enabled?width:0;
+	disparityWidth_ = disparityWidth;
-	if(compandingWidth_ != 0 && extendedDisparity_)
+	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...");
+		UWARN("Maximum disparity value '%d' exceeds the maximum supported '1024' by median filter, disabling median filter...", maxDisp);
-		extendedDisparity_ = false;
+		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
+	device_ = std::make_unique<dai::Device>(deviceToUse);
-	stereoModel_ = StereoCameraModel();
+	auto deviceName = device_->getDeviceName();
-	targetSize_ = cv::Size(resolution_<2?1280:resolution_==4?1920:640, resolution_==0?720:resolution_==1?800:resolution_==2?400:resolution_==3?480:1200);
+	auto imuType = device_->getConnectedIMU();
-
+	UINFO("Device Name: %s, Device Serial: %s", deviceName.c_str(), device_->getMxId().c_str());
-	dai::Pipeline p;
+	UINFO("Available Camera Sensors: ");
-	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: ");
 	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();
+	auto cameraId = outputMode_==2?dai::CameraBoardSocket::CAM_A:dai::CameraBoardSocket::CAM_B;
-	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;
 	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" ||
+		if(deviceName == "OAK-D")
-		   eeprom.boardName == "BW1098OBC")
 		{
 			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(
+			if(imuType == "BMI270")
-				 0,  1,  0,  0.04,
+			{
-				 1,  0,  0,  0.020265,
+				imuLocalTransform_ = Transform(
-				 0,  0, -1,  0);
+				 	 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())
+	this->setImageRate(0);
-	{
-		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.");
-	}
-
 	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 rgbOrLeft = messageGroup->get<dai::ImgFrame>(outputMode_==2?"rgb":"left");
-	auto rectifRightOrDepth = messageGroup->get<dai::ImgFrame>(imagesRectified_ && outputMode_?"depth":"right");
+	auto depthOrRight = messageGroup->get<dai::ImgFrame>(outputMode_?"depth":"right");
 
-	cv::Mat leftOrColor, depthOrRight;
+	double stamp = std::chrono::duration<double>(depthOrRight->getTimestampDevice(dai::CameraExposureOffset::MIDDLE).time_since_epoch()).count();
-	if(device_->getDeviceInfo().protocol == X_LINK_TCP_IP || mxidOrName_.find(".") != std::string::npos)
+	if(outputMode_)
-	{
+		data = SensorData(cv::imdecode(rgbOrLeft->getData(), cv::IMREAD_ANYCOLOR), depthOrRight->getCvFrame(), stereoModel_.left(), this->getNextSeqID(), stamp);
-		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);
-		}
-	}
 	else
-	{
+		data = SensorData(cv::imdecode(rgbOrLeft->getData(), cv::IMREAD_GRAYSCALE), cv::imdecode(depthOrRight->getData(), cv::IMREAD_GRAYSCALE), stereoModel_, this->getNextSeqID(), stamp);
-		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);
 
 	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(!kpts.empty()){
-		if(detectFeatures_ == 2)
+			std::vector<cv::KeyPoint> keypoints;
-			coarse_desc.forEach<cv::Vec<float, 256>>([&](cv::Vec<float, 256>& descriptor, const int position[]) -> void {
+			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);
+			descriptors = descriptors.reshape(1);
-		cv::Mat mapY(keypoints.size(), 1, CV_32FC1);
+
-		for(size_t i=0; i<keypoints.size(); ++i)
+			data.setFeatures(keypoints, std::vector<cv::Point3f>(), descriptors);
-		{
-			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);
-		});
-		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);

corelib/src/camera/CameraStereoVideo.cpp

@@ -56,7 +56,8 @@ CameraStereoVideo::CameraStereoVideo(
 		usbDevice_(0),
 		usbDevice2_(-1),
 		_width(0),
-		_height(0)
+		_height(0),
+		rightGrayScale_(true)
 {
 }
 
@@ -74,7 +75,8 @@ CameraStereoVideo::CameraStereoVideo(
 		usbDevice_(0),
 		usbDevice2_(-1),
 		_width(0),
-		_height(0)
+		_height(0),
+		rightGrayScale_(true)
 {
 }
 
@@ -89,7 +91,8 @@ CameraStereoVideo::CameraStereoVideo(
 	usbDevice_(device),
 	usbDevice2_(-1),
 	_width(0),
-	_height(0)
+	_height(0),
+	rightGrayScale_(true)
 {
 }
 
@@ -105,7 +108,8 @@ CameraStereoVideo::CameraStereoVideo(
 	usbDevice_(deviceLeft),
 	usbDevice2_(deviceRight),
 	_width(0),
-	_height(0)
+	_height(0),
+	rightGrayScale_(true)
 {
 }
 
@@ -379,7 +383,7 @@ SensorData CameraStereoVideo::captureImage(SensorCaptureInfo * info)
 
 		// Rectification
 		bool rightCvt = false;
-		if(rightImage.type() != CV_8UC1)
+		if(rightImage.type() != CV_8UC1 && rightGrayScale_)
 		{
 			cv::Mat tmp;
 			cv::cvtColor(rightImage, tmp, CV_BGR2GRAY);

corelib/src/camera/CameraStereoZed.cpp

@@ -783,7 +783,10 @@ SensorData CameraStereoZed::captureImage(SensorCaptureInfo * info)
 #endif
 				cv::Mat rgbaRight = slMat2cvMat(tmp);
 				cv::Mat right;
-				cv::cvtColor(rgbaRight, right, cv::COLOR_BGRA2GRAY);
+				if(rightGrayScale_)
+					cv::cvtColor(rgbaRight, right, cv::COLOR_BGRA2GRAY);
+				else
+					cv::cvtColor(rgbaRight, right, cv::COLOR_BGRA2BGR);
 #if ZED_SDK_MAJOR_VERSION < 3
 				data = SensorData(left, right, stereoModel_, this->getNextSeqID(), UTimer::now());
 #else
@@ -891,4 +894,11 @@ void CameraStereoZed::postInterIMUPublic(const IMU & imu, double stamp)
 	postInterIMU(imu, stamp);
 }
 
+void CameraStereoZed::setRightGrayScale(bool enabled)
+{
+#ifdef RTABMAP_ZED
+	rightGrayScale_ = enabled;
+#endif
+}
+
 } // namespace rtabmap

corelib/src/camera/CameraStereoZedOC.cpp

@@ -755,7 +755,10 @@ SensorData CameraStereoZedOC::captureImage(SensorCaptureInfo * info)
 
 					// ----> Extract left and right images from side-by-side
 					left = frameBGR(cv::Rect(0, 0, frameBGR.cols / 2, frameBGR.rows));
-					cv::cvtColor(frameBGR(cv::Rect(frameBGR.cols / 2, 0, frameBGR.cols / 2, frameBGR.rows)),right,cv::COLOR_BGR2GRAY);
+					if(rightGrayScale_)
+						cv::cvtColor(frameBGR(cv::Rect(frameBGR.cols / 2, 0, frameBGR.cols / 2, frameBGR.rows)),right,cv::COLOR_BGR2GRAY);
+					else
+						right = frameBGR(cv::Rect(frameBGR.cols / 2, 0, frameBGR.cols / 2, frameBGR.rows));
 					// <---- Extract left and right images from side-by-side
 
 					if(stereoModel_.isValidForRectification())
@@ -792,4 +795,11 @@ SensorData CameraStereoZedOC::captureImage(SensorCaptureInfo * info)
 	return data;
 }
 
+void CameraStereoZedOC::setRightGrayScale(bool enabled)
+{
+#ifdef RTABMAP_ZEDOC
+	rightGrayScale_ = enabled;
+#endif
+}
+
 } // namespace rtabmap

corelib/src/odometry/OdometryF2M.cpp

@@ -60,6 +60,8 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
 	keyFrameThr_(Parameters::defaultOdomKeyFrameThr()),
 	visKeyFrameThr_(Parameters::defaultOdomVisKeyFrameThr()),
 	maxNewFeatures_(Parameters::defaultOdomF2MMaxNewFeatures()),
+	initDepthFactor_(Parameters::defaultOdomF2MInitDepthFactor()),
+	floorThreshold_(Parameters::defaultOdomF2MFloorThreshold()),
 	scanKeyFrameThr_(Parameters::defaultOdomScanKeyFrameThr()),
 	scanMaximumMapSize_(Parameters::defaultOdomF2MScanMaxSize()),
 	scanSubtractRadius_(Parameters::defaultOdomF2MScanSubtractRadius()),
@@ -67,6 +69,9 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
 	scanMapMaxRange_(Parameters::defaultOdomF2MScanRange()),
 	bundleAdjustment_(Parameters::defaultOdomF2MBundleAdjustment()),
 	bundleMaxFrames_(Parameters::defaultOdomF2MBundleAdjustmentMaxFrames()),
+	bundleMinMotion_(Parameters::defaultOdomF2MBundleAdjustmentMinMotion()),
+	bundleMaxKeyFramesPerFeature_(Parameters::defaultOdomF2MBundleAdjustmentMaxKeyFramesPerFeature()),
+	bundleUpdateFeatureMapOnAllFrames_(Parameters::defaultOdomF2MBundleUpdateFeatureMapOnAllFrames()),
 	validDepthRatio_(Parameters::defaultOdomF2MValidDepthRatio()),
 	pointToPlaneK_(Parameters::defaultIcpPointToPlaneK()),
 	pointToPlaneRadius_(Parameters::defaultIcpPointToPlaneRadius()),
@@ -81,6 +86,8 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
 	Parameters::parse(parameters, Parameters::kOdomKeyFrameThr(), keyFrameThr_);
 	Parameters::parse(parameters, Parameters::kOdomVisKeyFrameThr(), visKeyFrameThr_);
 	Parameters::parse(parameters, Parameters::kOdomF2MMaxNewFeatures(), maxNewFeatures_);
+	Parameters::parse(parameters, Parameters::kOdomF2MInitDepthFactor(), initDepthFactor_);
+	Parameters::parse(parameters, Parameters::kOdomF2MFloorThreshold(), floorThreshold_);
 	Parameters::parse(parameters, Parameters::kOdomScanKeyFrameThr(), scanKeyFrameThr_);
 	Parameters::parse(parameters, Parameters::kOdomF2MScanMaxSize(), scanMaximumMapSize_);
 	Parameters::parse(parameters, Parameters::kOdomF2MScanSubtractRadius(), scanSubtractRadius_);
@@ -91,6 +98,9 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
 	Parameters::parse(parameters, Parameters::kOdomF2MScanRange(), scanMapMaxRange_);
 	Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustment(), bundleAdjustment_);
 	Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustmentMaxFrames(), bundleMaxFrames_);
+	Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustmentMinMotion(), bundleMinMotion_);
+	Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustmentMaxKeyFramesPerFeature(), bundleMaxKeyFramesPerFeature_);
+	Parameters::parse(parameters, Parameters::kOdomF2MBundleUpdateFeatureMapOnAllFrames(), bundleUpdateFeatureMapOnAllFrames_);
 	Parameters::parse(parameters, Parameters::kOdomF2MValidDepthRatio(), validDepthRatio_);
 
 	Parameters::parse(parameters, Parameters::kIcpPointToPlaneK(), pointToPlaneK_);
@@ -120,6 +130,7 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
 	UASSERT(visKeyFrameThr_>=0);
 	UASSERT(scanKeyFrameThr_ >= 0.0f && scanKeyFrameThr_<=1.0f);
 	UASSERT(maxNewFeatures_ >= 0);
+	UASSERT(initDepthFactor_>0.0f);
 
 	int corType = Parameters::defaultVisCorType();
 	Parameters::parse(parameters, Parameters::kVisCorType(), corType);
@@ -143,16 +154,6 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
 	}
 	uInsert(bundleParameters, ParametersPair(Parameters::kVisEstimationType(), uNumber2Str(estType)));
 
-	bool forwardEst = Parameters::defaultVisForwardEstOnly();
-	Parameters::parse(parameters, Parameters::kVisForwardEstOnly(), forwardEst);
-	if(!forwardEst)
-	{
-		UWARN("%s=false is not supported by OdometryF2M, setting to true.",
-				Parameters::kVisForwardEstOnly().c_str());
-		forwardEst = true;
-	}
-	uInsert(bundleParameters, ParametersPair(Parameters::kVisForwardEstOnly(), uBool2Str(forwardEst)));
-
 	regPipeline_ = Registration::create(bundleParameters);
 	if(bundleAdjustment_>0 && regPipeline_->isScanRequired())
 	{
@@ -280,6 +281,7 @@ Transform OdometryF2M::computeTransform(
 			std::map<int, Transform> bundlePoses;
 			std::multimap<int, Link> bundleLinks;
 			std::map<int, std::vector<CameraModel> > bundleModels;
+			float bundleAvgInlierDistance = 0.0f;
 
 			for(int guessIteration=0;
 					guessIteration<(!guess.isNull()&&regPipeline_->isImageRequired()?2:1) && transform.isNull();
@@ -386,6 +388,7 @@ Transform OdometryF2M::computeTransform(
 
 							UDEBUG("Fill matches (%d)", (int)regInfo.inliersIDs.size());
 							std::map<int, std::map<int, FeatureBA> > wordReferences;
+							size_t maxKeyFramesForInlier = 0;
 							for(unsigned int i=0; i<regInfo.inliersIDs.size(); ++i)
 							{
 								int wordId =regInfo.inliersIDs[i];
@@ -398,6 +401,10 @@ Transform OdometryF2M::computeTransform(
 								// all other references
 								std::map<int, std::map<int, FeatureBA> >::iterator refIter = bundleWordReferences_.find(wordId);
 								UASSERT_MSG(refIter != bundleWordReferences_.end(), uFormat("wordId=%d", wordId).c_str());
+								if(info && refIter->second.size() > maxKeyFramesForInlier)
+								{
+									maxKeyFramesForInlier = refIter->second.size();
+								}
 
 								std::map<int, FeatureBA> references;
 								int step = bundleMaxFrames_>0?(refIter->second.size() / bundleMaxFrames_):1;
@@ -472,6 +479,7 @@ Transform OdometryF2M::computeTransform(
 							{
 								info->localBundlePoses = bundlePoses;
 								info->localBundleModels = bundleModels;
+								info->localBundleMaxKeyFramesForInlier = maxKeyFramesForInlier;
 							}
 
 							UDEBUG("Local Bundle Adjustment Before: %s", transform.prettyPrint().c_str());
@@ -479,8 +487,13 @@ Transform OdometryF2M::computeTransform(
 							{
 								if(!bundlePoses.rbegin()->second.isNull())
 								{
+									if(info)
+									{
+										info->localBundleOutliersPerCam = std::vector<int>(lastFrameModels.size(),0);
+									}
 									if(sbaOutliers.size())
 									{
+										regInfo.inliersPerCam = std::vector<int>(lastFrameModels.size(),0);
 										std::vector<int> newInliers(regInfo.inliersIDs.size());
 										int oi=0;
 										for(unsigned int i=0; i<regInfo.inliersIDs.size(); ++i)
@@ -488,6 +501,11 @@ Transform OdometryF2M::computeTransform(
 											if(sbaOutliers.find(regInfo.inliersIDs[i]) == sbaOutliers.end())
 											{
 												newInliers[oi++] = regInfo.inliersIDs[i];
+												regInfo.inliersPerCam[wordReferences.at(regInfo.inliersIDs[i]).at(lastFrame_->id()).cameraIndex] += 1;
+											}
+											else if(info)
+											{
+												info->localBundleOutliersPerCam[wordReferences.at(regInfo.inliersIDs[i]).at(lastFrame_->id()).cameraIndex] += 1;
 											}
 										}
 										newInliers.resize(oi);
@@ -534,13 +552,52 @@ Transform OdometryF2M::computeTransform(
 											regInfo.covariance.at<double>(4,4) *= 0.1;
 										if(regInfo.covariance.at<double>(5,5)>thrAng)
 											regInfo.covariance.at<double>(5,5) *= 0.1;
+
+										// Estimate how much the new frame moved from previous frame in term of pixels
+										if(bundleMinMotion_ > 0.0f)
+										{
+											UASSERT(!bundlePoses_.empty());
+											int count = 0;
+											for(unsigned int i=0; i<regInfo.inliersIDs.size(); ++i)
+											{
+												std::map<int, std::map<int, FeatureBA> >::iterator wter = wordReferences.find(regInfo.inliersIDs[i]);
+												if(wter != wordReferences.end())
+												{
+													std::map<int, FeatureBA>::iterator fter = wter->second.find(bundlePoses_.rbegin()->first);
+													if(fter != wter->second.end())
+													{
+														const FeatureBA & f1 = fter->second; // previous key-frame
+														const FeatureBA & f2 = wter->second.find(lastFrame_->id())->second; // current key-frame
+														float dx = f1.kpt.pt.x - f2.kpt.pt.x;
+														float dy = f1.kpt.pt.y - f2.kpt.pt.y;
+														bundleAvgInlierDistance += sqrt(dx*dx + dy*dy);
+														++count;
+													}
+												}
+											}
+											if(count)
+											{
+												bundleAvgInlierDistance /= count;
+											}
+											UDEBUG("Average pixel distance between %d inliers: %f", count, bundleAvgInlierDistance);
+											if(info)
+											{
+												info->localBundleAvgInlierDistance = bundleAvgInlierDistance;
+											}
+										}
 									}
+									UDEBUG("Local Bundle Adjustment After : %s", transform.prettyPrint().c_str());
+								}
+								else
+								{
+									regInfo.rejectedMsg = "Last bundle pose is null?!";
+									transform.setNull();
 								}
-								UDEBUG("Local Bundle Adjustment After : %s", transform.prettyPrint().c_str());
 							}
 							else
 							{
-								UWARN("Local bundle adjustment failed! transform is not refined.");
+								regInfo.rejectedMsg = "Local bundle adjustment failed!";
+								transform.setNull();
 							}
 						}
 					}
@@ -594,19 +651,73 @@ Transform OdometryF2M::computeTransform(
 				std::vector<cv::Point3f> mapPoints = tmpMap.getWords3();
 				cv::Mat mapDescriptors = tmpMap.getWordsDescriptors();
 
+				// update last frame features without depth (if bundle adjustment was done)
+				// Do this before adding bundle frames to keep mono observations without depth
+				bool lastFrameWords3Updated = false;
+				std::vector<cv::Point3f> lastFrameWords3;
+				if( regPipeline_->isImageRequired() &&
+					!visDepthAsMask &&
+				    bundleAdjustment_>0 &&
+					!lastFrame_->getWords().empty() &&
+					lastFrame_->getWords().size() == lastFrame_->getWords3().size() &&
+					!points3DMap.empty())
+				{
+					lastFrameWords3 = lastFrame_->getWords3();
+					Transform newFramePoseInv = newFramePose.inverse();
+					for(std::multimap<int, int>::const_iterator iter=lastFrame_->getWords().begin();
+					    iter!=lastFrame_->getWords().end();
+						++iter)
+					{
+						cv::Point3f & pt = lastFrameWords3.at(iter->second);
+						if(!util3d::isFinite(pt))
+						{
+							std::map<int, cv::Point3f>::iterator mapIter = points3DMap.find(iter->first);
+							if(mapIter != points3DMap.end())
+							{
+								// in base frame
+								pt = util3d::transformPoint(mapIter->second, newFramePoseInv);
+								lastFrameWords3Updated = true;
+							}
+						}
+					}
+				}
+
+				if( regPipeline_->isImageRequired() &&
+					bundleAdjustment_>0 &&
+					bundleUpdateFeatureMapOnAllFrames_ &&
+					!points3DMap.empty())
+				{
+					// update local map 3D points (if bundle adjustment was done)
+					for(std::map<int, cv::Point3f>::iterator iter=points3DMap.begin(); iter!=points3DMap.end(); ++iter)
+					{
+						UASSERT(mapWords.count(iter->first) == 1);
+						mapPoints[mapWords.find(iter->first)->second] = iter->second;
+					}
+					modified = true;
+				}
+
 				bool addVisualKeyFrame = regPipeline_->isImageRequired() &&
 						 (keyFrameThr_ == 0.0f ||
 						  visKeyFrameThr_ == 0 ||
 						  float(regInfo.inliers) <= (keyFrameThr_*float(lastFrame_->getWords().size())) ||
-						  regInfo.inliers <= visKeyFrameThr_);
+						  regInfo.inliers <= visKeyFrameThr_) &&
+						  (bundleAdjustment_==0 || bundleAvgInlierDistance >= bundleMinMotion_);
 
 				bool addGeometricKeyFrame = regPipeline_->isScanRequired() &&
 					(scanKeyFrameThr_==0 || regInfo.icpInliersRatio <= scanKeyFrameThr_);
 
-				addKeyFrame = false;//bundleLinks.rbegin()->second.transform().getNorm() > 5.0f*0.075f;
+				addKeyFrame = addVisualKeyFrame || addGeometricKeyFrame;
-				addKeyFrame = addKeyFrame || addVisualKeyFrame || addGeometricKeyFrame;
+
+				UDEBUG("keyframeThr=%f visKeyFrameThr_=%d matches=%d inliers=%d (avg dist=%f, min=%f) features=%d mp=%d",
+					keyFrameThr_,
+					visKeyFrameThr_,
+					regInfo.matches,
+					regInfo.inliers,
+					bundleAvgInlierDistance,
+					bundleMinMotion_,
+					(int)lastFrame_->sensorData().keypoints().size(),
+					(int)mapPoints.size());
 
-				UDEBUG("keyframeThr=%f visKeyFrameThr_=%d matches=%d inliers=%d features=%d mp=%d", keyFrameThr_, visKeyFrameThr_, regInfo.matches, regInfo.inliers, (int)lastFrame_->sensorData().keypoints().size(), (int)mapPoints.size());
 				if(addKeyFrame)
 				{
 					//Visual
@@ -615,6 +726,7 @@ Transform OdometryF2M::computeTransform(
 					UTimer tmpTimer;
 
 					UDEBUG("Update local map");
+					modified = bundleAdjustment_>0; // We always add new references even if we don't add/remove points
 
 					// update local map
 					UASSERT(mapWords.size() == mapPoints.size());
@@ -639,12 +751,14 @@ Transform OdometryF2M::computeTransform(
 						bundleModels_.insert(*bundleModels.find(lastFrame_->id()));
 						iterBundlePosesRef = bundlePoseReferences_.find(lastFrame_->id());
 
-						// update local map 3D points (if bundle adjustment was done)
+						if(!bundleUpdateFeatureMapOnAllFrames_)
-						for(std::map<int, cv::Point3f>::iterator iter=points3DMap.begin(); iter!=points3DMap.end(); ++iter)
 						{
-							UASSERT(mapWords.count(iter->first) == 1);
+							// update local map 3D points (if bundle adjustment was done)
-							//UDEBUG("Updated %d (%f,%f,%f) -> (%f,%f,%f)", iter->first, mapPoints[mapWords.find(iter->first)->second].x, mapPoints[mapWords.find(iter->first)->second].y, mapPoints[mapWords.find(iter->first)->second].z, iter->second.x, iter->second.y, iter->second.z);
+							for(std::map<int, cv::Point3f>::iterator iter=points3DMap.begin(); iter!=points3DMap.end(); ++iter)
-							mapPoints[mapWords.find(iter->first)->second] = iter->second;
+							{
+								UASSERT(mapWords.count(iter->first) == 1);
+								mapPoints[mapWords.find(iter->first)->second] = iter->second;
+							}
 						}
 					}
 
@@ -733,7 +847,16 @@ Transform OdometryF2M::computeTransform(
 									}
 									else
 									{
-										bundleWordReferences_.find(iter->first)->second.insert(std::make_pair(lastFrame_->id(), FeatureBA(kpt, depth, cv::Mat(), cameraIndex)));
+										std::map<int, rtabmap::FeatureBA> & keyframes = bundleWordReferences_.find(iter->first)->second;
+										if(bundleMaxKeyFramesPerFeature_ != 0 && (int)keyframes.size() > bundleMaxKeyFramesPerFeature_)
+										{
+											// To keep number of keyframes looking at same feature bounded
+											int frameId = keyframes.rbegin()->first;
+											UASSERT(bundlePoseReferences_.find(frameId) != bundlePoseReferences_.end());
+											bundlePoseReferences_.at(frameId) -= 1;
+											keyframes.erase(frameId);
+										}
+										keyframes.insert(std::make_pair(lastFrame_->id(), FeatureBA(kpt, depth, cv::Mat(), cameraIndex)));
 									}
 								}
 							}
@@ -750,6 +873,28 @@ Transform OdometryF2M::computeTransform(
 						if(maxNewFeatures_ == 0  || added < maxNewFeatures_)
 						{
 							int cameraIndex = iter->second.second.second.second.second;
+							cv::Point3f pt = iter->second.second.second.first;
+							if(!util3d::isFinite(pt))
+							{
+								// get the ray instead
+								float x = iter->second.second.first.pt.x; //subImageWidth should be already removed
+								float y = iter->second.second.first.pt.y;
+								Eigen::Vector3f ray = util3d::projectDepthTo3DRay(
+										lastFrameModels[cameraIndex].imageSize(),
+										x,
+										y,
+										lastFrameModels[cameraIndex].cx(),
+										lastFrameModels[cameraIndex].cy(),
+										lastFrameModels[cameraIndex].fx(),
+										lastFrameModels[cameraIndex].fy());
+								float scaleInf = initDepthFactor_ * lastFrameModels[cameraIndex].fx();
+								pt = util3d::transformPoint(cv::Point3f(ray[0]*scaleInf, ray[1]*scaleInf, ray[2]*scaleInf), lastFrameModels[cameraIndex].localTransform()); // in base_link frame
+							}
+							if(floorThreshold_ != 0.0f && pt.z < floorThreshold_)
+							{
+								continue;
+							}
+
 							if(bundleAdjustment_>0)
 							{
 								if(lastFrame_->getWords().count(iter->second.first) == 1)
@@ -778,23 +923,6 @@ Transform OdometryF2M::computeTransform(
 
 							mapWords.insert(mapWords.end(), std::make_pair(iter->second.first, mapWords.size()));
 							mapWordsKpts.push_back(iter->second.second.first);
-							cv::Point3f pt = iter->second.second.second.first;
-							if(!util3d::isFinite(pt))
-							{
-								// get the ray instead
-								float x = iter->second.second.first.pt.x; //subImageWidth should be already removed
-								float y = iter->second.second.first.pt.y;
-								Eigen::Vector3f ray = util3d::projectDepthTo3DRay(
-										lastFrameModels[cameraIndex].imageSize(),
-										x,
-										y,
-										lastFrameModels[cameraIndex].cx(),
-										lastFrameModels[cameraIndex].cy(),
-										lastFrameModels[cameraIndex].fx(),
-										lastFrameModels[cameraIndex].fy());
-								float scaleInf = (0.05 * lastFrameModels[cameraIndex].fx()) / 0.01;
-								pt = util3d::transformPoint(cv::Point3f(ray[0]*scaleInf, ray[1]*scaleInf, ray[2]*scaleInf), lastFrameModels[cameraIndex].localTransform()); // in base_link frame
-							}
 							mapPoints.push_back(util3d::transformPoint(pt, newFramePose));
 							mapDescriptors.push_back(iter->second.second.second.second.first);
 							if(lastFrameOldestNewId_ > iter->second.first)
@@ -803,6 +931,10 @@ Transform OdometryF2M::computeTransform(
 							}
 							++added;
 						}
+						else
+						{
+							break;
+						}
 					}
 					UDEBUG("");
 
@@ -1125,6 +1257,12 @@ Transform OdometryF2M::computeTransform(
 
 					map_->setWords(mapWords, mapWordsKpts, mapPoints, mapDescriptors);
 				}
+
+				if(lastFrameWords3Updated)
+				{
+					// update output with refined 3d points from bundle adjustment
+					data.setFeatures(lastFrame_->getWordsKpts(), lastFrameWords3, lastFrame_->getWordsDescriptors());
+				}
 			}
 
 			if(info)

corelib/src/odometry/OdometryMono.cpp

@@ -205,17 +205,21 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 	int nFeatures = 0;
 
 	// convert to grayscale
-	if(data.imageRaw().channels() > 1)
+	if(data.imageRaw().channels() > 1 || data.rightRaw().channels() > 1)
 	{
-		cv::Mat newFrame;
+		cv::Mat newFrame = data.imageRaw();
-		cv::cvtColor(data.imageRaw(), newFrame, cv::COLOR_BGR2GRAY);
+		cv::Mat newFrameRight = data.rightRaw();
+		if(data.imageRaw().channels() > 1)
+			cv::cvtColor(data.imageRaw(), newFrame, cv::COLOR_BGR2GRAY);
+		if(data.rightRaw().channels() > 1)
+			cv::cvtColor(data.rightRaw(), newFrameRight, cv::COLOR_BGR2GRAY);
 		if(!data.stereoCameraModels().empty())
 		{
-			data.setStereoImage(newFrame, data.rightRaw(), data.stereoCameraModels());
+			data.setStereoImage(newFrame, newFrameRight, data.stereoCameraModels());
 		}
 		else
 		{
-			data.setRGBDImage(newFrame, data.depthRaw(), data.cameraModels());
+			data.setRGBDImage(newFrame, newFrameRight, data.cameraModels());
 		}
 	}
 

corelib/src/odometry/OdometryORBSLAM3.cpp

@@ -448,11 +448,16 @@ Transform OdometryORBSLAM3::computeTransform(
 	{
 		localTransform = data.stereoCameraModels()[0].localTransform();
 		cv::Mat leftMono = data.imageRaw();
-                if(data.imageRaw().channels() == 3) {
+        if(data.imageRaw().channels() == 3) {
 			leftMono = cv::Mat();
 			cv::cvtColor(data.imageRaw(), leftMono, CV_BGR2GRAY);
 		}
-		Tcw = orbslam_->TrackStereo(leftMono, data.rightRaw(), data.stamp(), orbslamImus_);
+		cv::Mat rightMono = data.rightRaw();
+        if(data.rightRaw().channels() == 3) {
+			rightMono = cv::Mat();
+			cv::cvtColor(data.imageRaw(), rightMono, CV_BGR2GRAY);
+		}
+		Tcw = orbslam_->TrackStereo(leftMono, rightMono, data.stamp(), orbslamImus_);
 		orbslamImus_.clear();
 	}
 	else

corelib/src/optimizer/OptimizerG2O.cpp

@@ -90,7 +90,14 @@ typedef g2o::LinearSolverCSparse<SlamBlockSolver::PoseMatrixType> SlamLinearCSpa
 typedef g2o::LinearSolverCholmod<SlamBlockSolver::PoseMatrixType> SlamLinearCholmodSolver;
 #endif
 
-#ifdef G2O_SRC_DIR
+// We use G2O_SRC_DIR to know we are version after December 24 2020
+// where VertexSBAPointXYZ has been renamed to VertexPointXYZ
+// (g2o: 0fcccb302787e70ff19f65e70fb103a1295b33a2)
+//
+// VCPKG commented G2O_SRC_DIR from their port so we cannot use
+// G2O_SRC_DIR on windows to deduce it, we then assume it is the
+// latest version without VertexSBAPointXYZ
+#if defined(G2O_SRC_DIR) or defined(WIN32)
 namespace g2o {
 typedef VertexPointXYZ VertexSBAPointXYZ;
 }

corelib/src/stereo/StereoBM.cpp

@@ -82,7 +82,8 @@ cv::Mat StereoBM::computeDisparity(
 {
 	UASSERT(!leftImage.empty() && !rightImage.empty());
 	UASSERT(leftImage.cols == rightImage.cols && leftImage.rows == rightImage.rows);
-	UASSERT((leftImage.type() == CV_8UC1 || leftImage.type() == CV_8UC3) && rightImage.type() == CV_8UC1);
+	UASSERT(leftImage.type() == CV_8UC1 || leftImage.type() == CV_8UC3);
+	UASSERT(rightImage.type() == CV_8UC1 || rightImage.type() == CV_8UC3);
 
 	cv::Mat leftMono;
 	if(leftImage.channels() == 3)
@@ -94,6 +95,16 @@ cv::Mat StereoBM::computeDisparity(
 		leftMono = leftImage;
 	}
 
+	cv::Mat rightMono;
+	if(rightImage.channels() == 3)
+	{
+		cv::cvtColor(rightImage, rightMono, CV_BGR2GRAY);
+	}
+	else
+	{
+		rightMono = rightImage;
+	}
+
 	cv::Mat disparity;
 #if CV_MAJOR_VERSION < 3
 	cv::StereoBM stereo(cv::StereoBM::BASIC_PRESET);
@@ -106,7 +117,7 @@ cv::Mat StereoBM::computeDisparity(
 	stereo.state->textureThreshold = textureThreshold_;
 	stereo.state->speckleWindowSize = speckleWindowSize_;
 	stereo.state->speckleRange = speckleRange_;
-	stereo(leftMono, rightImage, disparity, CV_16SC1);
+	stereo(leftMono, rightMono, disparity, CV_16SC1);
 #else
 	cv::Ptr<cv::StereoBM> stereo = cv::StereoBM::create();
 	stereo->setBlockSize(blockSize_);
@@ -119,7 +130,7 @@ cv::Mat StereoBM::computeDisparity(
 	stereo->setSpeckleWindowSize(speckleWindowSize_);
 	stereo->setSpeckleRange(speckleRange_);
 	stereo->setDisp12MaxDiff(disp12MaxDiff_);
-	stereo->compute(leftMono, rightImage, disparity);
+	stereo->compute(leftMono, rightMono, disparity);
 #endif
 
 	if(minDisparity_>0)

corelib/src/stereo/StereoSGBM.cpp

@@ -71,7 +71,8 @@ cv::Mat StereoSGBM::computeDisparity(
 {
 	UASSERT(!leftImage.empty() && !rightImage.empty());
 	UASSERT(leftImage.cols == rightImage.cols && leftImage.rows == rightImage.rows);
-	UASSERT((leftImage.type() == CV_8UC1 || leftImage.type() == CV_8UC3) && rightImage.type() == CV_8UC1);
+	UASSERT(leftImage.type() == CV_8UC1 || leftImage.type() == CV_8UC3);
+	UASSERT(rightImage.type() == CV_8UC1 || rightImage.type() == CV_8UC3);
 
 	cv::Mat leftMono;
 	if(leftImage.channels() == 3)
@@ -83,6 +84,16 @@ cv::Mat StereoSGBM::computeDisparity(
 		leftMono = leftImage;
 	}
 
+	cv::Mat rightMono;
+	if(rightImage.channels() == 3)
+	{
+		cv::cvtColor(rightImage, rightMono, CV_BGR2GRAY);
+	}
+	else
+	{
+		rightMono = rightImage;
+	}
+
 	cv::Mat disparity;
 #if CV_MAJOR_VERSION < 3
 	cv::StereoSGBM stereo(
@@ -97,7 +108,7 @@ cv::Mat StereoSGBM::computeDisparity(
 			speckleWindowSize_,
 			speckleRange_,
             mode_==1);
-	stereo(leftMono, rightImage, disparity);
+	stereo(leftMono, rightMono, disparity);
 #else
 	cv::Ptr<cv::StereoSGBM> stereo = cv::StereoSGBM::create(
 			minDisparity_,
@@ -111,7 +122,7 @@ cv::Mat StereoSGBM::computeDisparity(
 			speckleWindowSize_,
 			speckleRange_,
             mode_);
-	stereo->compute(leftMono, rightImage, disparity);
+	stereo->compute(leftMono, rightMono, disparity);
 #endif
 
 	if(minDisparity_>0)

corelib/src/util3d.cpp

@@ -822,14 +822,14 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromStereoImages(
 		const ParametersMap & parameters)
 {
 	UASSERT(!imageLeft.empty() && !imageRight.empty());
-	UASSERT(imageRight.type() == CV_8UC1);
+	UASSERT(imageRight.type() == CV_8UC1 || imageRight.type() == CV_8UC3);
 	UASSERT(imageLeft.channels() == 3 || imageLeft.channels() == 1);
 	UASSERT(imageLeft.rows == imageRight.rows &&
 			imageLeft.cols == imageRight.cols);
 	UASSERT(decimation >= 1.0f);
 
 	cv::Mat leftColor = imageLeft;
-	cv::Mat rightMono = imageRight;
+	cv::Mat rightColor = imageRight;
 
 	cv::Mat leftMono;
 	if(leftColor.channels() == 3)
@@ -841,6 +841,16 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromStereoImages(
 		leftMono = leftColor;
 	}
 
+	cv::Mat rightMono;
+	if(rightColor.channels() == 3)
+	{
+		cv::cvtColor(rightColor, rightMono, CV_BGR2GRAY);
+	}
+	else
+	{
+		rightMono = rightColor;
+	}
+
 	return cloudFromDisparityRGB(
 			leftColor,
 			util2d::disparityFromStereoImages(leftMono, rightMono, parameters),
@@ -954,7 +964,7 @@ std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloudsFromSensorData(
 	else if(!sensorData.imageRaw().empty() && !sensorData.rightRaw().empty() && !sensorData.stereoCameraModels().empty())
 	{
 		//stereo
-		UASSERT(sensorData.rightRaw().type() == CV_8UC1);
+		UASSERT(sensorData.rightRaw().type() == CV_8UC1 || sensorData.rightRaw().type() == CV_8UC3);
 
 		cv::Mat leftMono;
 		if(sensorData.imageRaw().channels() == 3)
@@ -966,6 +976,16 @@ std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloudsFromSensorData(
 			leftMono = sensorData.imageRaw();
 		}
 
+		cv::Mat rightMono;
+		if(sensorData.rightRaw().channels() == 3)
+		{
+			cv::cvtColor(sensorData.rightRaw(), rightMono, CV_BGR2GRAY);
+		}
+		else
+		{
+			rightMono = sensorData.rightRaw();
+		}
+
 		UASSERT(int((sensorData.imageRaw().cols/sensorData.stereoCameraModels().size())*sensorData.stereoCameraModels().size()) == sensorData.imageRaw().cols);
 		UASSERT(int((sensorData.rightRaw().cols/sensorData.stereoCameraModels().size())*sensorData.stereoCameraModels().size()) == sensorData.rightRaw().cols);
 		int subImageWidth = sensorData.rightRaw().cols/sensorData.stereoCameraModels().size();
@@ -979,7 +999,7 @@ std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloudsFromSensorData(
 			if(sensorData.stereoCameraModels()[i].isValidForProjection())
 			{
 				cv::Mat left(leftMono, cv::Rect(subImageWidth*i, 0, subImageWidth, leftMono.rows));
-				cv::Mat right(sensorData.rightRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.rightRaw().rows));
+				cv::Mat right(rightMono, cv::Rect(subImageWidth*i, 0, subImageWidth, rightMono.rows));
 				StereoCameraModel model = sensorData.stereoCameraModels()[i];
 				if( roiRatios.size() == 4 &&
 					((roiRatios[0] > 0.0f && roiRatios[0] <= 1.0f) ||

corelib/src/util3d_mapping.cpp

@@ -703,10 +703,7 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
 				cv::Point2i end((pt[0]-xMin)/cellSize, (pt[1]-yMin)/cellSize);
 				if(end!=start)
 				{
-					if(localScans.size() > 1 || map.at<signed char>(end.y, end.x) != 0)
+					rayTrace(start, end, map, true); // trace free space
-					{
-						rayTrace(start, end, map, true); // trace free space
-					}
 				}
 			}
 			// ray tracing for no hits
@@ -730,14 +727,7 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
 				cv::Point2i end((pt[0]-xMin)/cellSize, (pt[1]-yMin)/cellSize);
 				if(end!=start)
 				{
-					if(localScans.size() > 1 || map.at<signed char>(end.y, end.x) != 0)
+					rayTrace(start, end, map, true); // trace free space
-					{
-						rayTrace(start, end, map, true); // trace free space
-						if(map.at<signed char>(end.y, end.x) == -1)
-						{
-							map.at<signed char>(end.y, end.x) = 0; // empty
-						}
-					}
 				}
 			}
 		}
@@ -823,8 +813,6 @@ void rayTrace(const cv::Point2i & start, const cv::Point2i & end, cv::Mat & grid
 {
 	UASSERT_MSG(start.x >= 0 && start.x < grid.cols, uFormat("start.x=%d grid.cols=%d", start.x, grid.cols).c_str());
 	UASSERT_MSG(start.y >= 0 && start.y < grid.rows, uFormat("start.y=%d grid.rows=%d", start.y, grid.rows).c_str());
-	UASSERT_MSG(end.x >= 0 && end.x < grid.cols, uFormat("end.x=%d grid.cols=%d", end.x, grid.cols).c_str());
-	UASSERT_MSG(end.y >= 0 && end.y < grid.rows, uFormat("end.x=%d grid.cols=%d", end.y, grid.rows).c_str());
 
 	cv::Point2i ptA, ptB;
 	ptA = start;
@@ -832,6 +820,10 @@ void rayTrace(const cv::Point2i & start, const cv::Point2i & end, cv::Mat & grid
 
 	float slope = float(ptB.y - ptA.y)/float(ptB.x - ptA.x);
 
+	// clip end point
+	ptB.x = std::min(std::max(ptB.x, 0), grid.cols-1);
+	ptB.y = std::min(std::max(ptB.y, 0), grid.rows-1);
+
 	bool swapped = false;
 	if(slope<-1.0f || slope>1.0f)
 	{

corelib/src/util3d_motion_estimation.cpp

@@ -288,6 +288,62 @@ Transform estimateMotion3DTo2D(
 	return transform;
 }
 
+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<int> * matchesOut,
+	std::vector<int> * inliersOut,
+	bool splitLinearCovarianceComponents)
+{
+	std::vector<std::vector<int> > matchesPerCamera;
+	std::vector<std::vector<int> > inliersPerCamera;
+	Transform t = estimateMotion3DTo2D(
+		words3A,
+		words2B,
+		cameraModels,
+		samplingPolicy,
+		minInliers,
+		iterations,
+		reprojError,
+		flagsPnP,
+		refineIterations,
+		varianceMedianRatio,
+		maxVariance,
+		guess,
+		words3B,
+		covariance,
+		matchesOut?&matchesPerCamera:0,
+		inliersOut?&inliersPerCamera:0,
+		splitLinearCovarianceComponents);
+	if(matchesOut)
+	{
+		for(size_t i=0; i<matchesPerCamera.size(); ++i)
+		{
+			matchesOut->insert(matchesOut->end(), matchesPerCamera[i].begin(), matchesPerCamera[i].end());
+		}
+	}
+	if(inliersOut)
+	{
+		for(size_t i=0; i<inliersPerCamera.size(); ++i)
+		{
+			inliersOut->insert(inliersOut->end(), inliersPerCamera[i].begin(), inliersPerCamera[i].end());
+		}
+	}
+	return t;
+}
+
 Transform estimateMotion3DTo2D(
 			const std::map<int, cv::Point3f> & words3A,
 			const std::map<int, cv::KeyPoint> & words2B,
@@ -303,8 +359,8 @@ Transform estimateMotion3DTo2D(
 			const Transform & guess,
 			const std::map<int, cv::Point3f> & words3B,
 			cv::Mat * covariance,
-			std::vector<int> * matchesOut,
+			std::vector<std::vector<int> > * matchesOut,
-			std::vector<int> * inliersOut,
+			std::vector<std::vector<int> > * inliersOut,
 			bool splitLinearCovarianceComponents)
 {
 	Transform transform;
@@ -649,14 +705,22 @@ Transform estimateMotion3DTo2D(
 
 	if(matchesOut)
 	{
-		*matchesOut = matches;
+		matchesOut->resize(cameraModels.size());
+		UASSERT(matches.size() == cameraIndexes.size());
+		for(size_t i=0; i<matches.size(); ++i)
+		{
+			UASSERT(cameraIndexes[i]>=0 && cameraIndexes[i] < (int)cameraModels.size());
+			matchesOut->at(cameraIndexes[i]).push_back(matches[i]);
+		}
 	}
 	if(inliersOut)
 	{
-		inliersOut->resize(inliers.size());
+		inliersOut->resize(cameraModels.size());
 		for(unsigned int i=0; i<inliers.size(); ++i)
 		{
-			inliersOut->at(i) = matches[inliers[i]];
+			UASSERT(inliers[i]>=0 && inliers[i] < (int)cameraIndexes.size());
+			UASSERT(cameraIndexes[inliers[i]]>=0 && cameraIndexes[inliers[i]] < (int)cameraModels.size());
+			inliersOut->at(cameraIndexes[inliers[i]]).push_back(matches[inliers[i]]);
 		}
 	}
 #endif

corelib/src/util3d_surface.cpp

@@ -1454,6 +1454,7 @@ cv::Mat mergeTextures(
 		bool exposureFusion,
 		const ProgressState * state,
 		unsigned char blankValue,
+		bool clearVertexColorUnderTexture,
 		std::map<int, std::map<int, cv::Vec4d> > * gains,
 		std::map<int, std::map<int, cv::Mat> > * blendingGains,
 		std::pair<float, float> * contrastValues)
@@ -1483,6 +1484,7 @@ cv::Mat mergeTextures(
 			exposureFusion,
 			state,
 			blankValue,
+			clearVertexColorUnderTexture,
 			gains,
 			blendingGains,
 			contrastValues);
@@ -1506,6 +1508,7 @@ cv::Mat mergeTextures(
 		bool exposureFusion,
 		const ProgressState * state,
 		unsigned char blankValue,
+		bool clearVertexColorUnderTexture,
 		std::map<int, std::map<int, cv::Vec4d> > * gainsOut,
 		std::map<int, std::map<int, cv::Mat> > * blendingGainsOut,
 		std::pair<float, float> * contrastValuesOut)
@@ -2186,6 +2189,70 @@ cv::Mat mergeTextures(
 					if(state) state->callback(uFormat("Brightness and contrast auto %fs", timer.ticks()));
 				}
 			}
+			
+			// Cloud color
+			if(clearVertexColorUnderTexture)
+			{
+				int colorOffset = 0;
+				for(unsigned int i=0; i<mesh.cloud.fields.size(); ++i)
+				{
+					if(mesh.cloud.fields[i].name.compare("rgb") == 0)
+					{
+						colorOffset = mesh.cloud.fields[i].offset;
+						break;
+					}
+				}
+				if(colorOffset>0)
+				{
+					pcl::IndicesPtr notTexturedVertexIndices(new std::vector<int>);
+					UASSERT(mesh.tex_coordinates.size() == mesh.tex_polygons.size());
+					for(size_t t=0; t<mesh.tex_polygons.size(); ++t)
+					{
+						int pixelIndex = 0;
+						for(size_t p=0; p<mesh.tex_polygons[t].size(); ++p)
+						{
+							// only clear polygon color if all 3 vertices have valid tex coordinates.
+							bool valid = true;
+							for(size_t v=0; v<mesh.tex_polygons[t][p].vertices.size() && valid; ++v)
+							{
+								UASSERT(pixelIndex+v < mesh.tex_coordinates[t].size());
+								const Eigen::Vector2f & uv = mesh.tex_coordinates[t][pixelIndex+v];
+								if(uv[0] == -1 || uv[1] == -1)
+								{
+									valid = false;
+								}
+							}
+							if(!valid)
+							{
+								for(size_t v=0; v<mesh.tex_polygons[t][p].vertices.size(); ++v)
+								{
+									int vertex = mesh.tex_polygons[t][p].vertices[v];
+									notTexturedVertexIndices->push_back(vertex);
+								}
+							}
+							pixelIndex+=mesh.tex_polygons[t][p].vertices.size();
+						}
+					}
+					// Set up the full indices set
+					pcl::IndicesPtr full_indices(new std::vector<int>(mesh.cloud.width* mesh.cloud.height));
+					for (size_t fii = 0; fii < full_indices->size(); ++fii)  // fii = full indices iterator
+						full_indices->at(fii) = fii;
+
+					// Set up the sorted input indices
+					std::sort (notTexturedVertexIndices->begin (), notTexturedVertexIndices->end ());
+
+					// Store the difference in indices
+					pcl::IndicesPtr texturedVertexIndices(new std::vector<int>());
+					std::set_difference (full_indices->begin (), full_indices->end (), notTexturedVertexIndices->begin (), notTexturedVertexIndices->end (), std::inserter (*texturedVertexIndices, texturedVertexIndices->begin ()));
+
+					for(size_t i=0; i<texturedVertexIndices->size(); ++i)
+					{
+						std::uint32_t white = 0xffffff;
+						UASSERT(texturedVertexIndices->at(i) * mesh.cloud.point_step + colorOffset < mesh.cloud.data.size());
+						memcpy(&mesh.cloud.data.data()[texturedVertexIndices->at(i) * mesh.cloud.point_step + colorOffset], reinterpret_cast<float*>(&white), sizeof(float));
+					}
+				}
+			}
 		}
 	}
 	UDEBUG("globalTextures=%d", globalTextures.cols?globalTextures.cols / globalTextures.rows:0);
@@ -3881,6 +3948,359 @@ bool intersectRayTriangle(
     return true;                    // I is in T
 }
 
+/**
+ * This is a modified copy of https://github.com/PointCloudLibrary/pcl/blob/pcl-1.12.1/io/src/obj_io.cpp
+ * with added color on each vertex if provided
+ */
+int saveOBJFile(
+	const std::string &file_name,
+	const pcl::TextureMesh &tex_mesh,
+	unsigned precision)
+{
+  if (tex_mesh.cloud.data.empty ())
+  {
+    UERROR ("Input point cloud has no data!\n");
+    return (-1);
+  }
+  // Open file
+  std::ofstream fs;
+  fs.precision (precision);
+  fs.open (file_name.c_str ());
+
+  // Define material file
+  std::string mtl_file_name = file_name.substr (0, file_name.find_last_of ('.')) + ".mtl";
+  // Strip path for "mtllib" command
+  std::string mtl_file_name_nopath = mtl_file_name;
+  mtl_file_name_nopath.erase (0, mtl_file_name.find_last_of ('/') + 1);
+
+  /* Write 3D information */
+  // number of points
+  unsigned nr_points  = tex_mesh.cloud.width * tex_mesh.cloud.height;
+  unsigned point_size = static_cast<unsigned> (tex_mesh.cloud.data.size () / nr_points);
+
+  // mesh size
+  unsigned nr_meshes = static_cast<unsigned> (tex_mesh.tex_polygons.size ());
+  // number of faces for header
+  unsigned nr_faces = 0;
+  for (unsigned m = 0; m < nr_meshes; ++m)
+    nr_faces += static_cast<unsigned> (tex_mesh.tex_polygons[m].size ());
+
+  // Write the header information
+  fs << "####" << '\n';
+  fs << "# OBJ dataFile simple version. File name: " << file_name << '\n';
+  fs << "# Vertices: " << nr_points << '\n';
+  fs << "# Faces: " <<nr_faces << '\n';
+  fs << "# Material information:" << '\n';
+  fs << "mtllib " << mtl_file_name_nopath << '\n';
+  fs << "####" << '\n';
+
+  // Write vertex coordinates
+  fs << "# Vertices" << '\n';
+  for (unsigned i = 0; i < nr_points; ++i)
+  {
+    int xyz = 0;
+    // "v" just be written one
+    bool v_written = false;
+    for (std::size_t d = 0; d < tex_mesh.cloud.fields.size (); ++d)
+    {
+      // adding vertex
+      if ((tex_mesh.cloud.fields[d].datatype == pcl::PCLPointField::FLOAT32) && (
+          tex_mesh.cloud.fields[d].name == "x" ||
+          tex_mesh.cloud.fields[d].name == "y" ||
+          tex_mesh.cloud.fields[d].name == "z"))
+      {
+        if (!v_written)
+        {
+           // write vertices beginning with v
+          fs << "v ";
+          v_written = true;
+        }
+        float value;
+        memcpy (&value, &tex_mesh.cloud.data[i * point_size + tex_mesh.cloud.fields[d].offset], sizeof (float));
+        fs << value;
+        if (++xyz == 3)
+          continue;
+        fs << " ";
+      }
+	  else if(tex_mesh.cloud.fields[d].datatype == pcl::PCLPointField::FLOAT32 &&
+          	  tex_mesh.cloud.fields[d].name == "rgb")
+	  {
+		std::uint32_t rgb = *reinterpret_cast<const int*>(&tex_mesh.cloud.data[i * point_size + tex_mesh.cloud.fields[d].offset]);
+		std::uint8_t r = (rgb >> 16) & 0x0000ff;
+		std::uint8_t g = (rgb >> 8)  & 0x0000ff;
+		std::uint8_t b = (rgb)       & 0x0000ff;
+		fs << " " << float(r)/255.0f << " " << float(g)/255.0f << " " << float(b)/255.0f;
+		break;
+	  }
+    }
+    if (xyz != 3)
+    {
+      UERROR ("Input point cloud has no XYZ data!\n");
+      return (-2);
+    }
+    fs << '\n';
+  }
+  fs << "# "<< nr_points <<" vertices" << '\n';
+
+  // Write vertex normals
+  for (unsigned i = 0; i < nr_points; ++i)
+  {
+    int xyz = 0;
+    // "vn" just be written one
+    bool v_written = false;
+    for (std::size_t d = 0; d < tex_mesh.cloud.fields.size (); ++d)
+    {
+      // adding vertex
+      if ((tex_mesh.cloud.fields[d].datatype == pcl::PCLPointField::FLOAT32) && (
+          tex_mesh.cloud.fields[d].name == "normal_x" ||
+          tex_mesh.cloud.fields[d].name == "normal_y" ||
+          tex_mesh.cloud.fields[d].name == "normal_z"))
+      {
+    	  if (!v_written)
+    	  {
+          // write vertices beginning with vn
+          fs << "vn ";
+          v_written = true;
+    	  }
+        float value;
+        memcpy (&value, &tex_mesh.cloud.data[i * point_size + tex_mesh.cloud.fields[d].offset], sizeof (float));
+        fs << value;
+        if (++xyz == 3)
+          break;
+        fs << " ";
+      }
+    }
+    if (xyz != 3)
+    {
+      UERROR ("Input point cloud has no normals!\n");
+      return (-2);
+    }
+    fs << '\n';
+  }
+  // Write vertex texture with "vt" (adding latter)
+
+  for (unsigned m = 0; m < nr_meshes; ++m)
+  {
+    fs << "# " << tex_mesh.tex_coordinates[m].size() << " vertex textures in submesh " << m <<  '\n';
+    for (const auto &coordinate : tex_mesh.tex_coordinates[m])
+    {
+      fs << "vt ";
+      fs <<  coordinate[0] << " " << coordinate[1] << '\n';
+    }
+  }
+
+  unsigned f_idx = 0;
+
+  // int idx_vt =0;
+  for (unsigned m = 0; m < nr_meshes; ++m)
+  {
+    if (m > 0) f_idx += static_cast<unsigned> (tex_mesh.tex_polygons[m-1].size ());
+
+    fs << "# The material will be used for mesh " << m << '\n';
+    fs << "usemtl " <<  tex_mesh.tex_materials[m].tex_name << '\n';
+    fs << "# Faces" << '\n';
+
+    for (std::size_t i = 0; i < tex_mesh.tex_polygons[m].size(); ++i)
+    {
+      // Write faces with "f"
+      fs << "f";
+      // There's one UV per vertex per face, i.e., the same vertex can have
+      // different UV depending on the face.
+      for (std::size_t j = 0; j < tex_mesh.tex_polygons[m][i].vertices.size (); ++j)
+      {
+        std::uint32_t idx = tex_mesh.tex_polygons[m][i].vertices[j] + 1;
+        fs << " " << idx
+           << "/" << tex_mesh.tex_polygons[m][i].vertices.size () * (i+f_idx) +j+1
+           << "/" << idx; // vertex index in obj file format starting with 1
+      }
+      fs << '\n';
+    }
+    fs << "# "<< tex_mesh.tex_polygons[m].size() << " faces in mesh " << m << '\n';
+  }
+  fs << "# End of File" << std::flush;
+
+  // Close obj file
+  fs.close ();
+
+  /* Write material definition for OBJ file*/
+  // Open file
+
+  std::ofstream m_fs;
+  m_fs.precision (precision);
+  m_fs.open (mtl_file_name.c_str ());
+
+  // default
+  m_fs << "#" << '\n';
+  m_fs << "# Wavefront material file" << '\n';
+  m_fs << "#" << '\n';
+  for(unsigned m = 0; m < nr_meshes; ++m)
+  {
+    m_fs << "newmtl " << tex_mesh.tex_materials[m].tex_name << '\n';
+    m_fs << "Ka "<< tex_mesh.tex_materials[m].tex_Ka.r << " " << tex_mesh.tex_materials[m].tex_Ka.g << " " << tex_mesh.tex_materials[m].tex_Ka.b << '\n'; // defines the ambient color of the material to be (r,g,b).
+    m_fs << "Kd "<< tex_mesh.tex_materials[m].tex_Kd.r << " " << tex_mesh.tex_materials[m].tex_Kd.g << " " << tex_mesh.tex_materials[m].tex_Kd.b << '\n'; // defines the diffuse color of the material to be (r,g,b).
+    m_fs << "Ks "<< tex_mesh.tex_materials[m].tex_Ks.r << " " << tex_mesh.tex_materials[m].tex_Ks.g << " " << tex_mesh.tex_materials[m].tex_Ks.b << '\n'; // defines the specular color of the material to be (r,g,b). This color shows up in highlights.
+    m_fs << "d " << tex_mesh.tex_materials[m].tex_d << '\n'; // defines the transparency of the material to be alpha.
+    m_fs << "Ns "<< tex_mesh.tex_materials[m].tex_Ns  << '\n'; // defines the shininess of the material to be s.
+    m_fs << "illum "<< tex_mesh.tex_materials[m].tex_illum << '\n'; // denotes the illumination model used by the material.
+                                            // illum = 1 indicates a flat material with no specular highlights, so the value of Ks is not used.
+                                            // illum = 2 denotes the presence of specular highlights, and so a specification for Ks is required.
+    m_fs << "map_Kd " << tex_mesh.tex_materials[m].tex_file << '\n';
+    m_fs << "###" << '\n';
+  }
+  m_fs.close ();
+  return (0);
+}
+
+/**
+ * This is a modified copy of https://github.com/PointCloudLibrary/pcl/blob/pcl-1.12.1/io/src/obj_io.cpp
+ * with added color on each vertex if provided
+ */
+int saveOBJFile(
+	const std::string &file_name,
+	const pcl::PolygonMesh &mesh,
+	unsigned precision)
+{
+  if (mesh.cloud.data.empty ())
+  {
+    UERROR ("Input point cloud has no data!\n");
+    return (-1);
+  }
+  // Open file
+  std::ofstream fs;
+  fs.precision (precision);
+  fs.open (file_name.c_str ());
+
+  /* Write 3D information */
+  // number of points
+  int nr_points  = mesh.cloud.width * mesh.cloud.height;
+  // point size
+  unsigned point_size = static_cast<unsigned> (mesh.cloud.data.size () / nr_points);
+  // number of faces for header
+  unsigned nr_faces = static_cast<unsigned> (mesh.polygons.size ());
+  // Do we have vertices normals?
+  int normal_index = getFieldIndex (mesh.cloud, "normal_x");
+
+  // Write the header information
+  fs << "####" << '\n';
+  fs << "# OBJ dataFile simple version. File name: " << file_name << '\n';
+  fs << "# Vertices: " << nr_points << '\n';
+  if (normal_index != -1)
+    fs << "# Vertices normals : " << nr_points << '\n';
+  fs << "# Faces: " <<nr_faces << '\n';
+  fs << "####" << '\n';
+
+  // Write vertex coordinates
+  fs << "# List of Vertices, with (x,y,z) coordinates, w is optional." << '\n';
+  for (int i = 0; i < nr_points; ++i)
+  {
+    int xyz = 0;
+    for (std::size_t d = 0; d < mesh.cloud.fields.size (); ++d)
+    {
+      // adding vertex
+      if ((mesh.cloud.fields[d].datatype == pcl::PCLPointField::FLOAT32) && (
+          mesh.cloud.fields[d].name == "x" ||
+          mesh.cloud.fields[d].name == "y" ||
+          mesh.cloud.fields[d].name == "z"))
+      {
+        if (mesh.cloud.fields[d].name == "x")
+           // write vertices beginning with v
+          fs << "v ";
+
+        float value;
+        memcpy (&value, &mesh.cloud.data[i * point_size + mesh.cloud.fields[d].offset], sizeof (float));
+        fs << value;
+        if (++xyz == 3)
+          continue;
+        fs << " ";
+      }
+	  else if(mesh.cloud.fields[d].datatype == pcl::PCLPointField::FLOAT32 &&
+          	  mesh.cloud.fields[d].name == "rgb")
+	  {
+		std::uint32_t rgb = *reinterpret_cast<const int*>(&mesh.cloud.data[i * point_size + mesh.cloud.fields[d].offset]);
+		std::uint8_t r = (rgb >> 16) & 0x0000ff;
+		std::uint8_t g = (rgb >> 8)  & 0x0000ff;
+		std::uint8_t b = (rgb)       & 0x0000ff;
+		fs << " " << float(r)/255.0f << " " << float(g)/255.0f << " " << float(b)/255.0f;
+		break;
+	  }
+    }
+    if (xyz != 3)
+    {
+      UERROR ("Input point cloud has no XYZ data!\n");
+      return (-2);
+    }
+    fs << '\n';
+  }
+
+  fs << "# "<< nr_points <<" vertices" << '\n';
+
+  if(normal_index != -1)
+  {
+    fs << "# Normals in (x,y,z) form; normals might not be unit." <<  '\n';
+    // Write vertex normals
+    for (int i = 0; i < nr_points; ++i)
+    {
+      int nxyz = 0;
+      for (std::size_t d = 0; d < mesh.cloud.fields.size (); ++d)
+      {
+        // adding vertex
+        if ((mesh.cloud.fields[d].datatype == pcl::PCLPointField::FLOAT32) && (
+              mesh.cloud.fields[d].name == "normal_x" ||
+              mesh.cloud.fields[d].name == "normal_y" ||
+              mesh.cloud.fields[d].name == "normal_z"))
+        {
+          if (mesh.cloud.fields[d].name == "normal_x")
+            // write vertices beginning with vn
+            fs << "vn ";
+
+          float value;
+          memcpy (&value, &mesh.cloud.data[i * point_size + mesh.cloud.fields[d].offset], sizeof (float));
+          fs << value;
+          if (++nxyz == 3)
+            break;
+          fs << " ";
+        }
+      }
+      if (nxyz != 3)
+      {
+        UERROR ("Input point cloud has no normals!\n");
+        return (-2);
+      }
+      fs << '\n';
+    }
+
+    fs << "# "<< nr_points <<" vertices normals" << '\n';
+  }
+
+  fs << "# Face Definitions" << '\n';
+  // Write down faces
+  if(normal_index == -1)
+  {
+    for(unsigned i = 0; i < nr_faces; i++)
+    {
+      fs << "f ";      
+      for (std::size_t j = 0; j < mesh.polygons[i].vertices.size () - 1; ++j)
+        fs << mesh.polygons[i].vertices[j] + 1 << " ";
+      fs << mesh.polygons[i].vertices.back() + 1 << '\n';
+    }
+  }
+  else
+  {
+    for(unsigned i = 0; i < nr_faces; i++)
+    {
+      fs << "f ";
+      for (std::size_t j = 0; j < mesh.polygons[i].vertices.size () - 1; ++j)
+        fs << mesh.polygons[i].vertices[j] + 1 << "//" << mesh.polygons[i].vertices[j] + 1 << " ";
+      fs << mesh.polygons[i].vertices.back() + 1 << "//" << mesh.polygons[i].vertices.back() + 1 << '\n';
+    }
+  }
+  fs << "# End of File" << std::endl;
+
+  // Close obj file
+  fs.close ();
+  return 0;
+}
+
 }
 
 }

docker/jammy/Dockerfile

@@ -2,23 +2,6 @@
 
 FROM introlab3it/rtabmap:jammy-deps
 
-# June 19 2023: moved opengv here so that jammy-deps can be built on my computer. Not sure why but on my machine opengv arm64 fails, but not on CI.
-#commit Aug 6 2020
-RUN apt-get update && apt install -y wget && \
-    apt-get clean && rm -rf /var/lib/apt/lists/
-RUN git clone https://github.com/laurentkneip/opengv.git && \
-    cd opengv && \
-    git checkout 91f4b19c73450833a40e463ad3648aae80b3a7f3 && \
-    wget https://gist.githubusercontent.com/matlabbe/a412cf7c4627253874f81a00745a7fbb/raw/accc3acf465d1ffd0304a46b17741f62d4d354ef/opengv_disable_march_native.patch && \
-    git apply opengv_disable_march_native.patch && \
-    mkdir build && \
-    cd build && \
-    cmake -DCMAKE_BUILD_TYPE=Release .. && \
-    make -j$(nproc) && \
-    make install && \
-    cd && \
-    rm -r opengv
-
 # Will be used to read/store databases on host
 RUN mkdir -p /root/Documents/RTAB-Map && chmod 777 /root/Documents/RTAB-Map
 

docker/jammy/deps/Dockerfile

@@ -24,18 +24,11 @@ RUN apt-get update && \
     apt-get install -y git libtbb-dev ros-humble-ros-base ros-dev-tools && \
     apt-get clean && rm -rf /var/lib/apt/lists/
 
-RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then apt-get update && \
+RUN apt-get update && \
     apt upgrade -y && \
     apt-get install -y libfreenect-dev ros-humble-rtabmap-ros && \
-    apt-get remove -y ros-humble-rtabmap* && \
+    apt-get remove -y ros-humble-rtabmap* ros-humble-grid-map-core && \
-    apt-get clean && rm -rf /var/lib/apt/lists/; fi
+    apt-get clean && rm -rf /var/lib/apt/lists/
-
-# current issue that ros-humble-rtabmap-ros is not available on arm64
-RUN if [ "$TARGETPLATFORM" = "linux/arm64" ]; then apt-get update && \
-    apt upgrade -y && \
-    apt-get install -y libfreenect-dev ros-humble-rtabmap-launch && \
-    apt-get remove -y ros-humble-rtabmap* && \
-    apt-get clean && rm -rf /var/lib/apt/lists/; fi
 
 WORKDIR /root/
 
@@ -86,6 +79,22 @@ RUN git clone --branch 4.5.4 https://github.com/opencv/opencv.git && \
     cd ../.. && \
     rm -rf opencv opencv_contrib
 
+#commit Aug 6 2020
+RUN apt-get update && apt install -y wget && \
+    apt-get clean && rm -rf /var/lib/apt/lists/
+RUN git clone https://github.com/laurentkneip/opengv.git && \
+    cd opengv && \
+    git checkout 91f4b19c73450833a40e463ad3648aae80b3a7f3 && \
+    wget https://gist.githubusercontent.com/matlabbe/a412cf7c4627253874f81a00745a7fbb/raw/accc3acf465d1ffd0304a46b17741f62d4d354ef/opengv_disable_march_native.patch && \
+    git apply opengv_disable_march_native.patch && \
+    mkdir build && \
+    cd build && \
+    cmake -DCMAKE_BUILD_TYPE=Release .. && \
+    make -j$(nproc) && \
+    make install && \
+    cd && \
+    rm -r opengv
+
 RUN rm /bin/sh && ln -s /bin/bash /bin/sh
 
 COPY ./docker/jammy/deps/ros_entrypoint.sh /ros_entrypoint.sh

examples/BOWMapping/CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.14)
 
 if(POLICY CMP0020)
     cmake_policy(SET CMP0020 NEW)

examples/LidarMapping/CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8)
+cmake_minimum_required(VERSION 3.14)
 
 IF(DEFINED PROJECT_NAME)
    set(internal TRUE)

examples/LidarMapping/main.cpp

@@ -86,7 +86,11 @@ int main(int argc, char * argv[])
 	if(!ip.empty())
 	{
 		printf("Using ip=%s port=%d\n", ip.c_str(), port);
+#if BOOST_VERSION >= 108700  // Version 1.87.0
+		lidar = new LidarVLP16(boost::asio::ip::make_address(ip), port);
+#else
 		lidar = new LidarVLP16(boost::asio::ip::address_v4::from_string(ip), port);
+#endif
 	}
 	else
 	{

examples/NoEventsExample/CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.14)
 
 if(POLICY CMP0020)
     cmake_policy(SET CMP0020 NEW)

examples/RGBDMapping/CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.14)
 
 if(POLICY CMP0020)
     cmake_policy(SET CMP0020 NEW)

examples/WifiMapping/CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.14)
 
 if(POLICY CMP0020)
     cmake_policy(SET CMP0020 NEW)

guilib/include/rtabmap/gui/CloudViewer.h

@@ -350,6 +350,7 @@ public:
 	void setLighting(bool on);
 	void setShading(bool on);
 	void setEdgeVisibility(bool visible);
+	void setScalarVisibility(bool visible);
 	void setInteractorLayer(int layer);
 
 	bool isBackfaceCulling() const;
@@ -459,6 +460,7 @@ private:
     QAction * _aSetLighting;
     QAction * _aSetFlatShading;
     QAction * _aSetEdgeVisibility;
+	QAction * _aSetScalarVisibility;
     QAction * _aBackfaceCulling;
     QAction * _aPolygonPicking;
     QMenu * _menu;

guilib/include/rtabmap/gui/CloudViewerInteractorStyle.h

@@ -32,6 +32,7 @@ public:
 protected:
 	virtual void OnMouseMove();
 	virtual void OnLeftButtonDown();
+	virtual void OnRightButtonDown();
 
 protected:
 	friend class CloudViewer;

guilib/include/rtabmap/gui/ExportBundlerDialog.h

@@ -54,7 +54,7 @@ public:
 	void setWorkingDirectory(const QString & path);
 
 	void exportBundler(
-			const std::map<int, Transform> & poses,
+			std::map<int, Transform> & poses,
 			const std::multimap<int, Link> & links,
 			const QMap<int, Signature> & signatures,
 			const ParametersMap & parameters);