mergd master, disabled grid_map_core for jazzy

.github/workflows/cmake-ros.yml

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

.github/workflows/docker.yml

@@ -7,16 +7,6 @@ on:
 
 jobs:
   docker_deps:
-
-    # Disabling ###-deps step from CI because it is too flaky (seg faults, arm64 build timeout...)
-    # Only way I was able to build all images is to do it from a ubuntu 20.04 computer with:
-    #    $ sudo add-apt-repository ppa:canonical-server/server-backports
-    #    $ sudo apt-get update
-    #    $ sudo apt-get upgrade qemu-user-static
-    #    $ docker run --rm --privileged multiarch/qemu-user-static --reset -p yes -c yes
-    # More info: https://github.com/introlab/rtabmap/issues/1454
-    if: false
-
     runs-on: ubuntu-latest
     
     strategy:
@@ -59,21 +49,21 @@ jobs:
         uses: actions/checkout@v2
       - 
         name: Set up QEMU
-        uses: docker/setup-qemu-action@v3
+        uses: docker/setup-qemu-action@v1
         with:
           platforms: all
       -
         name: Set up Docker Buildx
-        uses: docker/setup-buildx-action@v3
+        uses: docker/setup-buildx-action@v1
       -
         name: Login to DockerHub
-        uses: docker/login-action@v3
+        uses: docker/login-action@v1
         with:
           username: ${{ secrets.DOCKERHUB_USERNAME }}
           password: ${{ secrets.DOCKERHUB_TOKEN }}
       -
         name: Build and push
-        uses: docker/build-push-action@v6
+        uses: docker/build-push-action@v2
         with:
           context: .
           push: true
@@ -84,7 +74,7 @@ jobs:
           cache-to: type=inline
   
   docker:
-    #needs: docker_deps
+    needs: docker_deps
     runs-on: ubuntu-latest
     
     strategy:
@@ -182,21 +172,21 @@ jobs:
         uses: actions/checkout@v2
       - 
         name: Set up QEMU
-        uses: docker/setup-qemu-action@v3
+        uses: docker/setup-qemu-action@v1
         with:
           platforms: all
       -
         name: Set up Docker Buildx
-        uses: docker/setup-buildx-action@v3
+        uses: docker/setup-buildx-action@v1
       -
         name: Login to DockerHub
-        uses: docker/login-action@v3
+        uses: docker/login-action@v1
         with:
           username: ${{ secrets.DOCKERHUB_USERNAME }}
           password: ${{ secrets.DOCKERHUB_TOKEN }}
       -
         name: Build and push
-        uses: docker/build-push-action@v6
+        uses: docker/build-push-action@v2
         with:
           context: .
           push: true

CMakeLists.txt

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

LICENSE

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

README.md

@@ -64,8 +64,8 @@ This project is supported by [IntRoLab - Intelligent / Interactive / Integrated
             <td><a href="http://build.ros2.org/job/Hbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary" alt="Build Status"/></td>
         </tr>
         <tr>
-            <td>Jazzy</td>
+            <td>Iron</td>
-            <td><a href="http://build.ros2.org/job/Jbin_uN64__rtabmap__ubuntu_noble_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Jbin_uN64__rtabmap__ubuntu_noble_amd64__binary" alt="Build Status"/></td>
+            <td><a href="http://build.ros2.org/job/Ibin_uJ64__rtabmap__ubuntu_jammy_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__rtabmap__ubuntu_jammy_amd64__binary" alt="Build Status"/></td>
         </tr>
         <tr>
             <td>Rolling</td>

app/android/jni/CMakeLists.txt

@@ -114,35 +114,7 @@ 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,9 +108,8 @@ void CameraMobile::close()
     dataReady_.release();
 }
 
-void CameraMobile::resetOrigin(const rtabmap::Transform & offset)
+void CameraMobile::resetOrigin()
 {
-    manualOriginOffset_ = offset;
 	originUpdate_ = true;
 }
 
@@ -194,7 +193,7 @@ void CameraMobile::poseReceived(const Transform & pose, double deviceStamp)
 			previousAnchorPose_.setNull();
 			previousAnchorLinearVelocity_.clear();
 			previousAnchorStamp_ = 0.0;
-            originOffset_ = manualOriginOffset_.isNull() ? pose.translation().inverse() : manualOriginOffset_;
+			originOffset_ = pose.translation().inverse();
 			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(const rtabmap::Transform & offset = rtabmap::Transform());
+	void resetOrigin();
 	virtual bool isCalibrated() const;
 
 	virtual bool odomProvided() const { return true; }
@@ -150,7 +150,6 @@ private:
 	EnvSensors lastEnvSensors_;
 	Transform originOffset_;
 	bool originUpdate_;
-    rtabmap::Transform manualOriginOffset_;
 	float upstreamRelocalizationAccThr_;
 	rtabmap::Transform previousAnchorPose_;
 	std::vector<float> previousAnchorLinearVelocity_;

app/android/jni/Measure.h

@@ -1,65 +0,0 @@
-/*
-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,7 +132,6 @@ 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")));
@@ -151,7 +150,6 @@ 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")));
@@ -266,43 +264,13 @@ RTABMapApp::RTABMapApp() :
 		lastPoseEventTime_(0.0),
 		visualizingMesh_(false),
 		exportedMeshUpdated_(false),
-		measuresUpdated_(false),
+		optMesh_(new pcl::TextureMesh),
-        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())
 
 {
-    pcl::PointXYZRGB ptWhite;
+	mappingParameters_.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpDetectorStrategy(), "5")); // GFTT/FREAK
-    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);
@@ -310,6 +278,19 @@ 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
@@ -423,16 +404,13 @@ 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)
-	optTextureMesh_.reset(new pcl::TextureMesh);
+	optMesh_.reset(new pcl::TextureMesh);
-    optMesh_ = rtabmap::Mesh();
 	optTexture_ = cv::Mat();
 	optRefId_ = 0;
 	if(optRefPose_)
@@ -440,7 +418,6 @@ 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)
@@ -459,20 +436,19 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 			if(!cloudMat.empty())
 			{
 				LOGI("Open: Found optimized mesh! Visualizing it.");
-				optTextureMesh_ = rtabmap::util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, true);
+				optMesh_ = rtabmap::util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, true);
-                optMesh_ = rtabmap::Mesh();
 				optTexture_ = textures;
-                if(!optTexture_.empty())
+				if(!optTexture_.empty())
 				{
 					LOGI("Open: Texture mesh: %dx%d.", optTexture_.cols, optTexture_.rows);
 					status=3;
 				}
-				else if(optTextureMesh_->tex_polygons.size())
+				else if(optMesh_->tex_polygons.size())
 				{
 					LOGI("Open: Polygon mesh");
 					status=2;
 				}
-				else if(!optTextureMesh_->cloud.data.empty())
+				else if(!optMesh_->cloud.data.empty())
 				{
 					LOGI("Open: Point cloud");
 					status=1;
@@ -1354,8 +1330,7 @@ int RTABMapApp::Render()
 #ifdef DEBUG_RENDERING_PERFORMANCE
 				LOGD("Camera updateOnRender %fs", time.ticks());
 #endif
-                // We detect if we are in measuring mode if rtabmap is not running
+				if(main_scene_.background_renderer_ == 0 && camera_->getTextureId() != 0)
-				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);
@@ -1449,190 +1424,41 @@ 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)...",
-						optTextureMesh_->cloud.point_step==0?0:(int)optTextureMesh_->cloud.data.size()/optTextureMesh_->cloud.point_step,
+						optMesh_->cloud.point_step==0?0:(int)optMesh_->cloud.data.size()/optMesh_->cloud.point_step,
-						optTextureMesh_->tex_polygons.size()!=1?0:(int)optTextureMesh_->tex_polygons[0].size(),
+						optMesh_->tex_polygons.size()!=1?0:(int)optMesh_->tex_polygons[0].size(),
-						optTextureMesh_->tex_coordinates.size()!=1?0:(int)optTextureMesh_->tex_coordinates[0].size(),
+						optMesh_->tex_coordinates.size()!=1?0:(int)optMesh_->tex_coordinates[0].size(),
-						(int)optTextureMesh_->tex_materials.size(),
+						(int)optMesh_->tex_materials.size(),
 						optTexture_.cols, optTexture_.rows);
-				if(optTextureMesh_->tex_polygons.size() && optTextureMesh_->tex_polygons[0].size())
+				if(optMesh_->tex_polygons.size() && optMesh_->tex_polygons[0].size())
 				{
-                    optMesh_ = rtabmap::Mesh();
+					rtabmap::Mesh mesh;
-                    optMesh_.gains[0] = optMesh_.gains[1] = optMesh_.gains[2] = 1.0;
+					mesh.gains[0] = mesh.gains[1] = mesh.gains[2] = 1.0;
-                    optMesh_.cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
+					mesh.cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
-                    optMesh_.normals.reset(new pcl::PointCloud<pcl::Normal>);
+					mesh.normals.reset(new pcl::PointCloud<pcl::Normal>);
-					pcl::fromPCLPointCloud2(optTextureMesh_->cloud, *optMesh_.cloud);
+					pcl::fromPCLPointCloud2(optMesh_->cloud, *mesh.cloud);
-					pcl::fromPCLPointCloud2(optTextureMesh_->cloud, *optMesh_.normals);
+					pcl::fromPCLPointCloud2(optMesh_->cloud, *mesh.normals);
-                    bool hasColors = false;
+					mesh.polygons = optMesh_->tex_polygons[0];
-                    for(unsigned int i=0; i<optTextureMesh_->cloud.fields.size(); ++i)
+					mesh.pose.setIdentity();
-                    {
+					if(optMesh_->tex_coordinates.size())
-                        if(optTextureMesh_->cloud.fields[i].name.compare("rgb") == 0)
-                        {
-                            hasColors = true;
-                            break;
-                        }
-                    }
-                    if(!hasColors)
-                    {
-                        std::uint8_t r = 255, g = 255, b = 255;    // White
-                        std::uint32_t rgb = ((std::uint32_t)r << 16 | (std::uint32_t)g << 8 | (std::uint32_t)b);
-                        for(size_t i=0; i<optMesh_.cloud->size(); ++i)
-                        {
-                            optMesh_.cloud->at(i).rgb = *reinterpret_cast<float*>(&rgb);
-                        }
-                    }
-                    optMesh_.polygons = optTextureMesh_->tex_polygons[0];
-					if(optTextureMesh_->tex_coordinates.size())
 					{
-                        optMesh_.texCoords = optTextureMesh_->tex_coordinates[0];
+						mesh.texCoords = optMesh_->tex_coordinates[0];
-                        optMesh_.texture = optTexture_;
+						mesh.texture = optTexture_;
 					}
-					main_scene_.addMesh(g_optMeshId, optMesh_, rtabmap::opengl_world_T_rtabmap_world, true);
+					main_scene_.addMesh(g_optMeshId, mesh, 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(optTextureMesh_->cloud, *cloud);
+					pcl::fromPCLPointCloud2(optMesh_->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_)
 			{
 				rtabmapMutex_.lock();
@@ -1750,8 +1576,7 @@ int RTABMapApp::Render()
 			if(main_scene_.hasCloud(g_optMeshId))
 			{
 				main_scene_.clear();
-				optTextureMesh_.reset(new pcl::TextureMesh);
+				optMesh_.reset(new pcl::TextureMesh);
-                optMesh_ = rtabmap::Mesh();
 				optTexture_ = cv::Mat();
 			}
 
@@ -1814,8 +1639,6 @@ 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;
@@ -2447,395 +2270,6 @@ 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);
@@ -2903,15 +2337,6 @@ 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)
@@ -2926,10 +2351,6 @@ void RTABMapApp::setWireframe(bool enabled)
 {
 	main_scene_.setWireframe(enabled);
 }
-void RTABMapApp::setTextureColorSeamsHidden(bool hidden)
-{
-    main_scene_.setTextureColorSeamsHidden(hidden);
-}
 
 void RTABMapApp::setLocalizationMode(bool enabled)
 {
@@ -3117,7 +2538,7 @@ void RTABMapApp::setDepthConfidence(int value)
 void RTABMapApp::setExportPointCloudFormat(const std::string & format)
 {
 #if defined(RTABMAP_PDAL) || defined(RTABMAP_LIBLAS)
-    if(format == "las" || format == "laz") {
+    if(format == "las") {
         exportPointCloudFormat_ = format;
     }
     else
@@ -3289,7 +2710,6 @@ bool RTABMapApp::exportMesh(
 		int optimizedMinClusterSize,
 		float optimizedMaxTextureDistance,
 		int optimizedMinTextureClusterSize,
-        int textureVertexColorPolicy,
 		bool blockRendering)
 {
 	// make sure createdMeshes_ is not modified while exporting! We don't
@@ -3578,7 +2998,7 @@ bool RTABMapApp::exportMesh(
 									0,
 									mergedClouds,
 									optimizedColorRadius,
-                                    !(textureSize > 0 && textureVertexColorPolicy == 0),
+									textureSize == 0,
 									optimizedCleanWhitePolygons,
 									optimizedMinClusterSize);
 
@@ -3820,9 +3240,7 @@ 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);
 				}
@@ -3863,28 +3281,9 @@ bool RTABMapApp::exportMesh(
 				}
 				else if(textureMesh->tex_materials.size())
 				{
-                    bool hasColors = false;
+					pcl::PointCloud<pcl::PointNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointNormal>);
-                    for(unsigned int i=0; i<textureMesh->cloud.fields.size(); ++i)
+					pcl::fromPCLPointCloud2(textureMesh->cloud, *cloud);
-                    {
+					cv::Mat cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*cloud, rtabmap::Transform(), false).data()); // for database
-                        if(textureMesh->cloud.fields[i].name.compare("rgb") == 0)
-                        {
-                            hasColors = true;
-                            break;
-                        }
-                    }
-                    cv::Mat cloudMat;
-                    if(hasColors)
-                    {
-                        pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
-                        pcl::fromPCLPointCloud2(textureMesh->cloud, *cloud);
-                        cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*cloud, rtabmap::Transform(), false).data()); // for database
-                    }
-                    else
-                    {
-                        pcl::PointCloud<pcl::PointNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointNormal>);
-                        pcl::fromPCLPointCloud2(textureMesh->cloud, *cloud);
-                        cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*cloud, rtabmap::Transform(), false).data()); // for database
-                    }
 
 					// save in database
 					std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons(textureMesh->tex_polygons.size());
@@ -4093,8 +3492,7 @@ bool RTABMapApp::exportMesh(
 bool RTABMapApp::postExportation(bool visualize)
 {
 	LOGI("postExportation(visualize=%d)", visualize?1:0);
-	optTextureMesh_.reset(new pcl::TextureMesh);
+	optMesh_.reset(new pcl::TextureMesh);
-    optMesh_= rtabmap::Mesh();
 	optTexture_ = cv::Mat();
 	exportedMeshUpdated_ = false;
 
@@ -4115,9 +3513,8 @@ bool RTABMapApp::postExportation(bool visualize)
 			if(!cloudMat.empty())
 			{
 				LOGI("postExportation: Found optimized mesh! Visualizing it.");
-				optTextureMesh_ = rtabmap::util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, true);
+				optMesh_ = rtabmap::util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, true);
-                optMesh_ = rtabmap::Mesh();
+				optTexture_ = textures;
-                optTexture_ = textures;
 
 				boost::mutex::scoped_lock  lock(renderingMutex_);
 				visualizingMesh_ = true;
@@ -4209,9 +3606,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" || exportPointCloudFormat_ == "laz")) {
+        if(polygonMesh->polygons.empty() && exportPointCloudFormat_ == "las") {
             // Point cloud LAS
-            std::string filePath = directory + UDirectory::separator() + name + (exportPointCloudFormat_ == "las"? ".las" : ".laz");
+            std::string filePath = directory + UDirectory::separator() + name + ".las";
             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);
@@ -4287,7 +3684,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 = rtabmap::util3d::saveOBJFile(filePath, *textureMesh) == 0;
+			success = pcl::io::saveOBJFile(filePath, *textureMesh) == 0;
 
 			if(success)
 			{
@@ -4402,66 +3799,6 @@ 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,7 +48,6 @@ 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 {
@@ -85,7 +84,7 @@ class RTABMapApp : public UEventsHandler {
 #ifdef __ANDROID__
   bool startCamera(JNIEnv* env, jobject iBinder, jobject context, jobject activity, int driver);
 #else // __APPLE__
-  bool startCamera();
+    bool startCamera();
 #endif
   // Allocate OpenGL resources for rendering, mainly for initializing the Scene.
   void InitializeGLContent();
@@ -128,7 +127,6 @@ 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);
@@ -178,19 +176,10 @@ 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,
@@ -214,7 +203,6 @@ 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;
@@ -276,26 +264,11 @@ class RTABMapApp : public UEventsHandler {
 
   bool visualizingMesh_;
   bool exportedMeshUpdated_;
-  pcl::TextureMesh::Ptr optTextureMesh_;
+  pcl::TextureMesh::Ptr optMesh_;
   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/point_cloud_drawable.cpp

@@ -170,12 +170,10 @@ 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"
 
@@ -189,14 +187,12 @@ 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"
 
@@ -204,7 +200,6 @@ const std::string kTextureMeshLightingVertexShader =
     "uniform mat3 uN;\n"
     "uniform vec3 uLightingDirection;\n"
 
-	"varying vec3 vColor;\n"
     "varying vec2 vTexCoord;\n"
     "varying float vLightWeighting;\n"
 
@@ -218,7 +213,6 @@ 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"
@@ -231,22 +225,11 @@ 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;\n"
+    "  vec4 textureColor = texture2D(uTexture, vTexCoord);\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 =
@@ -260,12 +243,11 @@ const std::string kTextureMeshBlendingFragmentShader =
 	"uniform vec2 uScreenScale;\n"
 	"uniform float uNearZ;\n"
 	"uniform float uFarZ;\n"
-	"varying vec3 vColor;\n"
     "varying vec2 vTexCoord;\n"
 	"varying float vLightWeighting;\n"
 	""
     "void main() {\n"
-	"  vec4 textureColor = texture2D(uTexture, vTexCoord);\n"
+    "  vec4 textureColor = texture2D(uTexture, vTexCoord);\n"
     "  float alpha = 1.0;\n"
 	"  vec2 coord = uScreenScale * gl_FragCoord.xy;\n;"
 	"  vec4 depthPacked = texture2D(uDepthTexture, coord);\n"
@@ -1026,8 +1008,7 @@ void PointCloudDrawable::Render(
 		float nearClipPlane,
 	    float farClipPlane,
 		bool packDepthToColorChannel,
-		bool wireFrame,
+		bool wireFrame) const
-        bool hideSeams) const
 {
 	if(vertex_buffer_ && nPoints_ && visible_ && !shaderPrograms_.empty())
 	{
@@ -1146,16 +1127,12 @@ void PointCloudDrawable::Render(
 
 				attribute_texture = glGetAttribLocation(program, "aTexCoord");
 				glEnableVertexAttribArray(attribute_texture);
-                
-                if(depthTexture == 0)
-                {
-                    GLuint hideSeams_handle = glGetUniformLocation(program, "uHideSeams");
-                    glUniform1i(hideSeams_handle, hideSeams?1:0);
-                }
 			}
-
+			else
-            attribute_color = glGetAttribLocation(program, "aColor");
+			{
-            glEnableVertexAttribArray(attribute_color);
+				attribute_color = glGetAttribLocation(program, "aColor");
+				glEnableVertexAttribArray(attribute_color);
+			}
 		}
 		tango_gl::util::CheckGlError("Pointcloud::Render() common");
 
@@ -1167,7 +1144,7 @@ void PointCloudDrawable::Render(
 			{
 				glVertexAttribPointer(attribute_texture, 2, GL_FLOAT, GL_FALSE, (hasNormals_?9:6)*sizeof(GLfloat), (GLvoid*) (4 * sizeof(GLfloat)));
 			}
-			if(!packDepthToColorChannel)
+			else 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,8 +98,7 @@ private:
 		  float nearClipPlane = 0, // nonnull if depthTexture>0
 		  float farClipPlane = 0,  // nonnull if depthTexture>0
 		  bool packDepthToColorChannel = false,
-		  bool wireFrame = false,
+		  bool wireFrame = false) const;
-          bool hideSeams = false) const;
 
  private:
   template<class PointT>

app/android/jni/quad_color.cpp

@@ -1,69 +0,0 @@
-/*
- * 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

@@ -1,35 +0,0 @@
-/*
- * 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 Scene::kHeightOffset = glm::vec3(0.0f, -1.3f, 0.0f);
+const glm::vec3 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,7 +93,6 @@ Scene::Scene() :
 		lighting_(false),
 		backfaceCulling_(true),
 		wireFrame_(false),
-		textureColorSeamsHidden_(true),
 		r_(0.0f),
 		g_(0.0f),
 		b_(0.0f),
@@ -125,7 +124,6 @@ void Scene::InitGLContent()
 
 	UASSERT(axis_ == 0);
 
-    TextDrawable::createShaderProgram();
 
 	axis_ = new tango_gl::Axis();
 	frustum_ = new tango_gl::Frustum();
@@ -168,7 +166,6 @@ void Scene::DeleteResources() {
 		background_renderer_ = 0;
 	}
 
-    TextDrawable::releaseShaderProgram();
 	PointCloudDrawable::releaseShaderPrograms();
 
 	if (graph_shader_program_) {
@@ -201,10 +198,6 @@ void Scene::clear()
 	{
 		delete iter->second;
 	}
-    clearLines();
-    clearQuads();
-    clearTexts();
-    clearCircles();
 	if(trace_)
 	{
 		trace_->ClearVertexArray();
@@ -222,39 +215,6 @@ 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) {
@@ -484,14 +444,8 @@ 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;
@@ -533,7 +487,7 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
         (blending_ &&
          gesture_camera_->GetCameraType()!=tango_gl::GestureCamera::kTopOrtho &&
          mapRendering_ && meshRendering_ &&
-         (positiveCloudIds > 1 || (renderBackgroundCamera && wireFrame_)));
+         (cloudsToDraw.size() > 1 || (renderBackgroundCamera && wireFrame_)));
 
 	if(onlineBlending && fboId_)
 	{
@@ -675,18 +629,9 @@ 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_, textureColorSeamsHidden_);
+		cloud->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, meshRenderingTexture_, lighting_, distanceToCameraSqr, onlineBlending?depthTexture_:0, screenWidth_, screenHeight_, gesture_camera_->getNearClipPlane(), gesture_camera_->getFarClipPlane(), false, wireFrame_);
 	}
 
-    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)
 	{
         if(renderBackgroundCamera && meshRendering_)
@@ -698,57 +643,6 @@ 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)
 	{
@@ -906,7 +800,12 @@ 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);
+	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);
@@ -914,16 +813,6 @@ void Scene::addCloud(
 	pointClouds_.insert(std::make_pair(id, drawable));
 }
 
-void Scene::removeCloudOrMesh(int id)
-{
-    std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
-    if(iter != pointClouds_.end())
-    {
-        delete iter->second;
-        pointClouds_.erase(iter);
-    }
-}
-
 void Scene::addMesh(
 		int id,
 		const rtabmap::Mesh & mesh,
@@ -931,7 +820,12 @@ void Scene::addMesh(
 		bool createWireframe)
 {
 	LOGI("add mesh %d", id);
-    removeCloudOrMesh(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(mesh, createWireframe);
@@ -991,160 +885,6 @@ 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,7 +33,6 @@
 #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>
@@ -42,16 +41,12 @@
 #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();
@@ -72,10 +67,6 @@ 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.
@@ -128,50 +119,11 @@ 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);
@@ -193,7 +145,6 @@ 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);
 
@@ -237,10 +188,6 @@ 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_;
 
@@ -256,7 +203,6 @@ 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(sin(theta) * radius);
     vertices.push_back(0);
+    vertices.push_back(sin(theta) * radius);
   }
   SetVertices(vertices);
 }

app/android/jni/text_drawable.cpp

@@ -1,366 +0,0 @@
-/*
-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

@@ -1,71 +0,0 @@
-/*
-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,15 +8,12 @@
 
 /* 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 */; };
@@ -158,21 +155,13 @@
 /* End PBXBuildFile section */
 
 /* Begin PBXFileReference section */
-		44D4681D2D538A4100B094BA /* RTABMapApp.entitlements */ = {isa = PBXFileReference; lastKnownFileType = text.plist.entitlements; name = RTABMapApp.entitlements; path = RTABMapApp/RTABMapApp.entitlements; sourceTree = "<group>"; };
 		4E0D83822621F52C00C879AC /* Settings.bundle */ = {isa = PBXFileReference; lastKnownFileType = "wrapper.plug-in"; path = Settings.bundle; sourceTree = "<group>"; };
