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: 
-      - master
+      - noetic-devel
   pull_request:
     branches: 
       - '**'
@@ -23,12 +23,10 @@ jobs:
     strategy:
       fail-fast: false
       matrix:
-        ros_distribution: [ noetic, humble ]
+        ros_distribution: [ noetic ]
         include:
         - ros_distribution: 'noetic'
           os: ubuntu-20.04
-        - ros_distribution: 'humble'
-          os: ubuntu-22.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

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 11)
+SET(RTABMAP_PATCH_VERSION 13)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   

README.md

@@ -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

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

app/android/jni/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())
 				{
 					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,39 +1449,188 @@ 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,
-						optMesh_->tex_polygons.size()!=1?0:(int)optMesh_->tex_polygons[0].size(),
-						optMesh_->tex_coordinates.size()!=1?0:(int)optMesh_->tex_coordinates[0].size(),
-						(int)optMesh_->tex_materials.size(),
+						optTextureMesh_->cloud.point_step==0?0:(int)optTextureMesh_->cloud.data.size()/optTextureMesh_->cloud.point_step,
+						optTextureMesh_->tex_polygons.size()!=1?0:(int)optTextureMesh_->tex_polygons[0].size(),
+						optTextureMesh_->tex_coordinates.size()!=1?0:(int)optTextureMesh_->tex_coordinates[0].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;
-					mesh.gains[0] = mesh.gains[1] = mesh.gains[2] = 1.0;
-					mesh.cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
-					mesh.normals.reset(new pcl::PointCloud<pcl::Normal>);
-					pcl::fromPCLPointCloud2(optMesh_->cloud, *mesh.cloud);
-					pcl::fromPCLPointCloud2(optMesh_->cloud, *mesh.normals);
-					mesh.polygons = optMesh_->tex_polygons[0];
-					mesh.pose.setIdentity();
-					if(optMesh_->tex_coordinates.size())
+                    optMesh_ = rtabmap::Mesh();
+                    optMesh_.gains[0] = optMesh_.gains[1] = optMesh_.gains[2] = 1.0;
+                    optMesh_.cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
+                    optMesh_.normals.reset(new pcl::PointCloud<pcl::Normal>);
+					pcl::fromPCLPointCloud2(optTextureMesh_->cloud, *optMesh_.cloud);
+					pcl::fromPCLPointCloud2(optTextureMesh_->cloud, *optMesh_.normals);
+                    bool hasColors = false;
+                    for(unsigned int i=0; i<optTextureMesh_->cloud.fields.size(); ++i)
                     {
-						mesh.texCoords = optMesh_->tex_coordinates[0];
-						mesh.texture = optTexture_;
+                        if(optTextureMesh_->cloud.fields[i].name.compare("rgb") == 0)
+                        {
+                            hasColors = true;
+                            break;
                         }
-					main_scene_.addMesh(g_optMeshId, mesh, rtabmap::opengl_world_T_rtabmap_world, true);
+                    }
+                    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())
+					{
+                        optMesh_.texCoords = optTextureMesh_->tex_coordinates[0];
+                        optMesh_.texture = optTexture_;
+					}
+					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())
 				{
+                    bool hasColors = false;
+                    for(unsigned int i=0; i<textureMesh->cloud.fields.size(); ++i)
+                    {
+                        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);
-					cv::Mat cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*cloud, rtabmap::Transform(), false).data()); // for database
+                        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,7 +4115,8 @@ 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);
+                optMesh_ = rtabmap::Mesh();
                 optTexture_ = textures;
 
 				boost::mutex::scoped_lock  lock(renderingMutex_);
@@ -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 {
@@ -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,6 +260,7 @@ 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"
 	""
@@ -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,13 +1146,17 @@ void PointCloudDrawable::Render(
 
 				attribute_texture = glGetAttribLocation(program, "aTexCoord");
 				glEnableVertexAttribArray(attribute_texture);
-			}
-			else
+                
+                if(depthTexture == 0)
                 {
+                    GLuint hideSeams_handle = glGetUniformLocation(program, "uHideSeams");
+                    glUniform1i(hideSeams_handle, hideSeams?1:0);
+                }
+			}
+
             attribute_color = glGetAttribLocation(program, "aColor");
             glEnableVertexAttribArray(attribute_color);
 		}
-		}
 		tango_gl::util::CheckGlError("Pointcloud::Render() common");
 
 		glBindBuffer(GL_ARRAY_BUFFER, vertex_buffer_);
@@ -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,7 +675,16 @@ 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,17 +906,22 @@ 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);
+    removeCloudOrMesh(id);
+
+	//create
+	PointCloudDrawable * drawable = new PointCloudDrawable(cloud, indices);
+	drawable->setPose(pose);
+	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);
     }