-		4E1E9CF22D617D08000CB881 /* Measure.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; name = Measure.h; path = ../android/jni/Measure.h; sourceTree = "<group>"; };
-		4E1E9CF32D618079000CB881 /* quad_color.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; name = quad_color.h; path = ../android/jni/quad_color.h; sourceTree = "<group>"; };
-		4E1E9CF42D618079000CB881 /* text_drawable.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; name = text_drawable.h; path = ../android/jni/text_drawable.h; sourceTree = "<group>"; };
-		4E1E9CF52D618079000CB881 /* text_drawable.cpp */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.cpp.cpp; name = text_drawable.cpp; path = ../android/jni/text_drawable.cpp; sourceTree = "<group>"; };
-		4E1E9CF62D618079000CB881 /* quad_color.cpp */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.cpp.cpp; name = quad_color.cpp; path = ../android/jni/quad_color.cpp; sourceTree = "<group>"; };
-		4E1E9CF92D619025000CB881 /* text_atlas_png.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; name = text_atlas_png.h; path = RTABMapApp/text_atlas_png.h; sourceTree = "<group>"; };
 		4E20B24D266AB94300316EE6 /* Images.xcassets */ = {isa = PBXFileReference; lastKnownFileType = folder.assetcatalog; name = Images.xcassets; path = RTABMapApp/Images.xcassets; sourceTree = SOURCE_ROOT; };
 		4E20B24F266AB95600316EE6 /* Images.xcassets */ = {isa = PBXFileReference; lastKnownFileType = folder.assetcatalog; name = Images.xcassets; path = RTABMapApp/Images.xcassets; sourceTree = "<group>"; };
 		4E2C516525A63119005CEDBD /* DatabaseView.swift */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.swift; name = DatabaseView.swift; path = RTABMapApp/DatabaseView.swift; sourceTree = "<group>"; };
 		4E2C516625A63119005CEDBD /* VerticalScrollerView.swift */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.swift; name = VerticalScrollerView.swift; path = RTABMapApp/VerticalScrollerView.swift; sourceTree = "<group>"; };
 		4E401BE825CC92C100240A56 /* ProgressionStatus.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; name = ProgressionStatus.h; path = ../android/jni/ProgressionStatus.h; sourceTree = "<group>"; };
 		4EB1A66525A0D90B0043C7BF /* RTABMap.swift */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.swift; name = RTABMap.swift; path = RTABMapApp/RTABMap.swift; sourceTree = "<group>"; };
-		4ED73AD02D7FECF3007899C2 /* liblaszip.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = liblaszip.a; path = RTABMapApp/Libraries/lib/liblaszip.a; sourceTree = "<group>"; };
 		4EE015C2259A2AF0008CCE65 /* RTABMapApp.app */ = {isa = PBXFileReference; explicitFileType = wrapper.application; includeInIndex = 0; path = RTABMapApp.app; sourceTree = BUILT_PRODUCTS_DIR; };
 		4EE016AF259BE441008CCE65 /* ViewController.swift */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.swift; name = ViewController.swift; path = RTABMapApp/ViewController.swift; sourceTree = "<group>"; };
 		4EE016B0259BE441008CCE65 /* SceneDelegate.swift */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.swift; name = SceneDelegate.swift; path = RTABMapApp/SceneDelegate.swift; sourceTree = "<group>"; };
@@ -357,7 +346,6 @@
 			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 */,
@@ -487,7 +475,6 @@
 		4EE01634259BDCC7008CCE65 /* RTABMapApp */ = {
 			isa = PBXGroup;
 			children = (
-				44D4681D2D538A4100B094BA /* RTABMapApp.entitlements */,
 				4E20B24F266AB95600316EE6 /* Images.xcassets */,
 				4EFD0BB0259D503200575D88 /* NativeWrapper */,
 				4EFD0B62259D501E00575D88 /* tango-gl */,
@@ -509,7 +496,6 @@
 		4EFD0B2A259D4DE900575D88 /* Frameworks */ = {
 			isa = PBXGroup;
 			children = (
-				4ED73AD02D7FECF3007899C2 /* liblaszip.a */,
 				4EFAA9422CAE4E960055DA51 /* liblas.a */,
 				4EFD0F4D259D67D900575D88 /* liblibjpeg-turbo.a */,
 				4EFD0F4C259D67D900575D88 /* liblibpng.a */,
@@ -693,12 +679,6 @@
 		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 */,
@@ -811,7 +791,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:24: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:21:DefaultValue $version\" \"${SRCROOT}/Settings.bundle/Root.plist\"\n";
 		};
 /* End PBXShellScriptBuildPhase section */
 
@@ -820,8 +800,6 @@
 			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 */,
@@ -1008,7 +986,6 @@
 				ASSETCATALOG_COMPILER_GLOBAL_ACCENT_COLOR_NAME = AccentColor;
 				CLANG_CXX_LIBRARY = "libc++";
 				CLANG_USE_OPTIMIZATION_PROFILE = NO;
-				CODE_SIGN_ENTITLEMENTS = RTABMapApp/RTABMapApp.entitlements;
 				CODE_SIGN_IDENTITY = "Apple Development";
 				CODE_SIGN_STYLE = Automatic;
 				CURRENT_PROJECT_VERSION = 1;
@@ -1036,7 +1013,7 @@
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/lib",
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/share/OpenCV/3rdparty/lib",
 				);
-				MARKETING_VERSION = 0.21.11;
+				MARKETING_VERSION = 0.21.8;
 				OTHER_CFLAGS = "";
 				PRODUCT_BUNDLE_IDENTIFIER = com.introlab.rtabmap;
 				PRODUCT_NAME = "$(TARGET_NAME)";
@@ -1066,7 +1043,6 @@
 				ASSETCATALOG_COMPILER_GLOBAL_ACCENT_COLOR_NAME = AccentColor;
 				CLANG_CXX_LIBRARY = "libc++";
 				CLANG_USE_OPTIMIZATION_PROFILE = NO;
-				CODE_SIGN_ENTITLEMENTS = RTABMapApp/RTABMapApp.entitlements;
 				CODE_SIGN_IDENTITY = "Apple Development";
 				CODE_SIGN_STYLE = Automatic;
 				CURRENT_PROJECT_VERSION = 1;
@@ -1094,7 +1070,7 @@
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/lib",
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/share/OpenCV/3rdparty/lib",
 				);
-				MARKETING_VERSION = 0.21.11;
+				MARKETING_VERSION = 0.21.8;
 				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" spacing="30" translatesAutoresizingMaskIntoConstraints="NO" id="kPv-DO-hpd">
+                            <stackView opaque="NO" contentMode="scaleToFill" axis="vertical" translatesAutoresizingMaskIntoConstraints="NO" id="kPv-DO-hpd">
-                                <rect key="frame" x="323" y="261" width="60" height="330"/>
+                                <rect key="frame" x="323" y="366" width="60" height="120"/>
                                 <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="91" width="60" height="58.666666666666686"/>
+                                        <rect key="frame" x="0.0" y="61.000000000000007" width="60" height="58.666666666666664"/>
                                         <constraints>
                                             <constraint firstAttribute="height" constant="60" id="DQI-eQ-v3X"/>
                                             <constraint firstAttribute="width" constant="60" id="Oeu-YT-wMz"/>
@@ -47,32 +47,6 @@
                                             <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">
@@ -105,10 +79,22 @@
                             </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"/>
@@ -160,17 +146,8 @@
                                     <action selector="clipDistanceAction:" destination="zah-iI-EPt" eventType="valueChanged" id="iKj-nz-0KW"/>
                                 </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="637.33333333333337" width="291" height="20.333333333333371"/>
-                                <accessibility key="accessibilityConfiguration">
-                                    <accessibilityTraits key="traits" staticText="YES" notEnabled="YES"/>
-                                </accessibility>
-                                <fontDescription key="fontDescription" type="system" pointSize="17"/>
-                                <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"/>
+                                <rect key="frame" x="135.66666666666666" y="727" width="121.99999999999997" height="31"/>
                                 <constraints>
                                     <constraint firstAttribute="width" constant="118" id="KDl-lP-Jj7"/>
                                 </constraints>
@@ -178,84 +155,41 @@
                                     <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">
+                            <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="10" y="396.66666666666669" width="62.666666666666671" height="60"/>
+                                <rect key="frame" x="51" y="668.33333333333337" width="291" height="20.333333333333371"/>
-                                <constraints>
+                                <color key="backgroundColor" white="0.0" alpha="0.3002232142857143" colorSpace="custom" customColorSpace="genericGamma22GrayColorSpace"/>
-                                    <constraint firstAttribute="height" constant="60" id="fe3-pR-D97"/>
+                                <accessibility key="accessibilityConfiguration">
-                                    <constraint firstAttribute="width" constant="60" id="t1H-Yp-Ayy"/>
+                                    <accessibilityTraits key="traits" staticText="YES" notEnabled="YES"/>
-                                </constraints>
+                                </accessibility>
-                                <color key="tintColor" systemColor="systemGreenColor"/>
+                                <fontDescription key="fontDescription" type="system" pointSize="17"/>
-                                <inset key="imageEdgeInsets" minX="0.0" minY="0.0" maxX="2.2250738585072014e-308" maxY="0.0"/>
+                                <color key="textColor" white="1" alpha="1" colorSpace="custom" customColorSpace="genericGamma22GrayColorSpace"/>
-                                <state key="normal" backgroundImage="accessibility.badge.arrow.up.right" catalog="system"/>
+                                <nil key="highlightedColor"/>
-                                <connections>
+                            </label>
-                                    <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="bottom" secondItem="wOV-tT-ZlN" secondAttribute="bottom" constant="25" id="Kab-84-pPU"/>
+                            <constraint firstItem="qAR-pI-uT2" firstAttribute="trailing" secondItem="773-n4-lob" secondAttribute="trailing" constant="20" id="N8s-Yg-VsL"/>
                             <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="qAR-pI-uT2" firstAttribute="bottom" secondItem="773-n4-lob" secondAttribute="bottom" constant="20" id="h09-nI-8yH"/>
+                            <constraint firstItem="Z1x-Af-hdf" firstAttribute="centerX" secondItem="sqF-4e-2HU" secondAttribute="centerX" id="i2j-zQ-m1V"/>
-                            <constraint firstItem="9oq-op-NGV" firstAttribute="top" secondItem="JMv-UE-cvK" secondAttribute="bottom" constant="113.33" id="icg-DR-ZpX"/>
+                            <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="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>
@@ -264,29 +198,24 @@
                         </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="-235.87786259541983" y="-586.61971830985919"/>
+            <point key="canvasLocation" x="-234.375" y="-586.25"/>
         </scene>
         <!--Unsupported View Controller-->
         <scene sceneID="sJB-xy-u63">
@@ -297,14 +226,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="21" y="352.33333333333331" width="351.33333333333331" height="33.666666666666686"/>
+                                <rect key="frame" x="28" y="341.33333333333331" width="337.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="21" y="394" width="351.33333333333331" height="64.333333333333314"/>
+                                <rect key="frame" x="28" y="383" width="337.33333333333331" height="86.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>
+                                <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>
                                 <fontDescription key="fontDescription" style="UICTFontTextStyleBody"/>
                                 <nil key="textColor"/>
                                 <nil key="highlightedColor"/>
@@ -329,26 +258,16 @@
         </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,19 +55,11 @@ class DatabaseView: UIView {
         super.init(frame: frame)
         self.databaseURL = databaseURL
         commonInit(databasePath: databaseURL.path)
-        if let image = ViewController.previewImages[databaseURL.path]
+        DispatchQueue.global().async {
-        {
+            let downloadedImage = getPreviewImage(databasePath: databaseURL.path)
-            self.coverImageView.image = image
+          DispatchQueue.main.async {
-        }
+            self.coverImageView.image = downloadedImage
-        else
+          }
-        {
-            DispatchQueue.global().async {
-                let downloadedImage = getPreviewImage(databasePath: databaseURL.path)
-                DispatchQueue.main.async {
-                    self.coverImageView.image = downloadedImage
-                    ViewController.previewImages[databaseURL.path] = downloadedImage
-                }
-            }
         }
     }
 

app/ios/RTABMapApp/NativeWrapper.cpp

@@ -150,12 +150,11 @@ 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, textureVertexColorPolicy, blockRendering);
+        return native(object)->exportMesh(cloudVoxelSize, regenerateCloud, meshing, textureSize, textureCount, normalK, optimized, optimizedVoxelSize, optimizedDepth, optimizedMaxPolygons, optimizedColorRadius, optimizedCleanWhitePolygons, optimizedMinClusterSize, optimizedMaxTextureDistance, optimizedMinTextureClusterSize, blockRendering);
     }
     else
     {
@@ -435,13 +434,6 @@ 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)
@@ -630,53 +622,3 @@ 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,7 +55,6 @@ 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);
@@ -91,7 +90,6 @@ 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);
@@ -117,14 +115,6 @@ 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,27 +152,6 @@ 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);
     }
@@ -197,7 +176,6 @@ class RTABMap {
         optimizedMinClusterSize: Int,
         optimizedMaxTextureDistance: Float,
         optimizedMinTextureClusterSize: Int,
-        textureVertexColorPolicy: Int,
         blockRendering: Bool) -> Bool
     {
        return exportMeshNative(native_rtabmap,
@@ -216,7 +194,6 @@ class RTABMap {
                                     Int32(optimizedMinClusterSize),
                                     optimizedMaxTextureDistance,
                                     Int32(optimizedMinTextureClusterSize),
-                            		Int32(textureVertexColorPolicy),
                                     blockRendering)
     }
     
@@ -246,7 +223,7 @@ class RTABMap {
         return Int(renderNative(native_rtabmap))
     }
     
-    func startCamera(imageOverlayInFirstPerson: Bool = true) -> Bool {
+    func startCamera() -> Bool {
         return startCameraNative(native_rtabmap)
     }
     
@@ -426,9 +403,6 @@ class RTABMap {
     func setWireframe(enabled: Bool) {
         setWireframeNative(native_rtabmap, enabled)
     }
-    func setTextureColorSeamsHidden(hidden: Bool) {
-        setTextureColorSeamsHiddenNative(native_rtabmap, hidden)
-    }
     func setLocalizationMode(enabled: Bool) {
         setLocalizationModeNative(native_rtabmap, enabled)
     }

app/ios/RTABMapApp/RTABMapApp.entitlements

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

app/ios/RTABMapApp/ViewController.swift

@@ -45,8 +45,6 @@ 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
@@ -55,9 +53,8 @@ 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 and localizing
+        STATE_VISUALIZING_CAMERA,     // Camera/Motion on  - Showing optimized mesh
-        STATE_VISUALIZING_WHILE_LOADING, // Camera/Motion off - Loading data while showing optimized mesh
+        STATE_VISUALIZING_WHILE_LOADING // Camera/Motion off - Loading data while showing optimized mesh
-        STATE_VISUALIZING_AND_MEASURING    // Camera/Motion on  - Showing optimized mesh without localizing and measuring tools enabled
     }
     private var mState: State = State.STATE_WELCOME;
     private func getStateString(state: State) -> String {
@@ -76,8 +73,6 @@ 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"
         }
@@ -98,14 +93,9 @@ 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!
@@ -116,11 +106,6 @@ 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{
@@ -143,7 +128,6 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
     }
     
     @objc func defaultsChanged(){
-        print("defaultsChanged()")
         updateDisplayFromDefaults()
     }
     
@@ -191,46 +175,13 @@ 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()
@@ -286,20 +237,6 @@ 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)
@@ -325,17 +262,14 @@ 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)
                 }
 
@@ -346,11 +280,10 @@ 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 availableMem = self.getAvailableMemory()
+            let usedMem = self.getMemoryUsage()
-            let usedMem = self.mMaximumMemory - availableMem;
             self.statusLabel.text =
                 "Status: " + (status == 1 && msg.isEmpty ? self.mState == State.STATE_CAMERA ? "Camera Preview" : "Idle" : msg) + "\n" +
-            	"RAM Usage (MB): \(usedMem) / \(self.mMaximumMemory)"
+                "Memory Usage: \(usedMem) MB"
         }
     }
         
@@ -383,8 +316,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
                            pitch: Float,
                            yaw: Float)
     {
-        let availableMem = self.getAvailableMemory()
+        let usedMem = self.getMemoryUsage()
-        let usedMem = self.mMaximumMemory - availableMem;
         
         if(loopClosureId > 0)
         {
@@ -407,7 +339,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
                 self.statusLabel.text =
                     self.statusLabel.text! +
                     "Status: \(self.getStateString(state: self.mState))\n" +
-                	"RAM Usage (MB): \(usedMem) / \(self.mMaximumMemory)"
+                    "Memory Usage : \(usedMem) MB"
             }
             if self.debugShown {
                 self.statusLabel.text =
@@ -494,43 +426,6 @@ 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)
-                }
-            }
         }
     }
     
@@ -544,22 +439,34 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         }
     }
     
-    func getAvailableMemory() -> Int {
+    func getMemoryUsage() -> UInt64 {
-        return os_proc_available_memory()/(1024*1024)
+        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
+        }
     }
     
     @objc func appMovedToBackground() {
-        print("appMovedToBackground()")
+        if(mState == .STATE_VISUALIZING_CAMERA || mState == .STATE_MAPPING || mState == .STATE_CAMERA)
-        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)
         {
@@ -603,33 +510,29 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         switch AVCaptureDevice.authorizationStatus(for: .video) {
             case .authorized: // The user has previously granted access to the camera.
                 print("Start Camera")
-            	rtabmap!.startCamera()
+                rtabmap!.startCamera()
                 let configuration = ARWorldTrackingConfiguration()
                 var message = ""
-            	if(mState != .STATE_VISUALIZING_AND_MEASURING)
+                if(!UserDefaults.standard.bool(forKey: "LidarMode"))
-            	{
+                {
-                	if(!UserDefaults.standard.bool(forKey: "LidarMode"))
+                    message = "LiDAR is disabled (Settings->Mapping->LiDAR Mode = OFF), only tracked features will be mapped."
-                	{
+                    self.setMeshRendering(viewMode: 0)
-       		        	message = "LiDAR is disabled (Settings->Mapping->LiDAR Mode = OFF), only tracked features will be mapped."
+                }
-       		        	self.setMeshRendering(viewMode: 0)
+                else if !depthSupported
-        	    	}
+                {
-                	else if !depthSupported
+                    message = "The device does not have a LiDAR, only tracked features will be mapped. A LiDAR is required for accurate 3D reconstruction."
-                	{
+                    self.setMeshRendering(viewMode: 0)
-                    	message = "The device does not have a LiDAR, only tracked features will be mapped. A LiDAR is required for accurate 3D reconstruction."
+                }
-                    	self.setMeshRendering(viewMode: 0)
+                else
-                	}
+                {
-                	else
+                    configuration.frameSemantics = .sceneDepth
-                	{
+                }
-                    	configuration.frameSemantics = .sceneDepth
-                	}
-            	}
                 
                 session.run(configuration, options: [.resetSceneReconstruction, .resetTracking, .removeExistingAnchors])
                 
                 switch mState {
-                case .STATE_VISUALIZING_AND_MEASURING,
+                case .STATE_VISUALIZING:
-                    .STATE_VISUALIZING_CAMERA:
+                    updateState(state: .STATE_VISUALIZING_CAMERA)
-                    break // State should be already set
                 default:
                     locationManager?.startUpdatingLocation()
                     updateState(state: .STATE_CAMERA)
@@ -696,7 +599,6 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         var actionExportEnabled: Bool
         var actionOptimizeEnabled: Bool
         var actionSettingsEnabled: Bool
-        var actionMeasuringEnabled: Bool
         
         switch mState {
         case .STATE_CAMERA:
@@ -709,14 +611,9 @@ 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
@@ -724,7 +621,6 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
             actionExportEnabled = false
             actionOptimizeEnabled = false
             actionSettingsEnabled = false
-            actionMeasuringEnabled = false
         case .STATE_MAPPING:
             libraryButton.isEnabled = false
             libraryButton.isHidden = !mHudVisible
@@ -735,14 +631,9 @@ 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
@@ -750,11 +641,9 @@ 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
@@ -764,14 +653,9 @@ 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_PROCESSING
+            orthoDistanceSlider.isHidden = cameraMode != 3 || mState != .STATE_VISUALIZING_WHILE_LOADING
-            orthoGridSlider.isHidden = cameraMode != 3 || mState == .STATE_PROCESSING
+            orthoGridSlider.isHidden = cameraMode != 3 || mState != .STATE_VISUALIZING_WHILE_LOADING
-            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
@@ -779,7 +663,6 @@ 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
@@ -790,14 +673,9 @@ 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
@@ -805,7 +683,6 @@ 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
@@ -816,14 +693,9 @@ 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
@@ -831,11 +703,10 @@ 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 && mState != .STATE_VISUALIZING_AND_MEASURING)
+        if(mState != .STATE_MAPPING && mState != .STATE_CAMERA && mState != .STATE_VISUALIZING_CAMERA)
         {
             self.isPaused = true
             view?.enableSetNeedsDisplay = true
@@ -846,7 +717,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         {
             view?.enableSetNeedsDisplay = false
             self.isPaused = false
-            print("disableSetNeedsDisplay")
+            print("diaableSetNeedsDisplay")
         }
         
         if !self.isPaused {
@@ -855,23 +726,6 @@ 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
@@ -933,25 +787,6 @@ 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()
@@ -976,13 +811,6 @@ 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
@@ -998,7 +826,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 || self.mState == .STATE_VISUALIZING_AND_MEASURING) ? [] : .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) ? [] : .disabled, handler: { _ in
                 self.odomShown = !self.odomShown
                 self.rtabmap!.setOdomCloudShown(shown: self.odomShown)
                 self.resetNoTouchTimer(true)
@@ -1037,7 +865,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, name: UserDefaults.didChangeNotification, object: nil)
+                    notificationCenter.removeObserver(self)
                     UserDefaults.standard.reset()
                     self.registerSettingsBundle()
                     self.updateDisplayFromDefaults();
@@ -1053,17 +881,12 @@ 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_AND_MEASURING || 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_WHILE_LOADING ? [] : .disabled, handler: { _ in
                 self.lightingShown = !self.lightingShown
                 self.rtabmap!.setLighting(enabled: self.lightingShown)
                 self.resetNoTouchTimer(true)
@@ -1076,21 +899,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 || self.mState == .STATE_VISUALIZING_AND_MEASURING) ? [] : .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) ? [] : .disabled, handler: { _ in
                 self.setGLCamera(type: 0)
                 if(self.mState == .STATE_VISUALIZING)
                 {
                     self.rtabmap?.setLocalizationMode(enabled: true)
                     self.rtabmap?.setPausedMapping(paused: false);
-                    self.updateState(state: .STATE_VISUALIZING_CAMERA)
                     self.startCamera()
+                    self.updateState(state: .STATE_VISUALIZING_CAMERA)
                 }
                 else
                 {
                     self.resetNoTouchTimer(true)
                 }
             }),
-            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
+            UIAction(title: "Third-P. View", image: cameraMode == 1 ? UIImage(systemName: "checkmark.circle") : UIImage(systemName: "circle"), handler: { _ in
                 self.setGLCamera(type: 1)
                 self.resetNoTouchTimer(true)
             }),
@@ -1104,7 +927,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
             })
         ])
         
-        let showCloudMeshActions = mState != .STATE_VISUALIZING && mState != .STATE_VISUALIZING_CAMERA && mState != .STATE_VISUALIZING_AND_MEASURING && mState != .STATE_PROCESSING && mState != .STATE_VISUALIZING_WHILE_LOADING
+        let showCloudMeshActions = mState != .STATE_VISUALIZING && mState != .STATE_VISUALIZING_CAMERA && 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)
@@ -1555,7 +1378,6 @@ 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))
@@ -1577,17 +1399,16 @@ 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()
@@ -1896,7 +1717,6 @@ 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?
@@ -1930,8 +1750,6 @@ 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: {
                 
@@ -1951,7 +1769,6 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
                     optimizedMinClusterSize: optimizedMinClusterSize,
                     optimizedMaxTextureDistance: maxTextureDistance,
                     optimizedMinTextureClusterSize: minTextureClusterSize,
-                    textureVertexColorPolicy: textureVertexColorPolicy,
                     blockRendering: blockRendering)
                 
             }, completion:{
@@ -1972,17 +1789,14 @@ 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)
                             }
 
@@ -2042,8 +1856,6 @@ 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);
@@ -2086,7 +1898,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         })
     }
     