-
-	//create
-	PointCloudDrawable * drawable = new PointCloudDrawable(cloud, indices);
-	drawable->setPose(pose);
-	pointClouds_.insert(std::make_pair(id, drawable));
 }
 
 void Scene::addMesh(
@@ -820,12 +931,7 @@ void Scene::addMesh(
 		bool createWireframe)
 {
 	LOGI("add mesh %d", id);
-	std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
-	if(iter != pointClouds_.end())
-	{
-		delete iter->second;
-		pointClouds_.erase(iter);
-	}
+    removeCloudOrMesh(id);
 
 	//create
 	PointCloudDrawable * drawable = new PointCloudDrawable(mesh, createWireframe);
@@ -885,6 +991,160 @@ void Scene::addMesh(
 	}
 }
 
+void Scene::addLine(
+        int id,
+        const cv::Point3f & pt1,
+        const cv::Point3f & pt2,
+        const tango_gl::Color & color)
+{
+    LOGI("add line %d", id);
+    removeLine(id);
+
+    //create
+    tango_gl::Line * line = new tango_gl::Line(2.0f, GL_LINES);
+    line->SetShader();
+    std::vector<glm::vec3> vertices(2);
+    vertices[0].x = pt1.x;
+    vertices[0].y = pt1.y;
+    vertices[0].z = pt1.z;
+    vertices[1].x = pt2.x;
+    vertices[1].y = pt2.y;
+    vertices[1].z = pt2.z;
+    line->UpdateLineVertices(vertices);
+    line->SetColor(color);
+    lines_.insert(std::make_pair(id, line));
+}
+
+void Scene::removeLine(int id)
+{
+    std::map<int, tango_gl::Line*>::iterator iter=lines_.find(id);
+    if(iter != lines_.end())
+    {
+        delete iter->second;
+        lines_.erase(iter);
+    }
+}
+
+void Scene::addText(
+        int id,
+        const std::string & text,
+        const rtabmap::Transform & pose,
+        float size,
+        const tango_gl::Color & color)
+{
+    LOGI("add text %d", id);
+    removeText(id);
+
+    //create
+    TextDrawable * textD = new TextDrawable(text, pose, size, color);
+    texts_.insert(std::make_pair(id, textD));
+}
+void Scene::removeText(int id)
+{
+    std::map<int, TextDrawable*>::iterator iter=texts_.find(id);
+    if(iter != texts_.end())
+    {
+        delete iter->second;
+        texts_.erase(iter);
+    }
+}
+
+void Scene::addQuad(
+        int id,
+        float size,
+        const rtabmap::Transform & pose,
+        const tango_gl::Color & color,
+        float alpha)
+{
+    //LOGI("add quad %d", id);
+    std::map<int, QuadColor*>::iterator iter=quads_.find(id);
+    if(iter != quads_.end())
+    {
+        delete iter->second;
+        quads_.erase(iter);
+    }
+
+    //create
+    QuadColor * quad = new QuadColor(size);
+    quad->SetTransformationMatrix(glmFromTransform(pose));
+    quad->SetColor(color);
+    quad->SetAlpha(alpha);
+    quads_.insert(std::make_pair(id, quad));
+}
+
+void Scene::addQuad(
+        int id,
+        float widthLeft,
+        float widthRight,
+        float heightBottom,
+        float heightTop,
+        const rtabmap::Transform & pose,
+        const tango_gl::Color & color,
+        float alpha)
+{
+    //LOGI("add quad %d", id);
+    removeQuad(id);
+
+    //create
+    QuadColor * quad = new QuadColor(widthLeft, widthRight, heightBottom, heightTop);
+    quad->SetTransformationMatrix(glmFromTransform(pose));
+    quad->SetColor(color);
+    quad->SetAlpha(alpha);
+    quads_.insert(std::make_pair(id, quad));
+}
+
+void Scene::removeQuad(int id)
+{
+    std::map<int, QuadColor*>::iterator iter=quads_.find(id);
+    if(iter != quads_.end())
+    {
+        delete iter->second;
+        quads_.erase(iter);
+    }
+}
+
+bool Scene::hasQuad(int id) const
+{
+    return quads_.find(id) != quads_.end();
+}
+
+void Scene::addCircle(
+        int id,
+        float radius,
+        const rtabmap::Transform & pose,
+        const tango_gl::Color & color,
+        float alpha)
+{
+    //LOGI("add quad %d", id);
+    std::map<int, tango_gl::Circle*>::iterator iter=circles_.find(id);
+    if(iter != circles_.end())
+    {
+        delete iter->second;
+        circles_.erase(iter);
+    }
+
+    //create
+    tango_gl::Circle * circle = new tango_gl::Circle(radius, 12);
+    circle->SetTransformationMatrix(glmFromTransform(pose));
+    circle->SetColor(color);
+    circle->SetAlpha(alpha);
+    circles_.insert(std::make_pair(id, circle));
+}
+
+void Scene::removeCircle(int id)
+{
+    std::map<int, tango_gl::Circle*>::iterator iter=circles_.find(id);
+    if(iter != circles_.end())
+    {
+        delete iter->second;
+        circles_.erase(iter);
+    }
+}
+
+bool Scene::hasCircle(int id) const
+{
+    return circles_.find(id) != circles_.end();
+}
 
 void Scene::setCloudPose(int id, const rtabmap::Transform & pose)
 {

app/android/jni/scene.h

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

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

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

app/android/jni/text_drawable.cpp

@@ -0,0 +1,366 @@
+/*
+Copyright (c) 2010-2025, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifdef __ANDROID__
+#include <jni.h>
+#endif
+
+#include <tango-gl/util.h>
+#include <vector>
+#include <rtabmap/core/Transform.h>
+#include "util.h"
+#include "text_drawable.h"
+#include "text_atlas_png.h"
+#include <rtabmap/utilite/UConversion.h>
+
+const std::string kTextVertexShader =
+	"uniform mat4 uMVPMatrix;"
+	"attribute vec4 vPosition;"
+	"attribute vec2 a_texCoord;"
+	"varying vec2 v_texCoord;"
+	"void main() {"
+	"  gl_Position = uMVPMatrix * vPosition;"
+	"  v_texCoord = a_texCoord;"
+	"}";
+const std::string kTextFragmentShader =
+	"precision mediump float;"
+	"uniform vec3 uColor;"
+	"varying vec2 v_texCoord;"
+	"uniform sampler2D s_texture;"
+	"void main() {"
+	"  gl_FragColor = texture2D( s_texture, v_texCoord );"
+	"  gl_FragColor.rgb = uColor;"
+	"}";
+
+const int RI_TEXT_TEXTURE_SIZE = 512; // 512
+const float RI_TEXT_HEIGHT_BASE = 32.0f;
+const char RI_TEXT_START = ' ';
+const char RI_TEXT_STOP = '~'+1;
+
+GLuint TextDrawable::textProgram_ = 0;
+GLuint TextDrawable::textTextureId_ = 0;
+float TextDrawable::textUVWidth_ = 0;
+float TextDrawable::textUVHeight_ = 0;
+float TextDrawable::textHeight_ = 0;
+std::vector<float> TextDrawable::textCharacterWidths_;
+
+void TextDrawable::createShaderProgram()
+{
+	releaseShaderProgram();
+
+	// hard-coded with text_atlas.png resource
+	textCharacterWidths_.resize(95);
+	textCharacterWidths_[0]=8;
+	textCharacterWidths_[1]=9.000000;
+	textCharacterWidths_[2]=10.000000;
+	textCharacterWidths_[3]=19.000000;
+	textCharacterWidths_[4]=18.000000;
+	textCharacterWidths_[5]=24.000000;
+	textCharacterWidths_[6]=21.000000;
+	textCharacterWidths_[7]=5.000000;
+	textCharacterWidths_[8]=11.000000;
+	textCharacterWidths_[9]=11.000000;
+	textCharacterWidths_[10]=15.000000;
+	textCharacterWidths_[11]=17.000000;
+	textCharacterWidths_[12]=8.000000;
+	textCharacterWidths_[13]=12.000000;
+	textCharacterWidths_[14]=9.000000;
+	textCharacterWidths_[15]=12.000000;
+	textCharacterWidths_[16]=18.000000;
+	textCharacterWidths_[17]=18.000000;
+	textCharacterWidths_[18]=18.000000;
+	textCharacterWidths_[19]=18.000000;
+	textCharacterWidths_[20]=18.000000;
+	textCharacterWidths_[21]=18.000000;
+	textCharacterWidths_[22]=18.000000;
+	textCharacterWidths_[23]=18.000000;
+	textCharacterWidths_[24]=18.000000;
+	textCharacterWidths_[25]=18.000000;
+	textCharacterWidths_[26]=9.000000;
+	textCharacterWidths_[27]=8.000000;
+	textCharacterWidths_[28]=16.000000;
+	textCharacterWidths_[29]=18.000000;
+	textCharacterWidths_[30]=17.000000;
+	textCharacterWidths_[31]=16.000000;
+	textCharacterWidths_[32]=29.000000;
+	textCharacterWidths_[33]=22.000000;
+	textCharacterWidths_[34]=20.000000;
+	textCharacterWidths_[35]=21.000000;
+	textCharacterWidths_[36]=21.000000;
+	textCharacterWidths_[37]=18.000000;
+	textCharacterWidths_[38]=18.000000;
+	textCharacterWidths_[39]=22.000000;
+	textCharacterWidths_[40]=23.000000;
+	textCharacterWidths_[41]=9.000000;
+	textCharacterWidths_[42]=18.000000;
+	textCharacterWidths_[43]=20.000000;
+	textCharacterWidths_[44]=17.000000;
+	textCharacterWidths_[45]=28.000000;
+	textCharacterWidths_[46]=23.000000;
+	textCharacterWidths_[47]=22.000000;
+	textCharacterWidths_[48]=21.000000;
+	textCharacterWidths_[49]=22.000000;
+	textCharacterWidths_[50]=20.000000;
+	textCharacterWidths_[51]=20.000000;
+	textCharacterWidths_[52]=20.000000;
+	textCharacterWidths_[53]=21.000000;
+	textCharacterWidths_[54]=21.000000;
+	textCharacterWidths_[55]=28.000000;
+	textCharacterWidths_[56]=20.000000;
+	textCharacterWidths_[57]=20.000000;
+	textCharacterWidths_[58]=19.000000;
+	textCharacterWidths_[59]=9.000000;
+	textCharacterWidths_[60]=14.000000;
+	textCharacterWidths_[61]=9.000000;
+	textCharacterWidths_[62]=14.000000;
+	textCharacterWidths_[63]=14.000000;
+	textCharacterWidths_[64]=11.000000;
+	textCharacterWidths_[65]=17.000000;
+	textCharacterWidths_[66]=18.000000;
+	textCharacterWidths_[67]=17.000000;
+	textCharacterWidths_[68]=18.000000;
+	textCharacterWidths_[69]=17.000000;
+	textCharacterWidths_[70]=11.000000;
+	textCharacterWidths_[71]=18.000000;
+	textCharacterWidths_[72]=18.000000;
+	textCharacterWidths_[73]=8.000000;
+	textCharacterWidths_[74]=8.000000;
+	textCharacterWidths_[75]=17.000000;
+	textCharacterWidths_[76]=8.000000;
+	textCharacterWidths_[77]=28.000000;
+	textCharacterWidths_[78]=18.000000;
+	textCharacterWidths_[79]=18.000000;
+	textCharacterWidths_[80]=18.000000;
+	textCharacterWidths_[81]=18.000000;
+	textCharacterWidths_[82]=12.000000;
+	textCharacterWidths_[83]=16.000000;
+	textCharacterWidths_[84]=11.000000;
+	textCharacterWidths_[85]=18.000000;
+	textCharacterWidths_[86]=16.000000;
+	textCharacterWidths_[87]=24.000000;
+	textCharacterWidths_[88]=16.000000;
+	textCharacterWidths_[89]=16.000000;
+	textCharacterWidths_[90]=16.000000;
+	textCharacterWidths_[91]=11.000000;
+	textCharacterWidths_[92]=8.000000;
+	textCharacterWidths_[93]=11.000000;
+	textCharacterWidths_[94]=21.000000;
+	textUVWidth_=0.062500;
+	textUVHeight_=0.082031;
+	textHeight_=42.000000;
+
+	textProgram_ = tango_gl::util::CreateProgram(kTextVertexShader.c_str(), kTextFragmentShader.c_str());
+	UASSERT(textProgram_ != 0);
+
+	glGenTextures(1, &textTextureId_);
+	UASSERT(textTextureId_);
+
+	// gen texture from image
+	glBindTexture(GL_TEXTURE_2D, textTextureId_);
+	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
+	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
+	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+
+	std::vector<char> data = uHex2Bytes(rtabmap::TEXT_ATLAS_PNG);
+
+	cv::Mat rgbImage = cv::imdecode(data, cv::IMREAD_UNCHANGED);
+
+	glPixelStorei(GL_UNPACK_ALIGNMENT, 4);
+	glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, rgbImage.cols, rgbImage.rows, 0, GL_RGBA, GL_UNSIGNED_BYTE, rgbImage.data);
+
+	GLint error = glGetError();
+	UASSERT(error == GL_NO_ERROR);
+}
+
+void TextDrawable::releaseShaderProgram()
+{
+	if(textProgram_)
+	{
+		glDeleteShader(textProgram_);
+		textProgram_ = 0;
+	}
+}
+
+TextDrawable::TextDrawable(
+		const std::string & text,
+		const rtabmap::Transform & pose,
+		float textSize, // meters
+		const tango_gl::Color & color) :
+		poseGl_(glmFromTransform(pose)),
+		color_(color)
+{
+	if(textProgram_ > 0)
+	{
+		vertexBuffer_ = std::vector<float>(text.size() * 12);
+		textureBuffer_ = std::vector<float>(text.size() * 8);
+		drawListBuffer_ = std::vector<unsigned short>(text.size() * 6);
+
+		int index_vecs = 0;
+		int index_indices = 0;
+		int index_uvs = 0;
+		float x=0.0f;
+		float y=0.0f;
+		float uniformscale = textSize/textHeight_;
+		for(unsigned int i=0; i<text.size(); ++i)
+		{
+			// get ascii value
+			char c = text[i];
+			int c_val = (int)c;
+
+			int indx = c_val-RI_TEXT_START;
+
+			if(indx<0 || indx>=textCharacterWidths_.size()) {
+				// unknown character, we will add a space for it to be save.
+				indx = 0;
+			}
+
+			int colCount = RI_TEXT_TEXTURE_SIZE/(int)RI_TEXT_HEIGHT_BASE;
+
+			// Calculate the uv parts
+			int row = indx / colCount;
+			int col = indx % colCount;
+
+			float v = row * textUVHeight_;
+			float v2 = v + textUVHeight_;
+			float u = col * textUVWidth_;
+			float u2 = u + textCharacterWidths_[indx]/(float)RI_TEXT_TEXTURE_SIZE;
+
+			// Creating the triangle information
+			std::vector<float> vec(12);
+			std::vector<float> uv(8);
+
+			vec[0] = x;
+			vec[1] = y + (textHeight_ * uniformscale);
+			vec[2] = 0;
+			vec[3] = x;
+			vec[4] = y;
+			vec[5] = 0;
+			vec[6] = x + (textCharacterWidths_[indx] * uniformscale);
+			vec[7] = y;
+			vec[8] = 0;
+			vec[9] = x + (textCharacterWidths_[indx] * uniformscale);
+			vec[10] = y + (textHeight_ * uniformscale);
+			vec[11] = 0;
+
+			// 0.001f = texture bleeding hack/fix
+			uv[0] = u+0.001f;
+			uv[1] = v+0.001f;
+			uv[2] = u+0.001f;
+			uv[3] = v2-0.001f;
+			uv[4] = u2-0.001f;
+			uv[5] = v2-0.001f;
+			uv[6] = u2-0.001f;
+			uv[7] = v+0.001f;
+
+			unsigned short inds[6] = {0, 1, 2, 0, 2, 3};
+
+			// We need a base value because the object has indices related to
+			// that object and not to this collection so basicly we need to
+			// translate the indices to align with the vertexlocation in ou
+			// vecs array of vectors.
+			short base = (short) (index_vecs / 3);
+
+			// We should add the vec, translating the indices to our saved vector
+			for(int i=0;i<vec.size();i++)
+			{
+				vertexBuffer_[index_vecs] = vec[i];
+				index_vecs++;
+			}
+
+			// We should add the uvs
+			for(int i=0;i<uv.size();i++)
+			{
+				textureBuffer_[index_uvs] = uv[i];
+				index_uvs++;
+			}
+
+			// We handle the indices
+			for(int j=0;j<6;j++)
+			{
+				drawListBuffer_[index_indices] = (unsigned short) (base + inds[j]);
+				index_indices++;
+			}
+
+			// Calculate the new position
+			x += (textCharacterWidths_[indx]  * uniformscale);
+		}
+	}
+}
+
+void TextDrawable::Render(
+		const glm::mat4 & projectionMatrix,
+		const glm::mat4 & viewMatrix,
+		const glm::mat4 & viewMatrixRotInv) const
+{
+	glUseProgram(textProgram_);
+
+	// get handle to vertex shader's vPosition member
+	int mPositionHandle = glGetAttribLocation(textProgram_, "vPosition");
+
+	// Enable a handle to the triangle vertices
+	glEnableVertexAttribArray(mPositionHandle);
+
+	// Prepare the background coordinate data
+	glVertexAttribPointer(mPositionHandle, 3, GL_FLOAT, false, 0, &vertexBuffer_[0]);
+
+	int mTexCoordLoc = glGetAttribLocation(textProgram_, "a_texCoord" );
+
+	// Prepare the texturecoordinates
+	glVertexAttribPointer ( mTexCoordLoc, 2, GL_FLOAT, false, 0, &textureBuffer_[0]);
+
+	glEnableVertexAttribArray ( mPositionHandle );
+	glEnableVertexAttribArray ( mTexCoordLoc );
+
+	// get handle to shape's transformation matrix
+	int mtrxhandle = glGetUniformLocation(textProgram_, "uMVPMatrix");
+
+	// Apply the projection and view transformation
+	//glUniformMatrix4fv(mtrxhandle, 1, false, m, 0);
+	glm::mat4 mvp_mat = projectionMatrix * viewMatrix * poseGl_ * viewMatrixRotInv;//glm::toMat4(gesture_camera_->GetParent()->GetRotation());
+	glUniformMatrix4fv(mtrxhandle, 1, GL_FALSE, glm::value_ptr(mvp_mat));
+
+	// get handle to color value
+	int colorhandle = glGetUniformLocation(textProgram_, "uColor");
+	glUniform3f(colorhandle, color_.r, color_.g, color_.b);
+
+	int mSamplerLoc = glGetUniformLocation (textProgram_, "s_texture" );
+
+	// Texture activate unit 0
+	glActiveTexture(GL_TEXTURE0);
+	// Bind the texture to this unit.
+	glBindTexture(GL_TEXTURE_2D, textTextureId_);
+	// Set the sampler texture unit to our selected id
+	glUniform1i ( mSamplerLoc, 0);
+
+	// Draw the triangle
+	glDrawElements(GL_TRIANGLES, drawListBuffer_.size(), GL_UNSIGNED_SHORT, &drawListBuffer_[0]);
+
+	// Disable vertex array
+	glDisableVertexAttribArray(mPositionHandle);
+	glDisableVertexAttribArray(mTexCoordLoc);
+}

app/android/jni/text_drawable.h

@@ -0,0 +1,71 @@
+/*
+Copyright (c) 2010-2025, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef TANGO_TEXT_DRAWABLE_H_
+#define TANGO_TEXT_DRAWABLE_H_
+
+#include <vector>
+#include <rtabmap/core/Transform.h>
+#include <tango-gl/color.h>
+
+// PointCloudDrawable is responsible for the point cloud rendering.
+class TextDrawable {
+private:
+	static GLuint textProgram_;
+	static GLuint textTextureId_;
+	static float textUVWidth_;
+	static float textUVHeight_;
+	static float textHeight_;
+	static std::vector<float> textCharacterWidths_;
+
+public:
+	static void createShaderProgram();
+	static void releaseShaderProgram();
+
+public:
+	TextDrawable(
+			const std::string & text,
+			const rtabmap::Transform & pose,
+			float textSize = 0.1f, // meters
+			const tango_gl::Color & color = tango_gl::Color(1,0,0));
+
+  virtual ~TextDrawable() {}
+
+  void Render(
+		  const glm::mat4 & projectionMatrix,
+		  const glm::mat4 & viewMatrix,
+		  const glm::mat4 & viewMatrixRotInv) const;
+
+ private:
+	std::vector<float> vertexBuffer_;
+	std::vector<float> textureBuffer_;
+	std::vector<unsigned short> drawListBuffer_;
+	glm::mat4 poseGl_;
+	tango_gl::Color color_;
+};
+
+#endif  // TANGO_POINT_CLOUD_POINT_CLOUD_DRAWABLE_H_

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

app/ios/RTABMapApp/DatabaseView.swift

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

app/ios/RTABMapApp/NativeWrapper.cpp

@@ -150,11 +150,12 @@ bool exportMeshNative(
             int optimizedMinClusterSize,
             float optimizedMaxTextureDistance,
             int optimizedMinTextureClusterSize,
+            int textureVertexColorPolicy,
             bool blockRendering)
 {
     if(object)
     {
-        return native(object)->exportMesh(cloudVoxelSize, regenerateCloud, meshing, textureSize, textureCount, normalK, optimized, optimizedVoxelSize, optimizedDepth, optimizedMaxPolygons, optimizedColorRadius, optimizedCleanWhitePolygons, optimizedMinClusterSize, optimizedMaxTextureDistance, optimizedMinTextureClusterSize, blockRendering);
+        return native(object)->exportMesh(cloudVoxelSize, regenerateCloud, meshing, textureSize, textureCount, normalK, optimized, optimizedVoxelSize, optimizedDepth, optimizedMaxPolygons, optimizedColorRadius, optimizedCleanWhitePolygons, optimizedMinClusterSize, optimizedMaxTextureDistance, optimizedMinTextureClusterSize, textureVertexColorPolicy, blockRendering);
     }
     else
     {
@@ -434,6 +435,13 @@ void setWireframeNative(const void *object, bool enabled)
     else
         UERROR("object is null!");
 }
+void setTextureColorSeamsHiddenNative(const void *object, bool hidden)
+{
+    if(object)
+        native(object)->setTextureColorSeamsHidden(hidden);
+    else
+        UERROR("object is null!");
+}
 void setLocalizationModeNative(const void *object, bool enabled)
 {
     if(object)
@@ -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/ViewController.swift

@@ -45,6 +45,8 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
     
     private var mReviewRequested = false
     
+    private var mMaximumMemory: Int = 0
+    
     // UI states
     private enum State {
         case STATE_WELCOME,    // Camera/Motion off - showing only buttons open and start new scan
@@ -53,8 +55,9 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         STATE_IDLE,            // Camera/Motion off
         STATE_PROCESSING,      // Camera/Motion off - post processing
         STATE_VISUALIZING,     // Camera/Motion off - Showing optimized mesh
-        STATE_VISUALIZING_CAMERA,     // Camera/Motion on  - Showing optimized mesh
-        STATE_VISUALIZING_WHILE_LOADING // Camera/Motion off - Loading data while showing optimized mesh
+        STATE_VISUALIZING_CAMERA,     // Camera/Motion on  - Showing optimized mesh and localizing
+        STATE_VISUALIZING_WHILE_LOADING, // Camera/Motion off - Loading data while showing optimized mesh
+        STATE_VISUALIZING_AND_MEASURING    // Camera/Motion on  - Showing optimized mesh without localizing and measuring tools enabled
     }
     private var mState: State = State.STATE_WELCOME;
     private func getStateString(state: State) -> String {
@@ -73,6 +76,8 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
             return "Visualizing with Camera"
         case .STATE_VISUALIZING_WHILE_LOADING:
             return "Visualizing while Loading"
+        case .STATE_VISUALIZING_AND_MEASURING:
+            return "Measuring"
         default: // IDLE
             return "Idle"
         }
@@ -93,9 +98,14 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
     private var wireframeShown: Bool = false
     private var backfaceShown: Bool = false
     private var lightingShown: Bool = false
+    private var textureColorSeamsShown: Bool = false
     private var mHudVisible: Bool = true
     private var mLastTimeHudShown: DispatchTime = .now()
     private var mMenuOpened: Bool = false
+    private var lowMemoryWarningShown: Bool = false
+    static var previewImages: [String: UIImage] = [:]
+    private var measuringMode: Int = 0
+    private var visualizationType: Int = 0 // 0=Cloud, 1=Mesh, 2=Texture Mesh
     
     @IBOutlet weak var stopButton: UIButton!
     @IBOutlet weak var recordButton: UIButton!
@@ -106,6 +116,11 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
     @IBOutlet weak var statusLabel: UILabel!
     @IBOutlet weak var closeVisualizationButton: UIButton!
     @IBOutlet weak var stopCameraButton: UIButton!
+    @IBOutlet weak var stopMeasuringButton: UIButton!
+    @IBOutlet weak var teleportButton: UIButton!
+    @IBOutlet weak var addMeasureButton: UIButton!
+    @IBOutlet weak var removeMeasureButton: UIButton!
+    @IBOutlet weak var measuringModeButton: UIButton!
     @IBOutlet weak var exportOBJPLYButton: UIButton!
     @IBOutlet weak var orthoDistanceSlider: UISlider!{
         didSet{
@@ -128,6 +143,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
     }
     
     @objc func defaultsChanged(){
+        print("defaultsChanged()")
         updateDisplayFromDefaults()
     }
     
@@ -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()
@@ -237,6 +286,20 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         }
     }
 
+    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 {
             self.progressView?.setProgress(Float(count)/Float(max), animated: true)
@@ -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 {
-        var taskInfo = mach_task_basic_info()
-        var count = mach_msg_type_number_t(MemoryLayout<mach_task_basic_info>.size)/4
-        let kerr: kern_return_t = withUnsafeMutablePointer(to: &taskInfo) {
-            $0.withMemoryRebound(to: integer_t.self, capacity: 1) {
-                task_info(mach_task_self_, task_flavor_t(MACH_TASK_BASIC_INFO), $0, &count)
-            }
-        }
-
-        if kerr == KERN_SUCCESS {
-            return taskInfo.resident_size / (1024*1024)
-        }
-        else {
-            print("Error with task_info(): " +
-                (String(cString: mach_error_string(kerr), encoding: String.Encoding.ascii) ?? "unknown error"))
-            return 0
-        }
+    func getAvailableMemory() -> Int {
+        return os_proc_available_memory()/(1024*1024)
     }
     
     @objc func appMovedToBackground() {
-        if(mState == .STATE_VISUALIZING_CAMERA || mState == .STATE_MAPPING || mState == .STATE_CAMERA)
+        print("appMovedToBackground()")
+        if(mState == .STATE_VISUALIZING_CAMERA || mState == .STATE_VISUALIZING_AND_MEASURING || mState == .STATE_MAPPING || mState == .STATE_CAMERA)
         {
             stopMapping(ignoreSaving: true)
         }
     }
     
     @objc func appMovedToForeground() {
+        print("appMovedToForeground()")
         updateDisplayFromDefaults()
+        updateState(state: mState)
         
         if(mMapNodes > 0 && self.openedDatabasePath == nil)
         {
@@ -513,6 +606,8 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
             	rtabmap!.startCamera()
                 let configuration = ARWorldTrackingConfiguration()
                 var message = ""
+            	if(mState != .STATE_VISUALIZING_AND_MEASURING)
+            	{
                 	if(!UserDefaults.standard.bool(forKey: "LidarMode"))
                 	{
        		        	message = "LiDAR is disabled (Settings->Mapping->LiDAR Mode = OFF), only tracked features will be mapped."
@@ -527,12 +622,14 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
                 	{
                     	configuration.frameSemantics = .sceneDepth
                 	}
+            	}
                 
                 session.run(configuration, options: [.resetSceneReconstruction, .resetTracking, .removeExistingAnchors])
                 
                 switch mState {
-                case .STATE_VISUALIZING:
-                    updateState(state: .STATE_VISUALIZING_CAMERA)
+                case .STATE_VISUALIZING_AND_MEASURING,
+                    .STATE_VISUALIZING_CAMERA:
+                    break // State should be already set
                 default:
                     locationManager?.startUpdatingLocation()
                     updateState(state: .STATE_CAMERA)
@@ -599,6 +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
-            orthoGridSlider.isHidden = cameraMode != 3 || mState != .STATE_VISUALIZING_WHILE_LOADING
+            orthoDistanceSlider.isHidden = cameraMode != 3 || mState == .STATE_PROCESSING
+            orthoGridSlider.isHidden = cameraMode != 3 || mState == .STATE_PROCESSING
+            teleportButton.isHidden = mState != .STATE_VISUALIZING_AND_MEASURING || cameraMode != 0
+            addMeasureButton.isHidden = mState != .STATE_VISUALIZING_AND_MEASURING || cameraMode == 3
+            removeMeasureButton.isHidden = mState != .STATE_VISUALIZING_AND_MEASURING || cameraMode == 3
+            measuringModeButton.isHidden = true
             actionNewScanEnabled = false
             actionNewDataRecording = false
             actionSaveEnabled = false
@@ -663,6 +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
@@ -787,6 +933,25 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         	})
         ])
         
+        // 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
             self.newScan()
@@ -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
@@ -2204,6 +2395,26 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
                     do {
                         let fileURLs = try FileManager.default.contentsOfDirectory(at: exportDir, includingPropertiesForKeys: nil)
                         if(!fileURLs.isEmpty)
+                        {
+                            if(isLAZ)
+                            {
+                                zipFileUrl = self.getDocumentDirectory().appendingPathComponent(fileName+".laz")
+                                do {
+                                    if FileManager.default.fileExists(atPath: zipFileUrl.path)
+                                    {
+                                        try FileManager.default.removeItem(at: zipFileUrl)
+                                    }
+                                    try FileManager.default.moveItem(at: fileURLs.first!, to: zipFileUrl)
+                                    print("LAZ file \(zipFileUrl.path) created (size=\(zipFileUrl.fileSizeString)")
+                                    success = true
+                                }
+                                catch
+                                {
+                                    print("Failed moving \(fileURLs.first!) to \(zipFileUrl.path)")
+                                    return
+                                }
+                            }
+                            else
                             {
                                 do {
                                     zipFileUrl = try Zip.quickZipFiles(fileURLs, fileName: fileName) // Zip
@@ -2214,6 +2425,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
                                     print("Something went wrong while zipping")
                                 }
                             }
+                        }
                     } catch {
                         print("No files exported to \(exportDir)")
                         return
@@ -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) {

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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
+   "9DD43B98BD49BF219F1E888BBFF8719B6E3798D9F165E853315FF7FEBFBB887BECA004577BDDE798AA75AA0A0652DCC0F3C17BB38269B8C0B4A2E48F027E41BD0934E4556083A27408F459C39B893D4D568805B760CDC6C056C05A7DCA9813B8AAC5F9025C19EC13BAADBEDB8F0E55917DDE3E95DE747055207DFE5C82CCBF31E05EDACB6B9F8701B7C02406B26EF292BB529BEF5C3BFE92A73E55006CDFC1799E5AB59E4583BB9FF9D6C7FD8B9639980A014D09DE4FAE448B0AC86E02EFBD247D32DB3445D28CAC7D75B99425C915BE59DCDB67DE3CF531B36966F65BB9B4B79D24FD572E12DA673E4967514EA5B823E5666048DA47D22CC0D9929E977489A433E532045EA2F7D5F4FE2E17715E0349610D880D831B6E1808385423C33F1CBCAFDAFA16DE0BDE1FED952366B699A41F4ABAD4CC7EEFB54F90F40B398BDB3A667646D1BA24462C5B69E0BB3E5D2544FF0F26174078D3A9FA2654267736699FA3C9B68F05EDDB4ACAFDC694E5A49F2067FAFC8AA46335303891DC20604FD5A76DE50AAEF84F6D118C4BCDEC02E03A49B115091794742C30C3CC3A9E19013F95F4E5A0F950497F924B4BF35D2F3FC4D542385BF5AE1A243DD4A9CC41C6CCE0FD609A18488DFA158299FD55D25F23ED67ABB89BF1F31A486BF55957D286DEFBD7E5BE9321A5954C4F148E6FFEBFC3CC0A5F106F300D00C251FF481A00540AAE9EFFFC5ED34C33BBD9EF636657E21625BA45F533B2D50AD46B09493573EC04493FC599D96B0FFF99922E90F4A4A48DE40AB698A45F4AEAC634FACDE0FD7366B65FA6C3E692AE96F4916C9B49FA8D5C3C849F9EF64256BA78281206FDCD16ED551E61DA5F786F183664C5A41A5278818FA97E00F08E99E5BA0AE520E242392B632B2A0D4C2D1A6041495FF09AAE6CD63731CCC0951EADBDFEE7F980EEF1DA7D7FFCCDC13E6BF729FFD4C0DF369326A54E813B82BE63FA91DD46AFCB3C397B676D7EACC8455EDF85A85F5276AD2EE484B10E3704DBBF4C636E78C8E9B99D7805509F7A54E94086FA3C6880D8CC7758434A031C51007B799FF5546091AA754A54007085F745D8CA6BBFC76BCFB5380F7078E481169AC46B7DEF0CFA15B25C2AB025034159B77BED7EA0DEAF837D1EF0B6751C90870B2CF4690882037E804BCB89319D3E8310AB86FA4254552C7EE5EB121685FA5195FA5401690030A200FEED7DD6FF294BEEA0F1F5E12291F7C02DF1DA2E256438B384F7BF9FE253F3BDBF6D667997698DA5D4ED43B0DA5DF6905B2EE897FBEA75B85AD8BF9533B74F95F4D34E77F5FEB72E443E1BBCFF1441EE79E61FFE5393FDFF66663776216F30E2AFFA3717152C04E4B160F07EA8C656248608B86C8FC37195468F02E66FBF576EB217917329D6B8A22CD9832906E05C79BE58606533FB5E8BFEC30E5CC9D345B3B7AF99D99359"
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
+   "81A5803371F786BA7304D6EB45764C11FFE1FF281006E7B4DA7F0EEACD98E380457AD063BF161F08B81B606E334FBAF3C1DD10D1A767FF5B76CCD8C02216D052EB7F7AA4FF2FFA90DFA909725DDC60C3673A7DBA41BAB9FE453008E49F1DF93CFFDAA2FFF691FE17F5213F0D00DA40B503801322D7670AF0F532E4174D44BECF5460D116FB2E4AEB98A97E07002F13995C91EF00E09C6E8E05FC39D2FFCA6EE5D6B900700FD4D3257D286B7A44D27A667657A70734B32966F66D49FFCC9A1690731774CBA6C1FB1BE522A06BCCA7CE4C43457075A4AD675F386E80B476D0FCA499FDAEC56EB140C82FF5AA43A798D975920E0A9ADF2769EFA2650F57805172EE149F49929A0E2ACDEC643997904FC703F5C4D001D8417113FFAFFBCD7E19C48CF3FE9F4DCEC4DF8C6D55EF1619D7AC638F7C4452AB7B715FE02CBC1B05CDE325EDD162B7BDD5E80E5C0F58BC1BD9610CC0019296C9FE7F43D21666F642B8136EE4FF4B5CD4FFFEC086611FB9D88137B2FFBF4A17D1F2C0CA923E1B349F25E7EBF289F94BCB2096AEF2521FC75B5FD2AC41DBBF5AED90A5FA3C27699AF7FA781F3A748C991DA5C61FD917CB903D4C59438D03C89B5BA520651C21E713AEBD86EBC360C492DD0B7FAFC694EDF3CCECD00A542A8B1B83F7E103B2D5B670DF3CD8823C4CEC71365063FCC2B566F672B31DCC6C92A47F04CDB32A1E43D694F70600C0FC9276F2B61D6E660FFA9D81E5801B24DD22E9377223945F4BBA127810F89AA7E05392CEF414FB51177A7D4FF55FF88992CE95149A38D6A0455C4211E0FCCDA17562B2A42BFA38ECE7226DF7B6DBC9CC3E6166B379AF4FF5A143B7DC16BC9F07D8A044F9C38955226D0FB4DBC9CCFE6C663B79AF580051628892DD6B4E56E3E0F01E49DB97AF51A93CA2FA49D567717527EA005650FD64F125498FE620FF31B9DA2735669574780EC78DF1F948DB1D1DEC774BA4AD65BD83107FD6B9B3064CFF6324D54515E20AD35C24A959D0D3B2922E055637B35BB3B633E48A99985C41834E09FD5AFF36B377329FC8AF3CBD6797FB21FCB28B6377CA07802DBDF71F94B4A8A44DE40A33F89C6C6663FA90154B7379B68FE395C11391B62515778FF4C242B86227AD78CACC8ACA839D1558BA4D9FC97D7EEE35E68BB43558DE4AA693EB3FA87CF4C390632485E9D42F4BDA723015482A8839E5267C3FC8DECF22E706D833E8B78DEA278BFFC8F6ED97B7E52C6C077B6D2B00FB9959DE75683E1A69EB6410F360A46D816E04FB0300DF8C72815FE90D57F9EF4C0D3CFC27CBCDC81F94AB52B64BD63E8BA4F7CC96667617F0ACDC83733160CE765F5C5CD6C0C241F3A5D9F19EC405A7F833A64D54CC00602949ED726BA7493ADDCC7ED2A7AC0F46DA720B302A888991B6D883AC571650FB188F3BE52C5045F019B959482BEE91B4620EB262"
+   "9F7FECFA964927D73F511C8BA9D10D2AB90AA48F95AD4C057C40EE99F303AFEDAB6A1C007C25787FA672720D9BD921C066927CCBC39EC0393957A2EDF5FEFF6A87C76ACA2C9294050ED4CA4A22E76FF7394452ADD0C85449DF36B3ED335FB06FFA9E2E29BC3063B2BFB34BEAA44A60587E72A224BF14E43F83EDCB0061005D59DCAF46B7442F8479BD92FB1C063353226D79E7E18E14668FB4CD0C1B8097706998AD5E69BD80E1C13C8A976ADF16880D0C861BEF37B3FFA87E10BE14B056ED4D76DFF753201F31B39B24BD3F473DF6567DF0F93CCABF36402C9B2D767F0D894D12BA3AF75A0CC0DA7291DC9234C6F7FD030B48FAAEB7CFD941E4A9EF1278235223DE8F546C59D1298B43583768BE31B01A9CADFA0F447251A055B092A4BF037FECF338B1E20E3DADD3502261D0A2E4D64B48744FA79FFF1C1DBC06FBF726D11F7329BFC26383999A193F0C6CDDB2C9FF927459B06FDF98D935AA9F804AD226F45FE4CE27B6AE4A27EB92C4060E5DADD152BB89FB8B4A840B496CAE7AB342581FD9F75F4C5023FEC3BADDE8E47B1AA83A58E312FF4DE606B847D2FF79CD5F01E630B34E464D9DF2801A2B11CE2F17EDBEAB06CCDD266937E03A33EB35107052A46DB0FB573F14696B17B5DE0D0F4BFA719B3E6FB6D9DE0F13D5DE0FF7BF9C64C5CEA3EACFFF09D5FB3F632CA802D3A312EF3143031334495A0DD877986701D49E15A7CA99FD6BCF2A3FE3CCFF7F7AD6D7DF372FF6918BD49F3F7B3F8BA44315AFDAD70BB1FB7F2769E5317F7FEC584DA95DD48F796DCF057D7CF3FA8B66767BB07D7EEFFF98DFC29FDDB41BD584D1F548FA421680E813CE72E697B495A4A685537A60BA99850F804725FD077840D2C59E1E269741D1EB002066CAC9A5C25C81C4F24DF30C5C9B9A9900ABE22933CB658D870E88FD6E62A6FC5355EF66595FCE575C0493CCAC65102039AD819168C985929E56639D8D9F03979BD94315E85406734A92993D06DCA601F7F1C2C02672F75CFF37729BB70A62AE0300337B11B72AED915EF3D2AA8F4FE887D8C4E95392AE6BB3DFFF8BB475153B561B007CB8C501FC45469E8E1CC30FFC8A9D88BF7D6C3345B2148F3085C1D4798DEBCD95EF00A0296676293056F503A74FF771C8D84C32F6E1D6011CAFFA51E07833FB611F7A7443587466AAA46B4A923DDC882DE9DC5037C3CCEA02A080EB54DC0020513DD7CA99B9DF27E96B1AA8D122B919E2B16A2C20365CF01FE297A83E7E6C73354E022FF6FECFCD0550C3CCFE9805A8FB01E83D95B98F705FA42D961A1E122BFCD5367DD8A71603E05FCCD0BFEE3FE4621596FC07FC6B91EDFEFEADA257B7578BC5893A604D5A948B2C80309BA19F55F16E88B4B50C98CCAA477D4FD237BD572995E080ADD4F8E0B9DBCC622EA0447BAE91CBACF1599341560A7738027C17B823786DD7A4"
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
+   "D6796BCF3A8756DBF9C95AC6DFB9DDE7EEEF7EAC67BD7FE879ED8F59DA5E8F6CB7FE3CFF3A33EFD5B3DEC6EDE7A2BBEEF7971BBFDDF6B13DFBB6A69F5F0E11BB6AC73BABCFDFC7878EB140EC59E700DCB9E7FBE7CED36EC76A33D703B07144D499A69F8B88C7F4FCCEDD22E26511F1C8CCDC79A8ABD26C7A1A3E19113BF52CDE22225E18110F8E886D23E2F69D65439C88F4EDFF9B23E2D9D5BC9B0688D52B33B78D888F47C4D8C1A7B54944EC18114FC8CC2795529E3B50E83BC6F8BECFD9BCF3FF49DE2EDAA4A70D7D6381FBFE4EB75D66ECD322628F6ADEEE11511F64778A886DAA79672C337644C49931BA4F5B64E6B6A594EF75E6ED111177EA4CFF75CF761E516D67AD7AEA4A292767E63763F473B77B66EE5A4AF95467DE7F54DBFF7D441CB63631D6E0F418BFDFBB6744BCB19AF7F71171CF6ADE2707881F11B159CCFF19988FDE38D6790B7DA9F71DFCBB768CE62039943745FFC1BFEB09317AF09FA4BD7BE64DE404A03DF9392EE63FF8779588D82F335F3389B6AC421F8C886BAB794FE859AF7E6F66447C6880F89F8DE660DA55F77EFC5535FDA09E5E8A0756D35F5F441B5E1EA3AF4189E6801F1111997950CFF6DF5B4A39771131E6F3C1685ECBAEBED1277FD3336F9699E2374C609BF549EF2462AC54D7C5F8FB60935934643559D355DD8511717444BC3B22CEEA59FE9CCCAC135716ADEDF2EBBBA23D3322DE19114746C45A75690EA8F4CCBB7142B1F689A667A3EBC888B85F343D2EFB47C455D5F27FCE616EC31C1E118746C4A77A96BDB75D76688C1FA4D60BA594CBA2E905E87A54CFAAF509F1B9A5946F0E10FF9A88F84E35FBA1D5747D85BC41443C656E2233B78E887B54EB7C7E116DF87644BCBF9AFDF0CCDCAFED867D49B5ECA288F8CFB5DDFE1A627F2BC6F7FFD1395E83A23E29BAB09472F2106D88E6BBE5AC057E6EEEF99DB1DB3003B84B357DE90462AC48A594AB63FC3BEEFEB368CBAA9399FBF4DC7FF972666E56AD777CCF7AAF1B20FE213DDB3DBE5A67D3CCFC7CCF7AFB0C10BF6FFF3333BF94992FCEA64AD6B3B349CC1B5C8E8F81BDA0679DB7F4B4AFBE325C4E1B665607606D63E7E87DCA397B0D10FF5F167A6D33F3AE99F9876AF960BD5F3D7FDBD33BCB9ED4D3B6CCCC4F77D6D9AB5A76D912DAB06536F93D5DE767E67B7A62EF37D4BEB7B15FDB13E3999DE58FEE59FEEE21DBB040DBDEBAD06B3F609CADB3C9B9E95A567ECB22E3CF3407A06DC317ABFD3F76066D383033CFCA262FE69799794A66EE36CFBA0FCFCCD7777E265EB06922B2FF00387660CDCC6D33F3C66ABD2F0E10FF0BD5366FC8CCFB2EB59D4B88DFB7DDA9752F66E607AAD8BFCDAA00483625429F59FD6C3D601B56F309C09699F9FB6ABB6FEA2C7F7EB5ECE6CCACAFD297137FD76AFB577696BDAD679F339B44B48DDA750EAF967D7689"
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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

@@ -58,7 +58,8 @@ public:
 			 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,
@@ -72,7 +73,8 @@ public:
 			 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;
@@ -106,6 +109,7 @@ private:
 	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,67 +38,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
-class OdometryInfo
+class RTABMAP_CORE_EXPORT OdometryInfo
 {
 public:
-	OdometryInfo() :
-		lost(true),
-		features(0),
-		localMapSize(0),
-		localScanMapSize(0),
-		localKeyFrames(0),
-		localBundleOutliers(0),
-		localBundleConstraints(0),
-		localBundleTime(0),
-		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 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.keyFrameAdded = keyFrameAdded;
-		output.timeDeskewing = timeDeskewing;
-		output.timeEstimation = timeEstimation;
-		output.timeParticleFiltering = timeParticleFiltering;
-		output.stamp = stamp;
-		output.interval = interval;
-		output.transform = transform;
-		output.transformFiltered = transformFiltered;
-		output.transformGroundTruth = transformGroundTruth;
-		output.guessVelocity = guessVelocity;
-		output.guess = guess;
-		output.distanceTravelled = distanceTravelled;
-		output.memoryUsage = memoryUsage;
-		output.gravityRollError = gravityRollError;
-		output.gravityPitchError = gravityPitchError;
-		output.type = type;
-		return output;
-	}
+	OdometryInfo();
+	OdometryInfo copyWithoutData() const;
+	std::map<std::string, float> statistics(const Transform & pose = Transform());
 
 	bool lost;
 	RegistrationInfo reg;
@@ -112,6 +58,7 @@ public:
 	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

@@ -678,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()));
@@ -883,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, VarianceAngular,        float, 0.01,  "Angular variance to set on marker detections. Set to >=9999 to use only position (xyz) constraint in graph optimization.");
+    RTABMAP_PARAM(Marker, VarianceLinear,         float, 0.001, uFormat("Linear variance to set on marker detections. If %s is enabled and %s=2 (GTSAM): it is the variance of the range factor, with 9999 to disable range factor and to do only bearing.", kMarkerVarianceOrientationIgnored().c_str(), kOptimizerStrategy().c_str()));
+    RTABMAP_PARAM(Marker, VarianceAngular,        float, 0.01,  uFormat("Angular variance to set on marker detections. If %s is enabled, it is ignored with %s=1 (g2o) and it corresponds to bearing variance with %s=2 (GTSAM).", kMarkerVarianceOrientationIgnored().c_str(), kOptimizerStrategy().c_str(), kOptimizerStrategy().c_str()));
+    RTABMAP_PARAM(Marker, VarianceOrientationIgnored, bool, false, uFormat("When this setting is false, the landmark's orientation is optimized during graph optimization. When this setting is true, only the position of the landmark is optimized. This can be useful when the landmark's orientation estimation is not reliable. Note that for %s=1 (g2o), only %s needs be set if we ignore orientation. For %s=2 (GTSAM), instead of optimizing the landmark's position directly, a bearing/range factor is used, with %s as the variance of the range factor (with 9999 to optimize the position with only a bearing factor) and %s as the variance of the bearing factor (pitch/yaw).", kOptimizerStrategy().c_str(), kMarkerVarianceLinear().c_str(), kOptimizerStrategy().c_str(), kMarkerVarianceLinear().c_str(), kMarkerVarianceAngular().c_str()));
     RTABMAP_PARAM(Marker, CornerRefinementMethod, int,   0,     "Corner refinement method (0: None, 1: Subpixel, 2:contour, 3: AprilTag2). For OpenCV <3.3.0, this is \"doCornerRefinement\" parameter: set 0 for false and 1 for true.");
     RTABMAP_PARAM(Marker, MaxRange,               float, 0.0,   "Maximum range in which markers will be detected. <=0 for unlimited range.");
     RTABMAP_PARAM(Marker, MinRange,               float, 0.0,   "Miniminum range in which markers will be detected. <=0 for unlimited range.");

corelib/include/rtabmap/core/RegistrationInfo.h

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

corelib/include/rtabmap/core/RegistrationVis.h

@@ -78,7 +78,6 @@ private:
 	int _refineIterations;
 	float _epipolarGeometryVar;
 	int _estimationType;
-	bool _forwardEstimateOnly;
 	float _PnPReprojError;
 	int _PnPFlags;
 	int _PnPRefineIterations;

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

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

@@ -55,7 +55,8 @@ DBReader::DBReader(const std::string & databasePath,
 				   bool featuresIgnored,
 				   int startMapId,
 				   int stopMapId,
-				   bool priorsIgnored) :
+				   bool priorsIgnored,
+				   const std::vector<Transform> & cameraLocalTransformOverrides) :
 	Camera(frameRate),
 	_paths(uSplit(databasePath, ';')),
 	_odometryIgnored(odometryIgnored),
@@ -70,6 +71,7 @@ DBReader::DBReader(const std::string & databasePath,
 	_priorsIgnored(priorsIgnored),
 	_startMapId(startMapId),
 	_stopMapId(stopMapId),
+	_cameraLocalTransformOverrides(cameraLocalTransformOverrides),
 	_dbDriver(0),
 	_currentId(_ids.end()),
 	_previousMapId(-1),
@@ -77,15 +79,7 @@ DBReader::DBReader(const std::string & databasePath,
 	_previousMapID(0),
 	_calibrated(false)
 {
-	if(_stopId>0 && _stopId<_startId)
-	{
-		_stopId = _startId;
-	}
-
-	if(_stopMapId>-1 && _stopMapId<_startMapId)
-	{
-		_stopMapId = _startMapId;
-	}
+	checkArguments();
 }
 
 DBReader::DBReader(const std::list<std::string> & databasePaths,
@@ -101,7 +95,8 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
 				   bool featuresIgnored,
 				   int startMapId,
 				   int stopMapId,
-				   bool priorsIgnored) :
+				   bool priorsIgnored,
+				   const std::vector<Transform> & cameraLocalTransformOverrides) :
 	Camera(frameRate),
    _paths(databasePaths),
 	_odometryIgnored(odometryIgnored),
@@ -116,12 +111,18 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
 	_priorsIgnored(priorsIgnored),
 	_startMapId(startMapId),
 	_stopMapId(stopMapId),
+	_cameraLocalTransformOverrides(cameraLocalTransformOverrides),
 	_dbDriver(0),
 	_currentId(_ids.end()),
 	_previousMapId(-1),
 	_previousStamp(0),
 	_previousMapID(0),
 	_calibrated(false)
+{
+	checkArguments();
+}
+
+void DBReader::checkArguments()
 {
 	if(_stopId>0 && _stopId<_startId)
 	{
@@ -132,6 +133,28 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
 	{
 		_stopMapId = _startMapId;
 	}
+
+	if(!_cameraLocalTransformOverrides.empty())
+	{
+		if(!_cameraIndices.empty() &&
+			_cameraIndices.size() != _cameraLocalTransformOverrides.size())
+		{
+			UERROR("Camera local transform overrides (%d) are not the same size than the camera indices (%d). The overrides are ignored.",
+				_cameraLocalTransformOverrides.size(),
+				_cameraIndices.size()
+			);
+			_cameraLocalTransformOverrides.clear();
+		}
+		for(size_t i=0; i<_cameraLocalTransformOverrides.size(); ++i)
+		{
+			if(_cameraLocalTransformOverrides[i].isNull())
+			{
+				UERROR("Camera local transform overrides vector cannot contains null transforms! Clearing overrides.");
+				_cameraLocalTransformOverrides.clear();
+				break;
+			}
+		}
+	}
 }
 
 DBReader::~DBReader()
@@ -144,8 +167,8 @@ DBReader::~DBReader()
 }
 
 bool DBReader::init(
-		const std::string & calibrationFolder,
-		const std::string & cameraName)
+		const std::string &,
+		const std::string &)
 {
 	if(_dbDriver)
 	{
@@ -566,9 +589,22 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
 					dbModels.push_back(data.stereoCameraModels()[i].left());
 				}
 			}
-			if(dbModels.size() > 1 &&
-				!_cameraIndices.empty())
+
+			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;
@@ -613,11 +649,23 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
 
 					if(!data.cameraModels().empty())
 					{
-						combinedModels.push_back(data.cameraModels()[_cameraIndices[i]]);
+						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
 					{
-						combinedStereoModels.push_back(data.stereoCameraModels()[_cameraIndices[i]]);
+						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));
 				}
@@ -630,6 +678,38 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
 					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);
 			data.setStamp(s->getStamp());
 			data.setGroundTruth(s->getGroundTruthPose());
@@ -686,7 +766,9 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
 							newKeypoints.back().pt.x += (newCameraIndex-cameraIndex)*subImageWidth;
 							if(!keypoints3D.empty())
 							{
-								newKeypoints3D.push_back(keypoints3D.at(i));
+								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())
 							{
@@ -696,6 +778,19 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
 					}
 					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);
@@ -706,7 +801,7 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
 				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/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(iter->second.transform().y() - t.y()),
-					force3DoF?0:fabs(iter->second.transform().z() - t.z()));
+					fabs(linkT.x() - t.x()),
+					fabs(linkT.y() - t.y()),
+					force3DoF?0:fabs(linkT.z() - t.z()));
 			UASSERT(iter->second.transVariance(false)>0.0);
 			float stddevLinear = sqrt(iter->second.transVariance(false));
 			float linearErrorRatio = linearError/stddevLinear;
@@ -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);
-			rgbToDepthFactorY = 1.0f/(model.imageHeight()>0?model.imageHeight()/depth.rows:1);
+			rgbToDepthFactorX = 1.0f/(model.imageWidth()>0?float(model.imageWidth())/float(depth.cols):1.0f);
+			rgbToDepthFactorY = 1.0f/(model.imageHeight()>0?float(model.imageHeight())/float(depth.rows):1.0f);
 		}
         else if(markerLength_ == 0)
         {

corelib/src/Memory.cpp

@@ -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());
@@ -5593,8 +5610,32 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 						continue;
 					}
 					cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
+					if(_markerOrientationIgnored)
+					{
+						covariance(cv::Range(3,6), cv::Range(3,6)) *= 9999; // disable orientation estimation
+						bool isGTSAM = uStr2Int(uValue(parameters_, Parameters::kOptimizerStrategy(), uNumber2Str(Parameters::defaultOptimizerStrategy()))) == Optimizer::kTypeGTSAM;
+						if(!isGTSAM)
+						{
+							covariance(cv::Range(0,3), cv::Range(0,3)) *= _markerLinVariance;
+						}
+						else if(_registrationPipeline->force3DoF())
+						{
+							// Bearing/Range in 2D, set X as bearing and Y as range (see OptimizerGTSAM)
+							covariance(cv::Range(0,1), cv::Range(0,1)) *= _markerAngVariance;
+							covariance(cv::Range(1,3), cv::Range(1,3)) *= _markerLinVariance;
+						}
+						else
+						{
+							// Bearing/Range in 3D, set X and Y as bearing and Z as range (see OptimizerGTSAM)
+							covariance(cv::Range(0,2), cv::Range(0,2)) *= _markerAngVariance;
+							covariance(cv::Range(2,3), cv::Range(2,3)) *= _markerLinVariance;
+						}
+					}
+					else
+					{
 						covariance(cv::Range(0,3), cv::Range(0,3)) *= _markerLinVariance;
 						covariance(cv::Range(3,6), cv::Range(3,6)) *= _markerAngVariance;
+					}
 					landmarks.insert(std::make_pair(iter->first, Landmark(iter->first, iter->second.length(), iter->second.pose(), covariance)));
 				}
 				UDEBUG("Markers detected = %d", (int)markers.size());
@@ -6169,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/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/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, iter->second.transform(), &info);
+					Transform t = reg.computeTransformationMod(sFrom, sTo, useLinkTransformAsGuess?iter->second.transform():Transform(), &info);
 					UDEBUG("%d->%d, inliers=%d",sFrom.id(), sTo.id(), (int)info.inliersIDs.size());
 
 					if(!t.isNull())

corelib/src/PDALWriter.cpp

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

corelib/src/Parameters.cpp

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

corelib/src/RegistrationVis.cpp

@@ -71,7 +71,6 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
 		_refineIterations(Parameters::defaultVisRefineIterations()),
 		_epipolarGeometryVar(Parameters::defaultVisEpipolarGeometryVar()),
 		_estimationType(Parameters::defaultVisEstimationType()),
-		_forwardEstimateOnly(Parameters::defaultVisForwardEstOnly()),
 		_PnPReprojError(Parameters::defaultVisPnPReprojError()),
 		_PnPFlags(Parameters::defaultVisPnPFlags()),
 		_PnPRefineIterations(Parameters::defaultVisPnPRefineIterations()),
@@ -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,63 +1573,45 @@ Transform RegistrationVis::computeTransformationImpl(
 	info.matchesIDs.clear();
 	if(toSignature.getWords().size())
 	{
-		Transform transforms[2];
-		std::vector<int> inliers[2];
-		std::vector<int> matches[2];
-		cv::Mat covariances[2];
-		covariances[0] = cv::Mat::eye(6,6,CV_64FC1);
-		covariances[1] = cv::Mat::eye(6,6,CV_64FC1);
-		for(int dir=0; dir<(!_forwardEstimateOnly?2:1); ++dir)
-		{
-			// A to B
-			Signature * signatureA;
-			Signature * signatureB;
-			if(dir == 0)
-			{
-				signatureA = &fromSignature;
-				signatureB = &toSignature;
-			}
-			else
-			{
-				signatureA = &toSignature;
-				signatureB = &fromSignature;
-			}
+		std::vector<int> inliers;
+		std::vector<int> matches;
+
 		if(_estimationType == 2) // Epipolar Geometry
 		{
 			UDEBUG("");
-				if((signatureB->sensorData().stereoCameraModels().size() != 1 ||
-					!signatureB->sensorData().stereoCameraModels()[0].isValidForProjection()) &&
-				   (signatureB->sensorData().cameraModels().size() != 1 ||
-					!signatureB->sensorData().cameraModels()[0].isValidForProjection()))
+			if((toSignature.sensorData().stereoCameraModels().size() != 1 ||
+				!toSignature.sensorData().stereoCameraModels()[0].isValidForProjection()) &&
+				(toSignature.sensorData().cameraModels().size() != 1 ||
+				!toSignature.sensorData().cameraModels()[0].isValidForProjection()))
 			{
 				UERROR("Calibrated camera required (multi-cameras not supported).");
 			}
-				else if((int)signatureA->getWords().size() >= _minInliers &&
-						(int)signatureB->getWords().size() >= _minInliers)
+			else if((int)fromSignature.getWords().size() >= _minInliers &&
+					(int)toSignature.getWords().size() >= _minInliers)
 			{
-					UASSERT((signatureA->sensorData().stereoCameraModels().size() == 1 && signatureA->sensorData().stereoCameraModels()[0].isValidForProjection()) || (signatureA->sensorData().cameraModels().size() == 1 && signatureA->sensorData().cameraModels()[0].isValidForProjection()));
-					const CameraModel & cameraModel = signatureA->sensorData().stereoCameraModels().size()?signatureA->sensorData().stereoCameraModels()[0].left():signatureA->sensorData().cameraModels()[0];
+				UASSERT((fromSignature.sensorData().stereoCameraModels().size() == 1 && fromSignature.sensorData().stereoCameraModels()[0].isValidForProjection()) || (fromSignature.sensorData().cameraModels().size() == 1 && fromSignature.sensorData().cameraModels()[0].isValidForProjection()));
+				const CameraModel & cameraModel = fromSignature.sensorData().stereoCameraModels().size()?fromSignature.sensorData().stereoCameraModels()[0].left():fromSignature.sensorData().cameraModels()[0];
 
 				// we only need the camera transform, send guess words3 for scale estimation
 				Transform cameraTransform;
 				double variance = 1.0f;
 				std::vector<int> matchesV;
-					std::map<int, int> uniqueWordsA = uMultimapToMapUnique(signatureA->getWords());
-					std::map<int, int> uniqueWordsB = uMultimapToMapUnique(signatureB->getWords());
+				std::map<int, int> uniqueWordsA = uMultimapToMapUnique(fromSignature.getWords());
+				std::map<int, int> uniqueWordsB = uMultimapToMapUnique(toSignature.getWords());
 				std::map<int, cv::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(!signatureA->getWords3().empty())
+					wordsA.insert(std::make_pair(iter->first, fromSignature.getWordsKpts()[iter->second]));
+					if(!fromSignature.getWords3().empty())
 					{
-							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)
 				{
-						wordsB.insert(std::make_pair(iter->first, signatureB->getWordsKpts()[iter->second]));
+					wordsB.insert(std::make_pair(iter->first, toSignature.getWordsKpts()[iter->second]));
 				}
 				std::map<int, cv::Point3f> inliers3D = util3d::generateWords3DMono(
 						wordsA,
@@ -1644,9 +1623,9 @@ Transform RegistrationVis::computeTransformationImpl(
 						words3A, // for scale estimation
 						&variance,
 						&matchesV);
-					covariances[dir] *= variance;
-					inliers[dir] = uKeys(inliers3D);
-					matches[dir] = matchesV;
+				covariance *= variance;
+				inliers = uKeys(inliers3D);
+				matches = matchesV;
 
 				if(!cameraTransform.isNull())
 				{
@@ -1656,11 +1635,11 @@ Transform RegistrationVis::computeTransformationImpl(
 						{
 							if(this->force3DoF())
 							{
-									transforms[dir] = cameraTransform.to3DoF();
+								transform = cameraTransform.to3DoF();
 							}
 							else
 							{
-									transforms[dir] = cameraTransform;
+								transform = cameraTransform;
 							}
 						}
 						else
@@ -1681,9 +1660,9 @@ Transform RegistrationVis::computeTransformationImpl(
 					UINFO(msg.c_str());
 				}
 			}
-				else if(signatureA->getWords().size() == 0)
+			else if(fromSignature.getWords().size() == 0)
 			{
-					msg = uFormat("No enough features (%d)", (int)signatureA->getWords().size());
+				msg = uFormat("No enough features (%d)", (int)fromSignature.getWords().size());
 				UWARN(msg.c_str());
 			}
 			else
@@ -1695,17 +1674,17 @@ Transform RegistrationVis::computeTransformationImpl(
 		else if(_estimationType == 1) // PnP
 		{
 			UDEBUG("");
-				if((signatureB->sensorData().stereoCameraModels().empty() || !signatureB->sensorData().stereoCameraModels()[0].isValidForProjection()) &&
-				   (signatureB->sensorData().cameraModels().empty() || !signatureB->sensorData().cameraModels()[0].isValidForProjection()))
+			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",
-							signatureB->id(),
-							(int)signatureB->sensorData().cameraModels().size(),
-							signatureB->sensorData().stereoCameraModels().size(),
-							signatureB->getWeight());
+						toSignature.id(),
+						(int)toSignature.sensorData().cameraModels().size(),
+						toSignature.sensorData().stereoCameraModels().size(),
+						toSignature.getWeight());
 			}
 #ifndef RTABMAP_OPENGV
-				else if(signatureB->sensorData().cameraModels().size() > 1)
+			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.");
@@ -1713,42 +1692,42 @@ Transform RegistrationVis::computeTransformationImpl(
 #endif
 			else
 			{
-					UDEBUG("words from3D=%d to2D=%d", (int)signatureA->getWords3().size(), (int)signatureB->getWords().size());
+				UDEBUG("words from3D=%d to2D=%d", (int)fromSignature.getWords3().size(), (int)toSignature.getWords().size());
 				// 3D to 2D
-					if((int)signatureA->getWords3().size() >= _minInliers &&
-					   (int)signatureB->getWords().size() >= _minInliers)
+				if((int)fromSignature.getWords3().size() >= _minInliers &&
+					(int)toSignature.getWords().size() >= _minInliers)
 				{
 					std::vector<int> inliersV;
 					std::vector<int> matchesV;
-						std::map<int, int> uniqueWordsA = uMultimapToMapUnique(signatureA->getWords());
-						std::map<int, int> uniqueWordsB = uMultimapToMapUnique(signatureB->getWords());
+					std::map<int, int> uniqueWordsA = uMultimapToMapUnique(fromSignature.getWords());
+					std::map<int, int> uniqueWordsB = uMultimapToMapUnique(toSignature.getWords());
 					std::map<int, cv::Point3f> words3A;
 					std::map<int, cv::Point3f> words3B;
 					std::map<int, cv::KeyPoint> wordsB;
 					for(std::map<int, int>::iterator iter=uniqueWordsA.begin(); iter!=uniqueWordsA.end(); ++iter)
 					{
-							words3A.insert(std::make_pair(iter->first, signatureA->getWords3()[iter->second]));
+						words3A.insert(std::make_pair(iter->first, fromSignature.getWords3()[iter->second]));
 					}
 					for(std::map<int, int>::iterator iter=uniqueWordsB.begin(); iter!=uniqueWordsB.end(); ++iter)
 					{
-							wordsB.insert(std::make_pair(iter->first, signatureB->getWordsKpts()[iter->second]));
-							if(!signatureB->getWords3().empty())
+						wordsB.insert(std::make_pair(iter->first, toSignature.getWordsKpts()[iter->second]));
+						if(!toSignature.getWords3().empty())
 						{
-								words3B.insert(std::make_pair(iter->first, signatureB->getWords3()[iter->second]));
+							words3B.insert(std::make_pair(iter->first, toSignature.getWords3()[iter->second]));
 						}
 					}
 
 					std::vector<CameraModel> models;
-						if(signatureB->sensorData().stereoCameraModels().size())
+					if(toSignature.sensorData().stereoCameraModels().size())
 					{
-							for(size_t i=0; i<signatureB->sensorData().stereoCameraModels().size(); ++i)
+						for(size_t i=0; i<toSignature.sensorData().stereoCameraModels().size(); ++i)
 						{
-								models.push_back(signatureB->sensorData().stereoCameraModels()[i].left());
+							models.push_back(toSignature.sensorData().stereoCameraModels()[i].left());
 						}
 					}
 					else
 					{
-							models = signatureB->sensorData().cameraModels();
+						models = toSignature.sensorData().cameraModels();
 					}
 
 					if(models.size()>1)
@@ -1756,7 +1735,10 @@ Transform RegistrationVis::computeTransformationImpl(
 						// Multi-Camera
 						UASSERT(models[0].isValidForProjection());
 
-							transforms[dir] = util3d::estimateMotion3DTo2D(
+						std::vector<std::vector<int> > matchesPerCam;
+						std::vector<std::vector<int> > inliersPerCam;
+
+						transform = util3d::estimateMotion3DTo2D(
 								words3A,
 								wordsB,
 								models,
@@ -1768,20 +1750,31 @@ Transform RegistrationVis::computeTransformationImpl(
 								_PnPRefineIterations,
 								_PnPVarMedianRatio,
 								_PnPMaxVar,
-									dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()),
+								!guess.isNull()?guess:Transform::getIdentity(),
 								words3B,
-									&covariances[dir],
-									&matchesV,
-									&inliersV,
+								&covariance,
+								&matchesPerCam,
+								&inliersPerCam,
 								_PnPSplitLinearCovarianceComponents);
-							inliers[dir] = inliersV;
-							matches[dir] = matchesV;
+
+						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());
 
-							transforms[dir] = util3d::estimateMotion3DTo2D(
+						transform = util3d::estimateMotion3DTo2D(
 								words3A,
 								wordsB,
 								models[0],
@@ -1792,31 +1785,31 @@ Transform RegistrationVis::computeTransformationImpl(
 								_PnPRefineIterations,
 								_PnPVarMedianRatio,
 								_PnPMaxVar,
-									dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()),
+								!guess.isNull()?guess:Transform::getIdentity(),
 								words3B,
-									&covariances[dir],
+								&covariance,
 								&matchesV,
 								&inliersV,
 								_PnPSplitLinearCovarianceComponents);
-							inliers[dir] = inliersV;
-							matches[dir] = matchesV;
+						inliers = inliersV;
+						matches = 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 features in images (old=%d, new=%d, min=%d)",
-								(int)signatureA->getWords3().size(), (int)signatureB->getWords().size(), _minInliers);
+							(int)fromSignature.getWords3().size(), (int)toSignature.getWords().size(), _minInliers);
 					UINFO(msg.c_str());
 				}
 			}
@@ -1826,97 +1819,59 @@ Transform RegistrationVis::computeTransformationImpl(
 		{
 			UDEBUG("");
 			// 3D -> 3D
-				if((int)signatureA->getWords3().size() >= _minInliers &&
-				   (int)signatureB->getWords3().size() >= _minInliers)
+			if((int)fromSignature.getWords3().size() >= _minInliers &&
+				(int)toSignature.getWords3().size() >= _minInliers)
 			{
 				std::vector<int> inliersV;
 				std::vector<int> matchesV;
-					std::map<int, int> uniqueWordsA = uMultimapToMapUnique(signatureA->getWords());
-					std::map<int, int> uniqueWordsB = uMultimapToMapUnique(signatureB->getWords());
+				std::map<int, int> uniqueWordsA = uMultimapToMapUnique(fromSignature.getWords());
+				std::map<int, int> uniqueWordsB = uMultimapToMapUnique(toSignature.getWords());
 				std::map<int, cv::Point3f> words3A;
 				std::map<int, cv::Point3f> words3B;
 				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]));
+					words3B.insert(std::make_pair(iter->first, toSignature.getWords3()[iter->second]));
 				}
-					transforms[dir] = util3d::estimateMotion3DTo3D(
+				transform = util3d::estimateMotion3DTo3D(
 						words3A,
 						words3B,
 						_minInliers,
 						_inlierDistance,
 						_iterations,
 						_refineIterations,
-							&covariances[dir],
+						&covariance,
 						&matchesV,
 						&inliersV);
-					inliers[dir] = inliersV;
-					matches[dir] = matchesV;
+				inliers = inliersV;
+				matches = matchesV;
 				UDEBUG("inliers: %d/%d", (int)inliersV.size(), (int)matchesV.size());
-					if(transforms[dir].isNull())
+				if(transform.isNull())
 				{
 					msg = uFormat("Not enough inliers %d/%d (matches=%d) between %d and %d",
-								(int)inliers[dir].size(), _minInliers, (int)matches[dir].size(), signatureA->id(), signatureB->id());
+							(int)inliers.size(), _minInliers, (int)matches.size(), fromSignature.id(), toSignature.id());
 					UINFO(msg.c_str());
 				}
 				else if(this->force3DoF())
 				{
-						transforms[dir] = transforms[dir].to3DoF();
+					transform = transform.to3DoF();
 				}
 			}
 			else
 			{
 				msg = uFormat("Not enough 3D features in images (old=%d, new=%d, min=%d)",
-							(int)signatureA->getWords3().size(), (int)signatureB->getWords3().size(), _minInliers);
+						(int)fromSignature.getWords3().size(), (int)toSignature.getWords3().size(), _minInliers);
 				UINFO(msg.c_str());
 			}
 		}
-		}
-
-		if(!_forwardEstimateOnly)
-		{
-			UDEBUG("from->to=%s", transforms[0].prettyPrint().c_str());
-			UDEBUG("to->from=%s", transforms[1].prettyPrint().c_str());
-		}
-
-		std::vector<int> allInliers = inliers[0];
-		if(inliers[1].size())
-		{
-			std::set<int> allInliersSet(allInliers.begin(), allInliers.end());
-			unsigned int oi = allInliers.size();
-			allInliers.resize(allInliers.size() + inliers[1].size());
-			for(unsigned int i=0; i<inliers[1].size(); ++i)
-			{
-				if(allInliersSet.find(inliers[1][i]) == allInliersSet.end())
-				{
-					allInliers[oi++] = inliers[1][i];
-				}
-			}
-			allInliers.resize(oi);
-		}
-		std::vector<int> allMatches = matches[0];
-		if(matches[1].size())
-		{
-			std::set<int> allMatchesSet(allMatches.begin(), allMatches.end());
-			unsigned int oi = allMatches.size();
-			allMatches.resize(allMatches.size() + matches[1].size());
-			for(unsigned int i=0; i<matches[1].size(); ++i)
-			{
-				if(allMatchesSet.find(matches[1][i]) == allMatchesSet.end())
-				{
-					allMatches[oi++] = matches[1][i];
-				}
-			}
-			allMatches.resize(oi);
-		}
 
 		if(_bundleAdjustment > 0 &&
 			_estimationType < 2 &&
-			!transforms[0].isNull() &&
-			allInliers.size() &&
+			!transform.isNull() &&
+			inliers.size() &&
 			fromSignature.getWords3().size() &&
 			toSignature.getWords().size() &&
 			(fromSignature.sensorData().stereoCameraModels().size() >= 1 || fromSignature.sensorData().cameraModels().size() >= 1) &&
@@ -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(covariances[i].cols==6 && covariances[i].rows == 6 && covariances[i].type() == CV_64FC1);
-				if(covariances[i].at<double>(0,0)<=COVARIANCE_LINEAR_EPSILON)
-					covariances[i].at<double>(0,0) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
-				if(covariances[i].at<double>(1,1)<=COVARIANCE_LINEAR_EPSILON)
-					covariances[i].at<double>(1,1) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
-				if(covariances[i].at<double>(2,2)<=COVARIANCE_LINEAR_EPSILON)
-					covariances[i].at<double>(2,2) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
-				if(covariances[i].at<double>(3,3)<=COVARIANCE_ANGULAR_EPSILON)
-					covariances[i].at<double>(3,3) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
-				if(covariances[i].at<double>(4,4)<=COVARIANCE_ANGULAR_EPSILON)
-					covariances[i].at<double>(4,4) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
-				if(covariances[i].at<double>(5,5)<=COVARIANCE_ANGULAR_EPSILON)
-					covariances[i].at<double>(5,5) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
-			}
-
-			cv::Mat cov = covariances[0].clone();
-
-			links.insert(std::make_pair(1, Link(1, 2, Link::kNeighbor, transforms[0], cov.inv())));
-			if(!transforms[1].isNull() && inliers[1].size())
-			{
-				cov = covariances[1].clone();
-				links.insert(std::make_pair(2, Link(2, 1, Link::kNeighbor, transforms[1], cov.inv())));
-			}
+			UASSERT(covariance.cols==6 && covariance.rows == 6 && covariance.type() == CV_64FC1);
+			if(covariance.at<double>(0,0)<=COVARIANCE_LINEAR_EPSILON)
+				covariance.at<double>(0,0) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
+			if(covariance.at<double>(1,1)<=COVARIANCE_LINEAR_EPSILON)
+				covariance.at<double>(1,1) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
+			if(covariance.at<double>(2,2)<=COVARIANCE_LINEAR_EPSILON)
+				covariance.at<double>(2,2) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
+			if(covariance.at<double>(3,3)<=COVARIANCE_ANGULAR_EPSILON)
+				covariance.at<double>(3,3) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
+			if(covariance.at<double>(4,4)<=COVARIANCE_ANGULAR_EPSILON)
+				covariance.at<double>(4,4) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
+			if(covariance.at<double>(5,5)<=COVARIANCE_ANGULAR_EPSILON)
+				covariance.at<double>(5,5) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
 
+			links.insert(std::make_pair(1, Link(1, 2, Link::kNeighbor, transform, covariance.inv())));
 			std::map<int, Transform> optimizedPoses;
 
 			UASSERT((toSignature.sensorData().stereoCameraModels().size()  >= 1 && toSignature.sensorData().stereoCameraModels()[0].isValidForProjection()) ||
@@ -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(!_forwardEstimateOnly, uFormat("3D point %d is not finite!?", wordId).c_str());
-					sbaOutliers.insert(wordId);
-					continue;
-				}
+				UASSERT_MSG(util3d::isFinite(pt3D), uFormat("3D point %d is not finite!?", wordId).c_str());
 
 				points3DMap.insert(std::make_pair(wordId, pt3D));
 
@@ -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()));
-					allInliers = newInliers;
+					UDEBUG("BA outliers ratio %f", float(sbaOutliers.size())/float(inliers.size()));
+					inliers = newInliers;
 				}
-				if((int)allInliers.size() < _minInliers)
+				if((int)inliers.size() < _minInliers)
 				{
 					msg = uFormat("Not enough inliers after bundle adjustment %d/%d (matches=%d) between %d and %d",
-							(int)allInliers.size(), _minInliers, (int)allInliers.size()+sbaOutliers.size(), fromSignature.id(), toSignature.id());
-					transforms[0].setNull();
+							(int)inliers.size(), _minInliers, (int)inliers.size()+sbaOutliers.size(), fromSignature.id(), toSignature.id());
+					transform.setNull();
 				}
 				else
 				{
-					transforms[0] = optimizedPoses.rbegin()->second;
+					transform = optimizedPoses.rbegin()->second;
 				}
 				// update 3D points, both from and to signatures
 				/*std::multimap<int, cv::Point3f> cpyWordsFrom3 = fromSignature.getWords3();
@@ -2137,36 +2076,16 @@ Transform RegistrationVis::computeTransformationImpl(
 			}
 			else
 			{
-				transforms[0].setNull();
+				transform.setNull();
 			}
-			transforms[1].setNull();
 		}
 
-		info.inliersIDs = allInliers;
-		info.matchesIDs = allMatches;
-		inliersCount = (int)allInliers.size();
-		matchesCount = (int)allMatches.size();
-		if(!transforms[1].isNull())
-		{
-			transforms[1] = transforms[1].inverse();
-			if(transforms[0].isNull())
-			{
-				transform = transforms[1];
-				covariance = covariances[1];
-			}
-			else
-			{
-				transform = transforms[0].interpolate(0.5f, transforms[1]);
-				covariance = (covariances[0]+covariances[1])/2.0f;
-			}
-		}
-		else
-		{
-			transform = transforms[0];
-			covariance = covariances[0];
-		}
+		info.inliersIDs = inliers;
+		info.matchesIDs = matches;
+		inliersCount = (int)inliers.size();
+		matchesCount = (int)matches.size();
 
-		if(!transform.isNull() && !allInliers.empty() && (_minInliersDistributionThr>0.0f || _maxInliersMeanDistance>0.0f))
+		if(!transform.isNull() && !inliers.empty() && (_minInliersDistributionThr>0.0f || _maxInliersMeanDistance>0.0f))
 		{
 			cv::Mat pcaData;
 			std::vector<CameraModel> cameraModelsTo;
@@ -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();
-				if(rejectedGlobalLoopClosure)
+				rejectedLoopClosure = transform.isNull();
+				if(rejectedLoopClosure)
 				{
 					UWARN("Rejected loop closure %d -> %d: %s",
 							_loopClosureHypothesis.first, signature->id(), info.rejectedMsg.c_str());
 				}
 				else if(_maxLoopClosureDistance>0.0f && transform.getNorm() > _maxLoopClosureDistance)
 				{
-					rejectedGlobalLoopClosure = true;
+					rejectedLoopClosure = true;
 					UWARN("Rejected localization %d -> %d because distance to map (%fm) is over %s=%fm.",
 							_loopClosureHypothesis.first, signature->id(), transform.getNorm(), Parameters::kRGBDMaxLoopClosureDistance().c_str(), _maxLoopClosureDistance);
 				}
@@ -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));
-				if(!rejectedGlobalLoopClosure)
+				rejectedLoopClosure = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, information));
+				if(!rejectedLoopClosure)
 				{
 					loopClosureLinksAdded.push_back(std::make_pair(signature->id(), _loopClosureHypothesis.first));
 				}
 			}
 
-			if(rejectedGlobalLoopClosure)
+			if(rejectedLoopClosure)
 			{
 				_loopClosureHypothesis.first = 0;
 			}
@@ -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;
-				rejectedLandmark = true;
+				rejectedLoopClosure = true;
 			}
 		}
 		else
@@ -3804,8 +3802,7 @@ bool Rtabmap::process(
 				updateConstraints = false;
 				_loopClosureHypothesis.first = 0;
 				lastProximitySpaceClosureId = 0;
-				rejectedGlobalLoopClosure = true;
-				rejectedLandmark = true;
+				rejectedLoopClosure = true;
 			}
 			else if(_memory->isIncremental() &&
 			  loopClosureLinksAdded.size() &&
@@ -3915,8 +3912,7 @@ bool Rtabmap::process(
 					updateConstraints = false;
 					_loopClosureHypothesis.first = 0;
 					lastProximitySpaceClosureId = 0;
-					rejectedGlobalLoopClosure = true;
-					rejectedLandmark = true;
+					rejectedLoopClosure = 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/camera/CameraDepthAI.cpp

@@ -191,11 +191,6 @@ void CameraDepthAI::setDetectFeatures(int detectFeatures, const std::string & bl
 #ifdef RTABMAP_DEPTHAI
 	detectFeatures_ = detectFeatures;
 	blobPath_ = blobPath;
-	if(detectFeatures_ && outputMode_==2)
-	{
-		UWARN("On-device feature detectors cannot be enabled on color camera input, disabling on-device feature detector...");
-		detectFeatures_ = 0;
-	}
 	if(detectFeatures_>=2 && blobPath_.empty())
 	{
 		UWARN("Missing MyriadX blob file, disabling on-device feature detector");
@@ -475,9 +470,10 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 	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)
 	{
-		auto rgbCamera = pipeline.create<dai::node::Camera>();
+		rgbCamera = pipeline.create<dai::node::Camera>();
 		rgbCamera->setCamera("color");
 		if(boardName == "BC2087")
 			rgbCamera->setSize(1920, 1200);
@@ -485,7 +481,6 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 			rgbCamera->setSize(1280, 720);
 		else
 			rgbCamera->setSize(1920, 1080);
-		rgbCamera->setSize(1920, 1200);
 		rgbCamera->setVideoSize(targetSize_.width, targetSize_.height);
 		rgbCamera->setPreviewSize(targetSize_.width, targetSize_.height);
 		rgbCamera->setFps(this->getImageRate());
@@ -608,7 +603,9 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 		cfg.featureMaintainer.minimumDistanceBetweenFeatures = minDistance_ * minDistance_;
 		gfttDetector->initialConfig.set(cfg);
 
-		if(imagesRectified_)
+		if(outputMode_ == 2)
+			rgbCamera->video.link(gfttDetector->inputImage);
+		else if(imagesRectified_)
 			stereoDepth->rectifiedLeft.link(gfttDetector->inputImage);
 		else
 			stereoDepth->syncedLeft.link(gfttDetector->inputImage);
@@ -620,6 +617,7 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 		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_);
@@ -627,7 +625,9 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 		neuralNetwork->setNumNCEPerInferenceThread(1);
 		neuralNetwork->input.setBlocking(false);
 
-		if(imagesRectified_)
+		if(outputMode_ == 2)
+			rgbCamera->video.link(imageManip->inputImage);
+		else if(imagesRectified_)
 			stereoDepth->rectifiedLeft.link(imageManip->inputImage);
 		else
 			stereoDepth->syncedLeft.link(imageManip->inputImage);
@@ -701,6 +701,7 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 		});
 	}
 
+	this->setImageRate(0);
 	uSleep(2000); // avoid bad frames on start
 
 	return true;
@@ -842,6 +843,8 @@ SensorData CameraDepthAI::captureImage(SensorCaptureInfo * info)
 
 		std::vector<cv::Point> kpts;
 		cv::findNonZero(scores > threshold_, kpts);
+
+		if(!kpts.empty()){
 			std::vector<cv::KeyPoint> keypoints;
 			for(auto& kpt : kpts)
 			{
@@ -870,6 +873,8 @@ SensorData CameraDepthAI::captureImage(SensorCaptureInfo * info)
 			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,8 +632,11 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 		if ("Stereo Module" == module_name)
 		{
 			sensors[1] = elem;
+			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/odometry/OdometryF2M.cpp

@@ -154,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())
 	{
@@ -497,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)
@@ -506,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);

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

@@ -306,6 +306,62 @@ Transform estimateMotion3DTo2D(
 	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,
+			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 transform;
 #ifndef RTABMAP_OPENGV
@@ -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;
+}
+
 }
 
 }

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);

guilib/include/rtabmap/gui/GraphViewer.h

@@ -74,7 +74,9 @@ public:
 			const std::map<int, GPS> & gpsValues);
 	void updateReferentialPosition(const Transform & t);
 	void updateMap(const cv::Mat & map8U, float resolution, float xMin, float yMin);
-	void updatePosterior(const std::map<int, float> & posterior, float fixedMax = 0.0f, int zValueOffset = 0);
+	// Use updateNodeColorByValue() instead with valueName="Posterior".
+	RTABMAP_DEPRECATED void updatePosterior(const std::map<int, float> & posterior, float fixedMax = 0.0f, int zValueOffset = 0);
+	void updateNodeColorByValue(const std::string & valueName, const std::map<int, float> & values, float fixedMax = 0.0f, bool invertedColorScale = false, int zValueOffset = 0);
 	void updateLocalPath(const std::vector<int> & localPath);
 	void setGlobalPath(const std::vector<std::pair<int, Transform> > & globalPath);
 	void setCurrentGoalID(int id, const Transform & pose = Transform());
@@ -82,9 +84,10 @@ public:
 	void highlightNode(int nodeId, int highlightIndex);
 	void clearGraph();
 	void clearMap();
-	void clearPosterior();
+	// Use clearNodeColorByValue() instead.
+	RTABMAP_DEPRECATED void clearPosterior();
+	void clearNodeColorByValue();
 	void clearAll();
-
 	void saveSettings(QSettings & settings, const QString & group = "") const;
 	void loadSettings(QSettings & settings, const QString & group = "");
 
@@ -238,6 +241,7 @@ private:
 	bool _mouseTracking;
 	ViewPlane _viewPlane;
 	bool _ensureFrameVisible;
+	QPoint _previousMousePos;
 };
 
 } /* namespace rtabmap */

guilib/src/CloudViewer.cpp

@@ -80,6 +80,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <vtkObjectFactory.h>
 #include <vtkQuad.h>
 #include <vtkWarpScalar.h>
+#include <vtkUnsignedCharArray.h>
 #include <opencv/vtkImageMatSource.h>
 
 #if VTK_MAJOR_VERSION >= 7
@@ -96,6 +97,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/global_map/OctoMap.h>
 #endif
 
+// For compatibility with new VTK generic data arrays.
+#ifdef vtkGenericDataArray_h
+#define InsertNextTupleValue InsertNextTypedTuple
+#endif
+
 namespace rtabmap {
 
 CloudViewer::CloudViewer(QWidget *parent, CloudViewerInteractorStyle * style) :
@@ -124,6 +130,7 @@ CloudViewer::CloudViewer(QWidget *parent, CloudViewerInteractorStyle * style) :
 		_aSetLighting(0),
 		_aSetFlatShading(0),
 		_aSetEdgeVisibility(0),
+		_aSetScalarVisibility(0),
 		_aBackfaceCulling(0),
 		_menu(0),
 		_trajectory(new pcl::PointCloud<pcl::PointXYZ>),
@@ -347,6 +354,9 @@ void CloudViewer::createMenu()
 	_aSetEdgeVisibility = new QAction("Show edges", this);
 	_aSetEdgeVisibility->setCheckable(true);
 	_aSetEdgeVisibility->setChecked(false);
+	_aSetScalarVisibility = new QAction("Show vertex colors", this);
+	_aSetScalarVisibility->setCheckable(true);
+	_aSetScalarVisibility->setChecked(true);
 	_aBackfaceCulling = new QAction("Backface culling", this);
 	_aBackfaceCulling->setCheckable(true);
 	_aBackfaceCulling->setChecked(true);
@@ -408,6 +418,7 @@ void CloudViewer::createMenu()
 	_menu->addAction(_aSetLighting);
 	_menu->addAction(_aSetFlatShading);
 	_menu->addAction(_aSetEdgeVisibility);
+	_menu->addAction(_aSetScalarVisibility);
 	_menu->addAction(_aBackfaceCulling);
 	_menu->addAction(_aPolygonPicking);
 }
@@ -1408,10 +1419,20 @@ bool CloudViewer::addTextureMesh (
   // Create points from mesh.cloud
   vtkSmartPointer<vtkPoints> poly_points = vtkSmartPointer<vtkPoints>::New ();
   vtkSmartPointer<vtkUnsignedCharArray> colors = vtkSmartPointer<vtkUnsignedCharArray>::New ();
-  bool has_color = false;
+  colors->SetNumberOfComponents(3);
+  colors->SetName ("Colors");
+  bool hasColors = false;
+  for(unsigned int i=0; i<mesh.cloud.fields.size(); ++i)
+  {
+	if(mesh.cloud.fields[i].name.compare("rgb") == 0)
+	{
+		hasColors = true;
+		break;
+	}
+  }
   vtkSmartPointer<vtkMatrix4x4> transformation = vtkSmartPointer<vtkMatrix4x4>::New ();
   
-    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ> ());
+    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB> ());
     pcl::fromPCLPointCloud2 (mesh.cloud, *cloud);
     // no points --> exit
     if (cloud->points.size () == 0)
@@ -1423,8 +1444,17 @@ bool CloudViewer::addTextureMesh (
     poly_points->SetNumberOfPoints (cloud->points.size ());
     for (std::size_t i = 0; i < cloud->points.size (); ++i)
     {
-      const pcl::PointXYZ &p = cloud->points[i];
+      const pcl::PointXYZRGB &p = cloud->points[i];
       poly_points->InsertPoint (i, p.x, p.y, p.z);
+
+	  if(hasColors) {
+	  	unsigned char color[3] = {p.r, p.g, p.b}; 
+#if VTK_MAJOR_VERSION > 7 || (VTK_MAJOR_VERSION==7 && VTK_MINOR_VERSION >= 1)
+	  	colors->InsertNextTypedTuple(color);
+#else
+	  	colors->InsertNextTupleValue(color);
+#endif
+	  }
     }
 
   //create polys from polyMesh.tex_polygons
@@ -1443,8 +1473,9 @@ bool CloudViewer::addTextureMesh (
   vtkSmartPointer<vtkPolyData> polydata = vtkSmartPointer<vtkPolyData>::New();
   polydata->SetPolys (polys);
   polydata->SetPoints (poly_points);
-  if (has_color)
+  if (hasColors) {
     polydata->GetPointData()->SetScalars(colors);
+  }
 
   vtkSmartPointer<vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New ();
 #if VTK_MAJOR_VERSION < 6
@@ -1524,6 +1555,7 @@ bool CloudViewer::addTextureMesh (
   actor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked());
   actor->GetProperty()->SetBackfaceCulling(_aBackfaceCulling->isChecked());
   actor->GetProperty()->SetFrontfaceCulling(_frontfaceCulling);
+  actor->GetMapper()->SetScalarVisibility(_aSetScalarVisibility->isChecked());
   return true;
 }
 
@@ -2892,6 +2924,21 @@ void CloudViewer::setEdgeVisibility(bool visible)
 	this->refreshView();
 }
 
+void CloudViewer::setScalarVisibility(bool visible)
+{
+	_aSetScalarVisibility->setChecked(visible);
+	pcl::visualization::ShapeActorMapPtr shapeActorMap = _visualizer->getShapeActorMap();
+	for(pcl::visualization::ShapeActorMap::iterator iter=shapeActorMap->begin(); iter!=shapeActorMap->end(); ++iter)
+	{
+		vtkActor* actor = vtkActor::SafeDownCast (iter->second);
+		if(actor && _addedClouds.contains(iter->first))
+		{
+			actor->GetMapper()->SetScalarVisibility(_aSetScalarVisibility->isChecked());
+		}
+	}
+	this->refreshView();
+}
+
 void CloudViewer::setInteractorLayer(int layer)
 {
 	_visualizer->getRendererCollection()->InitTraversal ();
@@ -3983,6 +4030,10 @@ void CloudViewer::handleAction(QAction * a)
 	{
 		this->setEdgeVisibility(_aSetEdgeVisibility->isChecked());
 	}
+	else if(a == _aSetScalarVisibility)
+	{
+		this->setScalarVisibility(_aSetScalarVisibility->isChecked());
+	}
 	else if(a == _aBackfaceCulling)
 	{
 		this->setBackfaceCulling(_aBackfaceCulling->isChecked(), _frontfaceCulling);

guilib/src/CloudViewerInteractorStyle.cpp

@@ -333,4 +333,9 @@ void CloudViewerInteractorStyle::OnLeftButtonDown()
 	PCLVisualizerInteractorStyle::OnLeftButtonDown();
 }
 
+void CloudViewerInteractorStyle::OnRightButtonDown()
+{
+	// ignore
+}
+
 } /* namespace rtabmap */

guilib/src/DatabaseViewer.cpp

@@ -1062,9 +1062,6 @@ bool DatabaseViewer::closeDatabase()
 				}
 				generatedLocalMaps_.clear();
 				localMaps_.clear();