-    func stopMapping(ignoreSaving: Bool, offerPostProcessing: Bool = true)
+    func stopMapping(ignoreSaving: Bool)
     {
         session.pause()
         locationManager?.stopUpdatingLocation()
@@ -2101,11 +1913,11 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         {
             self.rtabmap?.setLocalizationMode(enabled: false)
         }
-        updateState(state: mState == .STATE_VISUALIZING_CAMERA || mState == .STATE_VISUALIZING_AND_MEASURING ? .STATE_VISUALIZING : .STATE_IDLE);
+        updateState(state: mState == .STATE_VISUALIZING_CAMERA ? .STATE_VISUALIZING : .STATE_IDLE);
         
         if !ignoreSaving
         {
-            if(mDataRecording || !offerPostProcessing)
+            if(mDataRecording)
             {
                 // Go directly to save
                 self.save()
@@ -2201,7 +2013,6 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
             }
             else {
                 if(status >= 1 && status<=3) {
-                    self.visualizationType = status-1;
                     self.updateState(state: .STATE_VISUALIZING);
                     self.resetNoTouchTimer(true);
                 }
@@ -2297,10 +2108,10 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
             if textField.text != "" {
                 self.dismiss(animated: true)
                 //Read TextFields text data
-                let fileName = textField.text! + (self.exportOBJPLYButton.title(for: .normal)!.contains("LAZ") ? ".laz" : ".zip")
+                let fileName = textField.text!+".zip"
                 let filePath = self.getDocumentDirectory().appendingPathComponent(fileName).path
                 if FileManager.default.fileExists(atPath: filePath) {
-                    let alert = UIAlertController(title: "File Already Exists", message: "\(fileName) already exists, do you want to overwrite it?", preferredStyle: .alert)
+                    let alert = UIAlertController(title: "File Already Exists", message: "Do you want to overwrite the existing file?", preferredStyle: .alert)
                     let yes = UIAlertAction(title: "Yes", style: .default) {
                         (UIAlertAction) -> Void in
                         self.writeExportedFiles(fileName: textField.text!);
@@ -2347,9 +2158,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
 
     func writeExportedFiles(fileName: String)
     {
-        let isLAZ = self.visualizationType==0 && self.exportOBJPLYButton.title(for: .normal)!.contains("LAZ")
+        let alertView = UIAlertController(title: "Exporting", message: "Please wait while zipping data to \(fileName+".zip")...", preferredStyle: .alert)
-        
-        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
@@ -2396,34 +2205,13 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
                         let fileURLs = try FileManager.default.contentsOfDirectory(at: exportDir, includingPropertiesForKeys: nil)
                         if(!fileURLs.isEmpty)
                         {
-                            if(isLAZ)
+                            do {
-                            {
+                                zipFileUrl = try Zip.quickZipFiles(fileURLs, fileName: fileName) // Zip
-                                zipFileUrl = self.getDocumentDirectory().appendingPathComponent(fileName+".laz")
+                                print("Zip file \(zipFileUrl.path) created (size=\(zipFileUrl.fileSizeString)")
-                                do {
+                                success = true
-                                    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
+                            catch {
-                            {
+                              print("Something went wrong while zipping")
-                                do {
-                                    zipFileUrl = try Zip.quickZipFiles(fileURLs, fileName: fileName) // Zip
-                                    print("Zip file \(zipFileUrl.path) created (size=\(zipFileUrl.fileSizeString)")
-                                    success = true
-                                }
-                                catch {
-                                    print("Something went wrong while zipping")
-                                }
                             }
                         }
                     } catch {
@@ -2439,7 +2227,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
                 }
                 if(success)
                 {
-                    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 alertShare = UIAlertController(title: "Mesh/Cloud Saved!", message: "\(fileName+".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)
@@ -2525,7 +2313,6 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
 
     @IBAction func recordAction(_ sender: UIButton) {
         rtabmap?.setPausedMapping(paused: false);
-        lowMemoryWarningShown = false
         updateState(state: .STATE_MAPPING)
     }
     
@@ -2557,26 +2344,6 @@ 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,27 +222,6 @@ 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
@@ -254,7 +233,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 -DWITH_LASZIP=ON -DWITH_STATIC_LASZIP=ON ..
+cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=12.0 -DCMAKE_INSTALL_PREFIX=$prefix -DCMAKE_FIND_ROOT_PATH=$prefix -DWITH_UTILITIES=OFF -DWITH_TESTS=OFF -DWITH_GEOTIFF=OFF ..
 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

@@ -1,40 +0,0 @@
-/*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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
-   "D6796BCF3A8756DBF9C95AC6DFB9DDE7EEEF7EAC67BD7FE879ED8F59DA5E8F6CB7FE3CFF3A33EFD5B3DEC6EDE7A2BBEEF7971BBFDDF6B13DFBB6A69F5F0E11BB6AC73BABCFDFC7878EB140EC59E700DCB9E7FBE7CED36EC76A33D703B07144D499A69F8B88C7F4FCCEDD22E26511F1C8CCDC79A8ABD26C7A1A3E19113BF52CDE22225E18110F8E886D23E2F69D65439C88F4EDFF9B23E2D9D5BC9B0688D52B33B78D888F47C4D8C1A7B54944EC18114FC8CC2795529E3B50E83BC6F8BECFD9BCF3FF49DE2EDAA4A70D7D6381FBFE4EB75D66ECD322628F6ADEEE11511F64778A886DAA79672C337644C49931BA4F5B64E6B6A594EF75E6ED111177EA4CFF75CF761E516D67AD7AEA4A292767E63763F473B77B66EE5A4AF95467DE7F54DBFF7D441CB63631D6E0F418BFDFBB6744BCB19AF7F71171CF6ADE2707881F11B159CCFF19988FDE38D6790B7DA9F71DFCBB768CE62039943745FFC1BFEB09317AF09FA4BD7BE64DE404A03DF9392EE63FF8779588D82F335F3389B6AC421F8C886BAB794FE859AF7E6F66447C6880F89F8DE660DA55F77EFC5535FDA09E5E8A0756D35F5F441B5E1EA3AF4189E6801F1111997950CFF6DF5B4A39771131E6F3C1685ECBAEBED1277FD3336F9699E2374C609BF549EF2462AC54D7C5F8FB60935934643559D355DD8511717444BC3B22CEEA59FE9CCCAC135716ADEDF2EBBBA23D3322DE19114746C45A75690EA8F4CCBB7142B1F689A667A3EBC888B85F343D2EFB47C455D5F27FCE616EC31C1E118746C4A77A96BDB75D76688C1FA4D60BA594CBA2E905E87A54CFAAF509F1B9A5946F0E10FF9A88F84E35FBA1D5747D85BC41443C656E2233B78E887B54EB7C7E116DF87644BCBF9AFDF0CCDCAFED867D49B5ECA288F8CFB5DDFE1A627F2BC6F7FFD1395E83A23E29BAB09472F2106D88E6BBE5AC057E6EEEF99DB1DB3003B84B357DE90462AC48A594AB63FC3BEEFEB368CBAA9399FBF4DC7FF972666E56AD777CCF7AAF1B20FE213DDB3DBE5A67D3CCFC7CCF7AFB0C10BF6FFF3333BF94992FCEA64AD6B3B349CC1B5C8E8F81BDA0679DB7F4B4AFBE325C4E1B665607606D63E7E87DCA397B0D10FF5F167A6D33F3AE99F9876AF960BD5F3D7FDBD33BCB9ED4D3B6CCCC4F77D6D9AB5A76D912DAB06536F93D5DE767E67B7A62EF37D4BEB7B15FDB13E3999DE58FEE59FEEE21DBB040DBDEBAD06B3F609CADB3C9B9E95A567ECB22E3CF3407A06DC317ABFD3F76066D383033CFCA262FE69799794A66EE36CFBA0FCFCCD7777E265EB06922B2FF00387660CDCC6D33F3C66ABD2F0E10FF0BD5366FC8CCFB2EB59D4B88DFB7DDA9752F66E607AAD8BFCDAA00483625429F59FD6C3D601B56F309C09699F9FB6ABB6FEA2C7F7EB5ECE6CCACAFD297137FD76AFB577696BDAD679F339B44B48DDA750EAF967D7689"
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
-   "0000000000000000000000000000000000000000000000000000000000000000000000000000000000000000D629FF0F4A0CD12A5E549F170000000049454E44AE426082";
-
-}
-
-#endif //TEXT_ATLAS_PNG_H
-

app/ios/Settings.bundle/Assembling.plist

@@ -320,34 +320,6 @@
 			<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>
@@ -393,13 +365,11 @@
 			<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,14 +482,6 @@
 			<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>
@@ -505,14 +497,6 @@
 			<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>
@@ -544,14 +528,6 @@
 			<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>
@@ -566,14 +542,6 @@
 			<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>
@@ -836,56 +804,6 @@
 			<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,60 +430,6 @@
 				<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>
@@ -494,7 +440,7 @@
 		</dict>
 		<dict>
 			<key>DefaultValue</key>
-			<string>0.21.11</string>
+			<string>0.21.8</string>
 			<key>Key</key>
 			<string>Version</string>
 			<key>Title</key>

corelib/include/rtabmap/core/Compression.h

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

corelib/include/rtabmap/core/DBDriver.h

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

corelib/include/rtabmap/core/DBDriverSqlite3.h

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

corelib/include/rtabmap/core/DBReader.h

@@ -51,7 +51,7 @@ public:
 			 bool ignoreGoalDelay = false,
 			 bool goalsIgnored = false,
 			 int startId = 0,
-			 const std::vector<unsigned int> & cameraIndices = std::vector<unsigned int>(),
+			 int cameraIndex = -1,
 			 int stopId = 0,
 			 bool intermediateNodesIgnored = false,
 			 bool landmarksIgnored = false,
@@ -65,7 +65,7 @@ public:
 			 bool ignoreGoalDelay = false,
 			 bool goalsIgnored = false,
 			 int startId = 0,
-			 const std::vector<unsigned int> & cameraIndices = std::vector<unsigned int>(),
+			 int cameraIndex = -1,
 			 int stopId = 0,
 			 bool intermediateNodesIgnored = false,
 			 bool landmarksIgnored = false,
@@ -99,7 +99,7 @@ private:
 	bool _goalsIgnored;
 	int _startId;
 	int _stopId;
-	std::vector<unsigned int> _cameraIndices;
+	int _cameraIndex;
 	bool _intermediateNodesIgnored;
 	bool _landmarksIgnored;
 	bool _featuresIgnored;

corelib/include/rtabmap/core/Graph.h

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

corelib/include/rtabmap/core/Memory.h

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

corelib/include/rtabmap/core/OdometryInfo.h

@@ -49,8 +49,6 @@ public:
 		localBundleOutliers(0),
 		localBundleConstraints(0),
 		localBundleTime(0),
-		localBundleAvgInlierDistance(0.0f),
-		localBundleMaxKeyFramesForInlier(0),
 		keyFrameAdded(false),
 		timeDeskewing(0.0f),
 		timeEstimation(0.0f),
@@ -78,8 +76,6 @@ public:
 		output.localBundleTime = localBundleTime;
 		output.localBundlePoses = localBundlePoses;
 		output.localBundleModels = localBundleModels;
-		output.localBundleAvgInlierDistance = localBundleAvgInlierDistance;
-		output.localBundleMaxKeyFramesForInlier = localBundleMaxKeyFramesForInlier;
 		output.keyFrameAdded = keyFrameAdded;
 		output.timeDeskewing = timeDeskewing;
 		output.timeEstimation = timeEstimation;
@@ -110,8 +106,6 @@ public:
 	float localBundleTime;
 	std::map<int, Transform> localBundlePoses;
 	std::map<int, std::vector<CameraModel> > localBundleModels;
-	float localBundleAvgInlierDistance;
-	int localBundleMaxKeyFramesForInlier;
 	bool keyFrameAdded;
 	float timeDeskewing;
 	float timeEstimation;

corelib/include/rtabmap/core/Parameters.h

@@ -209,7 +209,6 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(Mem, NotLinkedNodesKept,          bool, true,     "Keep not linked nodes in db (rehearsed nodes and deleted nodes).");
     RTABMAP_PARAM(Mem, IntermediateNodeDataKept,    bool, false,    "Keep intermediate node data in db.");
     RTABMAP_PARAM_STR(Mem, ImageCompressionFormat,   ".jpg",        "RGB image compression format. It should be \".jpg\" or \".png\".");
-    RTABMAP_PARAM_STR(Mem, DepthCompressionFormat,   ".rvl",        "Depth image compression format for 16UC1 depth type. It should be \".png\" or \".rvl\". If depth type is 32FC1, \".png\" is used.");
     RTABMAP_PARAM(Mem, STMSize,                   unsigned int, 10, "Short-term memory size.");
     RTABMAP_PARAM(Mem, IncrementalMemory,           bool, true,     "SLAM mode, otherwise it is Localization mode.");
     RTABMAP_PARAM(Mem, LocalizationDataSaved,       bool, false,     uFormat("Save localization data during localization session (when %s=false). When enabled, the database will then also grow in localization mode. This mode would be used only for debugging purpose.", kMemIncrementalMemory().c_str()).c_str());
@@ -222,7 +221,6 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(Mem, BadSignaturesIgnored,        bool, false,    "Bad signatures are ignored.");
     RTABMAP_PARAM(Mem, InitWMWithAllNodes,          bool, false,    "Initialize the Working Memory with all nodes in Long-Term Memory. When false, it is initialized with nodes of the previous session.");
     RTABMAP_PARAM(Mem, DepthAsMask,                 bool, true,     "Use depth image as mask when extracting features for vocabulary.");
-    RTABMAP_PARAM(Mem, DepthMaskFloorThr,           float, 0.0,     uFormat("Filter floor from depth mask below specified threshold (m) before extracting features. 0 means disabled, negative means remove all objects above the floor threshold instead. Ignored if %s is false.", kMemDepthAsMask().c_str()));
     RTABMAP_PARAM(Mem, StereoFromMotion,            bool, false,    uFormat("Triangulate features without depth using stereo from motion (odometry). It would be ignored if %s is true and the feature detector used supports masking.", kMemDepthAsMask().c_str()));
     RTABMAP_PARAM(Mem, ImagePreDecimation,          unsigned int, 1, uFormat("Decimation of the RGB image before visual feature detection. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. If %s is true and if depth is smaller than decimated RGB, depth may be interpolated to match RGB size for feature detection.",kMemDepthAsMask().c_str()));
     RTABMAP_PARAM(Mem, ImagePostDecimation,         unsigned int, 1, uFormat("Decimation of the RGB image before saving it to database. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. Decimation is done from the original image. If set to same value than %s, data already decimated is saved (no need to re-decimate the image).", kMemImagePreDecimation().c_str()));
@@ -477,8 +475,6 @@ class RTABMAP_CORE_EXPORT Parameters
     // Odometry Frame-to-Map
     RTABMAP_PARAM(OdomF2M, MaxSize,             int, 2000,    "[Visual] Local map size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words.");
     RTABMAP_PARAM(OdomF2M, MaxNewFeatures,      int, 0,       "[Visual] Maximum features (sorted by keypoint response) added to local map from a new key-frame. 0 means no limit.");
-    RTABMAP_PARAM(OdomF2M, InitDepthFactor,     float, 0.05,  "[Visual] Depth factor used to initialize depth of features without depth. Depth = Factor * fx.");
-    RTABMAP_PARAM(OdomF2M, FloorThreshold,      float, 0.0,   "[Visual] Only track features in 3D feature map that are over this threshold (height in base frame). Can be useful to ignore reflections on the floor. 0 means disabled.");
     RTABMAP_PARAM(OdomF2M, ScanMaxSize,         int, 2000,    "[Geometry] Maximum local scan map size.");
     RTABMAP_PARAM(OdomF2M, ScanSubtractRadius,  float, 0.05,  "[Geometry] Radius used to filter points of a new added scan to local map. This could match the voxel size of the scans.");
     RTABMAP_PARAM(OdomF2M, ScanSubtractAngle,   float, 45,    uFormat("[Geometry] Max angle (degrees) used to filter points of a new added scan to local map (when \"%s\">0). 0 means any angle.", kOdomF2MScanSubtractRadius().c_str()).c_str());
@@ -490,9 +486,6 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(OdomF2M, BundleAdjustment,          int, 0, "Local bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres.");
 #endif
     RTABMAP_PARAM(OdomF2M, BundleAdjustmentMaxFrames, int, 10, "Maximum frames used for bundle adjustment (0=inf or all current frames in the local map).");
-    RTABMAP_PARAM(OdomF2M, BundleAdjustmentMinMotion, float, 0.0, "To create a new keyframe with bundle adjustment, a minimum motion (in pixels) can be required. The motion is computed by the average distance between inliers of the previous keyframe and new frame.");
-    RTABMAP_PARAM(OdomF2M, BundleAdjustmentMaxKeyFramesPerFeature, int, 0, "Maximum keyframes per feature for bundle adjustment. 0 means not limit.");
-    RTABMAP_PARAM(OdomF2M, BundleUpdateFeatureMapOnAllFrames, bool, false, uFormat("Update 3D local feature map on every frame with bundle adjustment. Recommended if %s=false and %s=true so that features without depth are better triangulated on every frame (not only on keyframes). If disabled, the feature map is updated only when a new keyframe is added (legacy approach).", kVisDepthAsMask().c_str(), kMemUseOdomFeatures().c_str()));
 
     // Odometry Mono
     RTABMAP_PARAM(OdomMono, InitMinFlow,        float, 100,  "Minimum optical flow required for the initialization step.");
@@ -710,7 +703,6 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(Vis, MaxDepth,                  float, 0,     "Max depth of the features (0 means no limit).");
     RTABMAP_PARAM(Vis, MinDepth,                  float, 0,     "Min depth of the features (0 means no limit).");
     RTABMAP_PARAM(Vis, DepthAsMask,               bool,  true,  "Use depth image as mask when extracting features.");
-    RTABMAP_PARAM(Vis, DepthMaskFloorThr,         float, 0.0,    uFormat("Filter floor from depth mask below specified threshold (m) before extracting features. 0 means disabled, negative means remove all objects above the floor threshold instead. Ignored if %s is false.", kVisDepthAsMask().c_str()));
     RTABMAP_PARAM_STR(Vis, RoiRatios,        "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
     RTABMAP_PARAM(Vis, SubPixWinSize,             int,   3,     "See cv::cornerSubPix().");
     RTABMAP_PARAM(Vis, SubPixIterations,          int,   0,     "See cv::cornerSubPix(). 0 disables sub pixel refining.");

corelib/include/rtabmap/core/RegistrationVis.h

@@ -101,7 +101,6 @@ private:
 	bool _guessMatchToProjection;
 	int _bundleAdjustment;
 	bool _depthAsMask;
-	float _maskFloorThreshold;
 	float _minInliersDistributionThr;
 	float _maxInliersMeanDistance;
 

corelib/include/rtabmap/core/SensorData.h

@@ -207,8 +207,8 @@ public:
 	void setStereoCameraModels(const std::vector<StereoCameraModel> & stereoCameraModels) {_stereoCameraModels = stereoCameraModels;}
 
 	//for convenience
-	cv::Mat depthRaw() const {return !(_depthOrRightRaw.type()==CV_8UC1 || _depthOrRightRaw.type()==CV_8UC3) ? _depthOrRightRaw : cv::Mat();}
+	cv::Mat depthRaw() const {return _depthOrRightRaw.type()!=CV_8UC1?_depthOrRightRaw:cv::Mat();}
-	cv::Mat rightRaw() const {return   _depthOrRightRaw.type()==CV_8UC1 || _depthOrRightRaw.type()==CV_8UC3  ? _depthOrRightRaw : cv::Mat();}
+	cv::Mat rightRaw() const {return _depthOrRightRaw.type()==CV_8UC1?_depthOrRightRaw:cv::Mat();}
 
 	// Use setRGBDImage() or setStereoImage() with clearNotUpdated=false or removeRawData() instead. To be backward compatible, this function doesn't clear compressed data.
 	RTABMAP_DEPRECATED void setImageRaw(const cv::Mat & image);
@@ -330,7 +330,7 @@ private:
 	LaserScan _laserScanCompressed;      // compressed data
 
 	cv::Mat _imageRaw;          // CV_8UC1 or CV_8UC3
-	cv::Mat _depthOrRightRaw;   // depth CV_16UC1 or CV_32FC1, right image CV_8UC1 or CV_8UC3
+	cv::Mat _depthOrRightRaw;   // depth CV_16UC1 or CV_32FC1, right image CV_8UC1
 	LaserScan _laserScanRaw;
 
 	std::vector<CameraModel> _cameraModels;

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

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

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

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

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

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

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

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

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

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

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

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

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

corelib/include/rtabmap/core/rvl_codec.h

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

corelib/include/rtabmap/core/util3d.h

@@ -364,22 +364,6 @@ void RTABMAP_CORE_EXPORT fillProjectedCloudHoles(
 		bool verticalDirection,
 		bool fillToBorder);
 
-/**
- * @brief Remove values below a floor threshold in a depth image.
- * 
- * @param depth the depth image to filter (can be a multi-camera depth image).
- * @param cameraModels corresponding camera model(s) to depth image, with valid 
- *                     local transform between base frame to camera frame.
- * @param threshold height from base frame at which pixels below it are set to 0.
- * @param depthBelow depth image of the pixels below the floor theshold.
- * @return cv::Mat depth image of the pixels above the floor theshold.
- */
-cv::Mat RTABMAP_CORE_EXPORT filterFloor(
-	const cv::Mat & depth,
-	const std::vector<CameraModel> & cameraModels,
-	float threshold,
-	cv::Mat * depthBelow = 0);
-
 /**
  * For each point, return pixel of the best camera (NodeID->CameraIndex)
  * looking at it based on the policy and parameters

corelib/include/rtabmap/core/util3d_surface.h

@@ -225,7 +225,6 @@ 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
@@ -248,7 +247,6 @@ 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
@@ -484,22 +482,6 @@ void RTABMAP_CORE_EXPORT adjustNormalsToViewPoint(
 		const Eigen::Vector3f & viewpoint = Eigen::Vector3f(0,0,0),
 		float groundNormalsUp = 0.0f);
 
-void RTABMAP_CORE_EXPORT adjustNormalsToViewPoints(
-		const std::map<int, Transform> & poses,
-		const std::vector<int> & cameraIndices,
-		pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
-		float groundNormalsUp = 0.0f);
-void RTABMAP_CORE_EXPORT adjustNormalsToViewPoints(
-		const std::map<int, Transform> & poses,
-		const std::vector<int> & cameraIndices,
-		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
-		float groundNormalsUp = 0.0f);
-void RTABMAP_CORE_EXPORT adjustNormalsToViewPoints(
-		const std::map<int, Transform> & poses,
-		const std::vector<int> & cameraIndices,
-		pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
-		float groundNormalsUp = 0.0f);
-
 void RTABMAP_CORE_EXPORT adjustNormalsToViewPoints(
 		const std::map<int, Transform> & poses,
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,
@@ -588,15 +570,6 @@ bool intersectRayMesh(
 		Eigen::Vector3f & normal,
 		int & index);
 
-int RTABMAP_CORE_EXPORT saveOBJFile(
-	const std::string &file_name,
-    const pcl::TextureMesh &tex_mesh,
-	unsigned precision = 5);
-
-int RTABMAP_CORE_EXPORT saveOBJFile(
-	const std::string &file_name,
-	const pcl::PolygonMesh &mesh,
-	unsigned precision = 5);
 
 } // namespace util3d
 } // namespace rtabmap

corelib/src/CMakeLists.txt

@@ -64,8 +64,6 @@ SET(SRC_FILES
     util3d_correspondences.cpp
     util3d_motion_estimation.cpp
     
-	rvl_codec.cpp
-	
     SensorData.cpp
     Graph.cpp
     Compression.cpp
@@ -645,10 +643,10 @@ IF(octomap_FOUND)
 ENDIF(octomap_FOUND)
 
 IF(grid_map_core_FOUND)
-    IF(TARGET grid_map_core::grid_map_core)
+    IF(TARGET grid_map_core)
         SET(LIBRARIES
             ${LIBRARIES}
-            grid_map_core::grid_map_core
+            grid_map_core
         )
     ELSE()
         SET(INCLUDE_DIRS

corelib/src/Compression.cpp

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

corelib/src/DBDriver.cpp

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

corelib/src/DBDriverSqlite3.cpp

@@ -2427,13 +2427,9 @@ void DBDriverSqlite3::getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildre
 			  << "FROM Node ";
 		if(ignoreChildren)
 		{
-			// use to_id to ignore all children (which don't have link pointing on them)
+			query << "INNER JOIN Link ";
-			// ignore self referring links
+			query << "ON id = to_id "; // use to_id to ignore all children (which don't have link pointing on them)
-			// keep nodes without link to other nodes (map has only a single node)
+			query << "WHERE from_id != to_id "; // ignore self referring links
-			query << "WHERE ";
-			query << "(EXISTS (select 1 from Link where Node.id=to_id and from_id != to_id) OR ";
-			query << " NOT EXISTS (select 1 from Link where id=to_id and from_id != to_id)) ";
-			
 			query << "AND weight>-9 "; //ignore invalid nodes
 			if(ignoreIntermediateNodes)
 			{
@@ -2487,75 +2483,6 @@ void DBDriverSqlite3::getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildre
 	}
 }
 
-void DBDriverSqlite3::getAllOdomPosesQuery(std::map<int, Transform> & poses, bool ignoreChildren, bool ignoreIntermediateNodes) const
-{
-	if(_ppDb)
-	{
-		UTimer timer;
-		timer.start();
-		int rc = SQLITE_OK;
-		sqlite3_stmt * ppStmt = 0;
-		std::stringstream query;
-
-		query << "SELECT DISTINCT id, pose "
-			  << "FROM Node ";
-		if(ignoreChildren)
-		{
-			query << "INNER JOIN Link ";
-			query << "ON id = to_id "; // use to_id to ignore all children (which don't have link pointing on them)
-			query << "WHERE from_id != to_id "; // ignore self referring links
-			query << "AND weight>-9 "; //ignore invalid nodes
-			if(ignoreIntermediateNodes)
-			{
-				query << "AND weight!=-1 "; //ignore intermediate nodes
-			}
-		}
-		else if(ignoreIntermediateNodes)
-		{
-			query << "WHERE weight!=-1 "; //ignore intermediate nodes
-		}
-
-		query  << "ORDER BY id";
-
-		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
-		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
-
-		const void * data = 0;
-		int dataSize = 0;
-
-		// Process the result if one
-		rc = sqlite3_step(ppStmt);
-		while(rc == SQLITE_ROW)
-		{
-			int id = sqlite3_column_int(ppStmt, 0); // Signature Id
-			data = sqlite3_column_blob(ppStmt, 1);  // Pose
-			dataSize = sqlite3_column_bytes(ppStmt, 1);
-
-			Transform pose;
-			if((unsigned int)dataSize == pose.size()*sizeof(float) && data)
-			{
-				memcpy(pose.data(), data, dataSize);
-				if(uStrNumCmp(_version, "0.15.2") < 0)
-				{
-					pose.normalizeRotation();
-				}
-			}
-			else if(dataSize)
-			{
-				UERROR("Error while loading pose for node %d! Setting to null...", id);
-			}
-			poses.insert(std::make_pair(id, pose));
-			rc = sqlite3_step(ppStmt);
-		}
-		UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
-
-		// Finalize (delete) the statement
-		rc = sqlite3_finalize(ppStmt);
-		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
-		ULOGGER_DEBUG("Time=%f ids=%d", timer.ticks(), (int)poses.size());
-	}
-}
-
 void DBDriverSqlite3::getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks, bool withLandmarks) const
 {
 	links.clear();
@@ -4723,8 +4650,7 @@ void DBDriverSqlite3::updateCalibrationQuery(
 
 void DBDriverSqlite3::updateDepthImageQuery(
 		int nodeId,
-		const cv::Mat & image,
+		const cv::Mat & image) const
-		const std::string & format) const
 {
 	UDEBUG("");
 	if(_ppDb)
@@ -4743,8 +4669,7 @@ void DBDriverSqlite3::updateDepthImageQuery(
 		// Save depth
 		stepDepthUpdate(ppStmt,
 				nodeId,
-				image,
+				image);
-				format);
 
 		// Finalize (delete) the statement
 		rc = sqlite3_finalize(ppStmt);
@@ -6015,7 +5940,7 @@ std::string DBDriverSqlite3::queryStepDepthUpdate() const
 		return "UPDATE Data SET depth=? WHERE id=?;";
 	}
 }
-void DBDriverSqlite3::stepDepthUpdate(sqlite3_stmt * ppStmt, int nodeId, const cv::Mat & image, const std::string & format) const
+void DBDriverSqlite3::stepDepthUpdate(sqlite3_stmt * ppStmt, int nodeId, const cv::Mat & image) const
 {
 	if(!ppStmt)
 	{
@@ -6029,7 +5954,7 @@ void DBDriverSqlite3::stepDepthUpdate(sqlite3_stmt * ppStmt, int nodeId, const c
 	if(!image.empty() && (image.type()!=CV_8UC1 || image.rows > 1))
 	{
 		// compress
-		imageCompressed = compressImage2(image, format);
+		imageCompressed = compressImage2(image, ".png");
 	}
 	else
 	{

corelib/src/DBReader.cpp

@@ -48,7 +48,7 @@ DBReader::DBReader(const std::string & databasePath,
 				   bool ignoreGoalDelay,
 				   bool goalsIgnored,
 				   int startId,
-				   const std::vector<unsigned int> & cameraIndices,
+				   int cameraIndex,
 				   int stopId,
 				   bool intermediateNodesIgnored,
 				   bool landmarksIgnored,
@@ -63,7 +63,7 @@ DBReader::DBReader(const std::string & databasePath,
 	_goalsIgnored(goalsIgnored),
 	_startId(startId),
 	_stopId(stopId),
-	_cameraIndices(cameraIndices),
+	_cameraIndex(cameraIndex),
 	_intermediateNodesIgnored(intermediateNodesIgnored),
 	_landmarksIgnored(landmarksIgnored),
 	_featuresIgnored(featuresIgnored),
@@ -94,7 +94,7 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
 				   bool ignoreGoalDelay,
 				   bool goalsIgnored,
 				   int startId,
-				   const std::vector<unsigned int> & cameraIndices,
+				   int cameraIndex,
 				   int stopId,
 				   bool intermediateNodesIgnored,
 				   bool landmarksIgnored,
@@ -109,7 +109,7 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
 	_goalsIgnored(goalsIgnored),
 	_startId(startId),
 	_stopId(stopId),
-	_cameraIndices(cameraIndices),
+	_cameraIndex(cameraIndex),
 	_intermediateNodesIgnored(intermediateNodesIgnored),
 	_landmarksIgnored(landmarksIgnored),
 	_featuresIgnored(featuresIgnored),
@@ -557,77 +557,34 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
 			}
 
 			data.uncompressData();
-			std::map<int, int> cameraOldNewIndices;
+			if(data.cameraModels().size() > 1 &&
-			std::vector<CameraModel> dbModels = data.cameraModels();
+				_cameraIndex >= 0)
-			if(dbModels.empty() && !data.stereoCameraModels().empty())
 			{
-				for(size_t i=0; i<data.stereoCameraModels().size(); ++i)
+				if(_cameraIndex < (int)data.cameraModels().size())
 				{
-					dbModels.push_back(data.stereoCameraModels()[i].left());
+					// select one camera
-				}
+					int subImageWidth = data.imageRaw().cols/data.cameraModels().size();
-			}
+					cv::Mat image;
-			if(dbModels.size() > 1 &&
-				!_cameraIndices.empty())
-			{
-				cv::Mat combinedImages;
-				cv::Mat combinedDepthImages;
-				std::vector<CameraModel> combinedModels;
-				std::vector<StereoCameraModel> combinedStereoModels;
-				for(size_t i=0; i<_cameraIndices.size(); ++i)
-				{
-					UASSERT_MSG(_cameraIndices[i] < dbModels.size(), uFormat("DBReader: camera index %ld is not valid (should be between 0 and %ld)",
-						_cameraIndices[i], dbModels.size()-1).c_str());
-
-					int addedCameras = std::max(combinedModels.size(), combinedStereoModels.size());
-
-					int subImageWidth = data.imageRaw().cols/dbModels.size();
 					UASSERT(!data.imageRaw().empty() &&
-							data.imageRaw().cols % dbModels.size() == 0 &&
+							data.imageRaw().cols % data.cameraModels().size() == 0 &&
-							(int)_cameraIndices[i]*subImageWidth < data.imageRaw().cols);
+							_cameraIndex*subImageWidth < data.imageRaw().cols);
-					if(combinedImages.empty())
+					image= cv::Mat(data.imageRaw(),
-					{
+						   cv::Rect(_cameraIndex*subImageWidth, 0, subImageWidth, data.imageRaw().rows)).clone();
-						// initialize with first camera
-						combinedImages = cv::Mat(data.imageRaw().rows, subImageWidth*(_cameraIndices.size()-i), data.imageRaw().type());
-					}
-
-					cv::Mat fromROI = cv::Mat(data.imageRaw(), cv::Rect(_cameraIndices[i]*subImageWidth, 0, subImageWidth, data.imageRaw().rows));
-					cv::Mat toROI = cv::Mat(combinedImages, cv::Rect(addedCameras*subImageWidth, 0, subImageWidth, combinedImages.rows));
-					fromROI.copyTo(toROI);
 
 					cv::Mat depth;
 					if(!data.depthOrRightRaw().empty())
 					{
-						subImageWidth = data.depthOrRightRaw().cols/dbModels.size();
+						UASSERT(data.depthOrRightRaw().cols % data.cameraModels().size() == 0 &&
-						UASSERT(data.depthOrRightRaw().cols % dbModels.size() == 0 &&
+								subImageWidth == data.depthOrRightRaw().cols/(int)data.cameraModels().size() &&
-								subImageWidth == data.depthOrRightRaw().cols/(int)dbModels.size() &&
+								_cameraIndex*subImageWidth < data.depthOrRightRaw().cols);
-								(int)_cameraIndices[i]*subImageWidth < data.depthOrRightRaw().cols);
+						depth = cv::Mat(data.depthOrRightRaw(),
-						if(combinedDepthImages.empty())
+							    cv::Rect(_cameraIndex*subImageWidth, 0, subImageWidth, data.depthOrRightRaw().rows)).clone();
-						{
-							// initialize with first camera
-							combinedDepthImages = cv::Mat(data.depthOrRightRaw().rows, subImageWidth*(_cameraIndices.size()-i), data.depthOrRightRaw().type());
-						}
-						fromROI = cv::Mat(data.depthOrRightRaw(), cv::Rect(_cameraIndices[i]*subImageWidth, 0, subImageWidth, data.depthOrRightRaw().rows));
-						toROI = cv::Mat(combinedDepthImages, cv::Rect(addedCameras*subImageWidth, 0, subImageWidth, combinedDepthImages.rows));
-						fromROI.copyTo(toROI);
 					}
-
+					data.setRGBDImage(image, depth, data.cameraModels().at(_cameraIndex));
-					if(!data.cameraModels().empty())
-					{
-						combinedModels.push_back(data.cameraModels()[_cameraIndices[i]]);
-					}
-					else
-					{
-						combinedStereoModels.push_back(data.stereoCameraModels()[_cameraIndices[i]]);
-					}
-					cameraOldNewIndices.insert(std::make_pair(_cameraIndices[i], i));
-				}
-				if(!combinedModels.empty())
-				{
-					data.setRGBDImage(combinedImages, combinedDepthImages, combinedModels);
 				}
 				else
 				{
-					data.setStereoImage(combinedImages, combinedDepthImages, combinedStereoModels);
+					UWARN("DBReader: Camera index %d doesn't exist! Camera models = %d.", _cameraIndex, (int)data.cameraModels().size());
 				}
 			}
 			data.setId(seq);
@@ -666,40 +623,7 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
 			   (keypoints3D.empty() || keypoints.size() == keypoints3D.size()) &&
 			   (descriptors.empty() || (int)keypoints.size() == descriptors.rows))
 			{
-				if(!cameraOldNewIndices.empty())
+				data.setFeatures(keypoints, keypoints3D, descriptors);
-				{
-					cv::Mat newDescriptors;
-					std::vector<cv::KeyPoint> newKeypoints;
-					std::vector<cv::Point3f> newKeypoints3D;
-					UASSERT(!dbModels.empty() && dbModels[0].imageWidth()>0);
-					int subImageWidth = dbModels[0].imageWidth();
-					for(size_t i = 0; i<keypoints.size(); ++i)
-					{
-						int cameraIndex = int(keypoints.at(i).pt.x / subImageWidth);
-						UASSERT_MSG(cameraIndex >= 0 && cameraIndex < (int)dbModels.size(),
-								uFormat("cameraIndex=%d, db models=%d, kpt.x=%f, image width=%d",
-										cameraIndex, (int)dbModels.size(), keypoints[i].pt.x, subImageWidth).c_str());
-						if(cameraOldNewIndices.find(cameraIndex) != cameraOldNewIndices.end())
-						{
-							int newCameraIndex = cameraOldNewIndices.at(cameraIndex);
-							newKeypoints.push_back(keypoints[i]);
-							newKeypoints.back().pt.x += (newCameraIndex-cameraIndex)*subImageWidth;
-							if(!keypoints3D.empty())
-							{
-								newKeypoints3D.push_back(keypoints3D.at(i));
-							}
-							if(!descriptors.empty())
-							{
-								newDescriptors.push_back(descriptors.row(i));
-							}
-						}
-					}
-					data.setFeatures(newKeypoints, newKeypoints3D, newDescriptors);
-				}
-				else
-				{
-					data.setFeatures(keypoints, keypoints3D, descriptors);
-				}
 			}
 			else if(!_featuresIgnored && !keypoints.empty() && (!keypoints3D.empty() || !descriptors.empty()))
 			{

corelib/src/Features2d.cpp

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

corelib/src/Graph.cpp

@@ -783,8 +783,7 @@ Transform calcRMSE (
 		float & rotational_median,
 		float & rotational_std,
 		float & rotational_min,
-		float & rotational_max,
+		float & rotational_max)
-		bool align2D)
 {
 
 	translational_rmse = 0.0f;
@@ -816,8 +815,8 @@ Transform calcRMSE (
 			{
 				idFirst = iter->first;
 			}
-			cloud1[oi] = pcl::PointXYZ(jter->second.x(), jter->second.y(), align2D?0:jter->second.z());
+			cloud1[oi] = pcl::PointXYZ(jter->second.x(), jter->second.y(), jter->second.z());
-			cloud2[oi++] = pcl::PointXYZ(iter->second.x(), iter->second.y(), align2D?0:iter->second.z());
+			cloud2[oi++] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
 		}
 	}
 

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

corelib/src/Memory.cpp

@@ -80,7 +80,6 @@ Memory::Memory(const ParametersMap & parameters) :
 	_notLinkedNodesKeptInDb(Parameters::defaultMemNotLinkedNodesKept()),
 	_saveIntermediateNodeData(Parameters::defaultMemIntermediateNodeDataKept()),
 	_rgbCompressionFormat(Parameters::defaultMemImageCompressionFormat()),
-	_depthCompressionFormat(Parameters::defaultMemDepthCompressionFormat()),
 	_incrementalMemory(Parameters::defaultMemIncrementalMemory()),
 	_localizationDataSaved(Parameters::defaultMemLocalizationDataSaved()),
 	_reduceGraph(Parameters::defaultMemReduceGraph()),
@@ -92,7 +91,6 @@ Memory::Memory(const ParametersMap & parameters) :
 	_badSignaturesIgnored(Parameters::defaultMemBadSignaturesIgnored()),
 	_mapLabelsAdded(Parameters::defaultMemMapLabelsAdded()),
 	_depthAsMask(Parameters::defaultMemDepthAsMask()),
-	_maskFloorThreshold(Parameters::defaultMemDepthMaskFloorThr()),
 	_stereoFromMotion(Parameters::defaultMemStereoFromMotion()),
     _imagePreDecimation(Parameters::defaultMemImagePreDecimation()),
 	_imagePostDecimation(Parameters::defaultMemImagePostDecimation()),
@@ -569,7 +567,6 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(params, Parameters::kMemNotLinkedNodesKept(), _notLinkedNodesKeptInDb);
 	Parameters::parse(params, Parameters::kMemIntermediateNodeDataKept(), _saveIntermediateNodeData);
 	Parameters::parse(params, Parameters::kMemImageCompressionFormat(), _rgbCompressionFormat);
-	Parameters::parse(params, Parameters::kMemDepthCompressionFormat(), _depthCompressionFormat);
 	Parameters::parse(params, Parameters::kMemRehearsalIdUpdatedToNewOne(), _idUpdatedToNewOneRehearsal);
 	Parameters::parse(params, Parameters::kMemGenerateIds(), _generateIds);
 	Parameters::parse(params, Parameters::kMemBadSignaturesIgnored(), _badSignaturesIgnored);
@@ -579,7 +576,6 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(params, Parameters::kMemTransferSortingByWeightId(), _transferSortingByWeightId);
 	Parameters::parse(params, Parameters::kMemSTMSize(), _maxStMemSize);
 	Parameters::parse(params, Parameters::kMemDepthAsMask(), _depthAsMask);
-	Parameters::parse(params, Parameters::kMemDepthMaskFloorThr(), _maskFloorThreshold);
 	Parameters::parse(params, Parameters::kMemStereoFromMotion(), _stereoFromMotion);
 	Parameters::parse(params, Parameters::kMemImagePreDecimation(), _imagePreDecimation);
 	Parameters::parse(params, Parameters::kMemImagePostDecimation(), _imagePostDecimation);
@@ -4519,55 +4515,17 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 {
 	UDEBUG("");
 	SensorData data = inputData;
-
-	bool isIntermediateNode = data.id() < 0;
-
-	// uncompress data if needed
-	
-	if(!isIntermediateNode)
-	{
-		// We need raw images if we need to extract features and/or do tag detection
-		bool needRawImages = _feature2D->getMaxFeatures() >= 0 && 
-			(!_useOdometryFeatures ||
-			 data.keypoints().empty() ||
-			 (int)data.keypoints().size() != data.descriptors().rows ||
-			 data.descriptors().empty() ||
-			 _detectMarkers ||
-			 _rotateImagesUpsideUp ||
-			 _imagePostDecimation > 1 ||
-			 (_createOccupancyGrid && _localMapMaker->isGridFromDepth()));
-
-		// Note: we could avoid uncompressing scan if we don't do any filtering
-		// and if we don't use it for local occupancy grid
-		bool needRawScan = true;
-
-		if( (needRawImages && data.imageRaw().empty() && !data.imageCompressed().empty()) ||
-			(needRawImages && data.depthOrRightRaw().empty() && !data.depthOrRightCompressed().empty()) ||
-			(needRawScan && data.laserScanRaw().empty() && !data.laserScanCompressed().empty()))
-		{
-			cv::Mat left, right;
-			LaserScan laserScan;
-			UDEBUG("Uncompressing data...");
-			data.uncompressData(
-				needRawImages && data.imageRaw().empty() && !data.imageCompressed().empty() ? &left : 0,
-				needRawImages && data.depthOrRightRaw().empty() && !data.depthOrRightCompressed().empty() ? &right : 0,
-				needRawScan && data.laserScanRaw().empty() && !data.laserScanCompressed().empty() ? &laserScan : 0);
-			UDEBUG("Uncompressing data...done!");
-		}
-	}
-
 	UASSERT(data.imageRaw().empty() ||
 			data.imageRaw().type() == CV_8UC1 ||
 			data.imageRaw().type() == CV_8UC3);
 	UASSERT_MSG(data.depthOrRightRaw().empty() ||
 			(  ( data.depthOrRightRaw().type() == CV_16UC1 ||
 				 data.depthOrRightRaw().type() == CV_32FC1 ||
-				 data.depthOrRightRaw().type() == CV_8UC1 ||
+				 data.depthOrRightRaw().type() == CV_8UC1)
-				 data.depthOrRightRaw().type() == CV_8UC3)
 			   &&
-				( (data.imageRaw().empty() && !(data.depthOrRightRaw().type() == CV_8UC1 || data.depthOrRightRaw().type() == CV_8UC3)) ||
+				( (data.imageRaw().empty() && data.depthOrRightRaw().type() != CV_8UC1) ||
 				  (data.depthOrRightRaw().rows <= data.imageRaw().rows && data.depthOrRightRaw().cols <= data.imageRaw().cols))),
-				uFormat("image=(%d/%d, type=%d, [accepted=%d,%d]) depth=(%d/%d, type=%d [accepted=%d(depth mm),%d(depth m),%d-%d(stereo)]). "
+				uFormat("image=(%d/%d, type=%d, [accepted=%d,%d]) depth=(%d/%d, type=%d [accepted=%d(depth mm),%d(depth m),%d(stereo)]). "
 						"For stereo, left and right images should be same size. "
 						"For RGB-D, depth can be X times smaller than RGB (where X is an integer).",
 						data.imageRaw().cols,
@@ -4578,7 +4536,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 						data.depthOrRightRaw().cols,
 						data.depthOrRightRaw().rows,
 						data.depthOrRightRaw().type(),
-						CV_16UC1, CV_32FC1, CV_8UC1, CV_8UC3).c_str());
+						CV_16UC1, CV_32FC1, CV_8UC1).c_str());
 
 	if(!data.depthOrRightRaw().empty() &&
 		data.cameraModels().empty() &&
@@ -4597,6 +4555,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 	float t;
 	std::vector<cv::KeyPoint> keypoints;
 	cv::Mat descriptors;
+	bool isIntermediateNode = data.id() < 0;
 	int id = data.id();
 	if(_generateIds)
 	{
@@ -4925,26 +4884,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 					imageMono.cols % decimatedData.depthRaw().cols == 0 &&
 					imageMono.rows/decimatedData.depthRaw().rows == imageMono.cols/decimatedData.depthRaw().cols)
 				{
-					depthMask = decimatedData.depthRaw();
+					depthMask = util2d::interpolate(decimatedData.depthRaw(), imageMono.rows/decimatedData.depthRaw().rows, 0.1f);
-
-					if(_maskFloorThreshold != 0.0f)
-					{
-						UASSERT(!decimatedData.cameraModels().empty());
-						UDEBUG("Masking floor (threshold=%f)", _maskFloorThreshold);
-						if(_maskFloorThreshold<0.0f)
-						{
-							cv::Mat depthBelow;
-							util3d::filterFloor(depthMask, decimatedData.cameraModels(), _maskFloorThreshold*-1.0f, &depthBelow);
-							depthMask = depthBelow;
-						}
-						else
-						{
-							depthMask = util3d::filterFloor(depthMask, decimatedData.cameraModels(), _maskFloorThreshold);
-						}
-						UDEBUG("Masking floor done.");
-					}
-
-					depthMask = util2d::interpolate(depthMask, imageMono.rows/depthMask.rows, 0.1f);
 				}
 				else
 				{
@@ -5791,8 +5731,8 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		}
 	}
 
-	LaserScan laserScan = data.laserScanRaw();
 	// Filter the laser scan?
+	LaserScan laserScan = data.laserScanRaw();
 	if(!isIntermediateNode && laserScan.size())
 	{
 		if(laserScan.rangeMax() == 0.0f)
@@ -5847,42 +5787,10 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		std::vector<unsigned char> imageBytes;
 		std::vector<unsigned char> depthBytes;
 
-		if(!depthOrRightImage.empty() && depthOrRightImage.type() == CV_32FC1)
+		if(_saveDepth16Format && !depthOrRightImage.empty() && depthOrRightImage.type() == CV_32FC1)
 		{
-			if(_saveDepth16Format)
+			UWARN("Save depth data to 16 bits format: depth type detected is 32FC1, use 16UC1 depth format to avoid this conversion (or set parameter \"Mem/SaveDepth16Format\"=false to use 32bits format).");
-			{
+			depthOrRightImage = util2d::cvtDepthFromFloat(depthOrRightImage);
-				static bool warned = false;
-				if(!warned)
-				{
-					UWARN("Converting depth data to 16 bits format because depth type detected is 32FC1, "
-					      "feed 16UC1 depth format directly to avoid this conversion (or set parameter %s=false "
-						  "to save 32bits format). This warning is only printed once.",
-						Parameters::kMemSaveDepth16Format().c_str());
-					warned = true;
-				}
-				depthOrRightImage = util2d::cvtDepthFromFloat(depthOrRightImage);
-			}
-			else if(_depthCompressionFormat == ".rvl")
-			{
-				static bool warned = false;
-				if(!warned)
-				{
-					UWARN("%s is set to false to use 32bits format but this is not "
-					 	  "compatible with the compressed depth format chosen (%s=\"%s\"), depth "
-						  "images will be compressed in \".png\" format instead. Explicitly "
-						  "set %s to true to keep using \"%s\" format and images will be "
-						  "converted to 16bits for convenience (warning: that would "
-						  "remove all depth values over 65 meters). Explicitly set %s=\".png\" "
-						  "to suppress this warning. This warning is only printed once.",
-						Parameters::kMemSaveDepth16Format().c_str(),
-						Parameters::kMemDepthCompressionFormat().c_str(),
-						_depthCompressionFormat.c_str(),
-						Parameters::kMemSaveDepth16Format().c_str(),
-						_depthCompressionFormat.c_str(),
-						Parameters::kMemDepthCompressionFormat().c_str());
-					warned = true;
-				}
-			}
 		}
 
 		cv::Mat compressedImage;
@@ -5892,7 +5800,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		if(_compressionParallelized)
 		{
 			rtabmap::CompressionThread ctImage(image, _rgbCompressionFormat);
-			rtabmap::CompressionThread ctDepth(depthOrRightImage, depthOrRightImage.type() == CV_32FC1 || depthOrRightImage.type() == CV_16UC1?_depthCompressionFormat:_rgbCompressionFormat);
+			rtabmap::CompressionThread ctDepth(depthOrRightImage, depthOrRightImage.type() == CV_32FC1 || depthOrRightImage.type() == CV_16UC1?std::string(".png"):_rgbCompressionFormat);
 			rtabmap::CompressionThread ctLaserScan(laserScan.data());
 			rtabmap::CompressionThread ctUserData(data.userDataRaw());
 			if(!image.empty())
@@ -5924,7 +5832,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		else
 		{
 			compressedImage = compressImage2(image, _rgbCompressionFormat);
-			compressedDepth = compressImage2(depthOrRightImage, depthOrRightImage.type() == CV_32FC1 || depthOrRightImage.type() == CV_16UC1?_depthCompressionFormat:_rgbCompressionFormat);
+			compressedDepth = compressImage2(depthOrRightImage, depthOrRightImage.type() == CV_32FC1 || depthOrRightImage.type() == CV_16UC1?std::string(".png"):_rgbCompressionFormat);
 			compressedScan = compressData2(laserScan.data());
 			compressedUserData = compressData2(data.userDataRaw());
 		}
@@ -5939,12 +5847,12 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			!stereoCameraModels.empty()?
 				SensorData(
 						laserScan.angleIncrement() == 0.0f?
-							LaserScan(compressedScan.empty()?data.laserScanCompressed().data():compressedScan,
+							LaserScan(compressedScan,
 								laserScan.maxPoints(),
 								laserScan.rangeMax(),
 								laserScan.format(),
 								laserScan.localTransform()):
-							LaserScan(compressedScan.empty()?data.laserScanCompressed().data():compressedScan,
+							LaserScan(compressedScan,
 								laserScan.format(),
 								laserScan.rangeMin(),
 								laserScan.rangeMax(),
@@ -5952,20 +5860,20 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 								laserScan.angleMax(),
 								laserScan.angleIncrement(),
 								laserScan.localTransform()),
-						compressedImage.empty()?data.imageCompressed():compressedImage,
+						compressedImage,
-						compressedDepth.empty()?data.depthOrRightCompressed():compressedDepth,
+						compressedDepth,
 						stereoCameraModels,
 						id,
 						0,
 						compressedUserData):
 				SensorData(
 						laserScan.angleIncrement() == 0.0f?
-							LaserScan(compressedScan.empty()?data.laserScanCompressed().data():compressedScan,
+							LaserScan(compressedScan,
 								laserScan.maxPoints(),
 								laserScan.rangeMax(),
 								laserScan.format(),
 								laserScan.localTransform()):
-							LaserScan(compressedScan.empty()?data.laserScanCompressed().data():compressedScan,
+							LaserScan(compressedScan,
 								laserScan.format(),
 								laserScan.rangeMin(),
 								laserScan.rangeMax(),
@@ -5973,8 +5881,8 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 								laserScan.angleMax(),
 								laserScan.angleIncrement(),
 								laserScan.localTransform()),
-						compressedImage.empty()?data.imageCompressed():compressedImage,
+						compressedImage,
-						compressedDepth.empty()?data.depthOrRightCompressed():compressedDepth,
+						compressedDepth,
 						cameraModels,
 						id,
 						0,
@@ -6023,12 +5931,12 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			!stereoCameraModels.empty()?
 				SensorData(
 						laserScan.angleIncrement() == 0.0f?
-								LaserScan(compressedScan.empty()?data.laserScanCompressed().data():compressedScan,
+								LaserScan(compressedScan,
 									laserScan.maxPoints(),
 									laserScan.rangeMax(),
 									laserScan.format(),
 									laserScan.localTransform()):
-								LaserScan(compressedScan.empty()?data.laserScanCompressed().data():compressedScan,
+								LaserScan(compressedScan,
 									laserScan.format(),
 									laserScan.rangeMin(),
 									laserScan.rangeMax(),
@@ -6044,12 +5952,12 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 						compressedUserData):
 				SensorData(
 						laserScan.angleIncrement() == 0.0f?
-								LaserScan(compressedScan.empty()?data.laserScanCompressed().data():compressedScan,
+								LaserScan(compressedScan,
 									laserScan.maxPoints(),
 									laserScan.rangeMax(),
 									laserScan.format(),
 									laserScan.localTransform()):
-								LaserScan(compressedScan.empty()?data.laserScanCompressed().data():compressedScan,
+								LaserScan(compressedScan,
 									laserScan.format(),
 									laserScan.rangeMin(),
 									laserScan.rangeMax(),

corelib/src/Odometry.cpp

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

corelib/src/OdometryThread.cpp

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

corelib/src/RegistrationVis.cpp

@@ -94,7 +94,6 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
 		_guessMatchToProjection(Parameters::defaultVisCorGuessMatchToProjection()),
 		_bundleAdjustment(Parameters::defaultVisBundleAdjustment()),
 		_depthAsMask(Parameters::defaultVisDepthAsMask()),
-		_maskFloorThreshold(Parameters::defaultVisDepthMaskFloorThr()),
 		_minInliersDistributionThr(Parameters::defaultVisMinInliersDistribution()),
 		_maxInliersMeanDistance(Parameters::defaultVisMeanInliersDistance()),
 		_detectorFrom(0),
@@ -156,7 +155,6 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kVisCorGuessMatchToProjection(), _guessMatchToProjection);
 	Parameters::parse(parameters, Parameters::kVisBundleAdjustment(), _bundleAdjustment);
 	Parameters::parse(parameters, Parameters::kVisDepthAsMask(), _depthAsMask);
-	Parameters::parse(parameters, Parameters::kVisDepthMaskFloorThr(), _maskFloorThreshold);
 	Parameters::parse(parameters, Parameters::kVisMinInliersDistribution(), _minInliersDistributionThr);
 	Parameters::parse(parameters, Parameters::kVisMeanInliersDistance(), _maxInliersMeanDistance);
 	uInsert(_bundleParameters, parameters);
@@ -425,32 +423,13 @@ Transform RegistrationVis::computeTransformationImpl(
 						   imageFrom.cols % fromSignature.sensorData().depthRaw().cols == 0 &&
 						   imageFrom.rows/fromSignature.sensorData().depthRaw().rows == fromSignature.sensorData().imageRaw().cols/fromSignature.sensorData().depthRaw().cols)
 						{
-							depthMask = fromSignature.sensorData().depthRaw();
+							depthMask = util2d::interpolate(fromSignature.sensorData().depthRaw(), fromSignature.sensorData().imageRaw().rows/fromSignature.sensorData().depthRaw().rows, 0.1f);
-
-							if(_maskFloorThreshold != 0.0f)
-							{
-								UASSERT(!fromSignature.sensorData().cameraModels().empty());
-								UDEBUG("Masking floor (threshold=%f)", _maskFloorThreshold);
-								if(_maskFloorThreshold<0.0f)
-								{
-									cv::Mat depthBelow;
-									util3d::filterFloor(depthMask, fromSignature.sensorData().cameraModels(), _maskFloorThreshold*-1.0f, &depthBelow);
-									depthMask = depthBelow;
-								}
-								else
-								{
-									depthMask = util3d::filterFloor(depthMask, fromSignature.sensorData().cameraModels(), _maskFloorThreshold);
-								}
-								UDEBUG("Masking floor done.");
-							}
-
-							depthMask = util2d::interpolate(depthMask, imageFrom.rows/depthMask.rows, 0.1f);
 						}
 						else
 						{
 							UWARN("%s is true, but RGB size (%dx%d) modulo depth size (%dx%d) is not 0. Ignoring depth mask for feature detection.",
 									Parameters::kVisDepthAsMask().c_str(),
-									imageFrom.rows, imageFrom.cols,
+									fromSignature.sensorData().imageRaw().rows, fromSignature.sensorData().imageRaw().cols,
 									fromSignature.sensorData().depthRaw().rows, fromSignature.sensorData().depthRaw().cols);
 						}
 					}
@@ -791,32 +770,13 @@ Transform RegistrationVis::computeTransformationImpl(
 						   imageTo.cols % toSignature.sensorData().depthRaw().cols == 0 &&
 						   imageTo.rows/toSignature.sensorData().depthRaw().rows == imageTo.cols/toSignature.sensorData().depthRaw().cols)
 						{
-							depthMask = toSignature.sensorData().depthRaw();
+							depthMask = util2d::interpolate(toSignature.sensorData().depthRaw(), imageTo.rows/toSignature.sensorData().depthRaw().rows, 0.1f);
-
-							if(_maskFloorThreshold != 0.0f)
-							{
-								UASSERT(!toSignature.sensorData().cameraModels().empty());
-								UDEBUG("Masking floor (threshold=%f)", _maskFloorThreshold);
-								if(_maskFloorThreshold<0.0f)
-								{
-									cv::Mat depthBelow;
-									util3d::filterFloor(depthMask, toSignature.sensorData().cameraModels(), _maskFloorThreshold*-1.0f, &depthBelow);
-									depthMask = depthBelow;
-								}
-								else
-								{
-									depthMask = util3d::filterFloor(depthMask, toSignature.sensorData().cameraModels(), _maskFloorThreshold);
-								}
-								UDEBUG("Masking floor done.");
-							}
-
-							depthMask = util2d::interpolate(depthMask, imageTo.rows/depthMask.rows, 0.1f);
 						}
 						else
 						{
 							UWARN("%s is true, but RGB size (%dx%d) modulo depth size (%dx%d) is not 0. Ignoring depth mask for feature detection.",
 									Parameters::kVisDepthAsMask().c_str(),
-									imageTo.rows, imageTo.cols,
+									toSignature.sensorData().imageRaw().rows, toSignature.sensorData().imageRaw().cols,
 									toSignature.sensorData().depthRaw().rows, toSignature.sensorData().depthRaw().cols);
 						}
 					}

corelib/src/Rtabmap.cpp

@@ -716,36 +716,6 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 
 	if(_memory)
 	{
-		bool isMemIncremental = _memory->isIncremental();
-		if(Parameters::parse(parameters, Parameters::kMemIncrementalMemory(), isMemIncremental) &&
-			isMemIncremental != _memory->isIncremental())
-		{
-			// Mode has changed from Mapping to Localization, cleanup the local graph
-			if(_memory->isGraphReduced() && _memory->isIncremental())
-			{
-				// Force reducing graph, then remove filtered nodes from the optimized poses
-				std::map<int, int> reducedIds;
-				_memory->incrementMapId(&reducedIds);
-				for(std::map<int, int>::iterator iter=reducedIds.begin(); iter!=reducedIds.end(); ++iter)
-				{
-					_optimizedPoses.erase(iter->first);
-				}
-			}
-
-			// In both cases, we save the latest optimized graph and latest localization pose
-			_memory->saveOptimizedPoses(_optimizedPoses, _lastLocalizationPose);
-
-			// Mode changed from Localization to Mapping, clear local graph
-			if(!_memory->isIncremental()) {
-				_optimizedPoses.clear();
-				_lastLocalizationPose.setNull();
-				_mapCorrection.setIdentity();
-				_mapCorrectionBackup.setNull();
-				_localizationCovariance = cv::Mat();
-				_lastLocalizationNodeId = 0;
-			}
-		}
-
 		_memory->parseParameters(parameters);
 		if(_memory->isIncremental() && !_globalScanMap.empty())
 		{
@@ -1750,7 +1720,6 @@ bool Rtabmap::process(
 					_constraints.erase(--_constraints.end());
 				}
 			}
-
 			_constraints.insert(std::make_pair(tmp.from(), tmp));
 		}
 		// Localization mode stuff

corelib/src/SensorCaptureThread.cpp

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

corelib/src/SensorData.cpp

@@ -365,8 +365,7 @@ void SensorData::setStereoImage(
 	}
 	else if(!right.empty())
 	{
-		UASSERT(right.type() == CV_8UC1 || // Mono
+		UASSERT(right.type() == CV_8UC1); // Mono
-				right.type() == CV_8UC3);  // RGB
 		_depthOrRightRaw = right;
 		if(clearData)
 		{

corelib/src/Stereo.cpp

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

corelib/src/camera/CameraDepthAI.cpp

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

corelib/src/camera/CameraStereoImages.cpp

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

corelib/src/camera/CameraStereoVideo.cpp

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

corelib/src/camera/CameraStereoZed.cpp

@@ -318,13 +318,12 @@ CameraStereoZed::CameraStereoZed(
     sl::RESOLUTION res = static_cast<sl::RESOLUTION>(resolution_);
     sl::DEPTH_MODE qual = static_cast<sl::DEPTH_MODE>(quality_);
 
+    UASSERT(res >= sl::RESOLUTION::HD2K && res < sl::RESOLUTION::LAST);
     UASSERT(qual >= sl::DEPTH_MODE::NONE && qual < sl::DEPTH_MODE::LAST);
 #if ZED_SDK_MAJOR_VERSION < 4
-	UASSERT(res >= sl::RESOLUTION::HD2K && res < sl::RESOLUTION::LAST);
     sl::SENSING_MODE sens = static_cast<sl::SENSING_MODE>(sensingMode_);
     UASSERT(sens >= sl::SENSING_MODE::STANDARD && sens < sl::SENSING_MODE::LAST);
 #else
-	UASSERT(res >= sl::RESOLUTION::HD4K && res < sl::RESOLUTION::LAST);
     UASSERT(sensingMode_ >= 0 && sensingMode_ < 2);
 #endif
     UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100);
@@ -783,10 +782,7 @@ SensorData CameraStereoZed::captureImage(SensorCaptureInfo * info)
 #endif
 				cv::Mat rgbaRight = slMat2cvMat(tmp);
 				cv::Mat right;
-				if(rightGrayScale_)
+				cv::cvtColor(rgbaRight, right, cv::COLOR_BGRA2GRAY);
-					cv::cvtColor(rgbaRight, right, cv::COLOR_BGRA2GRAY);
-				else
-					cv::cvtColor(rgbaRight, right, cv::COLOR_BGRA2BGR);
 #if ZED_SDK_MAJOR_VERSION < 3
 				data = SensorData(left, right, stereoModel_, this->getNextSeqID(), UTimer::now());
 #else
@@ -894,11 +890,4 @@ void CameraStereoZed::postInterIMUPublic(const IMU & imu, double stamp)
 	postInterIMU(imu, stamp);
 }
 
-void CameraStereoZed::setRightGrayScale(bool enabled)
-{
-#ifdef RTABMAP_ZED
-	rightGrayScale_ = enabled;
-#endif
-}
-
 } // namespace rtabmap

corelib/src/camera/CameraStereoZedOC.cpp

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

corelib/src/odometry/OdometryF2M.cpp

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

corelib/src/odometry/OdometryMono.cpp

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

corelib/src/odometry/OdometryORBSLAM3.cpp

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

corelib/src/optimizer/OptimizerCeres.cpp

@@ -37,47 +37,22 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #ifdef RTABMAP_CERES
 #include <ceres/ceres.h>
-
-#if CERES_VERSION_MAJOR >= 3 || \
-    (CERES_VERSION_MAJOR == 2 && CERES_VERSION_MINOR >= 1)
-#include <ceres/manifold.h>
-#else
 #include <ceres/local_parameterization.h>
-#endif
-
 #include "ceres/pose_graph_2d/types.h"
 #include "ceres/pose_graph_2d/pose_graph_2d_error_term.h"
-#include "ceres/pose_graph_2d/angle_manifold.h"
+#include "ceres/pose_graph_2d/angle_local_parameterization.h"
 #include "ceres/pose_graph_3d/types.h"
 #include "ceres/pose_graph_3d/pose_graph_3d_error_term.h"
 #include "ceres/bundle/BAProblem.h"
 #include "ceres/bundle/snavely_reprojection_error.h"
 
 #if not(CERES_VERSION_MAJOR > 1 || (CERES_VERSION_MAJOR == 1 && CERES_VERSION_MINOR >= 12))
-#include "ceres/pose_graph_3d/eigen_quaternion_manifold.h"
+#include "ceres/pose_graph_3d/eigen_quaternion_parameterization.h"
 #endif
 
 #endif
 
 namespace rtabmap {
-namespace {
-
-#ifdef RTABMAP_CERES
-#if CERES_VERSION_MAJOR >= 3 || \
-    (CERES_VERSION_MAJOR == 2 && CERES_VERSION_MINOR >= 1)
-inline void SetCeresProblemManifold(ceres::Problem& problem, double* params,
-                                    ceres::Manifold* manifold) {
-  problem.SetManifold(params, manifold);
-#else
-inline void SetCeresProblemManifold(
-    ceres::Problem& problem, double* params,
-    ceres::LocalParameterization* parameterization) {
-  problem.SetParameterization(params, parameterization);
-#endif
-}
-#endif
-
-}  // namespace
 
 bool OptimizerCeres::available()
 {
@@ -143,14 +118,8 @@ std::map<int, Transform> OptimizerCeres::optimize(
 		}
 
 		ceres::LossFunction* loss_function = NULL;
-#if CERES_VERSION_MAJOR >= 3 || \
+		ceres::LocalParameterization* angle_local_parameterization = NULL;
-    (CERES_VERSION_MAJOR == 2 && CERES_VERSION_MINOR >= 1)
+		ceres::LocalParameterization* quaternion_local_parameterization = NULL;
-		ceres::Manifold* angle_local_manifold = NULL;
-		ceres::Manifold* quaternion_local_manifold = NULL;
-#else
-		ceres::LocalParameterization* angle_local_manifold = NULL;
-		ceres::LocalParameterization* quaternion_local_manifold = NULL;
-#endif
 
 		for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
 		{
@@ -195,12 +164,12 @@ std::map<int, Transform> OptimizerCeres::optimize(
 						&pose_begin_iter->second.x,	&pose_begin_iter->second.y, &pose_begin_iter->second.yaw_radians,
 						&pose_end_iter->second.x, &pose_end_iter->second.y, &pose_end_iter->second.yaw_radians);
 
-					if(angle_local_manifold == NULL)
+					if(angle_local_parameterization == NULL)
 					{
-						angle_local_manifold = ceres::examples::AngleManfold::Create();
+						angle_local_parameterization = ceres::examples::AngleLocalParameterization::Create();
 					}
-					SetCeresProblemManifold(problem, &pose_begin_iter->second.yaw_radians, angle_local_manifold);
+					problem.SetParameterization(&pose_begin_iter->second.yaw_radians, angle_local_parameterization);
-					SetCeresProblemManifold(problem, &pose_end_iter->second.yaw_radians, angle_local_manifold);
+					problem.SetParameterization(&pose_end_iter->second.yaw_radians, angle_local_parameterization);
 				}
 				else
 				{
@@ -225,17 +194,12 @@ std::map<int, Transform> OptimizerCeres::optimize(
 					problem.AddResidualBlock(cost_function, loss_function,
 											  pose_begin_iter->second.p.data(), pose_begin_iter->second.q.coeffs().data(),
 											  pose_end_iter->second.p.data(), pose_end_iter->second.q.coeffs().data());
-					if(quaternion_local_manifold == NULL)
+					if(quaternion_local_parameterization == NULL)
 					{
-#if CERES_VERSION_MAJOR >= 3 || \
+						quaternion_local_parameterization = new ceres::EigenQuaternionParameterization;
-    (CERES_VERSION_MAJOR == 2 && CERES_VERSION_MINOR >= 1)
-						quaternion_local_manifold = new ceres::EigenQuaternionManifold;
-#else
-						quaternion_local_manifold = new ceres::EigenQuaternionParameterization;
-#endif
 					}
-					SetCeresProblemManifold(problem, pose_begin_iter->second.q.coeffs().data(), quaternion_local_manifold);
+					problem.SetParameterization(pose_begin_iter->second.q.coeffs().data(), quaternion_local_parameterization);
-					SetCeresProblemManifold(problem, pose_end_iter->second.q.coeffs().data(), quaternion_local_manifold);
+					problem.SetParameterization(pose_end_iter->second.q.coeffs().data(), quaternion_local_parameterization);
 				}
 			}
 			//else // not supporting pose prior and landmarks

corelib/src/optimizer/OptimizerG2O.cpp

@@ -1037,12 +1037,6 @@ std::map<int, Transform> OptimizerG2O::optimize(
 		int it = 0;
 		UTimer timer;
 		double lastError = 0.0;
-
-		if (!optimizer.solver()->init()) {
-			UERROR("g2o: Error while initializing solver");
-			return optimizedPoses;
-		}
-
 		if(intermediateGraphes || this->epsilon() > 0.0)
 		{
 			for(int i=0; i<iterations(); ++i)
@@ -1156,8 +1150,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
 					}
 				}
 
-				g2o::OptimizationAlgorithm::SolverResult result = optimizer.solver()->solve(i);
+				it += optimizer.optimize(1);
-				++it;
 
 				// early stop condition
 				optimizer.computeActiveErrors();
@@ -1170,12 +1163,6 @@ std::map<int, Transform> OptimizerG2O::optimize(
 					return optimizedPoses;
 				}
 
-				if(result == g2o::OptimizationAlgorithm::Fail)
-				{
-					UERROR("g2o: Solver failed, aborting optimization!");
-					return optimizedPoses;
-				}
-
 				double errorDelta = lastError - chi2;
 				if(i>0 && errorDelta < this->epsilon())
 				{

corelib/src/optimizer/OptimizerGTSAM.cpp

@@ -209,7 +209,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 			UDEBUG("hasGPSPrior=%s", hasGPSPrior?"true":"false");
 			if(isSlam2d())
 			{
-				gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector3(0.01, 0.01, hasGPSPrior?1e-2:1e-9));
+				gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector3(0.01, 0.01, hasGPSPrior?1e-2:std::numeric_limits<double>::min()));
 				graph.add(gtsam::PriorFactor<gtsam::Pose2>(rootId, gtsam::Pose2(initialPose.x(), initialPose.y(), initialPose.theta()), priorNoise));
 				addedPrior.push_back(ConstraintToFactor(rootId, rootId, -1));
 			}
@@ -217,7 +217,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 			{
 				gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances(
 						(gtsam::Vector(6) <<
-								(hasGravityConstraints?2:1e-2), (hasGravityConstraints?2:1e-2), (hasGPSPrior?1e-2:1e-9), // roll, pitch, fixed yaw if there are no priors
+								(hasGravityConstraints?2:1e-2), (hasGravityConstraints?2:1e-2), (hasGPSPrior?1e-2:std::numeric_limits<double>::min()), // roll, pitch, fixed yaw if there are no priors
 								(hasGPSPrior?2:1e-2), hasGPSPrior?2:1e-2, hasGPSPrior?2:1e-2 // xyz
 								).finished());
 				graph.add(gtsam::PriorFactor<gtsam::Pose3>(rootId, gtsam::Pose3(initialPose.toEigen4d()), priorNoise));
@@ -924,42 +924,22 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 			// early stop condition
 			UDEBUG("iteration %d error =%f", i+1, error);
 			double errorDelta = lastError - error;
-			if(this->epsilon() > 0.0 && fabs(error) > 1000000000000.0)
+			if((isam2_ || i>0) && errorDelta < this->epsilon())
 			{
-				UWARN("Error computed (%e) is very huge and/or diverging! Aborting! "
+				if(errorDelta < 0)
-					   "Set %s to 0 to ignore that check and keep iterating up to %s (%d).",
+				{
-					   error,
+					UDEBUG("Negative improvement?! Ignore and continue optimizing... (%f < %f)", errorDelta, this->epsilon());
-					   Parameters::kOptimizerEpsilon().c_str(),
+				}
-					   Parameters::kOptimizerIterations().c_str(),
+				else
-					   this->iterations());
+				{
-				return optimizedPoses;
+					UDEBUG("Stop optimizing, not enough improvement (%f < %f)", errorDelta, this->epsilon());
+					break;
+				}
 			}
-			else
+			else if(i==0 && error < this->epsilon())
 			{
-				if((isam2_ || i>0) && errorDelta < this->epsilon())
+				UINFO("Stop optimizing, error is already under epsilon (%f < %f)", error, this->epsilon());
-				{
+				break;
-					if(errorDelta < 0)
-					{
-						UDEBUG("Negative improvement?! Ignore and continue optimizing... (%f < %f)", errorDelta, this->epsilon());
-					}
-					else
-					{
-						UDEBUG("Stop optimizing, not enough improvement (%f < %f)", errorDelta, this->epsilon());
-						break;
-					}
-				}
-				else if(i==0)
-				{ 
-					if(error < 0)
-					{
-						UDEBUG("Negative error?! Ignore and continue optimizing... (%f)", error);
-					}
-					else if(error < this->epsilon())
-					{
-						UINFO("Stop optimizing, error is already under epsilon (%f < %f)", error, this->epsilon());
-						break;
-					}
-				}
 			}
 			lastError = error;
 		}

corelib/src/optimizer/ceres/pose_graph_2d/angle_local_parameterization.h

@@ -28,16 +28,10 @@
 //
 // Author: vitus@google.com (Michael Vitus)
 
-#ifndef CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_MANIFOLD_H_
+#ifndef CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_LOCAL_PARAMETERIZATION_H_
-#define CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_MANIFOLD_H_
+#define CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_LOCAL_PARAMETERIZATION_H_
 
-#if CERES_VERSION_MAJOR >= 3 || \
+#include "ceres/local_parameterization.h"
-    (CERES_VERSION_MAJOR == 2 && CERES_VERSION_MINOR >= 1)
-#include <ceres/autodiff_manifold.h>
-#include <ceres/manifold.h>
-#else
-#include <ceres/local_parameterization.h>
-#endif
 #include "normalize_angle.h"
 
 namespace ceres {
@@ -45,39 +39,7 @@ namespace examples {
 
 // Defines a local parameterization for updating the angle to be constrained in
 // [-pi to pi).
-
+class AngleLocalParameterization {
-#if CERES_VERSION_MAJOR >= 3 || \
-    (CERES_VERSION_MAJOR == 2 && CERES_VERSION_MINOR >= 1)
-
-// Defines a manifold for updating the angle to be constrained in [-pi to pi).
-class AngleManifold {
- public:
-  template <typename T>
-  bool Plus(const T* x_radians,
-            const T* delta_radians,
-            T* x_plus_delta_radians) const {
-    *x_plus_delta_radians = NormalizeAngle(*x_radians + *delta_radians);
-    return true;
-  }
-
-  template <typename T>
-  bool Minus(const T* y_radians,
-             const T* x_radians,
-             T* y_minus_x_radians) const {
-    *y_minus_x_radians =
-        NormalizeAngle(*y_radians) - NormalizeAngle(*x_radians);
-
-    return true;
-  }
-
-  static ceres::Manifold* Create() {
-    return new ceres::AutoDiffManifold<AngleManifold, 1, 1>;
-  }
-};
-
-#else
-
-class AngleManfold {
  public:
 
   template <typename T>
@@ -90,13 +52,12 @@ class AngleManfold {
   }
 
   static ceres::LocalParameterization* Create() {
-    return (new ceres::AutoDiffLocalParameterization<AngleManfold, 1, 1>);
+    return (new ceres::AutoDiffLocalParameterization<AngleLocalParameterization,
+                                                     1, 1>);
   }
 };
 
-#endif
-
 }  // namespace examples
 }  // namespace ceres
 
-#endif  // CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_MANIFOLD_H_
+#endif  // CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_LOCAL_PARAMETERIZATION_H_

corelib/src/optimizer/ceres/pose_graph_3d/eigen_quaternion_parameterization.h

@@ -31,7 +31,7 @@
 #ifndef CERES_EXAMPLES_POSE_GRAPH_3D_EIGEN_QUATERNION_PARAMETERIZATION_H_
 #define CERES_EXAMPLES_POSE_GRAPH_3D_EIGEN_QUATERNION_PARAMETERIZATION_H_
 
-#include "ceres/manifold.h"
+#include "ceres/local_parameterization.h"
 
 namespace ceres {
 
@@ -46,7 +46,7 @@ namespace ceres {
 //
 // Plus(x, delta) = [sin(|delta|) delta / |delta|, cos(|delta|)] * x
 // with * being the quaternion multiplication operator.
-class EigenQuaternionParameterization : public ceres::Manifold {
+class EigenQuaternionParameterization : public ceres::LocalParameterization {
  public:
   virtual ~EigenQuaternionParameterization() {}
   virtual bool Plus(const double* x_ptr,

corelib/src/rvl_codec.cpp

@@ -1,102 +0,0 @@
-// The following code is a C++ wrapper of the code presented by
-// Andrew D. Wilson in "Fast Lossless Depth Image Compression" at SIGCHI'17.
-// The original code is licensed under the MIT License.
-
-#include <rtabmap/core/rvl_codec.h>
-
-namespace rtabmap
-{
-
-RvlCodec::RvlCodec() {}
-
-void RvlCodec::EncodeVLE(int value)
-{
-    do
-    {
-        int nibble = value & 0x7;  // lower 3 bits
-        if (value >>= 3)
-            nibble |= 0x8;  // more to come
-        word_ <<= 4;
-        word_ |= nibble;
-        if (++nibblesWritten_ == 8)  // output word
-        {
-            *pBuffer_++ = word_;
-            nibblesWritten_ = 0;
-            word_ = 0;
-        }
-    } while (value);
-}
-
-int RvlCodec::DecodeVLE()
-{
-    unsigned int nibble;
-    int value = 0, bits = 29;
-    do
-    {
-        if (!nibblesWritten_)
-        {
-            word_ = *pBuffer_++;  // load word
-            nibblesWritten_ = 8;
-        }
-        nibble = word_ & 0xf0000000;
-        value |= (nibble << 1) >> bits;
-        word_ <<= 4;
-        nibblesWritten_--;
-        bits -= 3;
-    } while (nibble & 0x80000000);
-    return value;
-}
-
-int RvlCodec::CompressRVL(const uint16_t * input, unsigned char * output, int numPixels)
-{
-    buffer_ = pBuffer_ = reinterpret_cast<int *>(output);
-    nibblesWritten_ = 0;
-    const uint16_t * end = input + numPixels;
-    uint16_t previous = 0;
-    while (input != end)
-    {
-        int zeros = 0, nonzeros = 0;
-        for (; (input != end) && !*input; input++, zeros++) {}
-        EncodeVLE(zeros);  // number of zeros
-        for (const uint16_t * p = input; (p != end) && *p++; nonzeros++) {}
-        EncodeVLE(nonzeros);  // number of nonzeros
-        for (int i = 0; i < nonzeros; i++)
-        {
-            uint16_t current = *input++;
-            int delta = current - previous;
-            int positive = (delta << 1) ^ (delta >> 31);
-            EncodeVLE(positive);  // nonzero value
-            previous = current;
-        }
-    }
-    if (nibblesWritten_)  // last few values
-        *pBuffer_++ = word_ << 4 * (8 - nibblesWritten_);
-    return static_cast<int>((unsigned char *)pBuffer_ - (unsigned char *)buffer_);  // num bytes
-}
-
-void RvlCodec::DecompressRVL(const unsigned char * input, uint16_t * output, int numPixels)
-{
-    buffer_ = pBuffer_ = const_cast<int *>(reinterpret_cast<const int *>(input));
-    nibblesWritten_ = 0;
-    uint16_t current, previous = 0;
-    int numPixelsToDecode = numPixels;
-    while (numPixelsToDecode)
-    {
-        int zeros = DecodeVLE();  // number of zeros
-        numPixelsToDecode -= zeros;
-        for (; zeros; zeros--) 
-            *output++ = 0;
-        int nonzeros = DecodeVLE();  // number of nonzeros
-        numPixelsToDecode -= nonzeros;
-        for (; nonzeros; nonzeros--)
-        {
-            int positive = DecodeVLE();  // nonzero value
-            int delta = (positive >> 1) ^ -(positive & 1);
-            current = previous + delta;
-            *output++ = current;
-            previous = current;
-        }
-    }
-}
-
-}  // namespace rtabmap

corelib/src/stereo/StereoBM.cpp

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

corelib/src/stereo/StereoSGBM.cpp

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

corelib/src/util3d.cpp

@@ -822,14 +822,14 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromStereoImages(
 		const ParametersMap & parameters)
 {
 	UASSERT(!imageLeft.empty() && !imageRight.empty());
-	UASSERT(imageRight.type() == CV_8UC1 || imageRight.type() == CV_8UC3);
+	UASSERT(imageRight.type() == CV_8UC1);
 	UASSERT(imageLeft.channels() == 3 || imageLeft.channels() == 1);
 	UASSERT(imageLeft.rows == imageRight.rows &&
 			imageLeft.cols == imageRight.cols);
 	UASSERT(decimation >= 1.0f);
 
 	cv::Mat leftColor = imageLeft;
-	cv::Mat rightColor = imageRight;
+	cv::Mat rightMono = imageRight;
 
 	cv::Mat leftMono;
 	if(leftColor.channels() == 3)
@@ -841,16 +841,6 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromStereoImages(
 		leftMono = leftColor;
 	}
 
-	cv::Mat rightMono;
-	if(rightColor.channels() == 3)
-	{
-		cv::cvtColor(rightColor, rightMono, CV_BGR2GRAY);
-	}
-	else
-	{
-		rightMono = rightColor;
-	}
-
 	return cloudFromDisparityRGB(
 			leftColor,
 			util2d::disparityFromStereoImages(leftMono, rightMono, parameters),
@@ -964,7 +954,7 @@ std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloudsFromSensorData(
 	else if(!sensorData.imageRaw().empty() && !sensorData.rightRaw().empty() && !sensorData.stereoCameraModels().empty())
 	{
 		//stereo
-		UASSERT(sensorData.rightRaw().type() == CV_8UC1 || sensorData.rightRaw().type() == CV_8UC3);
+		UASSERT(sensorData.rightRaw().type() == CV_8UC1);
 
 		cv::Mat leftMono;
 		if(sensorData.imageRaw().channels() == 3)
@@ -976,16 +966,6 @@ std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloudsFromSensorData(
 			leftMono = sensorData.imageRaw();
 		}
 
-		cv::Mat rightMono;
-		if(sensorData.rightRaw().channels() == 3)
-		{
-			cv::cvtColor(sensorData.rightRaw(), rightMono, CV_BGR2GRAY);
-		}
-		else
-		{
-			rightMono = sensorData.rightRaw();
-		}
-
 		UASSERT(int((sensorData.imageRaw().cols/sensorData.stereoCameraModels().size())*sensorData.stereoCameraModels().size()) == sensorData.imageRaw().cols);
 		UASSERT(int((sensorData.rightRaw().cols/sensorData.stereoCameraModels().size())*sensorData.stereoCameraModels().size()) == sensorData.rightRaw().cols);
 		int subImageWidth = sensorData.rightRaw().cols/sensorData.stereoCameraModels().size();
@@ -999,7 +979,7 @@ std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloudsFromSensorData(
 			if(sensorData.stereoCameraModels()[i].isValidForProjection())
 			{
 				cv::Mat left(leftMono, cv::Rect(subImageWidth*i, 0, subImageWidth, leftMono.rows));
-				cv::Mat right(rightMono, cv::Rect(subImageWidth*i, 0, subImageWidth, rightMono.rows));
+				cv::Mat right(sensorData.rightRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.rightRaw().rows));
 				StereoCameraModel model = sensorData.stereoCameraModels()[i];
 				if( roiRatios.size() == 4 &&
 					((roiRatios[0] > 0.0f && roiRatios[0] <= 1.0f) ||
@@ -3027,115 +3007,6 @@ void fillProjectedCloudHoles(cv::Mat & registeredDepth, bool verticalDirection,
 	}
 }
 
-cv::Mat filterFloor(const cv::Mat & depth, const std::vector<CameraModel> & cameraModels, float threshold, cv::Mat * depthBelow)
-{
-	cv::Mat output = depth.clone();
-	if(depth.empty())
-	{
-		return output;
-	}
-	if(depthBelow)
-	{
-		*depthBelow = cv::Mat::zeros(output.size(), output.type());
-	}
-
-	UASSERT(!cameraModels.empty());
-	UASSERT(cameraModels[0].isValidForReprojection());
-	// Support camera model with different resolution than depth image
-	float rgbToDepthFactorX = float(cameraModels[0].imageWidth()) / float(output.cols/cameraModels.size());
-	float rgbToDepthFactorY = float(cameraModels[0].imageHeight()) / float(output.rows);
-	int depthWidth = output.cols/cameraModels.size();
-	UASSERT(depthWidth*(int)cameraModels.size() == output.cols);
-
-	// for each camera
-	for(size_t i=0; i<cameraModels.size(); ++i)
-	{
-		const CameraModel & cam = cameraModels[i];
-		UASSERT(cam.isValidForReprojection());
-		const Transform & localTransform = cam.localTransform();
-		UASSERT(!localTransform.isNull());
-		if(i>0)
-		{
-			// Make sure all models are the same resolution
-			UASSERT(cam.imageWidth() == cameraModels[i-1].imageWidth());
-			UASSERT(cam.imageHeight() == cameraModels[i-1].imageHeight());
-		}
-
-		float depthFx = cam.fx() / rgbToDepthFactorX;
-		float depthFy = cam.fy() / rgbToDepthFactorY;
-		float depthCx = cam.cx() / rgbToDepthFactorX;
-		float depthCy = cam.cy() / rgbToDepthFactorY;
-
-		cv::Mat subImage = output.colRange(cv::Range(i*depthWidth, (i+1)*depthWidth));
-		cv::Mat subImageBelow;
-		if(depthBelow)
-			subImageBelow = depthBelow->colRange(cv::Range(i*depthWidth, (i+1)*depthWidth));
-
-		for(int y=0; y<subImage.rows; ++y)
-		{
-			if(subImage.type() == CV_16UC1)
-			{
-				unsigned short * ptr = (unsigned short *)subImage.row(y).ptr();
-				unsigned short * ptrBelow = 0;
-				if(depthBelow)
-				{
-					ptrBelow = (unsigned short *)subImageBelow.row(y).ptr();
-				}
-				for(int x=0; x<subImage.cols; ++x)
-				{
-					if(ptr[x] > 0)
-					{
-						float d = float(ptr[x])/1000.0f;
-						cv::Point3f pt;
-						pt.x = (x - depthCx) * d / depthFx;
-						pt.y = (y - depthCy) * d / depthFy;
-						pt.z = d;
-						pt = util3d::transformPoint(pt, localTransform);
-						if(pt.z < threshold)
-						{
-							if(ptrBelow)
-							{
-								ptrBelow[x] = ptr[x];
-							}
-							ptr[x] = 0;
-						}
-					}
-				}
-			}
-			else // CV_32FC1
-			{
-				float * ptr = (float *)subImage.row(y).ptr();
-				float * ptrBelow = 0;
-				if(depthBelow)
-				{
-					ptrBelow = (float *)subImageBelow.row(y).ptr();
-				}
-				for(int x=0; x<subImage.cols; ++x)
-				{
-					if(ptr[x] > 0.0f)
-					{
-						float & d = ptr[x];
-						cv::Point3f pt;
-						pt.x = (x - depthCx) * d / depthFx;
-						pt.y = (y - depthCy) * d / depthFy;
-						pt.z = d;
-						pt = util3d::transformPoint(pt, localTransform);
-						if(pt.z < threshold)
-						{
-							if(ptrBelow)
-							{
-								ptrBelow[x] = ptr[x];
-							}
-							d = 0;
-						}
-					}
-				}
-			}
-		}
-	}
-	return output;
-}
-
 class ProjectionInfo {
 public:
 	ProjectionInfo():

corelib/src/util3d_filtering.cpp

@@ -694,7 +694,7 @@ typename pcl::PointCloud<PointT>::Ptr voxelizeImpl(
 
 		if ((dx*dy*dz) > static_cast<std::int64_t>(std::numeric_limits<std::int32_t>::max()))
 		{
-			UDEBUG("Leaf size is too small for the input dataset. Integer indices would overflow. "
+			UWARN("Leaf size is too small for the input dataset. Integer indices would overflow. "
 				  "We will split space to be able to voxelize (lvl=%d cloud=%d min=[%f %f %f] max=[%f %f %f] voxel=%f).",
 				  level,
 				  (int)(indices->empty()?cloud->size():indices->size()),
@@ -2155,7 +2155,7 @@ pcl::IndicesPtr normalFilteringImpl(
 		for(unsigned int i=0; i<cloud_normals->size(); ++i)
 		{
 			Eigen::Vector4f v(cloud_normals->at(i).normal_x, cloud_normals->at(i).normal_y, cloud_normals->at(i).normal_z, 0.0f);
-			if(groundNormalsUp>0.0f && v[2] < -groundNormalsUp && cloud->at(indices->size()!=0?indices->at(i):i).z < viewpoint[2]) // some far velodyne rays on road can have normals toward ground
+			if(groundNormalsUp>0.0f && v[2] < -groundNormalsUp && cloud->at(indices->size()!=0?indices->at(i):i).z < viewpoint[3]) // some far velodyne rays on road can have normals toward ground
 			{
 				//reverse normal
 				v *= -1.0f;
@@ -2226,7 +2226,7 @@ pcl::IndicesPtr normalFilteringImpl(
 			for(unsigned int i=0; i<indices->size(); ++i)
 			{
 				Eigen::Vector4f v(cloud->at(indices->at(i)).normal_x, cloud->at(indices->at(i)).normal_y, cloud->at(indices->at(i)).normal_z, 0.0f);
-				if(groundNormalsUp>0.0f && v[2] < -groundNormalsUp && cloud->at(indices->at(i)).z < viewpoint[2]) // some far velodyne rays on road can have normals toward ground
+				if(groundNormalsUp>0.0f && v[2] < -groundNormalsUp && cloud->at(indices->at(i)).z < viewpoint[3]) // some far velodyne rays on road can have normals toward ground
 				{
 					//reverse normal
 					v *= -1.0f;
@@ -2244,7 +2244,7 @@ pcl::IndicesPtr normalFilteringImpl(
 			for(unsigned int i=0; i<cloud->size(); ++i)
 			{
 				Eigen::Vector4f v(cloud->at(i).normal_x, cloud->at(i).normal_y, cloud->at(i).normal_z, 0.0f);
-				if(groundNormalsUp>0.0f && v[2] < -groundNormalsUp && cloud->at(i).z < viewpoint[2]) // some far velodyne rays on road can have normals toward ground
+				if(groundNormalsUp>0.0f && v[2] < -groundNormalsUp && cloud->at(i).z < viewpoint[3]) // some far velodyne rays on road can have normals toward ground
 				{
 					//reverse normal
 					v *= -1.0f;

corelib/src/util3d_mapping.cpp

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

corelib/src/util3d_surface.cpp

@@ -1454,7 +1454,6 @@ 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)
@@ -1484,7 +1483,6 @@ cv::Mat mergeTextures(
 			exposureFusion,
 			state,
 			blankValue,
-			clearVertexColorUnderTexture,
 			gains,
 			blendingGains,
 			contrastValues);
@@ -1508,7 +1506,6 @@ 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)
@@ -2189,70 +2186,6 @@ 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 pcl::Indices());
-					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::Indices full_indices (mesh.cloud.width * mesh.cloud.height);
-					for (size_t fii = 0; fii < full_indices.size(); ++fii)  // fii = full indices iterator
-						full_indices[fii] = fii;
-
-					// Set up the sorted input indices
-					std::sort (notTexturedVertexIndices->begin (), notTexturedVertexIndices->end ());
-
-					// Store the difference in indices
-					pcl::Indices texturedVertexIndices;
-					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[i] * mesh.cloud.point_step + colorOffset < mesh.cloud.data.size());
-						memcpy(&mesh.cloud.data.data()[texturedVertexIndices[i] * mesh.cloud.point_step + colorOffset], reinterpret_cast<float*>(&white), sizeof(float));
-					}
-				}
-			}
 		}
 	}
 	UDEBUG("globalTextures=%d", globalTextures.cols?globalTextures.cols / globalTextures.rows:0);
@@ -3597,7 +3530,7 @@ LaserScan adjustNormalsToViewPoint(
 
 					float result = v.dot(n);
 					if(result < 0
-					 || (groundNormalsUp>0.0f && ptr[nz] < -groundNormalsUp && ptr[2] < viewpoint[2])) // some far velodyne rays on road can have normals toward ground
+					 || (groundNormalsUp>0.0f && ptr[nz] < -groundNormalsUp && ptr[2] < viewpoint[3])) // some far velodyne rays on road can have normals toward ground
 					{
 						//reverse normal
 						ptr[nx] *= -1.0f;
@@ -3636,7 +3569,7 @@ void adjustNormalsToViewPointImpl(
 
 			float result = v.dot(n);
 			if(result < 0
-				|| (groundNormalsUp>0.0f && normal.z < -groundNormalsUp && cloud->points[i].z < viewpoint[2])) // some far velodyne rays on road can have normals toward ground
+				|| (groundNormalsUp>0.0f && normal.z < -groundNormalsUp && cloud->points[i].z < viewpoint[3])) // some far velodyne rays on road can have normals toward ground
 			{
 				//reverse normal
 				cloud->points[i].normal_x *= -1.0f;
@@ -3692,67 +3625,6 @@ void adjustNormalsToViewPoint(
 	adjustNormalsToViewPointImpl<pcl::PointXYZINormal>(cloud, viewpoint, groundNormalsUp);
 }
 
-template<typename PointT>
-void adjustNormalsToViewPointsImpl(
-		const std::map<int, Transform> & poses,
-		const std::vector<int> & cameraIndices,
-		typename pcl::PointCloud<PointT>::Ptr & cloud,
-		float groundNormalsUp)
-{
-	if(poses.size() && cloud->size() == cameraIndices.size() && cloud->size())
-	{
-		#pragma omp parallel for
-		for(int i=0; i<(int)cloud->size(); ++i)
-		{
-			pcl::PointXYZ normal(cloud->points[i].normal_x, cloud->points[i].normal_y, cloud->points[i].normal_z);
-			if(pcl::isFinite(normal))
-			{
-				const Transform & p = poses.at(cameraIndices[i]);
-				pcl::PointXYZ viewpoint(p.x(), p.y(), p.z());
-				Eigen::Vector3f v = viewpoint.getVector3fMap() - cloud->points[i].getVector3fMap();
-
-				Eigen::Vector3f n(normal.x, normal.y, normal.z);
-
-				float result = v.dot(n);
-				if(result < 0 ||
-					(groundNormalsUp>0.0f && normal.z < -groundNormalsUp && cloud->points[i].z < viewpoint.z)) // some far velodyne rays on road can have normals toward ground)
-				{
-					//reverse normal
-					cloud->points[i].normal_x *= -1.0f;
-					cloud->points[i].normal_y *= -1.0f;
-					cloud->points[i].normal_z *= -1.0f;
-				}
-			}
-		}
-	}
-}
-
-void adjustNormalsToViewPoints(
-		const std::map<int, Transform> & poses,
-		const std::vector<int> & cameraIndices,
-		pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
-		float groundNormalsUp)
-{
-	adjustNormalsToViewPointsImpl<pcl::PointNormal>(poses, cameraIndices, cloud, groundNormalsUp);
-}
-
-void adjustNormalsToViewPoints(
-		const std::map<int, Transform> & poses,
-		const std::vector<int> & cameraIndices,
-		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
-		float groundNormalsUp)
-{
-	adjustNormalsToViewPointsImpl<pcl::PointXYZRGBNormal>(poses, cameraIndices, cloud, groundNormalsUp);
-}
-
-void adjustNormalsToViewPoints(
-		const std::map<int, Transform> & poses,
-		const std::vector<int> & cameraIndices,
-		pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
-		float groundNormalsUp)
-{
-	adjustNormalsToViewPointsImpl<pcl::PointXYZINormal>(poses, cameraIndices, cloud, groundNormalsUp);
-}
 
 template<typename PointT>
 void adjustNormalsToViewPointsImpl(
@@ -3948,359 +3820,6 @@ bool intersectRayTriangle(
     return true;                    // I is in T
 }
 
-/**
- * This is a modified copy of https://github.com/PointCloudLibrary/pcl/blob/pcl-1.12.1/io/src/obj_io.cpp
- * with added color on each vertex if provided
- */
-int saveOBJFile(
-	const std::string &file_name,
-	const pcl::TextureMesh &tex_mesh,
-	unsigned precision)
-{
-  if (tex_mesh.cloud.data.empty ())
-  {
-    UERROR ("Input point cloud has no data!\n");
-    return (-1);
-  }
-  // Open file
-  std::ofstream fs;
-  fs.precision (precision);
-  fs.open (file_name.c_str ());
-
-  // Define material file
-  std::string mtl_file_name = file_name.substr (0, file_name.find_last_of ('.')) + ".mtl";
-  // Strip path for "mtllib" command
-  std::string mtl_file_name_nopath = mtl_file_name;
-  mtl_file_name_nopath.erase (0, mtl_file_name.find_last_of ('/') + 1);
-
-  /* Write 3D information */
-  // number of points
-  unsigned nr_points  = tex_mesh.cloud.width * tex_mesh.cloud.height;
-  unsigned point_size = static_cast<unsigned> (tex_mesh.cloud.data.size () / nr_points);
-
-  // mesh size
-  unsigned nr_meshes = static_cast<unsigned> (tex_mesh.tex_polygons.size ());
-  // number of faces for header
-  unsigned nr_faces = 0;
-  for (unsigned m = 0; m < nr_meshes; ++m)
-    nr_faces += static_cast<unsigned> (tex_mesh.tex_polygons[m].size ());
-
-  // Write the header information
-  fs << "####" << '\n';
-  fs << "# OBJ dataFile simple version. File name: " << file_name << '\n';
-  fs << "# Vertices: " << nr_points << '\n';
-  fs << "# Faces: " <<nr_faces << '\n';
-  fs << "# Material information:" << '\n';
-  fs << "mtllib " << mtl_file_name_nopath << '\n';
-  fs << "####" << '\n';
-
-  // Write vertex coordinates
-  fs << "# Vertices" << '\n';
-  for (unsigned i = 0; i < nr_points; ++i)
-  {
-    int xyz = 0;
-    // "v" just be written one
-    bool v_written = false;
-    for (std::size_t d = 0; d < tex_mesh.cloud.fields.size (); ++d)
-    {
-      // adding vertex
-      if ((tex_mesh.cloud.fields[d].datatype == pcl::PCLPointField::FLOAT32) && (
-          tex_mesh.cloud.fields[d].name == "x" ||
-          tex_mesh.cloud.fields[d].name == "y" ||
-          tex_mesh.cloud.fields[d].name == "z"))
-      {
-        if (!v_written)
-        {
-           // write vertices beginning with v
-          fs << "v ";
-          v_written = true;
-        }
-        float value;
-        memcpy (&value, &tex_mesh.cloud.data[i * point_size + tex_mesh.cloud.fields[d].offset], sizeof (float));
-        fs << value;
-        if (++xyz == 3)
-          continue;
-        fs << " ";
-      }
-	  else if(tex_mesh.cloud.fields[d].datatype == pcl::PCLPointField::FLOAT32 &&
-          	  tex_mesh.cloud.fields[d].name == "rgb")
-	  {
-		std::uint32_t rgb = *reinterpret_cast<const int*>(&tex_mesh.cloud.data[i * point_size + tex_mesh.cloud.fields[d].offset]);
-		std::uint8_t r = (rgb >> 16) & 0x0000ff;
-		std::uint8_t g = (rgb >> 8)  & 0x0000ff;
-		std::uint8_t b = (rgb)       & 0x0000ff;
-		fs << " " << float(r)/255.0f << " " << float(g)/255.0f << " " << float(b)/255.0f;
-		break;
-	  }
-    }
-    if (xyz != 3)
-    {
-      UERROR ("Input point cloud has no XYZ data!\n");
-      return (-2);
-    }
-    fs << '\n';
-  }
-  fs << "# "<< nr_points <<" vertices" << '\n';
-
-  // Write vertex normals
-  for (unsigned i = 0; i < nr_points; ++i)
-  {
-    int xyz = 0;
-    // "vn" just be written one
-    bool v_written = false;
-    for (std::size_t d = 0; d < tex_mesh.cloud.fields.size (); ++d)
-    {
-      // adding vertex
-      if ((tex_mesh.cloud.fields[d].datatype == pcl::PCLPointField::FLOAT32) && (
-          tex_mesh.cloud.fields[d].name == "normal_x" ||
-          tex_mesh.cloud.fields[d].name == "normal_y" ||
-          tex_mesh.cloud.fields[d].name == "normal_z"))
-      {
-    	  if (!v_written)
-    	  {
-          // write vertices beginning with vn
-          fs << "vn ";
-          v_written = true;
-    	  }
-        float value;
-        memcpy (&value, &tex_mesh.cloud.data[i * point_size + tex_mesh.cloud.fields[d].offset], sizeof (float));
-        fs << value;
-        if (++xyz == 3)
-          break;
-        fs << " ";
-      }
-    }
-    if (xyz != 3)
-    {
-      UERROR ("Input point cloud has no normals!\n");
-      return (-2);
-    }
-    fs << '\n';
-  }
-  // Write vertex texture with "vt" (adding latter)
-
-  for (unsigned m = 0; m < nr_meshes; ++m)
-  {
-    fs << "# " << tex_mesh.tex_coordinates[m].size() << " vertex textures in submesh " << m <<  '\n';
-    for (const auto &coordinate : tex_mesh.tex_coordinates[m])
-    {
-      fs << "vt ";
-      fs <<  coordinate[0] << " " << coordinate[1] << '\n';
-    }
-  }
-
-  unsigned f_idx = 0;
-
-  // int idx_vt =0;
-  for (unsigned m = 0; m < nr_meshes; ++m)
-  {
-    if (m > 0) f_idx += static_cast<unsigned> (tex_mesh.tex_polygons[m-1].size ());
-
-    fs << "# The material will be used for mesh " << m << '\n';
-    fs << "usemtl " <<  tex_mesh.tex_materials[m].tex_name << '\n';
-    fs << "# Faces" << '\n';
-
-    for (std::size_t i = 0; i < tex_mesh.tex_polygons[m].size(); ++i)
-    {
-      // Write faces with "f"
-      fs << "f";
-      // There's one UV per vertex per face, i.e., the same vertex can have
-      // different UV depending on the face.
-      for (std::size_t j = 0; j < tex_mesh.tex_polygons[m][i].vertices.size (); ++j)
-      {
-        std::uint32_t idx = tex_mesh.tex_polygons[m][i].vertices[j] + 1;
-        fs << " " << idx
-           << "/" << tex_mesh.tex_polygons[m][i].vertices.size () * (i+f_idx) +j+1
-           << "/" << idx; // vertex index in obj file format starting with 1
-      }
-      fs << '\n';
-    }
-    fs << "# "<< tex_mesh.tex_polygons[m].size() << " faces in mesh " << m << '\n';
-  }
-  fs << "# End of File" << std::flush;
-
-  // Close obj file
-  fs.close ();
-
-  /* Write material definition for OBJ file*/
-  // Open file
-
-  std::ofstream m_fs;
-  m_fs.precision (precision);
-  m_fs.open (mtl_file_name.c_str ());
-
-  // default
-  m_fs << "#" << '\n';
-  m_fs << "# Wavefront material file" << '\n';
-  m_fs << "#" << '\n';
-  for(unsigned m = 0; m < nr_meshes; ++m)
-  {
-    m_fs << "newmtl " << tex_mesh.tex_materials[m].tex_name << '\n';
-    m_fs << "Ka "<< tex_mesh.tex_materials[m].tex_Ka.r << " " << tex_mesh.tex_materials[m].tex_Ka.g << " " << tex_mesh.tex_materials[m].tex_Ka.b << '\n'; // defines the ambient color of the material to be (r,g,b).
-    m_fs << "Kd "<< tex_mesh.tex_materials[m].tex_Kd.r << " " << tex_mesh.tex_materials[m].tex_Kd.g << " " << tex_mesh.tex_materials[m].tex_Kd.b << '\n'; // defines the diffuse color of the material to be (r,g,b).
-    m_fs << "Ks "<< tex_mesh.tex_materials[m].tex_Ks.r << " " << tex_mesh.tex_materials[m].tex_Ks.g << " " << tex_mesh.tex_materials[m].tex_Ks.b << '\n'; // defines the specular color of the material to be (r,g,b). This color shows up in highlights.
-    m_fs << "d " << tex_mesh.tex_materials[m].tex_d << '\n'; // defines the transparency of the material to be alpha.
-    m_fs << "Ns "<< tex_mesh.tex_materials[m].tex_Ns  << '\n'; // defines the shininess of the material to be s.
-    m_fs << "illum "<< tex_mesh.tex_materials[m].tex_illum << '\n'; // denotes the illumination model used by the material.
-                                            // illum = 1 indicates a flat material with no specular highlights, so the value of Ks is not used.
-                                            // illum = 2 denotes the presence of specular highlights, and so a specification for Ks is required.
-    m_fs << "map_Kd " << tex_mesh.tex_materials[m].tex_file << '\n';
-    m_fs << "###" << '\n';
-  }
-  m_fs.close ();
-  return (0);
-}
-
-/**
- * This is a modified copy of https://github.com/PointCloudLibrary/pcl/blob/pcl-1.12.1/io/src/obj_io.cpp
- * with added color on each vertex if provided
- */
-int saveOBJFile(
-	const std::string &file_name,
-	const pcl::PolygonMesh &mesh,
-	unsigned precision)
-{
-  if (mesh.cloud.data.empty ())
-  {
-    UERROR ("Input point cloud has no data!\n");
-    return (-1);
-  }
-  // Open file
-  std::ofstream fs;
-  fs.precision (precision);
-  fs.open (file_name.c_str ());
-
-  /* Write 3D information */
-  // number of points
-  int nr_points  = mesh.cloud.width * mesh.cloud.height;
-  // point size
-  unsigned point_size = static_cast<unsigned> (mesh.cloud.data.size () / nr_points);
-  // number of faces for header
-  unsigned nr_faces = static_cast<unsigned> (mesh.polygons.size ());
-  // Do we have vertices normals?
-  int normal_index = getFieldIndex (mesh.cloud, "normal_x");
-
-  // Write the header information
-  fs << "####" << '\n';
-  fs << "# OBJ dataFile simple version. File name: " << file_name << '\n';
-  fs << "# Vertices: " << nr_points << '\n';
-  if (normal_index != -1)
-    fs << "# Vertices normals : " << nr_points << '\n';
-  fs << "# Faces: " <<nr_faces << '\n';
-  fs << "####" << '\n';
-
-  // Write vertex coordinates
-  fs << "# List of Vertices, with (x,y,z) coordinates, w is optional." << '\n';
-  for (int i = 0; i < nr_points; ++i)
-  {
-    int xyz = 0;
-    for (std::size_t d = 0; d < mesh.cloud.fields.size (); ++d)
-    {
-      // adding vertex
-      if ((mesh.cloud.fields[d].datatype == pcl::PCLPointField::FLOAT32) && (
-          mesh.cloud.fields[d].name == "x" ||
-          mesh.cloud.fields[d].name == "y" ||
-          mesh.cloud.fields[d].name == "z"))
-      {
-        if (mesh.cloud.fields[d].name == "x")
-           // write vertices beginning with v
-          fs << "v ";
-
-        float value;
-        memcpy (&value, &mesh.cloud.data[i * point_size + mesh.cloud.fields[d].offset], sizeof (float));
-        fs << value;
-        if (++xyz == 3)
-          continue;
-        fs << " ";
-      }
-	  else if(mesh.cloud.fields[d].datatype == pcl::PCLPointField::FLOAT32 &&
-          	  mesh.cloud.fields[d].name == "rgb")
-	  {
-		std::uint32_t rgb = *reinterpret_cast<const int*>(&mesh.cloud.data[i * point_size + mesh.cloud.fields[d].offset]);
-		std::uint8_t r = (rgb >> 16) & 0x0000ff;
-		std::uint8_t g = (rgb >> 8)  & 0x0000ff;
-		std::uint8_t b = (rgb)       & 0x0000ff;
-		fs << " " << float(r)/255.0f << " " << float(g)/255.0f << " " << float(b)/255.0f;
-		break;
-	  }
-    }
-    if (xyz != 3)
-    {
-      UERROR ("Input point cloud has no XYZ data!\n");
-      return (-2);
-    }
-    fs << '\n';
-  }
-
-  fs << "# "<< nr_points <<" vertices" << '\n';
-
-  if(normal_index != -1)
-  {
-    fs << "# Normals in (x,y,z) form; normals might not be unit." <<  '\n';
-    // Write vertex normals
-    for (int i = 0; i < nr_points; ++i)
-    {
-      int nxyz = 0;
-      for (std::size_t d = 0; d < mesh.cloud.fields.size (); ++d)
-      {
-        // adding vertex
-        if ((mesh.cloud.fields[d].datatype == pcl::PCLPointField::FLOAT32) && (
-              mesh.cloud.fields[d].name == "normal_x" ||
-              mesh.cloud.fields[d].name == "normal_y" ||
-              mesh.cloud.fields[d].name == "normal_z"))
-        {
-          if (mesh.cloud.fields[d].name == "normal_x")
-            // write vertices beginning with vn
-            fs << "vn ";
-
-          float value;
-          memcpy (&value, &mesh.cloud.data[i * point_size + mesh.cloud.fields[d].offset], sizeof (float));
-          fs << value;
-          if (++nxyz == 3)
-            break;
-          fs << " ";
-        }
-      }
-      if (nxyz != 3)
-      {
-        UERROR ("Input point cloud has no normals!\n");
-        return (-2);
-      }
-      fs << '\n';
-    }
-
-    fs << "# "<< nr_points <<" vertices normals" << '\n';
-  }
-
-  fs << "# Face Definitions" << '\n';
-  // Write down faces
-  if(normal_index == -1)
-  {
-    for(unsigned i = 0; i < nr_faces; i++)
-    {
-      fs << "f ";      
-      for (std::size_t j = 0; j < mesh.polygons[i].vertices.size () - 1; ++j)
-        fs << mesh.polygons[i].vertices[j] + 1 << " ";
-      fs << mesh.polygons[i].vertices.back() + 1 << '\n';
-    }
-  }
-  else
-  {
-    for(unsigned i = 0; i < nr_faces; i++)
-    {
-      fs << "f ";
-      for (std::size_t j = 0; j < mesh.polygons[i].vertices.size () - 1; ++j)
-        fs << mesh.polygons[i].vertices[j] + 1 << "//" << mesh.polygons[i].vertices[j] + 1 << " ";
-      fs << mesh.polygons[i].vertices.back() + 1 << "//" << mesh.polygons[i].vertices.back() + 1 << '\n';
-    }
-  }
-  fs << "# End of File" << std::endl;
-
-  // Close obj file
-  fs.close ();
-  return 0;
-}
-
 }
 
 }

docker/jammy/Dockerfile

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

docker/jammy/deps/Dockerfile

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

guilib/include/rtabmap/gui/CloudViewer.h

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

guilib/include/rtabmap/gui/CloudViewerInteractorStyle.h

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

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -278,7 +278,6 @@ public:
 	bool isSourceFeatureDetection() const;
 	bool isSourceStereoDepthGenerated() const;
 	bool isSourceStereoExposureCompensation() const;
-	bool isRightGrayScale() const;
 	bool isSourceScanFromDepth() const;
 	bool isSourceScanDeskewing() const;
 	int getSourceScanDownsampleStep() const;

guilib/src/CameraViewer.cpp

@@ -132,23 +132,9 @@ void CameraViewer::showImage(const rtabmap::SensorData & data)
 {
 	processingImages_ = true;
 	QString sizes;
-
+	imageView_->setVisible(!data.imageRaw().empty() || !data.imageRaw().empty());
-	cv::Mat left;
-	cv::Mat depthOrRight;
-	LaserScan scan;
-	if( !data.imageRaw().empty() || !data.imageCompressed().empty() ||
-		!data.depthOrRightRaw().empty() || !data.depthOrRightCompressed().empty() ||
-		!data.laserScanRaw().empty() || !data.laserScanCompressed().empty())
-	{
-		data.uncompressDataConst(
-			!data.imageRaw().empty() || !data.imageCompressed().empty()?&left:0, 
-			!data.depthOrRightRaw().empty() || !data.depthOrRightCompressed().empty()?&depthOrRight:0,
-			!data.laserScanRaw().empty() || !data.laserScanCompressed().empty()?&scan:0);
-	}
-
-	imageView_->setVisible(!left.empty() || !left.empty());
 	std::map<int, MarkerInfo> detections;
-	if(!left.empty())
+	if(!data.imageRaw().empty())
 	{
 		std::vector<CameraModel> models;
 		if(markerCheckbox_->isEnabled() && markerCheckbox_->isChecked())
@@ -166,58 +152,36 @@ void CameraViewer::showImage(const rtabmap::SensorData & data)
 		if(!models.empty() && models[0].isValidForProjection())
 		{
 			cv::Mat imageWithDetections;
-			detections = markerDetector_->detect(left, models, depthOrRight, std::map<int, float>(), &imageWithDetections);
+			detections = markerDetector_->detect(data.imageRaw(), models, data.depthRaw(), std::map<int, float>(), &imageWithDetections);
 			imageView_->setImage(uCvMat2QImage(imageWithDetections));
 		}
 		else
 		{
-			imageView_->setImage(uCvMat2QImage(left));
+			imageView_->setImage(uCvMat2QImage(data.imageRaw()));
 		}
-		sizes.append(QString("Color=%1x%2").arg(left.cols).arg(left.rows));
+		sizes.append(QString("Color=%1x%2").arg(data.imageRaw().cols).arg(data.imageRaw().rows));
 	}
-	if(!depthOrRight.empty())
+	if(!data.depthOrRightRaw().empty())
 	{
-		imageView_->setImageDepth(depthOrRight);
+		imageView_->setImageDepth(data.depthOrRightRaw());
-		sizes.append(QString(" Depth=%1x%2").arg(depthOrRight.cols).arg(depthOrRight.rows));
+		sizes.append(QString(" Depth=%1x%2").arg(data.depthOrRightRaw().cols).arg(data.depthOrRightRaw().rows));
 	}
 	imageSizeLabel_->setText(sizes);
 
-	if(!depthOrRight.empty() &&
+	if(!data.depthOrRightRaw().empty() &&
 	   ((data.stereoCameraModels().size() && data.stereoCameraModels()[0].isValidForProjection()) || (data.cameraModels().size() && data.cameraModels().at(0).isValidForProjection())))
 	{
 		if(showCloudCheckbox_->isChecked())
 		{
-			if(!left.empty() && !depthOrRight.empty())
+			if(!data.imageRaw().empty() && !data.depthOrRightRaw().empty())
 			{
 				showCloudCheckbox_->setEnabled(true);
-				if(data.imageRaw().empty())
+				cloudView_->addCloud("cloud", util3d::cloudRGBFromSensorData(data, decimationSpin_->value()!=0?decimationSpin_->value():1, 0, 0, 0, parameters_));
-				{
-					if(!data.stereoCameraModels().empty())
-					{
-						cloudView_->addCloud("cloud", util3d::cloudRGBFromSensorData(SensorData(left, depthOrRight, data.stereoCameraModels()), decimationSpin_->value()!=0?decimationSpin_->value():1, 0, 0, 0, parameters_));
-					}
-					else
-					{
-						cloudView_->addCloud("cloud", util3d::cloudRGBFromSensorData(SensorData(left, depthOrRight, data.cameraModels()), decimationSpin_->value()!=0?decimationSpin_->value():1, 0, 0, 0, parameters_));
-					}
-					
-				}
-				else
-				{
-					cloudView_->addCloud("cloud", util3d::cloudRGBFromSensorData(data, decimationSpin_->value()!=0?decimationSpin_->value():1, 0, 0, 0, parameters_));
-				}
 			}
-			else if(!depthOrRight.empty())
+			else if(!data.depthOrRightRaw().empty())
 			{
 				showCloudCheckbox_->setEnabled(true);
-				if(data.depthOrRightRaw().empty())
+				cloudView_->addCloud("cloud", util3d::cloudFromSensorData(data, decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1, 0, 0, 0, parameters_));
-				{
-					cloudView_->addCloud("cloud", util3d::cloudFromSensorData(SensorData(cv::Mat(), depthOrRight, data.cameraModels()), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1, 0, 0, 0, parameters_));
-				}
-				else
-				{
-					cloudView_->addCloud("cloud", util3d::cloudFromSensorData(data, decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1, 0, 0, 0, parameters_));
-				}
 			}
 
 			// Add landmarks to 3D Map view
@@ -244,37 +208,37 @@ void CameraViewer::showImage(const rtabmap::SensorData & data)
 		}
 	}
 
-	if(!scan.isEmpty())
+	if(!data.laserScanRaw().isEmpty())
 	{
 		showScanCheckbox_->setEnabled(true);
 		if(showScanCheckbox_->isChecked())
 		{
-			if(scan.hasNormals())
+			if(data.laserScanRaw().hasNormals())
 			{
-				if(scan.hasIntensity())
+				if(data.laserScanRaw().hasIntensity())
 				{
-					cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudINormal(scan), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), scan.localTransform(), Qt::yellow);
+					cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudINormal(data.laserScanRaw()), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), data.laserScanRaw().localTransform(), Qt::yellow);
 				}
-				else if(scan.hasRGB())
+				else if(data.laserScanRaw().hasRGB())
 				{
-					cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudRGBNormal(scan), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), scan.localTransform(), Qt::yellow);
+					cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudRGBNormal(data.laserScanRaw()), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), data.laserScanRaw().localTransform(), Qt::yellow);
 				}
 				else
 				{
-					cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudNormal(scan), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), scan.localTransform(), Qt::yellow);
+					cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudNormal(data.laserScanRaw()), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), data.laserScanRaw().localTransform(), Qt::yellow);
 				}
 			}
-			else if(scan.hasIntensity())
+			else if(data.laserScanRaw().hasIntensity())
 			{
-				cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudI(scan), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), scan.localTransform(), Qt::yellow);
+				cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudI(data.laserScanRaw()), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), data.laserScanRaw().localTransform(), Qt::yellow);
 			}
-			else if(scan.hasRGB())
+			else if(data.laserScanRaw().hasRGB())
 			{
-				cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudRGB(scan), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), scan.localTransform(), Qt::yellow);
+				cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudRGB(data.laserScanRaw()), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), data.laserScanRaw().localTransform(), Qt::yellow);
 			}
 			else
 			{
-				cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloud(scan), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), scan.localTransform(), Qt::yellow);
+				cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloud(data.laserScanRaw()), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), data.laserScanRaw().localTransform(), Qt::yellow);
 			}
 		}
 	}

guilib/src/CloudViewer.cpp

@@ -80,7 +80,6 @@ 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
@@ -97,11 +96,6 @@ 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) :
@@ -130,7 +124,6 @@ CloudViewer::CloudViewer(QWidget *parent, CloudViewerInteractorStyle * style) :
 		_aSetLighting(0),
 		_aSetFlatShading(0),
 		_aSetEdgeVisibility(0),
-		_aSetScalarVisibility(0),
 		_aBackfaceCulling(0),
 		_menu(0),
 		_trajectory(new pcl::PointCloud<pcl::PointXYZ>),
@@ -354,9 +347,6 @@ 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);
@@ -418,7 +408,6 @@ void CloudViewer::createMenu()
 	_menu->addAction(_aSetLighting);
 	_menu->addAction(_aSetFlatShading);
 	_menu->addAction(_aSetEdgeVisibility);