-
-				// This will force rtabmap_ros to regenerate the global occupancy grid if there was one
-				dbDriver_->save2DMap(cv::Mat(), 0, 0, 0);
 			}
 
 			if(button != QMessageBox::Yes && button != QMessageBox::No)
@@ -2907,11 +2904,10 @@ void DatabaseViewer::editSaved2DMap()
 		return;
 	}
 
-	if(linksAdded_.size() || linksRefined_.size() || linksRemoved_.size() || generatedLocalMaps_.size())
+	if(linksAdded_.size() || linksRefined_.size() || linksRemoved_.size())
 	{
 		QMessageBox::warning(this, tr("Cannot edit 2D map"),
-				tr("The database has modified links and/or modified local "
-				   "occupancy grids, the 2D optimized map cannot be modified."));
+				tr("The database has modified links, the 2D optimized map cannot be modified."));
 		return;
 	}
 
@@ -3038,7 +3034,7 @@ void DatabaseViewer::editSaved2DMap()
 								{
 									if(x+j>=0 && x+j<map8S.cols &&
 									   y+k>=0 && y+k<map8S.rows &&
-									   map8S.at<unsigned char>(y+k,x+j) == 100)
+									   map8S.at<signed char>(y+k,x+j) == 100)
 									{
 										obstacleDetected = true;
 									}
@@ -3102,7 +3098,7 @@ void DatabaseViewer::editSaved2DMap()
 								{
 									if(x+j>=0 && x+j<map8S.cols &&
 									   y+k>=0 && y+k<map8S.rows &&
-									   map8S.at<unsigned char>(y+k,x+j) == 100)
+									   map8S.at<signed char>(y+k,x+j) == 100)
 									{
 										obstacleDetected = true;
 									}
@@ -3238,11 +3234,10 @@ void DatabaseViewer::import2DMap()
 		return;
 	}
 
-	if(linksAdded_.size() || linksRefined_.size() || linksRemoved_.size() || generatedLocalMaps_.size())
+	if(linksAdded_.size() || linksRefined_.size() || linksRemoved_.size())
 	{
 		QMessageBox::warning(this, tr("Cannot import 2D map"),
-				tr("The database has modified links and/or modified local "
-				   "occupancy grids, the 2D optimized map cannot be modified."));
+				tr("The database has modified links, the 2D optimized map cannot be modified."));
 		return;
 	}
 
@@ -3885,6 +3880,10 @@ void DatabaseViewer::regenerateLocalMaps()
 			}
 			else
 			{
+				if(modifiedLaserScans_.find(s.id())!=modifiedLaserScans_.end())
+				{
+					s.sensorData().setLaserScan(modifiedLaserScans_.at(s.id()));
+				}
 				localMapMaker.createLocalMap(s, ground, obstacles, empty, viewpoint);
 			}
 