-	_menu->addAction(_aSetScalarVisibility);
 	_menu->addAction(_aBackfaceCulling);
 	_menu->addAction(_aPolygonPicking);
 }
@@ -1419,20 +1408,10 @@ bool CloudViewer::addTextureMesh (
   // Create points from mesh.cloud
   vtkSmartPointer<vtkPoints> poly_points = vtkSmartPointer<vtkPoints>::New ();
   vtkSmartPointer<vtkUnsignedCharArray> colors = vtkSmartPointer<vtkUnsignedCharArray>::New ();
-  colors->SetNumberOfComponents(3);
+  bool has_color = false;
-  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::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB> ());
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ> ());
     pcl::fromPCLPointCloud2 (mesh.cloud, *cloud);
     // no points --> exit
     if (cloud->points.size () == 0)
@@ -1444,17 +1423,8 @@ bool CloudViewer::addTextureMesh (
     poly_points->SetNumberOfPoints (cloud->points.size ());
     for (std::size_t i = 0; i < cloud->points.size (); ++i)
     {
-      const pcl::PointXYZRGB &p = cloud->points[i];
+      const pcl::PointXYZ &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
@@ -1473,9 +1443,8 @@ bool CloudViewer::addTextureMesh (
   vtkSmartPointer<vtkPolyData> polydata = vtkSmartPointer<vtkPolyData>::New();
   polydata->SetPolys (polys);
   polydata->SetPoints (poly_points);
-  if (hasColors) {
+  if (has_color)
     polydata->GetPointData()->SetScalars(colors);
-  }
 
   vtkSmartPointer<vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New ();
 #if VTK_MAJOR_VERSION < 6
@@ -1555,7 +1524,6 @@ bool CloudViewer::addTextureMesh (
   actor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked());
   actor->GetProperty()->SetBackfaceCulling(_aBackfaceCulling->isChecked());
   actor->GetProperty()->SetFrontfaceCulling(_frontfaceCulling);
-  actor->GetMapper()->SetScalarVisibility(_aSetScalarVisibility->isChecked());
   return true;
 }
 
@@ -2924,21 +2892,6 @@ 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 ();
@@ -4030,10 +3983,6 @@ 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,9 +333,4 @@ void CloudViewerInteractorStyle::OnLeftButtonDown()
 	PCLVisualizerInteractorStyle::OnLeftButtonDown();
 }
 
-void CloudViewerInteractorStyle::OnRightButtonDown()
-{
-	// ignore
-}
-
 } /* namespace rtabmap */