guilib/src/ExportBundlerDialog.cpp

@@ -165,7 +165,7 @@ void ExportBundlerDialog::getPath()
 }
 
 void ExportBundlerDialog::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)
@@ -185,7 +185,6 @@ void ExportBundlerDialog::exportBundler(
 
 		std::map<int, cv::Point3f> points3DMap;
 		std::map<int, std::map<int, FeatureBA> > wordReferences;
-		std::map<int, Transform> newPoses = poses;
 		if(_ui->groupBox_export_points->isEnabled() && _ui->groupBox_export_points->isChecked())
 		{
 			std::map<int, Transform> posesOut;
@@ -197,7 +196,7 @@ void ExportBundlerDialog::exportBundler(
 			uInsert(parametersSBA, std::make_pair(Parameters::kg2oPixelVariance(), uNumber2Str(_ui->sba_variance->value())));
 			Optimizer * sba = Optimizer::create(sbaType, parametersSBA);
 			sba->getConnectedGraph(poses.begin()->first, poses, links, posesOut, linksOut);
-			newPoses = sba->optimizeBA(
+			poses = sba->optimizeBA(
 					posesOut.begin()->first,
 					posesOut,
 					linksOut,
@@ -207,7 +206,7 @@ void ExportBundlerDialog::exportBundler(
 					_ui->sba_rematchFeatures->isChecked());
 			delete sba;
 
-			if(newPoses.empty())
+			if(poses.empty())
 			{
 				QMessageBox::warning(this, tr("Exporting cameras..."), tr("SBA optimization failed! Cannot export with 3D points.").arg(path));
 				return;
@@ -231,7 +230,7 @@ void ExportBundlerDialog::exportBundler(
 				std::map<int, int> cameraIndexes;
 				int camIndex = 0;
 				std::map<int, QColor> colors;
-				for(std::map<int, Transform>::const_iterator iter=newPoses.begin(); iter!=newPoses.end(); ++iter)
+				for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
 				{
 					QMap<int, Signature>::const_iterator ster = signatures.find(iter->first);
 					if(ster!= signatures.end())
@@ -536,10 +535,10 @@ void ExportBundlerDialog::exportBundler(
 				QMessageBox::information(this,
 						tr("Exporting cameras in Bundler format..."),
 						tr("%1 cameras/images and %2 points exported to directory \"%3\".%4")
-						.arg(newPoses.size())
+						.arg(poses.size())
 						.arg(points3DMap.size())
 						.arg(path)
-						.arg(newPoses.size()>cameras.size()?tr(" %1/%2 cameras ignored for too fast motion and/or blur level.").arg(newPoses.size()-cameras.size()).arg(newPoses.size()):""));
+						.arg(poses.size()>cameras.size()?tr(" %1/%2 cameras ignored for too fast motion and/or blur level.").arg(poses.size()-cameras.size()).arg(poses.size()):""));
 			}
 			else
 			{

guilib/src/ExportCloudsDialog.cpp

@@ -240,6 +240,8 @@ ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) :
 	connect(_ui->doubleSpinBox_meshingTextureMaxAngle, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
 	connect(_ui->spinBox_mesh_minTextureClusterSize, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->lineEdit_meshingTextureRoiRatios, SIGNAL(textChanged(const QString &)), this, SIGNAL(configChanged()));
+	connect(_ui->checkBox_distanceToCamPolicy, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->comboBox_texturingColorPolicy, SIGNAL(currentIndexChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->checkBox_cameraFilter, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->checkBox_cameraFilter, SIGNAL(stateChanged(int)), this, SLOT(updateReconstructionFlavor()));
 	connect(_ui->doubleSpinBox_cameraFilterRadius, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
@@ -467,6 +469,7 @@ void ExportCloudsDialog::saveSettings(QSettings & settings, const QString & grou
 	settings.setValue("mesh_textureMinCluster", _ui->spinBox_mesh_minTextureClusterSize->value());
 	settings.setValue("mesh_textureRoiRatios", _ui->lineEdit_meshingTextureRoiRatios->text());
 	settings.setValue("mesh_textureDistanceToCamPolicy", _ui->checkBox_distanceToCamPolicy->isChecked());
+	settings.setValue("mesh_textureVertexColorPolicy", _ui->comboBox_texturingColorPolicy->currentIndex());
 	settings.setValue("mesh_textureCameraFiltering", _ui->checkBox_cameraFilter->isChecked());
 	settings.setValue("mesh_textureCameraFilteringRadius", _ui->doubleSpinBox_cameraFilterRadius->value());
 	settings.setValue("mesh_textureCameraFilteringAngle", _ui->doubleSpinBox_cameraFilterAngle->value());
@@ -651,6 +654,7 @@ void ExportCloudsDialog::loadSettings(QSettings & settings, const QString & grou
 	_ui->spinBox_mesh_minTextureClusterSize->setValue(settings.value("mesh_textureMinCluster", _ui->spinBox_mesh_minTextureClusterSize->value()).toDouble());
 	_ui->lineEdit_meshingTextureRoiRatios->setText(settings.value("mesh_textureRoiRatios", _ui->lineEdit_meshingTextureRoiRatios->text()).toString());
 	_ui->checkBox_distanceToCamPolicy->setChecked(settings.value("mesh_textureDistanceToCamPolicy", _ui->checkBox_distanceToCamPolicy->isChecked()).toBool());
+	_ui->comboBox_texturingColorPolicy->setCurrentIndex(settings.value("mesh_textureVertexColorPolicy", _ui->comboBox_texturingColorPolicy->currentIndex()).toInt());
 	_ui->checkBox_cameraFilter->setChecked(settings.value("mesh_textureCameraFiltering", _ui->checkBox_cameraFilter->isChecked()).toBool());
 	_ui->doubleSpinBox_cameraFilterRadius->setValue(settings.value("mesh_textureCameraFilteringRadius", _ui->doubleSpinBox_cameraFilterRadius->value()).toDouble());
 	_ui->doubleSpinBox_cameraFilterAngle->setValue(settings.value("mesh_textureCameraFilteringAngle", _ui->doubleSpinBox_cameraFilterAngle->value()).toDouble());
@@ -832,6 +836,7 @@ void ExportCloudsDialog::restoreDefaults()
 	_ui->spinBox_mesh_minTextureClusterSize->setValue(50);
 	_ui->lineEdit_meshingTextureRoiRatios->setText("0.0 0.0 0.0 0.0");
 	_ui->checkBox_distanceToCamPolicy->setChecked(false);
+	_ui->comboBox_texturingColorPolicy->setCurrentIndex(0);
 	_ui->checkBox_cameraFilter->setChecked(false);
 	_ui->doubleSpinBox_cameraFilterRadius->setValue(0);
 	_ui->doubleSpinBox_cameraFilterAngle->setValue(30);
@@ -1352,7 +1357,10 @@ void ExportCloudsDialog::viewClouds(
 							blendingDecimation,
 							_ui->spinBox_textureBrightnessContrastRatioLow->value(),
 							_ui->spinBox_textureBrightnessContrastRatioHigh->value(),
-							_ui->checkBox_exposureFusion->isEnabled() && _ui->checkBox_exposureFusion->isChecked());
+							_ui->checkBox_exposureFusion->isEnabled() && _ui->checkBox_exposureFusion->isChecked(),
+							0,
+							255,
+							_ui->comboBox_texturingColorPolicy->currentIndex() == 1);
 					if(globalTextures.rows == globalTextures.cols)
 					{
 						globalTexture = globalTextures;
@@ -1362,23 +1370,90 @@ void ExportCloudsDialog::viewClouds(
 				_progressDialog->appendText(tr("Viewing the mesh %1 (%2 polygons)...").arg(iter->first).arg(mesh->tex_polygons.size()?mesh->tex_polygons[0].size():0));
 				_progressDialog->incrementStep();
 
+				bool hasColors = false;
+				for(unsigned int i=0; i<mesh->cloud.fields.size(); ++i)
+				{
+					if(mesh->cloud.fields[i].name.compare("rgb") == 0)
+					{
+						hasColors = true;
+					}
+				}
+
 				// VTK issue:
 				//  tex_coordinates should be linked to points, not
 				//  polygon vertices. Points linked to multiple different TCoords (different textures) should
 				//  be duplicated.
+				if(hasColors)
+				{
 					for (unsigned int t = 0; t < mesh->tex_coordinates.size(); ++t)
 					{
 						if(mesh->tex_polygons[t].size())
 						{
 
-						pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud(new pcl::PointCloud<pcl::PointXYZ>);
+							pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr originalCloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 							pcl::fromPCLPointCloud2(mesh->cloud, *originalCloud);
 
 							// make a cloud with as many points than polygon vertices
 							unsigned int nPoints = mesh->tex_coordinates[t].size();
 							UASSERT(nPoints == mesh->tex_polygons[t].size()*mesh->tex_polygons[t][0].vertices.size()); // assuming polygon size is constant!
 
-						pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+							pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+							cloud->resize(nPoints);
+
+							unsigned int oi = 0;
+							size_t texCoordIndex = 0;
+							for (unsigned int i = 0; i < mesh->tex_polygons[t].size(); ++i)
+							{
+								pcl::Vertices & vertices = mesh->tex_polygons[t][i];
+								bool hasTexture = true;
+								for(unsigned int j=0; j<vertices.vertices.size(); ++j)
+								{
+									if(mesh->tex_coordinates[t][texCoordIndex][0] == -1 || mesh->tex_coordinates[t][texCoordIndex][1] == -1)
+									{
+										hasTexture = false;
+										break;
+									}
+								}
+								
+								for(unsigned int j=0; j<vertices.vertices.size(); ++j)
+								{
+									UASSERT(oi < cloud->size());
+									UASSERT_MSG((int)vertices.vertices[j] < (int)originalCloud->size(), uFormat("%d vs %d", vertices.vertices[j], (int)originalCloud->size()).c_str());
+									cloud->at(oi) = originalCloud->at(vertices.vertices[j]);
+									if(hasTexture && _ui->comboBox_texturingColorPolicy->currentIndex() == 1) {
+										cloud->at(oi).r = 255;
+										cloud->at(oi).g = 255;
+										cloud->at(oi).b = 255;
+									}
+
+									vertices.vertices[j] = oi; // new vertice index
+									++oi;
+								}
+								texCoordIndex+=vertices.vertices.size();
+							}
+							pcl::toPCLPointCloud2(*cloud, mesh->cloud);
+						}
+						else
+						{
+							UWARN("No polygons for texture %d of mesh %d?!", t, iter->first);
+						}
+					}
+				}
+				else
+				{
+					for (unsigned int t = 0; t < mesh->tex_coordinates.size(); ++t)
+					{
+						if(mesh->tex_polygons[t].size())
+						{
+
+							pcl::PointCloud<pcl::PointNormal>::Ptr originalCloud(new pcl::PointCloud<pcl::PointNormal>);
+							pcl::fromPCLPointCloud2(mesh->cloud, *originalCloud);
+
+							// make a cloud with as many points than polygon vertices
+							unsigned int nPoints = mesh->tex_coordinates[t].size();
+							UASSERT(nPoints == mesh->tex_polygons[t].size()*mesh->tex_polygons[t][0].vertices.size()); // assuming polygon size is constant!
+
+							pcl::PointCloud<pcl::PointNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointNormal>);
 							cloud->resize(nPoints);
 
 							unsigned int oi = 0;
@@ -1402,6 +1477,7 @@ void ExportCloudsDialog::viewClouds(
 							UWARN("No polygons for texture %d of mesh %d?!", t, iter->first);
 						}
 					}
+				}
 
 				if (globalTexture.empty())
 				{
@@ -2682,7 +2758,7 @@ bool ExportCloudsDialog::getExportedClouds(
 								_ui->spinBox_meshMaxPolygons->isEnabled()?_ui->spinBox_meshMaxPolygons->value():0,
 								iter->second,
 								(float)_ui->doubleSpinBox_transferColorRadius->value(),
-								!(_ui->checkBox_textureMapping->isEnabled() && _ui->checkBox_textureMapping->isChecked()),
+								!(_ui->checkBox_textureMapping->isEnabled() && _ui->checkBox_textureMapping->isChecked() && _ui->comboBox_texturingColorPolicy->currentIndex()==0),
 								_ui->checkBox_cleanMesh->isChecked(),
 								_ui->spinBox_mesh_minClusterSize->value(),
 								&texturingState);
@@ -3068,7 +3144,7 @@ bool ExportCloudsDialog::getExportedClouds(
 						_ui->spinBox_meshMaxPolygons->isEnabled()?_ui->spinBox_meshMaxPolygons->value():0,
 						vertices,
 						(float)_ui->doubleSpinBox_transferColorRadius->value(),
-						!(_ui->checkBox_textureMapping->isEnabled() && _ui->checkBox_textureMapping->isChecked()),
+						!(_ui->checkBox_textureMapping->isEnabled() && _ui->checkBox_textureMapping->isChecked() && _ui->comboBox_texturingColorPolicy->currentIndex()==0),
 						_ui->checkBox_cleanMesh->isChecked(),
 						_ui->spinBox_mesh_minClusterSize->value(),
 						&texturingState);
@@ -3140,7 +3216,7 @@ bool ExportCloudsDialog::getExportedClouds(
 						_ui->spinBox_meshMaxPolygons->isEnabled()?_ui->spinBox_meshMaxPolygons->value():0,
 						mergedClouds,
 						(float)_ui->doubleSpinBox_transferColorRadius->value(),
-						!(_ui->checkBox_textureMapping->isEnabled() && _ui->checkBox_textureMapping->isChecked()),
+						!(_ui->checkBox_textureMapping->isEnabled() && _ui->checkBox_textureMapping->isChecked() && _ui->comboBox_texturingColorPolicy->currentIndex()==0),
 						_ui->checkBox_cleanMesh->isChecked(),
 						_ui->spinBox_mesh_minClusterSize->value(),
 						&texturingState);
@@ -4041,11 +4117,15 @@ void ExportCloudsDialog::saveClouds(
 		std::list<std::string> pdalFormats = uSplit(getPDALSupportedWriters(), ' ');
 		for(std::list<std::string>::iterator iter=pdalFormats.begin(); iter!=pdalFormats.end(); ++iter)
 		{
-			if(iter->compare("ply") == 0 || iter->compare("pcd") == 0)
+			if(iter->compare("ply") == 0 || iter->compare("pcd") == 0 || iter->compare("laz") == 0)
 			{
 				continue;
 			}
 			extensions += QString(" *.") + iter->c_str();
+			if(iter->compare("las") == 0)
+			{
+				extensions += QString(" *.laz");
+			}
 		}
 		extensions += ")";
 #elif defined(RTABMAP_LIBLAS)
@@ -4243,13 +4323,19 @@ void ExportCloudsDialog::saveClouds(
 		std::list<std::string> pdalFormats = uSplit(getPDALSupportedWriters(), ' ');
 		for(std::list<std::string>::iterator iter=pdalFormats.begin(); iter!=pdalFormats.end(); ++iter)
 		{
-			if(iter->compare("ply") == 0 || iter->compare("pcd") == 0)
+			if(iter->compare("ply") == 0 || iter->compare("pcd") == 0 || iter->compare("laz") == 0)
 			{
 				continue;
 			}
 			extensions += QString(" *.") + iter->c_str();
 
 			items.push_back(iter->c_str());
+
+			if(iter->compare("las") == 0)
+			{
+				extensions += QString(" *.laz");
+				items.push_back("laz");
+			}
 		}
 		extensions += ")...";
 #elif defined(RTABMAP_LIBLAS)
@@ -4697,7 +4783,8 @@ void ExportCloudsDialog::saveTextureMeshes(
 							_ui->spinBox_textureBrightnessContrastRatioHigh->value(),
 							_ui->checkBox_exposureFusion->isEnabled() && _ui->checkBox_exposureFusion->isChecked(),
 							0,
-							0,
+							255,
+							_ui->comboBox_texturingColorPolicy->currentIndex() == 1,
 							&gains,
 							&blendingGains,
 							&contrastValues);
@@ -4939,7 +5026,10 @@ void ExportCloudsDialog::saveTextureMeshes(
 									blendingDecimation,
 									_ui->spinBox_textureBrightnessContrastRatioLow->value(),
 									_ui->spinBox_textureBrightnessContrastRatioHigh->value(),
-									_ui->checkBox_exposureFusion->isEnabled() && _ui->checkBox_exposureFusion->isChecked());
+									_ui->checkBox_exposureFusion->isEnabled() && _ui->checkBox_exposureFusion->isChecked(),
+									0,
+									255,
+									_ui->comboBox_texturingColorPolicy->currentIndex() == 1);
 						}
 						bool singleTexture = mesh->tex_materials.size() == 1;
 						if(!singleTexture)
@@ -5115,6 +5205,9 @@ void ExportCloudsDialog::saveTextureMeshes(
 }
 
 bool ExportCloudsDialog::saveOBJFile(const QString &path, pcl::TextureMesh::Ptr &mesh) const {
+
+	
+
 #if PCL_VERSION_COMPARE(>=, 1, 13, 0)
 	mesh->tex_coord_indices = std::vector<std::vector<pcl::Vertices>>();
 	auto nr_meshes = static_cast<unsigned>(mesh->tex_polygons.size());
@@ -5130,11 +5223,11 @@ void ExportCloudsDialog::saveTextureMeshes(
 		f_idx += static_cast<unsigned>(mesh->tex_polygons[m].size());
 	}
 #endif
-        return pcl::io::saveOBJFile(path.toStdString(), *mesh) == 0;
+	return util3d::saveOBJFile(path.toStdString(), *mesh) == 0;
 }
 
 bool ExportCloudsDialog::saveOBJFile(const QString &path, pcl::PolygonMesh &mesh) const {
-        return pcl::io::saveOBJFile(path.toStdString(), mesh) == 0;
+	return util3d::saveOBJFile(path.toStdString(), mesh) == 0;
 }
 
 

guilib/src/GraphViewer.cpp

@@ -92,7 +92,7 @@ public:
 	}
 	virtual ~NodeItem() {}
 
-	void setColor(const QColor & color)
+	void setColor(const QColor & color, const QString & valueName = QString(), float value = 0.0f)
 	{
 		QPen p = this->pen();
 		p.setColor(color);
@@ -104,6 +104,9 @@ public:
 		QPen pen = _line->pen();
 		pen.setColor(QColor(255-color.red(), 255-color.green(), 255-color.blue()));
 		_line->setPen(pen);
+
+		_valueName = valueName;
+		_value = value;
 	}
 
 	void setRadius(float radius)
@@ -144,14 +147,22 @@ public:
 protected:
 	virtual void hoverEnterEvent ( QGraphicsSceneHoverEvent * event )
 	{
+		QString msg;
 		if(_weight>=0)
 		{
-			this->setToolTip(QString("%1 [map=%2, w=%3] %4").arg(_id).arg(_mapId).arg(_weight).arg(_pose.prettyPrint().c_str()));
+			msg = QString("%1 [map=%2, w=%3]\n%4").arg(_id).arg(_mapId).arg(_weight).arg(_pose.prettyPrint().c_str());
 		}
 		else
 		{
-			this->setToolTip(QString("%1 [map=%2] %3").arg(_id).arg(_mapId).arg(_pose.prettyPrint().c_str()));
+			msg = QString("%1 [map=%2]\n%3").arg(_id).arg(_mapId).arg(_pose.prettyPrint().c_str());
 		}
+		if(!_valueName.isEmpty())
+		{
+			msg += QString("\n%1=%2").arg(_valueName).arg(_value);
+		}
+
+		this->setToolTip(msg);
+		
 		this->setScale(2);
 		QGraphicsEllipseItem::hoverEnterEvent(event);
 	}
@@ -168,6 +179,8 @@ private:
 	int _weight;
 	Transform _pose;
 	QGraphicsLineItem * _line;
+	QString _valueName;
+	float _value;
 };
 
 class NodeGPSItem: public NodeItem
@@ -463,8 +476,9 @@ GraphViewer::GraphViewer(QWidget * parent) :
 	_odomCacheOverlay->setBrush(QBrush(QColor(255, 255, 255, 150)));
 	_odomCacheOverlay->setPen(QPen(Qt::NoPen));
 
-	_highlightedNodes.push_back(QPair<QColor, NodeItem*>(Qt::magenta, nullptr));
+	// Match by default scan colors from DatabaseViewer
 	_highlightedNodes.push_back(QPair<QColor, NodeItem*>(Qt::yellow, nullptr));
+	_highlightedNodes.push_back(QPair<QColor, NodeItem*>(Qt::magenta, nullptr));
 
 	this->restoreDefaults();
 
@@ -1070,11 +1084,16 @@ void GraphViewer::updateMap(const cv::Mat & map8U, float resolution, float xMin,
 }
 
 void GraphViewer::updatePosterior(const std::map<int, float> & posterior, float max, int zValueOffset)
+{
+	updateNodeColorByValue("Posterior Prob", posterior, max, false, zValueOffset);
+}
+
+void GraphViewer::updateNodeColorByValue(const std::string & valueName, const std::map<int, float> & values, float max, bool invertedColorScale, int zValueOffset)
 {
 	//find max
 	if(max <= 0.0f)
 	{
-		for(std::map<int, float>::const_iterator iter = posterior.begin(); iter!=posterior.end(); ++iter)
+		for(std::map<int, float>::const_iterator iter = values.begin(); iter!=values.end(); ++iter)
 		{
 			if(iter->first > 0 && iter->second>max)
 			{
@@ -1086,11 +1105,11 @@ void GraphViewer::updatePosterior(const std::map<int, float> & posterior, float
 	{
 		for(QMap<int, NodeItem*>::iterator iter = _nodeItems.begin(); iter!=_nodeItems.end(); ++iter)
 		{
-			std::map<int,float>::const_iterator jter = posterior.find(iter.key());
-			if(jter != posterior.end())
+			std::map<int,float>::const_iterator jter = values.find(iter.key());
+			if(jter != values.end())
 			{
 				float v = jter->second>max?max:jter->second;
-				iter.value()->setColor(QColor::fromHsvF((1-v/max)*240.0f/360.0f, 1, 1, 1)); //0=red 240=blue
+				iter.value()->setColor(QColor::fromHsvF(( invertedColorScale ? v/max : 1-v/max )*240.0f/360.0f, 1, 1, 1), valueName.c_str(), jter->second); //0=red 240=blue
 				iter.value()->setZValue(iter.value()->zValue()+zValueOffset);
 			}
 		}
@@ -1285,6 +1304,11 @@ void GraphViewer::clearMap()
 }
 
 void GraphViewer::clearPosterior()
+{
+	clearNodeColorByValue();
+}
+
+void GraphViewer::clearNodeColorByValue()
 {
 	for(QMap<int, NodeItem*>::iterator iter = _nodeItems.begin(); iter!=_nodeItems.end(); ++iter)
 	{
@@ -1832,8 +1856,8 @@ void GraphViewer::restoreDefaults()
 	setNodeColor(Qt::blue);
 	setNodeOdomCacheColor(Qt::darkGreen);
 	setCurrentGoalColor(Qt::darkMagenta);
-	setHighlightColor(Qt::magenta, 0);
-	setHighlightColor(Qt::yellow, 1);
+	setHighlightColor(Qt::yellow, 0);
+	setHighlightColor(Qt::magenta, 1);
 	setNeighborColor(Qt::blue);
 	setGlobalLoopClosureColor(Qt::red);
 	setLocalLoopClosureColor(Qt::yellow);
@@ -1883,6 +1907,22 @@ void GraphViewer::mouseMoveEvent(QMouseEvent * event)
 	{
 		QToolTip::hideText();
 	}
+	if (event->modifiers() & (Qt::ShiftModifier | Qt::ControlModifier) && event->buttons() & Qt::LeftButton) {
+		// same modifiers than 3D view, change zoom
+		if(_previousMousePos.y()!=0) {
+			if(event->pos().y() - _previousMousePos.y() > 0)
+			{
+				this->scale(0.98, 0.98);
+			}
+			else
+			{
+				this->scale(1.02, 1.02);
+			}
+		}
+		_previousMousePos = event->pos();
+		return;
+	}
+	_previousMousePos.setY(0);
 	QGraphicsView::mouseMoveEvent(event);
 }
 

guilib/src/MainWindow.cpp

@@ -1629,6 +1629,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 		_cloudViewer->updateCameraTargetPosition(_odometryCorrection*odom.pose());
 		UDEBUG("Time Update Pose: %fs", time.ticks());
 	}
+
 	_cloudViewer->refreshView();
 
 	if(_ui->graphicsView_graphView->isVisible())
@@ -1798,6 +1799,22 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 		_ui->statsToolBox->updateStat("Odometry/InliersMeanDistance/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.inliersMeanDistance, _preferencesDialog->isCacheSavedInFigures());
 		_ui->statsToolBox->updateStat("Odometry/InliersDistribution/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.inliersDistribution, _preferencesDialog->isCacheSavedInFigures());
 		_ui->statsToolBox->updateStat("Odometry/InliersRatio/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), odom.info().features<=0?0.0f:float(odom.info().reg.inliers)/float(odom.info().features), _preferencesDialog->isCacheSavedInFigures());
+		for(size_t i=0; i<odom.info().reg.matchesPerCam.size(); ++i)
+		{
+			_ui->statsToolBox->updateStat(QString("Odometry/matchesCam%1/").arg(i), _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.matchesPerCam[i], _preferencesDialog->isCacheSavedInFigures());
+		}
+		for(size_t i=0; i<odom.info().reg.inliersPerCam.size(); ++i)
+		{
+			_ui->statsToolBox->updateStat(QString("Odometry/inliersCam%1/").arg(i), _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.inliersPerCam[i], _preferencesDialog->isCacheSavedInFigures());
+		}
+		if(odom.info().reg.matchesPerCam.size() == odom.info().reg.inliersPerCam.size())
+		{
+			for(size_t i=0; i<odom.info().reg.matchesPerCam.size(); ++i)
+			{
+				_ui->statsToolBox->updateStat(QString("Odometry/inliersRatioCam%1/").arg(i), _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), odom.info().reg.matchesPerCam[i]>0 ? (float)odom.info().reg.inliersPerCam[i] / (float)odom.info().reg.matchesPerCam[i] : 0.0f, _preferencesDialog->isCacheSavedInFigures());
+			}
+		}
+		
 		_ui->statsToolBox->updateStat("Odometry/ICPInliersRatio/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.icpInliersRatio, _preferencesDialog->isCacheSavedInFigures());
 		_ui->statsToolBox->updateStat("Odometry/ICPRotation/rad", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.icpRotation, _preferencesDialog->isCacheSavedInFigures());
 		_ui->statsToolBox->updateStat("Odometry/ICPTranslation/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.icpTranslation, _preferencesDialog->isCacheSavedInFigures());
@@ -1833,6 +1850,10 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 			_ui->statsToolBox->updateStat("Odometry/localBundleTime/ms", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().localBundleTime*1000.0f, _preferencesDialog->isCacheSavedInFigures());
 			_ui->statsToolBox->updateStat("Odometry/localBundleAvgInlierDistance/pix", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().localBundleAvgInlierDistance, _preferencesDialog->isCacheSavedInFigures());
 			_ui->statsToolBox->updateStat("Odometry/localBundleMaxKeyFramesForInlier/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().localBundleMaxKeyFramesForInlier, _preferencesDialog->isCacheSavedInFigures());
+			for(size_t i=0; i<odom.info().localBundleOutliersPerCam.size(); ++i)
+			{
+				_ui->statsToolBox->updateStat(QString("Odometry/localBundleOutliersCam%1/").arg(i), _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().localBundleOutliersPerCam[i], _preferencesDialog->isCacheSavedInFigures());
+			}
 		}
 		_ui->statsToolBox->updateStat("Odometry/KeyFrameAdded/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().keyFrameAdded?1.0f:0.0f, _preferencesDialog->isCacheSavedInFigures());
 		_ui->statsToolBox->updateStat("Odometry/ID/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)data->id(), _preferencesDialog->isCacheSavedInFigures());
@@ -2599,19 +2620,19 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 			if(_preferencesDialog->isPosteriorGraphView() &&
 			   stat.posterior().size())
 			{
-				_ui->graphicsView_graphView->updatePosterior(stat.posterior());
+				_ui->graphicsView_graphView->updateNodeColorByValue("Posterior", stat.posterior());
 			}
 			else if(_preferencesDialog->isRGBDMode())
 			{
 				if(_preferencesDialog->isWordsCountGraphView() &&
 						_cachedWordsCount.size())
 				{
-					_ui->graphicsView_graphView->updatePosterior(_cachedWordsCount, (float)_preferencesDialog->getKpMaxFeatures());
+					_ui->graphicsView_graphView->updateNodeColorByValue("Visual Words", _cachedWordsCount, (float)_preferencesDialog->getKpMaxFeatures());
 				}
 				else if(_preferencesDialog->isLocalizationsCountGraphView() &&
 						_cachedLocalizationsCount.size())
 				{
-					_ui->graphicsView_graphView->updatePosterior(_cachedLocalizationsCount, 1.0f);
+					_ui->graphicsView_graphView->updateNodeColorByValue("Re-Localization Count", _cachedLocalizationsCount, 1.0f);
 				}
 			}
 			if(_preferencesDialog->isRelocalizationColorOdomCacheGraphView() && !stat.odomCachePoses().empty())
@@ -2632,7 +2653,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 						colors.insert(std::make_pair(iter->first, 240)); //red
 					}
 				}
-				_ui->graphicsView_graphView->updatePosterior(colors, 240);
+				_ui->graphicsView_graphView->updateNodeColorByValue("Re-Localized", colors, 240);
 			}
 			// update local path on the graph view
 			_ui->graphicsView_graphView->updateLocalPath(stat.localPath());
@@ -4750,7 +4771,7 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
 				_preferencesDialog->isRGBDMode()&&
 				_cachedWordsCount.size())
 			{
-				_ui->graphicsView_graphView->updatePosterior(_cachedWordsCount, (float)_preferencesDialog->getKpMaxFeatures());
+				_ui->graphicsView_graphView->updateNodeColorByValue("Visual Words", _cachedWordsCount, (float)_preferencesDialog->getKpMaxFeatures());
 			}
 
 			_progressDialog->appendText("Updating the 3D map cloud... done.");
@@ -4901,7 +4922,7 @@ void MainWindow::applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags)
 		}
 		if(!_preferencesDialog->isPosteriorGraphView() && _ui->graphicsView_graphView->isVisible())
 		{
-			_ui->graphicsView_graphView->clearPosterior();
+			_ui->graphicsView_graphView->clearNodeColorByValue();
 		}
 	}
 
@@ -8320,6 +8341,21 @@ void MainWindow::exportBundlerFormat()
 					_currentLinksMap,
 					_cachedSignatures,
 					_preferencesDialog->getAllParameters());
+		
+		if(!poses.empty())
+		{
+			UINFO("Updating map...");
+			this->updateMapCloud(
+					poses,
+					std::multimap<int, Link>(_currentLinksMap),
+					std::map<int, int>(_currentMapIds),
+					std::map<int, std::string>(_currentLabels),
+					std::map<int, Transform>(_currentGTPosesMap),
+					std::map<int, Transform>(),
+					std::multimap<int, Link>(),
+					false);
+			UINFO("Updating map... done!");
+		}
 	}
 	else
 	{

guilib/src/PreferencesDialog.cpp

@@ -1272,7 +1272,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->loopClosure_estimationType->setObjectName(Parameters::kVisEstimationType().c_str());
 	connect(_ui->loopClosure_estimationType, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_loopClosureEstimation, SLOT(setCurrentIndex(int)));
 	_ui->stackedWidget_loopClosureEstimation->setCurrentIndex(Parameters::defaultVisEstimationType());
-	_ui->loopClosure_forwardEst->setObjectName(Parameters::kVisForwardEstOnly().c_str());
 	_ui->loopClosure_bowEpipolarGeometryVar->setObjectName(Parameters::kVisEpipolarGeometryVar().c_str());
 	_ui->loopClosure_pnpReprojError->setObjectName(Parameters::kVisPnPReprojError().c_str());
 	_ui->loopClosure_pnpFlags->setObjectName(Parameters::kVisPnPFlags().c_str());
@@ -1669,6 +1668,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->ArucoMaxDepthError->setObjectName(Parameters::kMarkerMaxDepthError().c_str());
 	_ui->ArucoVarianceLinear->setObjectName(Parameters::kMarkerVarianceLinear().c_str());
 	_ui->ArucoVarianceAngular->setObjectName(Parameters::kMarkerVarianceAngular().c_str());
+	_ui->ArucoVarianceOrientationIgnored->setObjectName(Parameters::kMarkerVarianceOrientationIgnored().c_str());
 	_ui->ArucoMarkerRangeMin->setObjectName(Parameters::kMarkerMinRange().c_str());
 	_ui->ArucoMarkerRangeMax->setObjectName(Parameters::kMarkerMaxRange().c_str());
 	_ui->ArucoMarkerPriors->setObjectName(Parameters::kMarkerPriors().c_str());
@@ -4809,8 +4809,6 @@ void PreferencesDialog::setParameter(const std::string & key, const std::string
 	QWidget * obj = _ui->stackedWidget->findChild<QWidget*>(key.c_str());
 	if(obj)
 	{
-		uInsert(_parameters, ParametersPair(key, value));
-
 		QSpinBox * spin = qobject_cast<QSpinBox *>(obj);
 		QDoubleSpinBox * doubleSpin = qobject_cast<QDoubleSpinBox *>(obj);
 		QComboBox * combo = qobject_cast<QComboBox *>(obj);
@@ -4821,18 +4819,28 @@ void PreferencesDialog::setParameter(const std::string & key, const std::string
 		bool ok;
 		if(spin)
 		{
-			spin->setValue(QString(value.c_str()).toInt(&ok));
+			int v = QString(value.c_str()).toInt(&ok);
 			if(!ok)
 			{
-				UERROR("Conversion failed from \"%s\" for parameter %s", value.c_str(), key.c_str());
+				UERROR("Conversion failed from \"%s\" for parameter %s. Original value (%d) is kept.", value.c_str(), key.c_str(), spin->value());
+			}
+			else
+			{
+				spin->setValue(v);
+				uInsert(_parameters, ParametersPair(key, value));
 			}
 		}
 		else if(doubleSpin)
 		{
-			doubleSpin->setValue(QString(value.c_str()).toDouble(&ok));
+			double v = QString(value.c_str()).toDouble(&ok);
 			if(!ok)
 			{
-				UERROR("Conversion failed from \"%s\" for parameter %s", value.c_str(), key.c_str());
+				UERROR("Conversion failed from \"%s\" for parameter %s. Original value (%f) is kept.", value.c_str(), key.c_str(), doubleSpin->value());
+			}
+			else
+			{
+				doubleSpin->setValue(v);
+				uInsert(_parameters, ParametersPair(key, value));
 			}
 		}
 		else if(combo)
@@ -4855,7 +4863,7 @@ void PreferencesDialog::setParameter(const std::string & key, const std::string
 			int valueInt = QString(valueCpy.c_str()).toInt(&ok);
 			if(!ok)
 			{
-				UERROR("Conversion failed from \"%s\" for parameter %s", valueCpy.c_str(), key.c_str());
+				UERROR("Conversion failed from \"%s\" for parameter %s. Original value (%d) is kept.", valueCpy.c_str(), key.c_str(), combo->currentIndex());
 			}
 			else
 			{
@@ -4934,6 +4942,7 @@ void PreferencesDialog::setParameter(const std::string & key, const std::string
 				if(ok)
 				{
 					combo->setCurrentIndex(valueInt);
+					uInsert(_parameters, ParametersPair(key, uNumber2Str(valueInt)));
 				}
 			}
 
@@ -4943,18 +4952,22 @@ void PreferencesDialog::setParameter(const std::string & key, const std::string
 			_ui->checkBox_useOdomFeatures->blockSignals(true);
 			check->setChecked(uStr2Bool(value.c_str()));
 			_ui->checkBox_useOdomFeatures->blockSignals(false);
+			uInsert(_parameters, ParametersPair(key, uBool2Str(check->isChecked())));
 		}
 		else if(radio)
 		{
 			radio->setChecked(uStr2Bool(value.c_str()));
+			uInsert(_parameters, ParametersPair(key, uBool2Str(radio->isChecked())));
 		}
 		else if(lineEdit)
 		{
 			lineEdit->setText(value.c_str());
+			uInsert(_parameters, ParametersPair(key, value));
 		}
 		else if(groupBox)
 		{
 			groupBox->setChecked(uStr2Bool(value.c_str()));
+			uInsert(_parameters, ParametersPair(key, uBool2Str(groupBox->isChecked())));
 		}
 		else
 		{
@@ -7143,7 +7156,11 @@ Lidar * PreferencesDialog::createLidar()
 			// Connect to sensor
 
 			lidar = new LidarVLP16(
+#if BOOST_VERSION >= 108700  // Version 1.87.0
+					boost::asio::ip::make_address(uFormat("%ld.%ld.%ld.%ld",
+#else
 					boost::asio::ip::address_v4::from_string(uFormat("%ld.%ld.%ld.%ld",
+#endif
 							(size_t)_ui->spinBox_vlp16_ip1->value(),
 							(size_t)_ui->spinBox_vlp16_ip2->value(),
 							(size_t)_ui->spinBox_vlp16_ip3->value(),

guilib/src/ui/exportCloudsDialog.ui

@@ -23,9 +23,9 @@
       <property name="geometry">
        <rect>
         <x>0</x>
-        <y>-2995</y>
+        <y>-3199</y>
         <width>885</width>
-        <height>6152</height>
+        <height>6183</height>
        </rect>
       </property>
       <layout class="QVBoxLayout" name="verticalLayout_13">
@@ -2438,20 +2438,106 @@ By Node ID and Camera Index: NodeID*10+CameraIndex</string>
             <layout class="QVBoxLayout" name="verticalLayout_16">
              <item>
               <layout class="QGridLayout" name="gridLayout_15" columnstretch="0,1">
-               <item row="13" column="0">
-                <widget class="QSpinBox" name="spinBox_textureBrightnessContrastRatioHigh">
+               <item row="11" column="0">
+                <widget class="QCheckBox" name="checkBox_blending">
+                 <property name="text">
+                  <string/>
+                 </property>
+                </widget>
+               </item>
+               <item row="10" column="1">
+                <widget class="QLabel" name="label_meshingTextureSize_3">
+                 <property name="text">
+                  <string>Camera filtering. Some criteria to select which cameras are used for texturing.</string>
+                 </property>
+                 <property name="wordWrap">
+                  <bool>true</bool>
+                 </property>
+                </widget>
+               </item>
+               <item row="4" column="0">
+                <widget class="QDoubleSpinBox" name="doubleSpinBox_meshingTextureMaxDistance">
                  <property name="suffix">
-                  <string> %</string>
+                  <string> m</string>
+                 </property>
+                 <property name="decimals">
+                  <number>1</number>
                  </property>
                  <property name="minimum">
-                  <number>0</number>
+                  <double>0.000000000000000</double>
                  </property>
                  <property name="maximum">
-                  <number>49</number>
+                  <double>99.000000000000000</double>
+                 </property>
+                 <property name="singleStep">
+                  <double>1.000000000000000</double>
+                 </property>
+                 <property name="value">
+                  <double>3.000000000000000</double>
+                 </property>
+                </widget>
+               </item>
+               <item row="0" column="1">
+                <widget class="QLabel" name="label_meshingTextureFormat">
+                 <property name="text">
+                  <string>Texture format.</string>
+                 </property>
+                 <property name="wordWrap">
+                  <bool>true</bool>
+                 </property>
+                </widget>
+               </item>
+               <item row="6" column="1">
+                <widget class="QLabel" name="label_meshingTextureSize_10">
+                 <property name="text">
+                  <string>Maximum angle between camera and face to apply texture (0 means disabled).</string>
+                 </property>
+                 <property name="wordWrap">
+                  <bool>true</bool>
+                 </property>
+                </widget>
+               </item>
+               <item row="16" column="1">
+                <widget class="QLabel" name="label_exposureFusion">
+                 <property name="text">
+                  <string>Exposure fusion. Only used when textures are merged and brightness/contrast balance ratios are used (merging original texture + 1 for each ratios).</string>
+                 </property>
+                 <property name="wordWrap">
+                  <bool>true</bool>
                  </property>
                 </widget>
                </item>
                <item row="13" column="1">
+                <widget class="QLabel" name="label_meshingTextureSize_5">
+                 <property name="text">
+                  <string>Brightness and contrast balance low ratio. Only used when textures are merged.</string>
+                 </property>
+                 <property name="wordWrap">
+                  <bool>true</bool>
+                 </property>
+                </widget>
+               </item>
+               <item row="4" column="1">
+                <widget class="QLabel" name="label_meshingTextureSize_2">
+                 <property name="text">
+                  <string>Maximum distance from the camera for polygons to be textured by this camera (0 means infinite). Only used with dense reconstruction flavor.</string>
+                 </property>
+                 <property name="wordWrap">
+                  <bool>true</bool>
+                 </property>
+                </widget>
+               </item>
+               <item row="8" column="1">
+                <widget class="QLabel" name="label_meshingTextureSize_11">
+                 <property name="text">
+                  <string>Distance to camera policy. The closest camera from a polygon is used to texture the polygon. If disabled, the camera for which the polygon projection is the closest of the image center is used to texture the polygon.</string>
+                 </property>
+                 <property name="wordWrap">
+                  <bool>true</bool>
+                 </property>
+                </widget>
+               </item>
+               <item row="14" column="1">
                 <widget class="QLabel" name="label_meshingTextureSize_6">
                  <property name="text">
                   <string>Brightness and contrast balance high ratio. Only used when textures are merged.</string>
@@ -2461,7 +2547,17 @@ By Node ID and Camera Index: NodeID*10+CameraIndex</string>
                  </property>
                 </widget>
                </item>
-               <item row="11" column="0">
+               <item row="2" column="0">
+                <widget class="QSpinBox" name="spinBox_mesh_maxTextures">
+                 <property name="minimum">
+                  <number>1</number>
+                 </property>
+                 <property name="maximum">
+                  <number>20</number>
+                 </property>
+                </widget>
+               </item>
+               <item row="12" column="0">
                 <widget class="QComboBox" name="comboBox_blendingDecimation">
                  <item>
                   <property name="text">
@@ -2505,23 +2601,93 @@ By Node ID and Camera Index: NodeID*10+CameraIndex</string>
                  </item>
                 </widget>
                </item>
-               <item row="10" column="0">
-                <widget class="QCheckBox" name="checkBox_blending">
+               <item row="7" column="1">
+                <widget class="QLabel" name="label_meshingTextureSize_4">
                  <property name="text">
-                  <string/>
+                  <string>Minimum polygon cluster size for texturing. This removes textures applied on sparse polygons. Only used with dense reconstruction flavor.</string>
+                 </property>
+                 <property name="wordWrap">
+                  <bool>true</bool>
+                 </property>
+                </widget>
+               </item>
+               <item row="14" column="0">
+                <widget class="QSpinBox" name="spinBox_textureBrightnessContrastRatioHigh">
+                 <property name="suffix">
+                  <string> %</string>
+                 </property>
+                 <property name="minimum">
+                  <number>0</number>
+                 </property>
+                 <property name="maximum">
+                  <number>49</number>
                  </property>
                 </widget>
                </item>
                <item row="11" column="1">
-                <widget class="QLabel" name="label_exposureFusion_3">
+                <widget class="QLabel" name="label_exposureFusion_2">
                  <property name="text">
-                  <string>Blending decimation. </string>
+                  <string>Blending. Only used with dense reconstruction flavor and if clouds are assembled.</string>
                  </property>
                  <property name="wordWrap">
                   <bool>true</bool>
                  </property>
                 </widget>
                </item>
+               <item row="17" column="1">
+                <widget class="QLabel" name="label_multiband">
+                 <property name="text">
+                  <string>MultiBand texturing. Only available on export and RTAB-Map should be built with AliceVision support.</string>
+                 </property>
+                 <property name="wordWrap">
+                  <bool>true</bool>
+                 </property>
+                </widget>
+               </item>
+               <item row="6" column="0">
+                <widget class="QDoubleSpinBox" name="doubleSpinBox_meshingTextureMaxAngle">
+                 <property name="suffix">
+                  <string> deg</string>
+                 </property>
+                 <property name="decimals">
+                  <number>0</number>
+                 </property>
+                 <property name="minimum">
+                  <double>0.000000000000000</double>
+                 </property>
+                 <property name="maximum">
+                  <double>180.000000000000000</double>
+                 </property>
+                 <property name="singleStep">
+                  <double>1.000000000000000</double>
+                 </property>
+                 <property name="value">
+                  <double>0.000000000000000</double>
+                 </property>
+                </widget>
+               </item>
+               <item row="5" column="0">
+                <widget class="QDoubleSpinBox" name="doubleSpinBox_meshingTextureMaxDepthError">
+                 <property name="suffix">
+                  <string> m</string>
+                 </property>
+                 <property name="decimals">
+                  <number>2</number>
+                 </property>
+                 <property name="minimum">
+                  <double>-1.000000000000000</double>
+                 </property>
+                 <property name="maximum">
+                  <double>99.000000000000000</double>
+                 </property>
+                 <property name="singleStep">
+                  <double>0.050000000000000</double>
+                 </property>
+                 <property name="value">
+                  <double>0.000000000000000</double>
+                 </property>
+                </widget>
+               </item>
                <item row="3" column="1">
                 <widget class="QLabel" name="label_meshingTextureSize_7">
                  <property name="text">
@@ -2532,72 +2698,24 @@ By Node ID and Camera Index: NodeID*10+CameraIndex</string>
                  </property>
                 </widget>
                </item>
-               <item row="2" column="1">
-                <widget class="QLabel" name="label_meshingTextureSize_8">
+               <item row="17" column="0">
+                <widget class="QCheckBox" name="checkBox_multiband">
                  <property name="text">
-                  <string>Maximum output textures.</string>
-                 </property>
-                 <property name="wordWrap">
-                  <bool>true</bool>
+                  <string/>
                  </property>
                 </widget>
                </item>
-               <item row="10" column="1">
-                <widget class="QLabel" name="label_exposureFusion_2">
+               <item row="16" column="0">
+                <widget class="QCheckBox" name="checkBox_exposureFusion">
                  <property name="text">
-                  <string>Blending. Only used with dense reconstruction flavor and if clouds are assembled.</string>
-                 </property>
-                 <property name="wordWrap">
-                  <bool>true</bool>
+                  <string/>
                  </property>
                 </widget>
                </item>
-               <item row="4" column="1">
-                <widget class="QLabel" name="label_meshingTextureSize_2">
+               <item row="1" column="1">
+                <widget class="QLabel" name="label_meshingTextureSize">
                  <property name="text">
-                  <string>Maximum distance from the camera for polygons to be textured by this camera (0 means infinite). Only used with dense reconstruction flavor.</string>
-                 </property>
-                 <property name="wordWrap">
-                  <bool>true</bool>
-                 </property>
-                </widget>
-               </item>
-               <item row="4" column="0">
-                <widget class="QDoubleSpinBox" name="doubleSpinBox_meshingTextureMaxDistance">
-                 <property name="suffix">
-                  <string> m</string>
-                 </property>
-                 <property name="decimals">
-                  <number>1</number>
-                 </property>
-                 <property name="minimum">
-                  <double>0.000000000000000</double>
-                 </property>
-                 <property name="maximum">
-                  <double>99.000000000000000</double>
-                 </property>
-                 <property name="singleStep">
-                  <double>1.000000000000000</double>
-                 </property>
-                 <property name="value">
-                  <double>3.000000000000000</double>
-                 </property>
-                </widget>
-               </item>
-               <item row="12" column="1">
-                <widget class="QLabel" name="label_meshingTextureSize_5">
-                 <property name="text">
-                  <string>Brightness and contrast balance low ratio. Only used when textures are merged.</string>
-                 </property>
-                 <property name="wordWrap">
-                  <bool>true</bool>
-                 </property>
-                </widget>
-               </item>
-               <item row="0" column="1">
-                <widget class="QLabel" name="label_meshingTextureFormat">
-                 <property name="text">
-                  <string>Texture format.</string>
+                  <string>Output texture size. If not set or when clouds are not assembled, all textures are saved separately. Warning: values higher than 2048 may not be compatible with all GPUs.</string>
                  </property>
                  <property name="wordWrap">
                   <bool>true</bool>
@@ -2618,6 +2736,16 @@ By Node ID and Camera Index: NodeID*10+CameraIndex</string>
                  </item>
                 </widget>
                </item>
+               <item row="7" column="0">
+                <widget class="QSpinBox" name="spinBox_mesh_minTextureClusterSize">
+                 <property name="minimum">
+                  <number>0</number>
+                 </property>
+                 <property name="maximum">
+                  <number>99999</number>
+                 </property>
+                </widget>
+               </item>
                <item row="1" column="0">
                 <widget class="QComboBox" name="comboBox_meshingTextureSize">
                  <item>
@@ -2667,17 +2795,17 @@ By Node ID and Camera Index: NodeID*10+CameraIndex</string>
                  </item>
                 </widget>
                </item>
-               <item row="15" column="0">
-                <widget class="QCheckBox" name="checkBox_exposureFusion">
+               <item row="8" column="0">
+                <widget class="QCheckBox" name="checkBox_distanceToCamPolicy">
                  <property name="text">
                   <string/>
                  </property>
                 </widget>
                </item>
-               <item row="7" column="1">
-                <widget class="QLabel" name="label_meshingTextureSize_4">
+               <item row="2" column="1">
+                <widget class="QLabel" name="label_meshingTextureSize_8">
                  <property name="text">
-                  <string>Minimum polygon cluster size for texturing. This removes textures applied on sparse polygons. Only used with dense reconstruction flavor.</string>
+                  <string>Maximum output textures.</string>
                  </property>
                  <property name="wordWrap">
                   <bool>true</bool>
@@ -2687,76 +2815,16 @@ By Node ID and Camera Index: NodeID*10+CameraIndex</string>
                <item row="3" column="0">
                 <widget class="QLineEdit" name="lineEdit_meshingTextureRoiRatios"/>
                </item>
-               <item row="1" column="1">
-                <widget class="QLabel" name="label_meshingTextureSize">
+               <item row="12" column="1">
+                <widget class="QLabel" name="label_exposureFusion_3">
                  <property name="text">
-                  <string>Output texture size. If not set or when clouds are not assembled, all textures are saved separately. Warning: values higher than 2048 may not be compatible with all GPUs.</string>
+                  <string>Blending decimation. </string>
                  </property>
                  <property name="wordWrap">
                   <bool>true</bool>
                  </property>
                 </widget>
                </item>
-               <item row="9" column="0">
-                <widget class="QCheckBox" name="checkBox_cameraFilter">
-                 <property name="text">
-                  <string/>
-                 </property>
-                </widget>
-               </item>
-               <item row="12" column="0">
-                <widget class="QSpinBox" name="spinBox_textureBrightnessContrastRatioLow">
-                 <property name="suffix">
-                  <string> %</string>
-                 </property>
-                 <property name="minimum">
-                  <number>0</number>
-                 </property>
-                 <property name="maximum">
-                  <number>49</number>
-                 </property>
-                </widget>
-               </item>
-               <item row="7" column="0">
-                <widget class="QSpinBox" name="spinBox_mesh_minTextureClusterSize">
-                 <property name="minimum">
-                  <number>0</number>
-                 </property>
-                 <property name="maximum">
-                  <number>99999</number>
-                 </property>
-                </widget>
-               </item>
-               <item row="9" column="1">
-                <widget class="QLabel" name="label_meshingTextureSize_3">
-                 <property name="text">
-                  <string>Camera filtering. Some criteria to select which cameras are used for texturing.</string>
-                 </property>
-                 <property name="wordWrap">
-                  <bool>true</bool>
-                 </property>
-                </widget>
-               </item>
-               <item row="15" column="1">
-                <widget class="QLabel" name="label_exposureFusion">
-                 <property name="text">
-                  <string>Exposure fusion. Only used when textures are merged and brightness/contrast balance ratios are used (merging original texture + 1 for each ratios).</string>
-                 </property>
-                 <property name="wordWrap">
-                  <bool>true</bool>
-                 </property>
-                </widget>
-               </item>
-               <item row="2" column="0">
-                <widget class="QSpinBox" name="spinBox_mesh_maxTextures">
-                 <property name="minimum">
-                  <number>1</number>
-                 </property>
-                 <property name="maximum">
-                  <number>20</number>
-                 </property>
-                </widget>
-               </item>
                <item row="5" column="1">
                 <widget class="QLabel" name="label_meshingTextureSize_9">
                  <property name="text">
@@ -2767,92 +2835,53 @@ By Node ID and Camera Index: NodeID*10+CameraIndex</string>
                  </property>
                 </widget>
                </item>
-               <item row="5" column="0">
-                <widget class="QDoubleSpinBox" name="doubleSpinBox_meshingTextureMaxDepthError">
+               <item row="13" column="0">
+                <widget class="QSpinBox" name="spinBox_textureBrightnessContrastRatioLow">
                  <property name="suffix">
-                  <string> m</string>
-                 </property>
-                 <property name="decimals">
-                  <number>2</number>
+                  <string> %</string>
                  </property>
                  <property name="minimum">
-                  <double>-1.000000000000000</double>
-                 </property>
-                 <property name="maximum">
-                  <double>99.000000000000000</double>
-                 </property>
-                 <property name="singleStep">
-                  <double>0.050000000000000</double>
-                 </property>
-                 <property name="value">
-                  <double>0.000000000000000</double>
-                 </property>
-                </widget>
-               </item>
-               <item row="6" column="1">
-                <widget class="QLabel" name="label_meshingTextureSize_10">
-                 <property name="text">
-                  <string>Maximum angle between camera and face to apply texture (0 means disabled).</string>
-                 </property>
-                 <property name="wordWrap">
-                  <bool>true</bool>
-                 </property>
-                </widget>
-               </item>
-               <item row="6" column="0">
-                <widget class="QDoubleSpinBox" name="doubleSpinBox_meshingTextureMaxAngle">
-                 <property name="suffix">
-                  <string> deg</string>
-                 </property>
-                 <property name="decimals">
                   <number>0</number>
                  </property>
-                 <property name="minimum">
-                  <double>0.000000000000000</double>
-                 </property>
                  <property name="maximum">
-                  <double>180.000000000000000</double>
-                 </property>
-                 <property name="singleStep">
-                  <double>1.000000000000000</double>
-                 </property>
-                 <property name="value">
-                  <double>0.000000000000000</double>
+                  <number>49</number>
                  </property>
                 </widget>
                </item>
-               <item row="16" column="1">
-                <widget class="QLabel" name="label_multiband">
+               <item row="10" column="0">
+                <widget class="QCheckBox" name="checkBox_cameraFilter">
                  <property name="text">
-                  <string>MultiBand texturing. Only available on export and RTAB-Map should be built with AliceVision support.</string>
+                  <string/>
+                 </property>
+                </widget>
+               </item>
+               <item row="9" column="1">
+                <widget class="QLabel" name="label_texturingVertexColorPolicy">
+                 <property name="text">
+                  <string>Output vertex color policy.</string>
                  </property>
                  <property name="wordWrap">
                   <bool>true</bool>
                  </property>
                 </widget>
                </item>
-               <item row="16" column="0">
-                <widget class="QCheckBox" name="checkBox_multiband">
+               <item row="9" column="0">
+                <widget class="QComboBox" name="comboBox_texturingColorPolicy">
+                 <item>
                   <property name="text">
-                  <string/>
+                   <string>Colorless (standard OBJ)</string>
                   </property>
-                </widget>
                  </item>
-               <item row="8" column="1">
-                <widget class="QLabel" name="label_meshingTextureSize_11">
+                 <item>
                   <property name="text">
-                  <string>Distance to camera policy. The closest camera from a polygon is used to texture the polygon. If disabled, the camera for which the polygon projection is the closest of the image center is used to texture the polygon.</string>
+                   <string>Keep color on textureless polygons only</string>
                   </property>
-                 <property name="wordWrap">
-                  <bool>true</bool>
-                 </property>
-                </widget>
                  </item>
-               <item row="8" column="0">
-                <widget class="QCheckBox" name="checkBox_distanceToCamPolicy">
+                 <item>
                   <property name="text">
-                  <string/>
+                   <string>Keep all color</string>
                   </property>
+                 </item>
                 </widget>
                </item>
               </layout>

guilib/src/ui/preferencesDialog.ui

@@ -63,7 +63,7 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-989</y>
+            <y>0</y>
             <width>713</width>
             <height>4705</height>
            </rect>
@@ -95,7 +95,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>19</number>
+                <number>21</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,0">
@@ -14891,16 +14891,25 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                    <layout class="QVBoxLayout" name="verticalLayout_136">
                     <item>
                      <layout class="QGridLayout" name="gridLayout_63" columnstretch="0,1">
-                      <item row="5" column="1">
-                       <widget class="QLabel" name="label_space2_13">
-                        <property name="text">
-                         <string>Minimum detection range (0=disabled).</string>
+                      <item row="7" column="0">
+                       <widget class="QDoubleSpinBox" name="ArucoMarkerRangeMax">
+                        <property name="suffix">
+                         <string> m</string>
                         </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
+                        <property name="decimals">
+                         <number>2</number>
                         </property>
-                        <property name="textInteractionFlags">
-                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        <property name="minimum">
+                         <double>0.000000000000000</double>
+                        </property>
+                        <property name="maximum">
+                         <double>999.000000000000000</double>
+                        </property>
+                        <property name="singleStep">
+                         <double>1.000000000000000</double>
+                        </property>
+                        <property name="value">
+                         <double>0.000000000000000</double>
                         </property>
                        </widget>
                       </item>
@@ -14926,7 +14935,26 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="7" column="1">
+                      <item row="1" column="0">
+                       <widget class="QDoubleSpinBox" name="ArucoMarkerLength">
+                        <property name="suffix">
+                         <string> m</string>
+                        </property>
+                        <property name="decimals">
+                         <number>4</number>
+                        </property>
+                        <property name="minimum">
+                         <double>-1.000000000000000</double>
+                        </property>
+                        <property name="singleStep">
+                         <double>0.010000000000000</double>
+                        </property>
+                        <property name="value">
+                         <double>0.100000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="8" column="1">
                        <widget class="QLabel" name="label_space2_15">
                         <property name="text">
                          <string>World prior locations of the markers. The map will be transformed in marker's world frame when a tag is detected. Format is the marker's ID followed by its position (angles in rad), markers are separated by a vertical line (&quot;id1 x y z roll pitch yaw|id2 x y z roll pitch yaw&quot;). Example:  &quot;1 0 0 1 0 0 0|2 1 0 1 0 0 1.57&quot; (marker 2 is 1 meter forward than marker 1 with 90 deg yaw rotation).</string>
@@ -14939,11 +14967,65 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="7" column="0">
-                       <widget class="QLineEdit" name="ArucoMarkerPriors">
-                        <property name="placeholderText">
+                      <item row="6" column="1">
+                       <widget class="QLabel" name="label_space2_13">
+                        <property name="text">
+                         <string>Minimum detection range (0=disabled).</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="10" column="0">
+                       <widget class="QDoubleSpinBox" name="ArucoPriorsVarianceAngular">
+                        <property name="suffix">
                          <string/>
                         </property>
+                        <property name="decimals">
+                         <number>6</number>
+                        </property>
+                        <property name="minimum">
+                         <double>0.000001000000000</double>
+                        </property>
+                        <property name="maximum">
+                         <double>9999.000000000000000</double>
+                        </property>
+                        <property name="singleStep">
+                         <double>0.001000000000000</double>
+                        </property>
+                        <property name="value">
+                         <double>0.001000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="9" column="1">
+                       <widget class="QLabel" name="label_space2_16">
+                        <property name="text">
+                         <string>Linear variance to set on marker priors.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="10" column="1">
+                       <widget class="QLabel" name="label_space2_17">
+                        <property name="text">
+                         <string>Angular variance to set on marker priors.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
                        </widget>
                       </item>
                       <item row="0" column="0">
@@ -14953,51 +15035,22 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="6" column="0">
-                       <widget class="QDoubleSpinBox" name="ArucoMarkerRangeMax">
+                      <item row="3" column="0">
+                       <widget class="QDoubleSpinBox" name="ArucoVarianceLinear">
                         <property name="suffix">
-                         <string> m</string>
+                         <string/>
                         </property>
                         <property name="decimals">
-                         <number>2</number>
+                         <number>6</number>
                         </property>
                         <property name="minimum">
-                         <double>0.000000000000000</double>
-                        </property>
-                        <property name="maximum">
-                         <double>999.000000000000000</double>
+                         <double>0.000001000000000</double>
                         </property>
                         <property name="singleStep">
-                         <double>1.000000000000000</double>
+                         <double>0.001000000000000</double>
                         </property>
                         <property name="value">
-                         <double>0.000000000000000</double>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="3" column="1">
-                       <widget class="QLabel" name="label_space2_6">
-                        <property name="text">
-                         <string>Linear variance to set on marker detections.</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                        <property name="textInteractionFlags">
-                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="4" column="1">
-                       <widget class="QLabel" name="label_space2_9">
-                        <property name="text">
-                         <string>Angular variance to set on marker detections. Set to &gt;=9999 to use only position (xyz) constraint in graph optimization.</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                        <property name="textInteractionFlags">
-                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         <double>0.001000000000000</double>
                         </property>
                        </widget>
                       </item>
@@ -15020,39 +15073,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="1" column="0">
-                       <widget class="QDoubleSpinBox" name="ArucoMarkerLength">
-                        <property name="suffix">
-                         <string> m</string>
-                        </property>
-                        <property name="decimals">
-                         <number>4</number>
-                        </property>
-                        <property name="minimum">
-                         <double>-1.000000000000000</double>
-                        </property>
-                        <property name="singleStep">
-                         <double>0.010000000000000</double>
-                        </property>
-                        <property name="value">
-                         <double>0.100000000000000</double>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="8" column="1">
-                       <widget class="QLabel" name="label_space2_16">
-                        <property name="text">
-                         <string>Linear variance to set on marker priors.</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                        <property name="textInteractionFlags">
-                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="5" column="0">
+                      <item row="6" column="0">
                        <widget class="QDoubleSpinBox" name="ArucoMarkerRangeMin">
                         <property name="suffix">
                          <string> m</string>
@@ -15087,10 +15108,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="6" column="1">
-                       <widget class="QLabel" name="label_space2_14">
+                      <item row="3" column="1">
+                       <widget class="QLabel" name="label_space2_6">
                         <property name="text">
-                         <string>Maximum detection range (0=unlimited).</string>
+                         <string>Linear variance to set on marker detections. If variance is adjusted to ignore orientation (see below) and Optimizer/Strategy=2 (GTSAM): it is the variance of the range factor, with 9999 to disable range factor and to do only bearing.</string>
                         </property>
                         <property name="wordWrap">
                          <bool>true</bool>
@@ -15100,29 +15121,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="3" column="0">
-                       <widget class="QDoubleSpinBox" name="ArucoVarianceLinear">
-                        <property name="suffix">
-                         <string/>
-                        </property>
-                        <property name="decimals">
-                         <number>6</number>
-                        </property>
-                        <property name="minimum">
-                         <double>0.000001000000000</double>
-                        </property>
-                        <property name="singleStep">
-                         <double>0.001000000000000</double>
-                        </property>
-                        <property name="value">
-                         <double>0.001000000000000</double>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="0" column="1">
-                       <widget class="QLabel" name="label_markerDetection">
+                      <item row="4" column="1">
+                       <widget class="QLabel" name="label_space2_9">
                         <property name="text">
-                         <string>Detect static markers to be added as landmarks for graph optimization. If input data have already landmarks, this will be ignored.</string>
+                         <string>Angular variance to set on marker detections. If variance is adjusted to ignore orientation (see below), the angular variance is ignored with Optimizer/Strategy=1 (g2o) and it corresponds to bearing variance with Optimizer/Strategy=2 (GTSAM).</string>
                         </property>
                         <property name="wordWrap">
                          <bool>true</bool>
@@ -15132,33 +15134,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="1" column="1">
-                       <widget class="QLabel" name="label_space2_7">
-                        <property name="text">
-                         <string>Marker length. 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).</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                        <property name="textInteractionFlags">
-                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="9" column="1">
-                       <widget class="QLabel" name="label_space2_17">
-                        <property name="text">
-                         <string>Angular variance to set on marker priors.</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                        <property name="textInteractionFlags">
-                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="8" column="0">
+                      <item row="9" column="0">
                        <widget class="QDoubleSpinBox" name="ArucoPriorsVarianceLinear">
                         <property name="suffix">
                          <string/>
@@ -15177,25 +15153,72 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="9" column="0">
-                       <widget class="QDoubleSpinBox" name="ArucoPriorsVarianceAngular">
-                        <property name="suffix">
+                      <item row="7" column="1">
+                       <widget class="QLabel" name="label_space2_14">
+                        <property name="text">
+                         <string>Maximum detection range (0=unlimited).</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="8" column="0">
+                       <widget class="QLineEdit" name="ArucoMarkerPriors">
+                        <property name="placeholderText">
                          <string/>
                         </property>
-                        <property name="decimals">
-                         <number>6</number>
+                       </widget>
+                      </item>
+                      <item row="1" column="1">
+                       <widget class="QLabel" name="label_space2_7">
+                        <property name="text">
+                         <string>Marker length. 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).</string>
                         </property>
-                        <property name="minimum">
-                         <double>0.000001000000000</double>
+                        <property name="wordWrap">
+                         <bool>true</bool>
                         </property>
-                        <property name="maximum">
-                         <double>9999.000000000000000</double>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
                         </property>
-                        <property name="singleStep">
-                         <double>0.001000000000000</double>
+                       </widget>
+                      </item>
+                      <item row="0" column="1">
+                       <widget class="QLabel" name="label_markerDetection">
+                        <property name="text">
+                         <string>Detect static markers to be added as landmarks for graph optimization. If input data have already landmarks, this will be ignored.</string>
                         </property>
-                        <property name="value">
-                         <double>0.001000000000000</double>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="5" column="1">
+                       <widget class="QLabel" name="label_space2_20">
+                        <property name="toolTip">
+                         <string>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 Optimizer/Strategy=1 (g2o), only Marker/VarianceLinear needs be set if we ignore orientation. For Optimizer/Strategy=2 (GTSAM), instead of optimizing the landmark's position directly, a bearing/range factor is used, with Marker/VarianceLinear as the variance of the range factor (with 9999 to optimize the position with only a bearing factor) and Marker/VarianceAngular as the variance of the bearing factor (pitch/yaw).</string>
+                        </property>
+                        <property name="text">
+                         <string>Adjust variance to ignore orientation during optimization. Mouseover for more details.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="5" column="0">
+                       <widget class="QCheckBox" name="ArucoVarianceOrientationIgnored">
+                        <property name="text">
+                         <string/>
                         </property>
                        </widget>
                       </item>
@@ -16182,7 +16205,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                               </widget>
                              </item>
                              <item row="2" column="1">
-                              <widget class="QLabel" name="label_764">
+                              <widget class="QLabel" name="label_7641">
                                <property name="text">
                                 <string>[Visual] Floor threshold. 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.</string>
                                </property>
@@ -16234,7 +16257,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                               </widget>
                              </item>
                              <item row="14" column="1">
-                              <widget class="QLabel" name="label_763">
+                              <widget class="QLabel" name="label_7631">
                                <property name="text">
                                 <string>[Visual] Update 3D local feature map on every frames with bundle adjustment. Recommended if Vis/DepthAsMask=false and Mem/UseOdomFeatures=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).</string>
                                </property>
@@ -21689,6 +21712,95 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                    <layout class="QVBoxLayout" name="verticalLayout_41" stretch="0,0,0,1,0">
                     <item>
                      <layout class="QGridLayout" name="gridLayout_23" columnstretch="0,1">
+                      <item row="2" column="1">
+                       <widget class="QLabel" name="label_554">
+                        <property name="text">
+                         <string>Maximum distance (m) of the mean distance of inliers from the camera to accept the transformation. 0 means disabled.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="5" column="0">
+                       <widget class="QComboBox" name="loopClosure_bundle">
+                        <property name="sizeAdjustPolicy">
+                         <enum>QComboBox::AdjustToContentsOnFirstShow</enum>
+                        </property>
+                        <item>
+                         <property name="text">
+                          <string>Disabled</string>
+                         </property>
+                        </item>
+                        <item>
+                         <property name="text">
+                          <string>g2o</string>
+                         </property>
+                        </item>
+                        <item>
+                         <property name="text">
+                          <string>cvsba</string>
+                         </property>
+                        </item>
+                        <item>
+                         <property name="text">
+                          <string>Ceres</string>
+                         </property>
+                        </item>
+                       </widget>
+                      </item>
+                      <item row="5" column="1">
+                       <widget class="QLabel" name="label_346">
+                        <property name="text">
+                         <string>Refine transformation with bundle adjustment. See Optimizer panel.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="2" column="0">
+                       <widget class="QDoubleSpinBox" name="visMeanDistance">
+                        <property name="suffix">
+                         <string> m</string>
+                        </property>
+                        <property name="maximum">
+                         <double>9999.000000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="1" column="1">
+                       <widget class="QLabel" name="label_2">
+                        <property name="text">
+                         <string>Minimum correspondences to accept the estimated transformation.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="3" column="0">
+                       <widget class="QDoubleSpinBox" name="visMinDistribution">
+                        <property name="decimals">
+                         <number>4</number>
+                        </property>
+                        <property name="maximum">
+                         <double>0.500000000000000</double>
+                        </property>
+                        <property name="singleStep">
+                         <double>0.010000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
                       <item row="0" column="0">
                        <widget class="QComboBox" name="loopClosure_estimationType">
                         <property name="sizeAdjustPolicy">
@@ -21711,22 +21823,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </item>
                        </widget>
                       </item>
-                      <item row="4" column="0">
-                       <widget class="QSpinBox" name="loopClosure_bowIterations">
-                        <property name="minimum">
-                         <number>1</number>
-                        </property>
-                        <property name="maximum">
-                         <number>10000</number>
-                        </property>
-                        <property name="singleStep">
-                         <number>1</number>
-                        </property>
-                        <property name="value">
-                         <number>100</number>
-                        </property>
-                       </widget>
-                      </item>
                       <item row="4" column="1">
                        <widget class="QLabel" name="label_15">
                         <property name="text">
@@ -21737,26 +21833,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="5" column="1">
-                       <widget class="QLabel" name="label_298">
-                        <property name="text">
-                         <string>Forward estimation only (A-&gt;B). If false, a transformation is also computed in backward direction (B-&gt;A), then the two resulting transforms are merged (middle interpolation between the transforms).</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                        <property name="textInteractionFlags">
-                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="5" column="0">
-                       <widget class="QCheckBox" name="loopClosure_forwardEst">
-                        <property name="text">
-                         <string/>
-                        </property>
-                       </widget>
-                      </item>
                       <item row="1" column="0">
                        <widget class="QSpinBox" name="loopClosure_bowMinInliers">
                         <property name="minimum">
@@ -21783,72 +21859,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="1" column="1">
-                       <widget class="QLabel" name="label_2">
-                        <property name="text">
-                         <string>Minimum correspondences to accept the estimated transformation.</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                        <property name="textInteractionFlags">
-                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="6" column="0">
-                       <widget class="QComboBox" name="loopClosure_bundle">
-                        <property name="sizeAdjustPolicy">
-                         <enum>QComboBox::AdjustToContentsOnFirstShow</enum>
-                        </property>
-                        <item>
-                         <property name="text">
-                          <string>Disabled</string>
-                         </property>
-                        </item>
-                        <item>
-                         <property name="text">
-                          <string>g2o</string>
-                         </property>
-                        </item>
-                        <item>
-                         <property name="text">
-                          <string>cvsba</string>
-                         </property>
-                        </item>
-                        <item>
-                         <property name="text">
-                          <string>Ceres</string>
-                         </property>
-                        </item>
-                       </widget>
-                      </item>
-                      <item row="6" column="1">
-                       <widget class="QLabel" name="label_346">
-                        <property name="text">
-                         <string>Refine transformation with bundle adjustment. See Optimizer panel.</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                        <property name="textInteractionFlags">
-                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="2" column="1">
-                       <widget class="QLabel" name="label_554">
-                        <property name="text">
-                         <string>Maximum distance (m) of the mean distance of inliers from the camera to accept the transformation. 0 means disabled.</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                        <property name="textInteractionFlags">
-                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                        </property>
-                       </widget>
-                      </item>
                       <item row="3" column="1">
                        <widget class="QLabel" name="label_555">
                         <property name="text">
@@ -21862,26 +21872,19 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="3" column="0">
-                       <widget class="QDoubleSpinBox" name="visMinDistribution">
-                        <property name="decimals">
-                         <number>4</number>
+                      <item row="4" column="0">
+                       <widget class="QSpinBox" name="loopClosure_bowIterations">
+                        <property name="minimum">
+                         <number>1</number>
                         </property>
                         <property name="maximum">
-                         <double>0.500000000000000</double>
+                         <number>10000</number>
                         </property>
                         <property name="singleStep">
-                         <double>0.010000000000000</double>
+                         <number>1</number>
                         </property>
-                       </widget>
-                      </item>
-                      <item row="2" column="0">
-                       <widget class="QDoubleSpinBox" name="visMeanDistance">
-                        <property name="suffix">
-                         <string> m</string>
-                        </property>
-                        <property name="maximum">
-                         <double>9999.000000000000000</double>
+                        <property name="value">
+                         <number>100</number>
                         </property>
                        </widget>
                       </item>

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>rtabmap</name>
-  <version>0.21.11</version>
+  <version>0.21.13</version>
   <description>RTAB-Map's standalone library. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
   <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>

tools/Export/main.cpp

@@ -85,6 +85,10 @@ void showUsage()
 			"    --texture_angle #     Maximum camera angle for texturing a polygon (default 0 deg: no limit).\n"
 			"    --texture_depth_error # Maximum depth error between reprojected mesh and depth image to texture a face\n"
 			"                                (-1=disabled, 0=edge length is used, default=0).\n"
+			"    --texture_color_policy # How vertices are colored when texturing: \n"
+			"                                 0=colorless (standard OBJ), \n"
+			"                                 1=keep color only on textureless polygons, \n"
+			"                                 2=keep all color.\n"
 			"    --texture_roi_ratios \"# # # #\" Region of interest from images to texture or to color scans. Format\n"
 			"                                         is \"left right top bottom\" (e.g. \"0 0 0 0.1\" means 10%%\n"
 			"                                         of the image bottom not used).\n"
@@ -236,6 +240,7 @@ int main(int argc, char * argv[])
 	double multibandAngleHardthr = 90;
 	bool multibandForceVisible = false;
 	float colorRadius = -1.0f;
+	int textureVertexColorPolicy = 0;
 	bool cloudFromScan = false;
 	bool saveInDb = false;
 	int lowBrightnessGain = 0;
@@ -859,6 +864,18 @@ int main(int argc, char * argv[])
 				showUsage();
 			}
 		}
+		else if(std::strcmp(argv[i], "--texture_color_policy") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				textureVertexColorPolicy = uStr2Int(argv[i]);
+			}
+			else
+			{
+				showUsage();
+			}
+		}
 		else if(std::strcmp(argv[i], "--scan") == 0)
 		{
 			cloudFromScan = true;
@@ -2306,7 +2323,7 @@ int main(int argc, char * argv[])
 						maxPolygons,
 						cloudToExport,
 						colorRadius,
-						!texture,
+						!(texture && textureVertexColorPolicy == 0),
 						doClean,
 						minCluster);
 				printf("Mesh color transfer... done (%fs).\n", timer.ticks());
@@ -2424,7 +2441,8 @@ int main(int argc, char * argv[])
 								lowBrightnessGain, highBrightnessGain, // low-high brightness/contrast balance
 								false, // exposure fusion
 								0,     // state
-								0,     // blank value (0=black)
+								255,   // blank value (0=white)
+								textureVertexColorPolicy == 1,
 								&gains,
 								&blendingGains,
 								&contrastValues);
@@ -2539,7 +2557,7 @@ int main(int argc, char * argv[])
 										f_idx += static_cast<unsigned>(textureMesh->tex_polygons[m].size());
 									}
 	#endif								
-									success = pcl::io::saveOBJFile(outputPath, *textureMesh) == 0;
+									success = util3d::saveOBJFile(outputPath, *textureMesh) == 0;
 
 									if(success)
 									{

tools/LidarViewer/main.cpp

@@ -100,7 +100,11 @@ int main(int argc, char * argv[])
 	rtabmap::LidarVLP16 * lidar = 0;
 	if(driver == 0)
 	{
+#if BOOST_VERSION >= 108700  // Version 1.87.0
+		lidar = new rtabmap::LidarVLP16(boost::asio::ip::make_address(ip), port);
+#else
 		lidar = new rtabmap::LidarVLP16(boost::asio::ip::address_v4::from_string(ip), port);
+#endif
 	}
 	else
 	{

tools/Reprocess/main.cpp

@@ -75,6 +75,9 @@ void showUsage()
 			"                 is used, it will be applied to odometry frames, not rtabmap frames. Multi-session\n"
 			"                 cannot be detected in this mode (assuming the database contains continuous frames\n"
 			"                 of a single session).\n"
+			"     -odom_input_guess   Forward input database's odometry (if exists) as guess when recompting odometry.\n"
+			"     -odom_lin_var #.#   Override computed odometry linear covariance."
+			"     -odom_ang_var #.#   Override computed odometry angular covariance."
 			"     -start #    Start from this node ID.\n"
 			"     -stop #     Last node to process.\n"
 			"     -start_s #  Start from this map session ID.\n"
@@ -83,6 +86,12 @@ void showUsage()
 			"                 then next databases are reprocessed on top of the first one.\n"
 			"     -cam #      Camera index to stream. Ignored if a database doesn't contain multi-camera data. Can also be multiple \n"
 			"                 indices split by spaces in a string like \"0 2\" to stream cameras 0 and 2 only.\n"
+			"     -cam_tf \"x y z roll pitch yaw\" Camera local transform override(s) without optical rotation. For multi-cameras, \n"
+			"                                      use a \";\" between each transform.\n"
+			"     -cam_tf_lens_offset #.#          Override camera local transform with an y-axis offset before optical rotation. \n"
+			"                                      If -cam_tf is also used, it is combined before optical rotation. For multi-cameras,\n"
+			"                                      -cam_tf should be also used, and explicitly enumerate offsets if they are different, \n"
+			"                                      e.g., \"0.05 0.075\" for two cameras setup.\n"
 			"     -nolandmark Don't republish landmarks contained in input database.\n"
 			"     -nopriors   Don't republish priors contained in input database.\n"
 			"     -pub_loops  Republish loop closures contained in input database.\n"
@@ -250,12 +259,17 @@ int main(int argc, char * argv[])
 	bool useDatabaseRate = false;
 	bool useDefaultParameters = false;
 	bool recomputeOdometry = false;
+	bool useInputOdometryAsGuess = false;
+	double odomLinVarOverride = 0.0;
+	double odomAngVarOverride = 0.0;
 	int startId = 0;
 	int stopId = 0;
 	int startMapId = 0;
 	int stopMapId = -1;
 	bool appendMode = false;
 	std::vector<unsigned int> cameraIndices;
+	std::vector<Transform> cameraLocalTransformOverrides;
+	std::vector<float> cameraLocalTransformOffsetOverrides;
 	int framesToSkip = 0;
 	bool ignoreLandmarks = false;
 	bool ignorePriors = false;
@@ -305,6 +319,38 @@ int main(int argc, char * argv[])
 		{
 			recomputeOdometry = true;
 		}
+		else if(strcmp(argv[i], "-odom_input_guess") == 0 || strcmp(argv[i], "--odom_input_guess") == 0)
+		{
+			useInputOdometryAsGuess = true;
+		}
+		else if (strcmp(argv[i], "-odom_lin_var") == 0 || strcmp(argv[i], "--odom_lin_var") == 0)
+		{
+			++i;
+			if(i < argc - 2)
+			{
+				odomLinVarOverride = uStr2Double(argv[i]);
+				printf("Odometry linear variance overriden to = %f.\n", odomLinVarOverride);
+			}
+			else
+			{
+				printf("-odom_lin_var option requires a value\n");
+				showUsage();
+			}
+		}
+		else if (strcmp(argv[i], "-odom_ang_var") == 0 || strcmp(argv[i], "--odom_ang_var") == 0)
+		{
+			++i;
+			if(i < argc - 2)
+			{
+				odomAngVarOverride = uStr2Double(argv[i]);
+				printf("Odometry angular variance overriden to = %f.\n", odomAngVarOverride);
+			}
+			else
+			{
+				printf("-odom_ang_var option requires a value\n");
+				showUsage();
+			}
+		}
 		else if (strcmp(argv[i], "-start") == 0 || strcmp(argv[i], "--start") == 0)
 		{
 			++i;
@@ -384,6 +430,42 @@ int main(int argc, char * argv[])
 				showUsage();
 			}
 		}
+		else if (strcmp(argv[i], "-cam_tf") == 0 || strcmp(argv[i], "--cam_tf") == 0)
+		{
+			++i;
+			if(i < argc - 2)
+			{
+				std::list<std::string> tfStr = uSplit(argv[i], ';');
+				for(std::list<std::string>::iterator iter=tfStr.begin(); iter!=tfStr.end(); ++iter)
+				{
+					cameraLocalTransformOverrides.push_back(Transform::fromString(*iter));
+					printf("Camera transform = %s\n", cameraLocalTransformOverrides.back().prettyPrint().c_str());
+				}
+			}
+			else
+			{
+				printf("-cam option requires a value\n");
+				showUsage();
+			}
+		}
+		else if (strcmp(argv[i], "-cam_tf_lens_offset") == 0 || strcmp(argv[i], "--cam_tf_lens_offset") == 0)
+		{
+			++i;
+			if(i < argc - 2)
+			{
+				std::list<std::string> offsetStr = uSplit(argv[i], ' ');
+				for(std::list<std::string>::iterator iter=offsetStr.begin(); iter!=offsetStr.end(); ++iter)
+				{
+					cameraLocalTransformOffsetOverrides.push_back(uStr2Float(*iter));
+					printf("Camera offset = %f\n", cameraLocalTransformOffsetOverrides.back());
+				}
+			}
+			else
+			{
+				printf("-cam option requires a value\n");
+				showUsage();
+			}
+		}
 		else if (strcmp(argv[i], "-skip") == 0 || strcmp(argv[i], "--skip") == 0)
 		{
 			++i;
@@ -566,6 +648,10 @@ int main(int argc, char * argv[])
 		if (!UFile::exists(*iter))
 		{
 			printf("Input database \"%s\" doesn't exist!\n", iter->c_str());
+			if(uStrContains(inputDatabasePath,":"))
+			{
+				printf("Did you mean \"%s\"?\n", uReplaceChar(inputDatabasePath, ':', ";").c_str());
+			}
 			return -1;
 		}
 
@@ -738,11 +824,6 @@ int main(int argc, char * argv[])
 	delete dbDriver;
 	dbDriver = 0;
 
-	if(framesToSkip)
-	{
-		totalIds/=framesToSkip+1;
-	}
-
 	std::string workingDirectory = UDirectory::getDir(outputDatabasePath);
 	printf("Set working directory to \"%s\".\n", workingDirectory.c_str());
 	if(!targetVersion.empty())
@@ -775,6 +856,35 @@ int main(int argc, char * argv[])
 	Parameters::parse(parameters, Parameters::kRGBDEnabled(), rgbdEnabled);
 	bool odometryIgnored = !rgbdEnabled;
 
+	if(!cameraLocalTransformOffsetOverrides.empty())
+	{
+		if(!cameraLocalTransformOverrides.empty() && cameraLocalTransformOffsetOverrides.size() > 1 && cameraLocalTransformOffsetOverrides.size() != cameraLocalTransformOverrides.size())
+		{
+			printf("Error: -cam_tf_lens_offset size (%ld) is not equal to -cam_tf argument (%ld). "
+				   "-cam_tf_lens_offset should be one to affect all cameras or same size than -cam_tf argument.\n",
+				   cameraLocalTransformOffsetOverrides.size(), cameraLocalTransformOverrides.size());
+			showUsage();
+			return 1;
+		}
+		if(cameraLocalTransformOverrides.empty())
+		{
+			if(cameraLocalTransformOffsetOverrides.size() > 1)
+			{
+				printf("Error: -cam_tf_lens_offset size (%ld) should be one if -cam_tf is not set.\n",
+				   cameraLocalTransformOffsetOverrides.size());
+				showUsage();
+				return 1;
+			}
+			cameraLocalTransformOverrides.push_back(Transform::getIdentity());
+		}
+		for(size_t i=0; i<cameraLocalTransformOverrides.size(); ++i)
+		{
+			float offset = cameraLocalTransformOffsetOverrides.size()==1?cameraLocalTransformOffsetOverrides[0]:cameraLocalTransformOffsetOverrides[i];
+			cameraLocalTransformOverrides[i] *= Transform(0, offset, 0);
+			printf("Overriding camera's local transform %ld to %s (offset=%f)\n", i, cameraLocalTransformOverrides[i].prettyPrint().c_str(), offset);
+		}
+	}
+
 	DBReader * dbReader = new DBReader(
 			inputDatabasePath,
 			useDatabaseRate?-1:0,
@@ -789,7 +899,8 @@ int main(int argc, char * argv[])
 			!useOdomFeatures,
 			startMapId,
 			stopMapId,
-			ignorePriors);
+			ignorePriors,
+			cameraLocalTransformOverrides);
 
 	dbReader->init();
 
@@ -840,22 +951,49 @@ int main(int argc, char * argv[])
 	}
 	camThread.postUpdate(&data, &info);
 	Transform lastLocalizationOdomPose = info.odomPose;
+	Transform previousOdomPose = info.odomPose;
+	cv::Mat odomCovariance;
 	bool inMotion = true;
 	while(data.isValid() && g_loopForever)
 	{
 		if(recomputeOdometry)
 		{
 			OdometryInfo odomInfo;
-			Transform pose = odometry->process(data, &odomInfo);
-			printf("Processed %d/%d frames (visual=%d/%d lidar=%f lost=%s)... odometry = %dms\n",
+			Transform pose = odometry->process(data, useInputOdometryAsGuess && !info.odomPose.isNull()?previousOdomPose.inverse() * info.odomPose:Transform(), &odomInfo);
+			previousOdomPose = info.odomPose;
+			if(odomInfo.reg.covariance.total() == 36)
+			{
+				if(odomLinVarOverride > 0.0)
+				{
+					odomInfo.reg.covariance.at<double>(0,0) = odomLinVarOverride;
+					odomInfo.reg.covariance.at<double>(1,1) = odomLinVarOverride;
+					odomInfo.reg.covariance.at<double>(2,2) = odomLinVarOverride;
+				}
+				if(odomAngVarOverride > 0.0)
+				{
+					odomInfo.reg.covariance.at<double>(3,3) = odomAngVarOverride;
+					odomInfo.reg.covariance.at<double>(4,4) = odomAngVarOverride;
+					odomInfo.reg.covariance.at<double>(5,5) = odomAngVarOverride;
+				}
+				if(uIsFinite(odomInfo.reg.covariance.at<double>(0,0)) &&
+					odomInfo.reg.covariance.at<double>(0,0) != 1.0 &&
+					odomInfo.reg.covariance.at<double>(0,0)>0.0)
+				{
+					// Use largest covariance error (to be independent of the odometry frame rate)
+					if(odomCovariance.empty() || odomInfo.reg.covariance.at<double>(0,0) > odomCovariance.at<double>(0,0))
+					{
+						odomCovariance = odomInfo.reg.covariance;
+					}
+				}
+			}
+			printf("Processed %d/%d frames (visual=%s lidar=%s lost=%s)... odometry = %dms\n",
 					processed+1,
 					totalIds,
-					odomInfo.reg.inliers,
-					odomInfo.reg.matches,
-					odomInfo.reg.icpInliersRatio,
+					odomInfo.reg.matches!=0?uFormat("%d/%d", odomInfo.reg.inliers, odomInfo.reg.matches).c_str():"NA",
+					odomInfo.reg.icpInliersRatio!=0.0?uNumber2Str(odomInfo.reg.icpInliersRatio).c_str():"NA",
 					odomInfo.lost?"true":"false",
 					int(odomInfo.timeEstimation * 1000));
-			if(lastUpdateStamp > 0.0 && data.stamp() < lastUpdateStamp + rtabmapUpdateRate)
+			if(lastUpdateStamp > 0.0 && (data.stamp() < lastUpdateStamp + rtabmapUpdateRate || framesToSkip>0))
 			{
 				if(framesToSkip>0)
 				{
@@ -877,8 +1015,11 @@ int main(int argc, char * argv[])
 				continue;
 			}
 			info.odomPose = pose;
-			info.odomCovariance = odomInfo.reg.covariance;
+			info.odomCovariance = odomCovariance;
+			odomCovariance = cv::Mat();
 			lastUpdateStamp = data.stamp();
+
+			uInsert(globalMapStats, odomInfo.statistics(pose));
 		}
 
 		UTimer iterationTime;
@@ -1037,8 +1178,9 @@ int main(int argc, char * argv[])
 
 		const rtabmap::Statistics & stats = rtabmap.getStatistics();
 		int refId = stats.refImageId();
+		bool rejected = uValue(stats.data(), rtabmap::Statistics::kLoopRejectedHypothesis(), 0.0f) != 0.0f;
 		int loopId = stats.loopClosureId() > 0? stats.loopClosureId(): stats.proximityDetectionId() > 0?stats.proximityDetectionId() :0;
-		int landmarkId = (int)uValue(stats.data(), rtabmap::Statistics::kLoopLandmark_detected(), 0.0f);
+		int landmarkId = rejected?0:(int)uValue(stats.data(), rtabmap::Statistics::kLoopLandmark_detected(), 0.0f);
 		int refMapId = stats.refImageMapId();
 		++totalFrames;