guilib/src/DatabaseViewer.cpp

@@ -2423,8 +2423,7 @@ void DatabaseViewer::editDepthImage()
 				UASSERT(data.depthRaw().type() == depth.type());
 				UASSERT(data.depthRaw().cols == depth.cols);
 				UASSERT(data.depthRaw().rows == depth.rows);
-				std::string depthFormat = compressedDepthFormat(data.depthOrRightCompressed());
+				dbDriver_->updateDepthImage(id, depth);
-				dbDriver_->updateDepthImage(id, depth, depthFormat);
 				this->update3dView();
 			}
 		}
@@ -2502,9 +2501,115 @@ void DatabaseViewer::exportPoses(int format)
 		return;
 	}
 
-	if(format == 5 && (gpsValues_.empty() || gpsPoses_.empty()))
+	if(format == 5)
 	{
-		QMessageBox::warning(this, tr("Cannot export poses in KML format"), tr("No GPS in database?!"));
+		if(gpsValues_.empty() || gpsPoses_.empty())
+		{
+			QMessageBox::warning(this, tr("Cannot export poses"), tr("No GPS in database?!"));
+		}
+		else
+		{
+			std::map<int, rtabmap::Transform> graph;
+			if(groundTruth)
+			{
+				graph = groundTruthPoses_;
+			}
+			else if(odometry)
+			{
+				graph = odomPoses_;
+			}
+			else
+			{
+				graph = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
+			}
+
+
+			//align with ground truth for more meaningful results
+			pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
+			cloud1.resize(graph.size());
+			cloud2.resize(graph.size());
+			int oi = 0;
+			int idFirst = 0;
+			for(std::map<int, Transform>::const_iterator iter=gpsPoses_.begin(); iter!=gpsPoses_.end(); ++iter)
+			{
+				std::map<int, Transform>::iterator iter2 = graph.find(iter->first);
+				if(iter2!=graph.end())
+				{
+					if(oi==0)
+					{
+						idFirst = iter->first;
+					}
+					cloud1[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
+					cloud2[oi++] = pcl::PointXYZ(iter2->second.x(), iter2->second.y(), iter2->second.z());
+				}
+			}
+
+			Transform t = Transform::getIdentity();
+			if(oi>5)
+			{
+				cloud1.resize(oi);
+				cloud2.resize(oi);
+
+				t = util3d::transformFromXYZCorrespondencesSVD(cloud2, cloud1);
+			}
+			else if(idFirst)
+			{
+				t = gpsPoses_.at(idFirst) * graph.at(idFirst).inverse();
+			}
+
+			std::map<int, GPS> values;
+			GeodeticCoords origin = gpsValues_.begin()->second.toGeodeticCoords();
+			for(std::map<int, Transform>::iterator iter=graph.begin(); iter!=graph.end(); ++iter)
+			{
+				iter->second = t * iter->second;
+
+				GeodeticCoords coord;
+				coord.fromENU_WGS84(cv::Point3d(iter->second.x(), iter->second.y(), iter->second.z()), origin);
+				double bearing = -(iter->second.theta()*180.0/M_PI-90.0);
+				if(bearing < 0)
+				{
+					bearing += 360;
+				}
+
+				Transform p, g;
+				int w;
+				std::string l;
+				double stamp=0.0;
+				int mapId;
+				std::vector<float> v;
+				GPS gps;
+				EnvSensors sensors;
+				dbDriver_->getNodeInfo(iter->first, p, mapId, w, l, stamp, g, v, gps, sensors);
+				values.insert(std::make_pair(iter->first, GPS(stamp, coord.longitude(), coord.latitude(), coord.altitude(), 0, 0)));
+			}
+
+			QString output = pathDatabase_ + QDir::separator() + "poses.kml";
+			QString path = QFileDialog::getSaveFileName(
+					this,
+					tr("Save File"),
+					output,
+					tr("Google Earth file (*.kml)"));
+
+			if(!path.isEmpty())
+			{
+				bool saved = graph::exportGPS(path.toStdString(), values, ui_->graphViewer->getNodeColor().rgba());
+
+				if(saved)
+				{
+					QMessageBox::information(this,
+							tr("Export poses..."),
+							tr("GPS coordinates saved to \"%1\".")
+							.arg(path));
+				}
+				else
+				{
+					QMessageBox::information(this,
+							tr("Export poses..."),
+							tr("Failed to save GPS coordinates to \"%1\"!")
+							.arg(path));
+				}
+			}
+		}
 		return;
 	}
 
@@ -2513,132 +2618,70 @@ void DatabaseViewer::exportPoses(int format)
 	{
 		optimizedPoses = groundTruthPoses_;
 	}
-	else if(odometry)
-	{
-		optimizedPoses = odomPoses_;
-	}
 	else
 	{
-		optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
+		if(odometry)
-	}
-
-	bool alignToGPS = 
-			(ui_->checkBox_alignPosesWithGPS->isEnabled() && 
-		     ui_->checkBox_alignPosesWithGPS->isChecked()) ||
-		    format == 5;
-
-	if(alignToGPS ||
-	   (ui_->checkBox_alignPosesWithGroundTruth->isEnabled() && ui_->checkBox_alignPosesWithGroundTruth->isChecked()))
-	{
-		std::map<int, Transform> refPoses = groundTruthPoses_;
-		if(alignToGPS)
 		{
-			refPoses = gpsPoses_;
+			optimizedPoses = odomPoses_;
+		}
+		else
+		{
+			optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
 		}
 
-		// Log ground truth statistics (in TUM's RGBD-SLAM format)
+		if((ui_->checkBox_alignPosesWithGPS->isEnabled() && ui_->checkBox_alignPosesWithGPS->isChecked()) ||
-		if(refPoses.size())
+		   (ui_->checkBox_alignPosesWithGroundTruth->isEnabled() && ui_->checkBox_alignPosesWithGroundTruth->isChecked()))
 		{
-			float translational_rmse = 0.0f;
+			std::map<int, Transform> refPoses = groundTruthPoses_;
-			float translational_mean = 0.0f;
+			if(ui_->checkBox_alignPosesWithGPS->isEnabled() && 
-			float translational_median = 0.0f;
+			   ui_->checkBox_alignPosesWithGPS->isChecked())
-			float translational_std = 0.0f;
-			float translational_min = 0.0f;
-			float translational_max = 0.0f;
-			float rotational_rmse = 0.0f;
-			float rotational_mean = 0.0f;
-			float rotational_median = 0.0f;
-			float rotational_std = 0.0f;
-			float rotational_min = 0.0f;
-			float rotational_max = 0.0f;
-
-			Transform gtToMap = graph::calcRMSE(
-					refPoses,
-					optimizedPoses,
-					translational_rmse,
-					translational_mean,
-					translational_median,
-					translational_std,
-					translational_min,
-					translational_max,
-					rotational_rmse,
-					rotational_mean,
-					rotational_median,
-					rotational_std,
-					rotational_min,
-					rotational_max,
-					alignToGPS);
-
-			if(!gtToMap.isIdentity())
 			{
-				for(std::map<int, Transform>::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
+				refPoses = gpsPoses_;
+			}
+
+			// Log ground truth statistics (in TUM's RGBD-SLAM format)
+			if(refPoses.size())
+			{
+				float translational_rmse = 0.0f;
+				float translational_mean = 0.0f;
+				float translational_median = 0.0f;
+				float translational_std = 0.0f;
+				float translational_min = 0.0f;
+				float translational_max = 0.0f;
+				float rotational_rmse = 0.0f;
+				float rotational_mean = 0.0f;
+				float rotational_median = 0.0f;
+				float rotational_std = 0.0f;
+				float rotational_min = 0.0f;
+				float rotational_max = 0.0f;
+
+				Transform gtToMap = graph::calcRMSE(
+						refPoses,
+						optimizedPoses,
+						translational_rmse,
+						translational_mean,
+						translational_median,
+						translational_std,
+						translational_min,
+						translational_max,
+						rotational_rmse,
+						rotational_mean,
+						rotational_median,
+						rotational_std,
+						rotational_min,
+						rotational_max);
+
+				if(!gtToMap.isIdentity())
 				{
-					iter->second = gtToMap * iter->second;
-				}
-				if(alignToGPS && format != 5 && optimizedPoses.find(gpsValues_.begin()->first)!=optimizedPoses.end())
-				{
-					// This will make the exported first pose the GPS origin. Don't do it for KML format as is it done implicitly below.
-					int originId = gpsValues_.begin()->first;
-					Transform offset = optimizedPoses.at(originId).translation().inverse();
 					for(std::map<int, Transform>::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
 					{
-						iter->second = offset * iter->second;
+						iter->second = gtToMap * iter->second;
 					}
 				}
 			}
 		}
 	}
 
-	if(format == 5)
-	{
-		std::map<int, GPS> values;
-		GeodeticCoords origin = gpsValues_.begin()->second.toGeodeticCoords();
-		for(std::map<int, Transform>::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
-		{
-			GeodeticCoords coord;
-			coord.fromENU_WGS84(cv::Point3d(iter->second.x(), iter->second.y(), iter->second.z()), origin);
-
-			Transform p, g;
-			int w;
-			std::string l;
-			double stamp=0.0;
-			int mapId;
-			std::vector<float> v;
-			GPS gps;
-			EnvSensors sensors;
-			dbDriver_->getNodeInfo(iter->first, p, mapId, w, l, stamp, g, v, gps, sensors);
-			values.insert(std::make_pair(iter->first, GPS(stamp, coord.longitude(), coord.latitude(), coord.altitude(), 0, 0)));
-		}
-
-		QString output = pathDatabase_ + QDir::separator() + "poses.kml";
-		QString path = QFileDialog::getSaveFileName(
-				this,
-				tr("Save File"),
-				output,
-				tr("Google Earth file (*.kml)"));
-
-		if(!path.isEmpty())
-		{
-			bool saved = graph::exportGPS(path.toStdString(), values, ui_->graphViewer->getNodeColor().rgba());
-
-			if(saved)
-			{
-				QMessageBox::information(this,
-						tr("Export poses..."),
-						tr("GPS coordinates saved to \"%1\".")
-						.arg(path));
-			}
-			else
-			{
-				QMessageBox::information(this,
-						tr("Export poses..."),
-						tr("Failed to save GPS coordinates to \"%1\"!")
-						.arg(path));
-			}
-		}
-		return;
-	}
-
 	if(optimizedPoses.size())
 	{
 		std::map<int, Transform> localTransforms;
@@ -4130,72 +4173,6 @@ void DatabaseViewer::generate3DMap()
 	else
 	{
 		optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
-
-		bool alignToGPS = 
-			ui_->checkBox_alignPosesWithGPS->isEnabled() && 
-		    ui_->checkBox_alignPosesWithGPS->isChecked();
-
-		if(alignToGPS ||
-		   (ui_->checkBox_alignPosesWithGroundTruth->isEnabled() && ui_->checkBox_alignPosesWithGroundTruth->isChecked()))
-		{
-			std::map<int, Transform> refPoses = groundTruthPoses_;
-			if(alignToGPS)
-			{
-				refPoses = gpsPoses_;
-			}
-
-			// Log ground truth statistics (in TUM's RGBD-SLAM format)
-			if(refPoses.size())
-			{
-				float translational_rmse = 0.0f;
-				float translational_mean = 0.0f;
-				float translational_median = 0.0f;
-				float translational_std = 0.0f;
-				float translational_min = 0.0f;
-				float translational_max = 0.0f;
-				float rotational_rmse = 0.0f;
-				float rotational_mean = 0.0f;
-				float rotational_median = 0.0f;
-				float rotational_std = 0.0f;
-				float rotational_min = 0.0f;
-				float rotational_max = 0.0f;
-
-				Transform gtToMap = graph::calcRMSE(
-						refPoses,
-						optimizedPoses,
-						translational_rmse,
-						translational_mean,
-						translational_median,
-						translational_std,
-						translational_min,
-						translational_max,
-						rotational_rmse,
-						rotational_mean,
-						rotational_median,
-						rotational_std,
-						rotational_min,
-						rotational_max,
-						alignToGPS);
-
-				if(!gtToMap.isIdentity())
-				{
-					for(std::map<int, Transform>::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
-					{
-						iter->second = gtToMap * iter->second;
-					}
-					if(alignToGPS && optimizedPoses.find(gpsValues_.begin()->first)!=optimizedPoses.end())
-					{
-						// This will make the exported first pose the GPS origin.
-						int originId = gpsValues_.begin()->first;
-						Transform offset = optimizedPoses.at(originId).translation().inverse();
-						for(std::map<int, Transform>::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
-						{
-							iter->second = offset * iter->second;
-						}
-					}
-				}
-			}
-		}
 	}
 	if(ui_->groupBox_posefiltering->isChecked())
 	{
@@ -4850,7 +4827,7 @@ void DatabaseViewer::update(int value,
 					{
 						keypoints.insert(std::make_pair(iter->first, signatures.front()->getWordsKpts()[iter->second]));
 					}
-					view->setFeatures(keypoints, data.depthOrRightRaw().type() == CV_8UC1||data.depthOrRightRaw().type() == CV_8UC3?cv::Mat():data.depthOrRightRaw(), Qt::yellow);
+					view->setFeatures(keypoints, data.depthOrRightRaw().type() == CV_8UC1?cv::Mat():data.depthOrRightRaw(), Qt::yellow);
 				}
 
 				Transform odomPose, g;
@@ -4893,7 +4870,9 @@ void DatabaseViewer::update(int value,
 				dbDriver_->loadLinks(id, gravityLink, Link::kGravity);
 				if(!gravityLink.empty())
 				{
-					Eigen::Vector3f v = gravityLink.begin()->second.transform().inverse().toEigen3f() * -Eigen::Vector3f::UnitZ();
+					float roll,pitch,yaw;
+					gravityLink.begin()->second.transform().getEulerAngles(roll, pitch, yaw);
+					Eigen::Vector3d v = Transform(0,0,0,roll,pitch,0).toEigen3d() * -Eigen::Vector3d::UnitZ();
 					labelGravity->setText(QString("x=%1 y=%2 z=%3").arg(v[0]).arg(v[1]).arg(v[2]));
 					labelGravity->setToolTip(QString("roll=%1 pitch=%2 yaw=%3").arg(roll).arg(pitch).arg(yaw));
 				}
@@ -5088,7 +5067,7 @@ void DatabaseViewer::update(int value,
 				}
 
 				//stereo
-				if(!data.depthOrRightRaw().empty() && (data.depthOrRightRaw().type() == CV_8UC1 || data.depthOrRightRaw().type() == CV_8UC3))
+				if(!data.depthOrRightRaw().empty() && data.depthOrRightRaw().type() == CV_8UC1)
 				{
 					this->updateStereo(&data);
 				}
@@ -5119,8 +5098,13 @@ void DatabaseViewer::update(int value,
 					if(!gravityLink.empty() && ui_->checkBox_gravity_3dview->isChecked())
 					{
 						Transform gravityT = gravityLink.begin()->second.transform();
-						Eigen::Vector3f gravity =  gravityT.inverse().toEigen3f()*-Eigen::Vector3f::UnitZ();
+						Eigen::Vector3f gravity(0,0,-1);
-						cloudViewer_->addOrUpdateLine("gravity", pose, pose*Transform(gravity[0], gravity[1], gravity[2], 0, 0, 0), Qt::yellow, true, false);
+						if(pose.isIdentity())
+						{
+							gravityT = gravityT.inverse();
+						}
+						gravity = (gravityT.rotation()*(pose).rotation().inverse()).toEigen3f()*gravity;
+						cloudViewer_->addOrUpdateLine("gravity", pose, (pose).translation()*Transform(gravity[0], gravity[1], gravity[2], 0, 0, 0)*pose.rotation().inverse(), Qt::yellow, true, false);
 					}
 
 					//add scan
@@ -5782,7 +5766,7 @@ void DatabaseViewer::updateStereo(const SensorData * data)
 		ui_->dockWidget_stereoView->isVisible() &&
 		!data->imageRaw().empty() &&
 		!data->depthOrRightRaw().empty() &&
-		(data->depthOrRightRaw().type() == CV_8UC1 || data->depthOrRightRaw().type() == CV_8UC3) &&
+		data->depthOrRightRaw().type() == CV_8UC1 &&
 		data->stereoCameraModels().size()==1 && // Not implemented for multiple stereo cameras
 		data->stereoCameraModels()[0].isValidForProjection())
 	{
@@ -5795,15 +5779,6 @@ void DatabaseViewer::updateStereo(const SensorData * data)
 		{
 			leftMono = data->imageRaw();
 		}
-		cv::Mat rightMono;
-		if(data->rightRaw().channels() == 3)
-		{
-			cv::cvtColor(data->rightRaw(), rightMono, CV_BGR2GRAY);
-		}
-		else
-		{
-			rightMono = data->rightRaw();
-		}
 
 		UTimer timer;
 		ParametersMap parameters = ui_->parameters_toolbox->getParameters();
@@ -5835,7 +5810,7 @@ void DatabaseViewer::updateStereo(const SensorData * data)
 
 		rightCorners = stereo->computeCorrespondences(
 				leftMono,
-				rightMono,
+				data->rightRaw(),
 				leftCorners,
 				status);
 		delete stereo;
@@ -6444,7 +6419,7 @@ void DatabaseViewer::updateConstraintView(
 		}
 		dataFrom.uncompressData();
 		UASSERT(dataFrom.imageRaw().empty() || dataFrom.imageRaw().type()==CV_8UC3 || dataFrom.imageRaw().type() == CV_8UC1);
-		UASSERT(dataFrom.depthOrRightRaw().empty() || dataFrom.depthOrRightRaw().type()==CV_8UC1 || dataFrom.depthOrRightRaw().type()==CV_8UC3 || dataFrom.depthOrRightRaw().type() == CV_16UC1 || dataFrom.depthOrRightRaw().type() == CV_32FC1);
+		UASSERT(dataFrom.depthOrRightRaw().empty() || dataFrom.depthOrRightRaw().type()==CV_8UC1 || dataFrom.depthOrRightRaw().type() == CV_16UC1 || dataFrom.depthOrRightRaw().type() == CV_32FC1);
 
 		if(signatureTo.id()>0)
 		{
@@ -6456,7 +6431,7 @@ void DatabaseViewer::updateConstraintView(
 		}
 		dataTo.uncompressData();
 		UASSERT(dataTo.imageRaw().empty() || dataTo.imageRaw().type()==CV_8UC3 || dataTo.imageRaw().type() == CV_8UC1);
-		UASSERT(dataTo.depthOrRightRaw().empty() || dataTo.depthOrRightRaw().type()==CV_8UC1 || dataTo.depthOrRightRaw().type()==CV_8UC3 || dataTo.depthOrRightRaw().type() == CV_16UC1 || dataTo.depthOrRightRaw().type() == CV_32FC1);
+		UASSERT(dataTo.depthOrRightRaw().empty() || dataTo.depthOrRightRaw().type()==CV_8UC1 || dataTo.depthOrRightRaw().type() == CV_16UC1 || dataTo.depthOrRightRaw().type() == CV_32FC1);
 
 		// get odom pose
 		Transform pose = Transform::getIdentity();
@@ -6985,8 +6960,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 		std::map<int, rtabmap::Transform> graph = uValueAt(graphes_, value);
 
 		std::map<int, Transform> refPoses = groundTruthPoses_;
-		bool alignToGPS = ui_->checkBox_alignPosesWithGPS->isEnabled() && ui_->checkBox_alignPosesWithGPS->isChecked();
+		if(ui_->checkBox_alignPosesWithGPS->isEnabled() && ui_->checkBox_alignPosesWithGPS->isChecked())
-		if(alignToGPS)
 		{
 			refPoses = gpsPoses_;
 		}
@@ -7032,8 +7006,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 					rotational_median,
 					rotational_std,
 					rotational_min,
-					rotational_max,
+					rotational_max);
-					alignToGPS);
 
 			// ground truth live statistics
 			ui_->label_rmse->setNum(translational_rmse);
@@ -7051,7 +7024,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 			UINFO("rotational_min=%f", rotational_min);
 			UINFO("rotational_max=%f", rotational_max);
 
-			if((alignToGPS ||
+			if(((ui_->checkBox_alignPosesWithGPS->isEnabled() && ui_->checkBox_alignPosesWithGPS->isChecked()) ||
 				(ui_->checkBox_alignPosesWithGroundTruth->isEnabled() && ui_->checkBox_alignPosesWithGroundTruth->isChecked())) &&
 			   !gtToMap.isIdentity())
 			{

guilib/src/ExportCloudsDialog.cpp

@@ -240,8 +240,6 @@ 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()));
@@ -469,7 +467,6 @@ 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());
@@ -654,7 +651,6 @@ 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());
@@ -820,7 +816,7 @@ void ExportCloudsDialog::restoreDefaults()
 	_ui->doubleSpinBox_gp3Mu->setValue(2.5);
 	_ui->doubleSpinBox_meshDecimationFactor->setValue(0.0);
 	_ui->spinBox_meshMaxPolygons->setValue(0);
-	_ui->doubleSpinBox_transferColorRadius->setValue(0.05);
+	_ui->doubleSpinBox_transferColorRadius->setValue(0.025);
 	_ui->checkBox_cleanMesh->setChecked(true);
 	_ui->spinBox_mesh_minClusterSize->setValue(0);
 
@@ -836,7 +832,6 @@ 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);
@@ -1357,10 +1352,7 @@ 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;
@@ -1370,112 +1362,44 @@ 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)
 				{
-					for (unsigned int t = 0; t < mesh->tex_coordinates.size(); ++t)
+					if(mesh->tex_polygons[t].size())
 					{
-						if(mesh->tex_polygons[t].size())
+
+						pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud(new pcl::PointCloud<pcl::PointXYZ>);
+						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>);
+						cloud->resize(nPoints);
+
+						unsigned int oi = 0;
+						for (unsigned int i = 0; i < mesh->tex_polygons[t].size(); ++i)
 						{
+							pcl::Vertices & vertices = mesh->tex_polygons[t][i];
 
-							pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr originalCloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+							for(unsigned int j=0; j<vertices.vertices.size(); ++j)
-							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::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];
+								UASSERT(oi < cloud->size());
-								bool hasTexture = true;
+								UASSERT_MSG((int)vertices.vertices[j] < (int)originalCloud->size(), uFormat("%d vs %d", vertices.vertices[j], (int)originalCloud->size()).c_str());
-								for(unsigned int j=0; j<vertices.vertices.size(); ++j)
+								cloud->at(oi) = originalCloud->at(vertices.vertices[j]);
-								{
+								vertices.vertices[j] = oi; // new vertice index
-									if(mesh->tex_coordinates[t][texCoordIndex][0] == -1 || mesh->tex_coordinates[t][texCoordIndex][1] == -1)
+								++oi;
-									{
-										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);
 						}
+						pcl::toPCLPointCloud2(*cloud, mesh->cloud);
 					}
-				}
+					else
-				else
-				{
-					for (unsigned int t = 0; t < mesh->tex_coordinates.size(); ++t)
 					{
-						if(mesh->tex_polygons[t].size())
+						UWARN("No polygons for texture %d of mesh %d?!", t, iter->first);
-						{
-
-							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;
-							for (unsigned int i = 0; i < mesh->tex_polygons[t].size(); ++i)
-							{
-								pcl::Vertices & vertices = mesh->tex_polygons[t][i];
-
-								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]);
-									vertices.vertices[j] = oi; // new vertice index
-									++oi;
-								}
-							}
-							pcl::toPCLPointCloud2(*cloud, mesh->cloud);
-						}
-						else
-						{
-							UWARN("No polygons for texture %d of mesh %d?!", t, iter->first);
-						}
 					}
 				}
 
@@ -2758,7 +2682,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->comboBox_texturingColorPolicy->currentIndex()==0),
+								!(_ui->checkBox_textureMapping->isEnabled() && _ui->checkBox_textureMapping->isChecked()),
 								_ui->checkBox_cleanMesh->isChecked(),
 								_ui->spinBox_mesh_minClusterSize->value(),
 								&texturingState);
@@ -3144,7 +3068,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->comboBox_texturingColorPolicy->currentIndex()==0),
+						!(_ui->checkBox_textureMapping->isEnabled() && _ui->checkBox_textureMapping->isChecked()),
 						_ui->checkBox_cleanMesh->isChecked(),
 						_ui->spinBox_mesh_minClusterSize->value(),
 						&texturingState);
@@ -3216,7 +3140,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->comboBox_texturingColorPolicy->currentIndex()==0),
+						!(_ui->checkBox_textureMapping->isEnabled() && _ui->checkBox_textureMapping->isChecked()),
 						_ui->checkBox_cleanMesh->isChecked(),
 						_ui->spinBox_mesh_minClusterSize->value(),
 						&texturingState);
@@ -4773,8 +4697,7 @@ void ExportCloudsDialog::saveTextureMeshes(
 							_ui->spinBox_textureBrightnessContrastRatioHigh->value(),
 							_ui->checkBox_exposureFusion->isEnabled() && _ui->checkBox_exposureFusion->isChecked(),
 							0,
-							255,
+							0,
-							_ui->comboBox_texturingColorPolicy->currentIndex() == 1,
 							&gains,
 							&blendingGains,
 							&contrastValues);
@@ -5016,10 +4939,7 @@ 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)
@@ -5194,31 +5114,28 @@ void ExportCloudsDialog::saveTextureMeshes(
 	}
 }
 
-bool ExportCloudsDialog::saveOBJFile(const QString &path, pcl::TextureMesh::Ptr &mesh) const {
+    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>>();
+        mesh->tex_coord_indices = std::vector<std::vector<pcl::Vertices>>();
-	auto nr_meshes = static_cast<unsigned>(mesh->tex_polygons.size());
+        auto nr_meshes = static_cast<unsigned>(mesh->tex_polygons.size());
-	unsigned f_idx = 0;
+        unsigned f_idx = 0;
-	for (unsigned m = 0; m < nr_meshes; m++) {
+        for (unsigned m = 0; m < nr_meshes; m++) {
-		std::vector<pcl::Vertices> ci = mesh->tex_polygons[m];
+            std::vector<pcl::Vertices> ci = mesh->tex_polygons[m];
-		for(std::size_t i = 0; i < ci.size(); i++) {
+            for(std::size_t i = 0; i < ci.size(); i++) {
-			for (std::size_t j = 0; j < ci[i].vertices.size(); j++) {
+                for (std::size_t j = 0; j < ci[i].vertices.size(); j++) {
-				ci[i].vertices[j] = ci[i].vertices.size() * (i + f_idx) + j;
+                    ci[i].vertices[j] = ci[i].vertices.size() * (i + f_idx) + j;
-			}
+                }
-		}
+            }
-		mesh->tex_coord_indices.push_back(ci);
+            mesh->tex_coord_indices.push_back(ci);
-		f_idx += static_cast<unsigned>(mesh->tex_polygons[m].size());
+            f_idx += static_cast<unsigned>(mesh->tex_polygons[m].size());
-	}
+        }
 #endif
-	return util3d::saveOBJFile(path.toStdString(), *mesh) == 0;
+        return pcl::io::saveOBJFile(path.toStdString(), *mesh) == 0;
-}
+    }
 
-bool ExportCloudsDialog::saveOBJFile(const QString &path, pcl::PolygonMesh &mesh) const {
+    bool ExportCloudsDialog::saveOBJFile(const QString &path, pcl::PolygonMesh &mesh) const {
-	return util3d::saveOBJFile(path.toStdString(), mesh) == 0;
+        return pcl::io::saveOBJFile(path.toStdString(), mesh) == 0;
-}
+    }
 
 
 }