merged master->jazzy

* New feature: Depth confidence

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

* Updated tools to show/extract depth confidence

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

* Fixed warning

* removed debug log

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

* fixed some warnings, android: added bleeding error option

* CI: try updating ros2 key

* added sudo

* antoher test

* bump ios app version

.github/workflows/cmake-ros.yml

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

.github/workflows/docker.yml

@@ -15,14 +15,14 @@ jobs:
     #    $ 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
+    # if: false
 
     runs-on: ubuntu-latest
     
     strategy:
       fail-fast: false
       matrix:
-        docker_tag: [focal-deps, jammy-deps, jammy-iron-deps, noble-deps]
+        docker_tag: [focal-deps, jammy-deps, noble-deps, noble-kilted-deps]
         include:
         - docker_tag: focal-deps
           docker_tags: |
@@ -30,7 +30,6 @@ jobs:
           docker_platforms: |
             linux/amd64
             linux/arm64
-            linux/arm/v7
           docker_path: 'focal/deps'
         - docker_tag: jammy-deps
           docker_tags: |
@@ -39,12 +38,6 @@ jobs:
             linux/amd64
             linux/arm64
           docker_path: 'jammy/deps'
-        - docker_tag: jammy-iron-deps
-          docker_tags: |
-            introlab3it/rtabmap:jammy-iron-deps
-          docker_platforms: |
-            linux/amd64
-          docker_path: 'jammy-iron/deps'
         - docker_tag: noble-deps
           docker_tags: |
             introlab3it/rtabmap:noble-deps
@@ -52,6 +45,13 @@ jobs:
             linux/amd64
             linux/arm64
           docker_path: 'noble/deps'
+        - docker_tag: noble-kilted-deps
+          docker_tags: |
+            introlab3it/rtabmap:noble-kilted-deps
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'noble-kilted/deps'
     
     steps:
       -
@@ -84,13 +84,13 @@ jobs:
           cache-to: type=inline
   
   docker:
-    #needs: docker_deps
+    needs: docker_deps
     runs-on: ubuntu-latest
     
     strategy:
       fail-fast: false
       matrix:
-        docker_tag: [bionic, focal, jammy, jammy-iron, noble, android23, android24, android26, android30]
+        docker_tag: [bionic, focal, jammy, noble, noble-kilted, android23, android24, android26, android30]
         include:
         - docker_tag: bionic
           docker_tags: |
@@ -112,7 +112,6 @@ jobs:
           docker_platforms: |
             linux/amd64
             linux/arm64
-            linux/arm/v7
           docker_path: 'focal'
         - docker_tag: jammy
           docker_tags: |
@@ -124,14 +123,6 @@ jobs:
             linux/amd64
             linux/arm64
           docker_path: 'jammy'
-        - docker_tag: jammy-iron
-          docker_tags: |
-            introlab3it/rtabmap:jammy-iron
-          docker_args: |
-            NOT_USED=0
-          docker_platforms: |
-            linux/amd64
-          docker_path: 'jammy-iron'
         - docker_tag: noble
           docker_tags: |
             introlab3it/rtabmap:noble
@@ -142,6 +133,15 @@ jobs:
             linux/amd64
             linux/arm64
           docker_path: 'noble'
+        - docker_tag: noble-kilted
+          docker_tags: |
+            introlab3it/rtabmap:noble-kilted
+          docker_args: |
+            NOT_USED=0
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'noble-kilted'
         - docker_tag: android23
           docker_tags: |
             introlab3it/rtabmap:android23

CMakeLists.txt

@@ -19,8 +19,8 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 # VERSION
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
-SET(RTABMAP_MINOR_VERSION 21)
-SET(RTABMAP_PATCH_VERSION 13)
+SET(RTABMAP_MINOR_VERSION 22)
+SET(RTABMAP_PATCH_VERSION 0)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   

app/android/jni/CameraARCore.cpp

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

app/android/jni/CameraARCore.h

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

app/android/jni/CameraAREngine.cpp

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

app/android/jni/CameraAREngine.h

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

app/android/jni/CameraMobile.cpp

@@ -41,9 +41,6 @@ namespace rtabmap {
 //////////////////////////////
 // CameraMobile
 //////////////////////////////
-const float CameraMobile::bilateralFilteringSigmaS = 2.0f;
-const float CameraMobile::bilateralFilteringSigmaR = 0.075f;
-
 const rtabmap::Transform CameraMobile::opticalRotation = Transform(
 		0.0f,  0.0f,  1.0f, 0.0f,
 	   -1.0f,  0.0f,  0.0f, 0.0f,
@@ -53,13 +50,12 @@ const rtabmap::Transform CameraMobile::opticalRotationInv = Transform(
 	    0.0f,  0.0f, -1.0f, 0.0f,
 		1.0f,  0.0f,  0.0f, 0.0f);
 
-CameraMobile::CameraMobile(bool smoothing, float upstreamRelocalizationAccThr) :
+CameraMobile::CameraMobile(float upstreamRelocalizationAccThr) :
 		Camera(10),
 		deviceTColorCamera_(Transform::getIdentity()),
 		textureId_(0),
 		uvs_initialized_(false),
 		stampEpochOffset_(0.0),
-		smoothing_(smoothing),
 		colorCameraToDisplayRotation_(ROTATION_0),
 		originUpdate_(true),
 		upstreamRelocalizationAccThr_(upstreamRelocalizationAccThr),
@@ -394,124 +390,135 @@ SensorData CameraMobile::updateDataOnRender(Transform & pose)
 void CameraMobile::postUpdate()
 {
 	if(data_.isValid())
-	{
-		// adjust origin
-		if(!originOffset_.isNull())
-		{
-			dataPose_ = originOffset_ * dataPose_;
-			viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * originOffset_ *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_));
-			occlusionModel_.setLocalTransform(originOffset_ * occlusionModel_.localTransform());
-		}
-
-		if(lastKnownGPS_.stamp() > 0.0 && data_.stamp()-lastKnownGPS_.stamp()<1.0)
-		{
-			data_.setGPS(lastKnownGPS_);
-		}
-		else if(lastKnownGPS_.stamp()>0.0)
-		{
-			LOGD("GPS too old (current time=%f, gps time = %f)", data_.stamp(), lastKnownGPS_.stamp());
-		}
-
-		if(lastEnvSensors_.size())
-		{
-			data_.setEnvSensors(lastEnvSensors_);
-			lastEnvSensors_.clear();
-		}
-
-		if(smoothing_ && !data_.depthRaw().empty())
-		{
-			//UTimer t;
-			data_.setDepthOrRightRaw(rtabmap::util2d::fastBilateralFiltering(data_.depthRaw(), bilateralFilteringSigmaS, bilateralFilteringSigmaR));
-			//LOGD("Bilateral filtering, time=%fs", t.ticks());
-		}
-
-		// Rotate image depending on the camera orientation
-		if(colorCameraToDisplayRotation_ == ROTATION_90)
-		{
-			UDEBUG("ROTATION_90");
-			cv::Mat rgb, depth;
-			cv::Mat rgbt(data_.imageRaw().cols, data_.imageRaw().rows, data_.imageRaw().type());
-			cv::flip(data_.imageRaw(),rgb,1);
-			cv::transpose(rgb,rgbt);
-			rgb = rgbt;
-			cv::Mat deptht(data_.depthRaw().cols, data_.depthRaw().rows, data_.depthRaw().type());
-			cv::flip(data_.depthRaw(),depth,1);
-			cv::transpose(depth,deptht);
-			depth = deptht;
-			CameraModel model = data_.cameraModels()[0];
-			cv::Size sizet(model.imageHeight(), model.imageWidth());
-			model = CameraModel(
-					model.fy(),
-					model.fx(),
-					model.cy(),
-					model.cx()>0?model.imageWidth()-model.cx():0,
-					model.localTransform()*rtabmap::Transform(0,-1,0,0, 1,0,0,0, 0,0,1,0));
-			model.setImageSize(sizet);
-			data_.setRGBDImage(rgb, depth, model);
-
-			std::vector<cv::KeyPoint> keypoints = data_.keypoints();
-			for(size_t i=0; i<keypoints.size(); ++i)
-			{
-				keypoints[i].pt.x = data_.keypoints()[i].pt.y;
-				keypoints[i].pt.y = rgb.rows - data_.keypoints()[i].pt.x;
-			}
-			data_.setFeatures(keypoints, data_.keypoints3D(), cv::Mat());
-		}
-		else if(colorCameraToDisplayRotation_ == ROTATION_180)
-		{
-			UDEBUG("ROTATION_180");
-			cv::Mat rgb, depth;
-			cv::flip(data_.imageRaw(),rgb,1);
-			cv::flip(rgb,rgb,0);
-			cv::flip(data_.depthOrRightRaw(),depth,1);
-			cv::flip(depth,depth,0);
-			CameraModel model = data_.cameraModels()[0];
-			cv::Size sizet(model.imageWidth(), model.imageHeight());
-			model = CameraModel(
-					model.fx(),
-					model.fy(),
-					model.cx()>0?model.imageWidth()-model.cx():0,
-					model.cy()>0?model.imageHeight()-model.cy():0,
-					model.localTransform()*rtabmap::Transform(0,0,0,0,0,1,0));
-			model.setImageSize(sizet);
-			data_.setRGBDImage(rgb, depth, model);
-
-			std::vector<cv::KeyPoint> keypoints = data_.keypoints();
-			for(size_t i=0; i<keypoints.size(); ++i)
-			{
-				keypoints[i].pt.x = rgb.cols - data_.keypoints()[i].pt.x;
-				keypoints[i].pt.y = rgb.rows - data_.keypoints()[i].pt.y;
-			}
-			data_.setFeatures(keypoints, data_.keypoints3D(), cv::Mat());
-		}
-		else if(colorCameraToDisplayRotation_ == ROTATION_270)
-		{
-			UDEBUG("ROTATION_270");
-			cv::Mat rgb(data_.imageRaw().cols, data_.imageRaw().rows, data_.imageRaw().type());
-			cv::transpose(data_.imageRaw(),rgb);
-			cv::flip(rgb,rgb,1);
-			cv::Mat depth(data_.depthOrRightRaw().cols, data_.depthOrRightRaw().rows, data_.depthOrRightRaw().type());
-			cv::transpose(data_.depthOrRightRaw(),depth);
-			cv::flip(depth,depth,1);
-			CameraModel model = data_.cameraModels()[0];
-			cv::Size sizet(model.imageHeight(), model.imageWidth());
-			model = CameraModel(
-					model.fy(),
-					model.fx(),
-					model.cy()>0?model.imageHeight()-model.cy():0,
-					model.cx(),
-					model.localTransform()*rtabmap::Transform(0,1,0,0, -1,0,0,0, 0,0,1,0));
-			model.setImageSize(sizet);
-			data_.setRGBDImage(rgb, depth, model);
-
-			std::vector<cv::KeyPoint> keypoints = data_.keypoints();
-			for(size_t i=0; i<keypoints.size(); ++i)
-			{
-				keypoints[i].pt.x = rgb.cols - data_.keypoints()[i].pt.y;
-				keypoints[i].pt.y = data_.keypoints()[i].pt.x;
-			}
-			data_.setFeatures(keypoints, data_.keypoints3D(), cv::Mat());
-		}
+    {
+        // adjust origin
+        if(!originOffset_.isNull())
+        {
+            dataPose_ = originOffset_ * dataPose_;
+            viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * originOffset_ *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_));
+            occlusionModel_.setLocalTransform(originOffset_ * occlusionModel_.localTransform());
+        }
+        
+        if(lastKnownGPS_.stamp() > 0.0 && data_.stamp()-lastKnownGPS_.stamp()<1.0)
+        {
+            data_.setGPS(lastKnownGPS_);
+        }
+        else if(lastKnownGPS_.stamp()>0.0)
+        {
+            LOGD("GPS too old (current time=%f, gps time = %f)", data_.stamp(), lastKnownGPS_.stamp());
+        }
+        
+        if(lastEnvSensors_.size())
+        {
+            data_.setEnvSensors(lastEnvSensors_);
+            lastEnvSensors_.clear();
+        }
+        
+        
+        // Rotate image depending on the camera orientation
+        if(colorCameraToDisplayRotation_ == ROTATION_90)
+        {
+            UDEBUG("ROTATION_90");
+            cv::Mat rgb, depth, confidence;
+            cv::Mat rgbt;
+            cv::flip(data_.imageRaw(),rgb,1);
+            cv::transpose(rgb,rgbt);
+            rgb = rgbt;
+            cv::Mat deptht;
+            cv::flip(data_.depthRaw(),depth,1);
+            cv::transpose(depth,deptht);
+            depth = deptht;
+            if(!data_.depthConfidenceRaw().empty()) {
+                cv::Mat conft;
+                cv::flip(data_.depthConfidenceRaw(),confidence,1);
+                cv::transpose(confidence,conft);
+                confidence = conft;
+            }
+            CameraModel model = data_.cameraModels()[0];
+            cv::Size sizet(model.imageHeight(), model.imageWidth());
+            model = CameraModel(
+                                model.fy(),
+                                model.fx(),
+                                model.cy(),
+                                model.cx()>0?model.imageWidth()-model.cx():0,
+                                model.localTransform()*rtabmap::Transform(0,-1,0,0, 1,0,0,0, 0,0,1,0));
+            model.setImageSize(sizet);
+            data_.setRGBDImage(rgb, depth, confidence, model);
+            
+            std::vector<cv::KeyPoint> keypoints = data_.keypoints();
+            for(size_t i=0; i<keypoints.size(); ++i)
+            {
+                keypoints[i].pt.x = data_.keypoints()[i].pt.y;
+                keypoints[i].pt.y = rgb.rows - data_.keypoints()[i].pt.x;
+            }
+            data_.setFeatures(keypoints, data_.keypoints3D(), cv::Mat());
+        }
+        else if(colorCameraToDisplayRotation_ == ROTATION_180)
+        {
+            UDEBUG("ROTATION_180");
+            cv::Mat rgb, depth, confidence;
+            cv::flip(data_.imageRaw(),rgb,1);
+            cv::flip(rgb,rgb,0);
+            cv::flip(data_.depthOrRightRaw(),depth,1);
+            cv::flip(depth,depth,0);
+            if(!data_.depthConfidenceRaw().empty()) {
+                cv::flip(data_.depthConfidenceRaw(),confidence,1);
+                cv::flip(confidence,confidence,0);
+            }
+            CameraModel model = data_.cameraModels()[0];
+            cv::Size sizet(model.imageWidth(), model.imageHeight());
+            model = CameraModel(
+                                model.fx(),
+                                model.fy(),
+                                model.cx()>0?model.imageWidth()-model.cx():0,
+                                model.cy()>0?model.imageHeight()-model.cy():0,
+                                model.localTransform()*rtabmap::Transform(0,0,0,0,0,1,0));
+            model.setImageSize(sizet);
+            data_.setRGBDImage(rgb, depth, confidence, model);
+            
+            std::vector<cv::KeyPoint> keypoints = data_.keypoints();
+            for(size_t i=0; i<keypoints.size(); ++i)
+            {
+                keypoints[i].pt.x = rgb.cols - data_.keypoints()[i].pt.x;
+                keypoints[i].pt.y = rgb.rows - data_.keypoints()[i].pt.y;
+            }
+            data_.setFeatures(keypoints, data_.keypoints3D(), cv::Mat());
+        }
+        else if(colorCameraToDisplayRotation_ == ROTATION_270)
+        {
+            UDEBUG("ROTATION_270");
+            cv::Mat rgb, depth, confidence;
+            cv::transpose(data_.imageRaw(),rgb);
+            cv::flip(rgb,rgb,1);
+            cv::transpose(data_.depthOrRightRaw(),depth);
+            cv::flip(depth,depth,1);
+            if(!data_.depthConfidenceRaw().empty()) {
+                cv::transpose(data_.depthConfidenceRaw(),confidence);
+                cv::flip(confidence,confidence,1);
+            }
+            CameraModel model = data_.cameraModels()[0];
+            cv::Size sizet(model.imageHeight(), model.imageWidth());
+            model = CameraModel(
+                                model.fy(),
+                                model.fx(),
+                                model.cy()>0?model.imageHeight()-model.cy():0,
+                                model.cx(),
+                                model.localTransform()*rtabmap::Transform(0,1,0,0, -1,0,0,0, 0,0,1,0));
+            model.setImageSize(sizet);
+            data_.setRGBDImage(rgb, depth, confidence, model);
+            
+            std::vector<cv::KeyPoint> keypoints = data_.keypoints();
+            for(size_t i=0; i<keypoints.size(); ++i)
+            {
+                keypoints[i].pt.x = rgb.cols - data_.keypoints()[i].pt.y;
+                keypoints[i].pt.y = data_.keypoints()[i].pt.x;
+            }
+            data_.setFeatures(keypoints, data_.keypoints3D(), cv::Mat());
+        }
+        else
+        {
+            UDEBUG("ROTATION_0");
+        }
 	}
 }
 

app/android/jni/CameraMobile.h

@@ -70,9 +70,6 @@ private:
 
 class CameraMobile : public Camera, public UEventsSender {
 public:
-	static const float bilateralFilteringSigmaS;
-	static const float bilateralFilteringSigmaR;
-
 	static const rtabmap::Transform opticalRotation;
 	static const rtabmap::Transform opticalRotationInv;
     
@@ -87,7 +84,7 @@ public:
             int kptsSize = 3);
 
 public:
-	CameraMobile(bool smoothing = false, float upstreamRelocalizationAccThr = 0.0f);
+	CameraMobile(float upstreamRelocalizationAccThr = 0.0f);
 	virtual ~CameraMobile();
 
 	// abstract functions
@@ -110,7 +107,6 @@ public:
 
 	const CameraModel & getCameraModel() const {return model_;}
 	const Transform & getDeviceTColorCamera() const {return deviceTColorCamera_;}
-	void setSmoothing(bool enabled) {smoothing_ = enabled;}
 	virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height) {colorCameraToDisplayRotation_ = colorCameraToDisplayRotation;}
 	void setGPS(const GPS & gps);
 	void addEnvSensor(int type, float value);
@@ -144,7 +140,6 @@ protected:
 private:
 	bool firstFrame_;
 	double stampEpochOffset_;
-	bool smoothing_;
 	ScreenRotation colorCameraToDisplayRotation_;
 	GPS lastKnownGPS_;
 	EnvSensors lastEnvSensors_;

app/android/jni/CameraTango.cpp

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

app/android/jni/CameraTango.h

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

app/android/jni/RTABMapApp.cpp

@@ -84,6 +84,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 const int g_optMeshId = -100;
 
+const float g_bilateralFilteringSigmaS = 2.0f;
+const float g_bilateralFilteringSigmaR = 0.075f;
+
 #ifdef __ANDROID__
 static JavaVM *jvm;
 static jobject RTABMapActivity = 0;
@@ -230,6 +233,7 @@ RTABMapApp::RTABMapApp() :
 		trajectoryMode_(false),
 		rawScanSaved_(false),
 		smoothing_(true),
+		depthBleedingError_(0.0f),
 		depthFromMotion_(false),
 		cameraColor_(true),
 		fullResolution_(false),
@@ -245,7 +249,11 @@ RTABMapApp::RTABMapApp() :
 		maxGainRadius_(0.02f),
 		renderingTextureDecimation_(4),
 		backgroundColor_(0.2f),
-        depthConfidence_(2),
+#ifndef RTABMAP_ARCORE
+        depthConfidence_(100), // iOS
+#else
+		depthConfidence_(0),
+#endif
         upstreamRelocalizationMaxAcc_(0.0f),
 		exportPointCloudFormat_("ply"),
 		dataRecorderMode_(false),
@@ -266,20 +274,20 @@ RTABMapApp::RTABMapApp() :
 		lastPoseEventTime_(0.0),
 		visualizingMesh_(false),
 		exportedMeshUpdated_(false),
+		optTextureMesh_(new pcl::TextureMesh),
+		optRefId_(0),
+		optRefPose_(0),
 		measuresUpdated_(false),
-        targetPoint_(new pcl::PointCloud<pcl::PointXYZRGB>),
-        quadSample_(new pcl::PointCloud<pcl::PointXYZ>),
-        quadSamplePolygons_(2),
-        metricSystem_(true),
+		metricSystem_(true),
         measuringTextSize_(0.05f),
 		snapAxisThr_(0.95),
         measuringMode_(0),
         addMeasureClicked_(false),
         teleportClicked_(false),
         removeMeasureClicked_(false),
-		optTextureMesh_(new pcl::TextureMesh),
-		optRefId_(0),
-		optRefPose_(0),
+		targetPoint_(new pcl::PointCloud<pcl::PointXYZRGB>),
+        quadSample_(new pcl::PointCloud<pcl::PointXYZ>),
+        quadSamplePolygons_(2),
 		mapToOdom_(rtabmap::Transform::getIdentity())
 
 {
@@ -461,7 +469,13 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 				LOGI("Open: Found optimized mesh! Visualizing it.");
 				optTextureMesh_ = rtabmap::util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, true);
                 optMesh_ = rtabmap::Mesh();
-				optTexture_ = textures;
+                if(textures.rows > 4096) // Limitation iOS, just for rendering on device
+                {
+                    cv::resize(textures, optTexture_, cv::Size(4096, 4096), 0.0f, 0.0f, cv::INTER_AREA);
+                }
+                else {
+                    optTexture_ = textures;
+                }
                 if(!optTexture_.empty())
 				{
 					LOGI("Open: Texture mesh: %dx%d.", optTexture_.cols, optTexture_.rows);
@@ -569,8 +583,8 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 						rtabmap::SensorData data = signatures.at(id).sensorData();
                         rawPoses_.insert(std::make_pair(id, signatures.at(id).getPose()));
 
-						cv::Mat tmpA, depth;
-						data.uncompressData(&tmpA, &depth);
+						cv::Mat tmpA, tmpB, tmpC;
+						data.uncompressData(&tmpA, &tmpB, 0, 0, 0, 0, 0, depthConfidence_>0?&tmpC:0);
 
 						if(!(!data.imageRaw().empty() && !data.depthRaw().empty()) && !data.laserScanCompressed().isEmpty())
 						{
@@ -586,8 +600,30 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 							if(!data.imageRaw().empty() && !data.depthRaw().empty() && (!useExternalLidar_ || data.laserScanRaw().isEmpty()))
 							{
                                 int meshDecimation = updateMeshDecimation(data.depthRaw().cols, data.depthRaw().rows);
-                                
-								cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get());
+
+								if(smoothing_ || depthBleedingError_>0.0f)
+								{
+									cv::Mat depth = data.depthRaw();
+									if(depthBleedingError_ > 0.0f)
+									{
+										rtabmap::util2d::depthBleedingFiltering(depth, depthBleedingError_);
+									}
+									if(smoothing_)
+									{
+										depth = rtabmap::util2d::fastBilateralFiltering(depth, g_bilateralFilteringSigmaS, g_bilateralFilteringSigmaR);
+									}
+									data.setRGBDImage(data.imageRaw(), depth, data.depthConfidenceRaw(), data.cameraModels());
+								}
+
+								cloud = rtabmap::util3d::cloudRGBFromSensorData(
+									data, 
+									meshDecimation, 
+									maxCloudDepth_, 
+									minCloudDepth_, 
+									indices.get(), 
+									rtabmap::ParametersMap(), 
+									std::vector<float>(), 
+									depthConfidence_);
 							}
 							else
 							{
@@ -875,11 +911,11 @@ int RTABMapApp::updateMeshDecimation(int width, int height)
         {
             meshDecimation = 5;
         }
-        else if(width % 3 == 0 && width % 3 == 0)
+        else if(width % 3 == 0 && height % 3 == 0)
         {
             meshDecimation = 3;
         }
-        else if(width % 2 == 0 && width % 2 == 0)
+        else if(width % 2 == 0 && height % 2 == 0)
         {
             meshDecimation = 2;
         }
@@ -946,7 +982,7 @@ bool RTABMapApp::startCamera()
 	if(cameraDriver_ == 0) // Tango
 	{
 #ifdef RTABMAP_TANGO
-		camera_ = new rtabmap::CameraTango(cameraColor_, !cameraColor_ || fullResolution_?1:2, rawScanSaved_, smoothing_);
+		camera_ = new rtabmap::CameraTango(cameraColor_, !cameraColor_ || fullResolution_?1:2, rawScanSaved_);
 
 		if (TangoService_setBinder(env, iBinder) != TANGO_SUCCESS) {
 		    UERROR("TangoHandler::ConnectTango, TangoService_setBinder error");
@@ -961,7 +997,7 @@ bool RTABMapApp::startCamera()
 	else if(cameraDriver_ == 1)
 	{
 #ifdef RTABMAP_ARCORE
-		camera_ = new rtabmap::CameraARCore(env, context, activity, depthFromMotion_, smoothing_, upstreamRelocalizationMaxAcc_);
+		camera_ = new rtabmap::CameraARCore(env, context, activity, depthFromMotion_, upstreamRelocalizationMaxAcc_);
 #else
 		UERROR("RTAB-Map is not built with ARCore support!");
 #endif
@@ -969,14 +1005,14 @@ bool RTABMapApp::startCamera()
 	else if(cameraDriver_ == 2)
 	{
 #ifdef RTABMAP_ARENGINE
-		camera_ = new rtabmap::CameraAREngine(env, context, activity, smoothing_, upstreamRelocalizationMaxAcc_);
+		camera_ = new rtabmap::CameraAREngine(env, context, activity, upstreamRelocalizationMaxAcc_);
 #else
 		UERROR("RTAB-Map is not built with AREngine support!");
 #endif
 	}
 	else if(cameraDriver_ == 3)
 	{
-		camera_ = new rtabmap::CameraMobile(smoothing_, upstreamRelocalizationMaxAcc_);
+		camera_ = new rtabmap::CameraMobile(upstreamRelocalizationMaxAcc_);
 	}
 
 	if(camera_ == 0)
@@ -1003,7 +1039,11 @@ bool RTABMapApp::startCamera()
 		cameraJustInitialized_ = true;
 		if(useExternalLidar_)
 		{
+#if BOOST_VERSION >= 108700
+            rtabmap::LidarVLP16 * lidar = new rtabmap::LidarVLP16(boost::asio::ip::make_address("192.168.1.201"), 2368, true);
+#else
 			rtabmap::LidarVLP16 * lidar = new rtabmap::LidarVLP16(boost::asio::ip::address_v4::from_string("192.168.1.201"), 2368, true);
+#endif
 			lidar->init();
 			camera_->setImageRate(0); // if lidar, to get close camera synchronization
 			sensorCaptureThread_ = new rtabmap::SensorCaptureThread(lidar, camera_, camera_, rtabmap::Transform::getIdentity());
@@ -1354,8 +1394,8 @@ 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 && !(rtabmapThread_ == 0 || !rtabmapThread_->isRunning()))
+                // We check if we are in measuring mode: not visualizing mesh or rtabmap is not started (localization mode)
+				if(main_scene_.background_renderer_ == 0 && camera_->getTextureId() != 0 && (!visualizingMesh_ || !(rtabmapThread_ == 0 || !rtabmapThread_->isRunning())))
 				{
 					main_scene_.background_renderer_ = new BackgroundRenderer();
 					main_scene_.background_renderer_->InitializeGlContent(((rtabmap::CameraMobile*)camera_)->getTextureId(), cameraDriver_ <= 2);
@@ -1536,7 +1576,6 @@ int RTABMapApp::Render()
                 int textId = 0;
                 int quadId = 0;
                 int circleId = 0;
-                float sphereRadius = 0.02f;
                 float quadSize=0.05f;
                 float quadAlpha = 0.3f;
 
@@ -2010,8 +2049,8 @@ int RTABMapApp::Render()
 								{
 									rtabmap::SensorData data = bufferedSensorData.at(id);
 
-									cv::Mat tmpA, depth;
-									data.uncompressData(&tmpA, &depth);
+									cv::Mat tmpA, tmpB, tmpC;
+									data.uncompressData(&tmpA, &tmpB, 0, 0, 0, 0, 0, depthConfidence_>0?&tmpC:0);
 									if(!(!data.imageRaw().empty() && !data.depthRaw().empty()) && !data.laserScanCompressed().isEmpty())
 									{
 										rtabmap::LaserScan scan;
@@ -2029,7 +2068,20 @@ int RTABMapApp::Render()
 										if(!data.imageRaw().empty() && !data.depthRaw().empty() && (!useExternalLidar_ || data.laserScanRaw().isEmpty()))
 										{
                                             int meshDecimation = updateMeshDecimation(data.depthRaw().cols, data.depthRaw().rows);
-											cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get());
+											if(smoothing_ || depthBleedingError_>0.0f)
+											{
+												cv::Mat depth = data.depthRaw();
+												if(depthBleedingError_ > 0.0f)
+												{
+													rtabmap::util2d::depthBleedingFiltering(depth, depthBleedingError_);
+												}
+												if(smoothing_)
+												{
+													depth = rtabmap::util2d::fastBilateralFiltering(depth, g_bilateralFilteringSigmaS, g_bilateralFilteringSigmaR);
+												}
+												data.setRGBDImage(data.imageRaw(), depth, data.depthConfidenceRaw(), data.cameraModels());
+											}
+											cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get(), rtabmap::ParametersMap(), std::vector<float>(), depthConfidence_);
 										}
 										else
 										{
@@ -2245,7 +2297,7 @@ int RTABMapApp::Render()
 							if(!sensorEvent.data().imageRaw().empty() && !sensorEvent.data().depthRaw().empty() && (!useExternalLidar_ || sensorEvent.data().laserScanRaw().isEmpty()))
 							{
                                 int meshDecimation = updateMeshDecimation(sensorEvent.data().depthRaw().cols, sensorEvent.data().depthRaw().rows);
-								cloud = rtabmap::util3d::cloudRGBFromSensorData(sensorEvent.data(), meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get());
+								cloud = rtabmap::util3d::cloudRGBFromSensorData(sensorEvent.data(), meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get(), rtabmap::ParametersMap(), std::vector<float>(), depthConfidence_);
 							}
 							else
 							{
@@ -3015,6 +3067,11 @@ void RTABMapApp::setSmoothing(bool enabled)
 	}
 }
 
+void RTABMapApp::setDepthBleedingError(float value)
+{
+	depthBleedingError_ = value;
+}
+
 void RTABMapApp::setDepthFromMotion(bool enabled)
 {
 	if(depthFromMotion_ != enabled)
@@ -3107,10 +3164,10 @@ void RTABMapApp::setBackgroundColor(float gray)
 
 void RTABMapApp::setDepthConfidence(int value)
 {
-    depthConfidence_ = value;
-    if(depthConfidence_>2)
+    depthConfidence_ = value*50; // [0,2] -> [0,100]
+    if(depthConfidence_>100)
     {
-        depthConfidence_ = 2;
+        depthConfidence_ = 100;
     }
 }
 
@@ -3410,6 +3467,19 @@ bool RTABMapApp::exportMesh(
 							if(!data.imageRaw().empty() && !data.depthRaw().empty() && data.cameraModels().size() == 1)
 							{
                                 int meshDecimation = updateMeshDecimation(data.depthRaw().cols, data.depthRaw().rows);
+								if(smoothing_ || depthBleedingError_>0.0f)
+								{
+									cv::Mat depth = data.depthRaw();
+									if(depthBleedingError_ > 0.0f)
+									{
+										rtabmap::util2d::depthBleedingFiltering(depth, depthBleedingError_);
+									}
+									if(smoothing_)
+									{
+										depth = rtabmap::util2d::fastBilateralFiltering(depth, g_bilateralFilteringSigmaS, g_bilateralFilteringSigmaR);
+									}
+									data.setRGBDImage(data.imageRaw(), depth, data.depthConfidenceRaw(), data.cameraModels());
+								}
 								cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get());
 								model = data.cameraModels()[0];
 								depth = data.depthRaw();
@@ -3677,7 +3747,20 @@ bool RTABMapApp::exportMesh(
 							if(!data.imageRaw().empty() && !data.depthRaw().empty() && data.cameraModels().size() == 1)
 							{
                                 int meshDecimation = updateMeshDecimation(data.depthRaw().cols, data.depthRaw().rows);
-								cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation, maxCloudDepth_, minCloudDepth_);
+								if(smoothing_ || depthBleedingError_>0.0f)
+								{
+									cv::Mat depth = data.depthRaw();
+									if(depthBleedingError_ > 0.0f)
+									{
+										rtabmap::util2d::depthBleedingFiltering(depth, depthBleedingError_);
+									}
+									if(smoothing_)
+									{
+										depth = rtabmap::util2d::fastBilateralFiltering(depth, g_bilateralFilteringSigmaS, g_bilateralFilteringSigmaR);
+									}
+									data.setRGBDImage(data.imageRaw(), depth, data.depthConfidenceRaw(), data.cameraModels());
+								}
+								cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation, maxCloudDepth_, minCloudDepth_, 0, rtabmap::ParametersMap(), std::vector<float>(), depthConfidence_);
 								polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
 							}
 						}
@@ -3935,7 +4018,20 @@ bool RTABMapApp::exportMesh(
 					if(!data.imageRaw().empty() && !data.depthRaw().empty())
 					{
 						// full resolution
-						cloud = rtabmap::util3d::cloudRGBFromSensorData(data, 1, maxCloudDepth_, minCloudDepth_, indices.get());
+						if(smoothing_ || depthBleedingError_>0.0f)
+						{
+							cv::Mat depth = data.depthRaw();
+							if(depthBleedingError_ > 0.0f)
+							{
+								rtabmap::util2d::depthBleedingFiltering(depth, depthBleedingError_);
+							}
+							if(smoothing_)
+							{
+								depth = rtabmap::util2d::fastBilateralFiltering(depth, g_bilateralFilteringSigmaS, g_bilateralFilteringSigmaR);
+							}
+							data.setRGBDImage(data.imageRaw(), depth, data.depthConfidenceRaw(), data.cameraModels());
+						}
+						cloud = rtabmap::util3d::cloudRGBFromSensorData(data, 1, maxCloudDepth_, minCloudDepth_, indices.get(), rtabmap::ParametersMap(), std::vector<float>(), depthConfidence_);
 					}
                     else if(!data.laserScanRaw().empty())
                     {
@@ -3965,7 +4061,20 @@ bool RTABMapApp::exportMesh(
 						if(!data.imageRaw().empty() && !data.depthRaw().empty())
 						{
                             int meshDecimation = updateMeshDecimation(data.depthRaw().cols, data.depthRaw().rows);
-							cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get());
+							if(smoothing_ || depthBleedingError_>0.0f)
+							{
+								cv::Mat depth = data.depthRaw();
+								if(depthBleedingError_ > 0.0f)
+								{
+									rtabmap::util2d::depthBleedingFiltering(depth, depthBleedingError_);
+								}
+								if(smoothing_)
+								{
+									depth = rtabmap::util2d::fastBilateralFiltering(depth, g_bilateralFilteringSigmaS, g_bilateralFilteringSigmaR);
+								}
+								data.setRGBDImage(data.imageRaw(), depth, data.depthConfidenceRaw(), data.cameraModels());
+							}
+							cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get(), rtabmap::ParametersMap(), std::vector<float>(), depthConfidence_);
 						}
                         else if(!data.laserScanRaw().empty())
                         {
@@ -4117,7 +4226,13 @@ bool RTABMapApp::postExportation(bool visualize)
 				LOGI("postExportation: Found optimized mesh! Visualizing it.");
 				optTextureMesh_ = rtabmap::util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, true);
                 optMesh_ = rtabmap::Mesh();
-                optTexture_ = textures;
+                if(textures.rows > 4096) // Limitation iOS, just for rendering on device
+                {
+                    cv::resize(textures, optTexture_, cv::Size(4096, 4096), 0.0f, 0.0f, cv::INTER_AREA);
+                }
+                else {
+                    optTexture_ = textures;
+                }
 
 				boost::mutex::scoped_lock  lock(renderingMutex_);
 				visualizingMesh_ = true;
@@ -4524,6 +4639,7 @@ void RTABMapApp::postOdometryEvent(
 
 
 				cv::Mat outputDepth;
+				cv::Mat outputDepthConfidence;
 				if(depth && depthHeight>0 && depthWidth>0)
 				{
 #ifndef DISABLE_LOG
@@ -4533,32 +4649,21 @@ void RTABMapApp::postOdometryEvent(
                     {
                         // IOS
                         outputDepth = cv::Mat(depthHeight, depthWidth, CV_32FC1, (void*)depth).clone();
-                        if(conf && confWidth == depthWidth && confHeight == depthHeight && confFormat == 1278226488 && depthConfidence_>0)
+                        if(conf && confWidth == depthWidth && confHeight == depthHeight && confFormat == 1278226488)
                         {
-                            const unsigned char * confPtr = (const unsigned char *)conf;
-                            float * depthPtr = outputDepth.ptr<float>();
-                            int i=0;
-                            for (int y = 0; y < outputDepth.rows; ++y)
-                            {
-                                for (int x = 0; x < outputDepth.cols; ++x)
-                                {
-                                    // https://developer.apple.com/documentation/arkit/arconfidencelevel
-                                    // 0 = low
-                                    // 1 = medium
-                                    // 2 = high
-                                    if(confPtr[y*outputDepth.cols + x] < depthConfidence_)
-                                    {
-                                        depthPtr[y*outputDepth.cols + x] = 0.0f;
-                                        ++i;
-                                    }
-                                }
-                            }
+							// https://developer.apple.com/documentation/arkit/arconfidencelevel
+							// 0 = low
+							// 1 = medium
+							// 2 = high
+							// Re-scale confidence from [0,2] to [0,100]
+							cv::Mat(depthHeight, depthWidth, CV_8UC1, (void*)conf).convertTo(outputDepthConfidence, CV_8UC1, 50, 0);
                         }
                     }
                     else if(depthLen == 2*depthWidth*depthHeight)
                     {
                         // ANDROID
                         outputDepth = cv::Mat(depthHeight, depthWidth, CV_16UC1);
+						outputDepthConfidence = cv::Mat(depthHeight, depthWidth, CV_8UC1);
                         uint16_t *dataShort = (uint16_t *)depth;
                         for (int y = 0; y < outputDepth.rows; ++y)
                         {
@@ -4567,6 +4672,13 @@ void RTABMapApp::postOdometryEvent(
                                 uint16_t depthSample = dataShort[y*outputDepth.cols + x];
                                 uint16_t depthRange = (depthSample & 0x1FFF); // first 3 bits are confidence
                                 outputDepth.at<uint16_t>(y,x) = depthRange;
+								// https://developer.android.com/reference/android/graphics/ImageFormat#DEPTH16
+								// The confidence value is an estimate of correctness for this sample. It 
+								// is encoded in the 3 most significant bits of the sample, with a value of 
+								// 0 representing 100% confidence, a value of 1 representing 0% confidence, a 
+								// value of 2 representing 1/7, a value of 3 representing 2/7, and so on.
+								uint8_t depthConfidence = uint8_t((depthSample >> 13) & 0x7);
+								outputDepthConfidence.at<uint8_t>(y,x) = depthConfidence == 0 ? 100 : (depthConfidence - 1)*100 / 7;
                             }
                         }
                     }
@@ -4626,7 +4738,9 @@ void RTABMapApp::postOdometryEvent(
 								depth_fx, 0, depth_cx,
 								0, depth_fy, depth_cy,
 								0, 0, 1);
-						outputDepth = rtabmap::util2d::registerDepth(outputDepth, depthK, outputDepth.size(), colorK, rgbToDepth);
+						cv::Mat regConfidence;
+						outputDepth = rtabmap::util2d::registerDepth(outputDepth, outputDepthConfidence, depthK, outputDepth.size(), colorK, rgbToDepth, regConfidence);
+						outputDepthConfidence = regConfidence;
 #ifndef DISABLE_LOG
 						UDEBUG("Depth registration time: %fs", time.elapsed());
 #endif
@@ -4666,8 +4780,8 @@ void RTABMapApp::postOdometryEvent(
                         depthModel.setLocalTransform(pose*model.localTransform());
                         camera_->setOcclusionImage(outputDepth, depthModel);
                     }
-                    
-					rtabmap::SensorData data(scan, outputRGB, outputDepth, model, 0, stamp);
+
+					rtabmap::SensorData data(scan, outputRGB, outputDepth, outputDepthConfidence, model, 0, stamp);
 					data.setFeatures(kpts,  kpts3, cv::Mat());
                     glm::mat4 projectionMatrix(0);
                     projectionMatrix[0][0] = p00;

app/android/jni/RTABMapApp.h

@@ -139,6 +139,7 @@ class RTABMapApp : public UEventsHandler {
   void setCameraColor(bool enabled);
   void setFullResolution(bool enabled);
   void setSmoothing(bool enabled);
+  void setDepthBleedingError(float value);
   void setDepthFromMotion(bool enabled);
   void setAppendMode(bool enabled);
   void setUpstreamRelocalizationAccThr(float value);
@@ -235,6 +236,7 @@ class RTABMapApp : public UEventsHandler {
   bool trajectoryMode_;
   bool rawScanSaved_;
   bool smoothing_;
+  float depthBleedingError_;
   bool depthFromMotion_;
   bool cameraColor_;
   bool fullResolution_;
@@ -250,7 +252,7 @@ class RTABMapApp : public UEventsHandler {
   float maxGainRadius_;
   int renderingTextureDecimation_;
   float backgroundColor_;
-  int depthConfidence_;
+  unsigned char depthConfidence_;
   float upstreamRelocalizationMaxAcc_;
   std::string exportPointCloudFormat_;
 

app/android/jni/jni_interface.cpp

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

app/android/res/layout/activity_settings.xml

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

app/android/res/values/strings.xml

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

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

@@ -1140,6 +1140,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 			RTABMapLib.setRawScanSaved(nativeApplication, sharedPref.getBoolean(getString(R.string.pref_key_raw_scan_saved), Boolean.parseBoolean(getString(R.string.pref_default_raw_scan_saved))));
 			RTABMapLib.setFullResolution(nativeApplication, sharedPref.getBoolean(getString(R.string.pref_key_resolution), Boolean.parseBoolean(getString(R.string.pref_default_resolution))));
 			RTABMapLib.setSmoothing(nativeApplication, sharedPref.getBoolean(getString(R.string.pref_key_smoothing), Boolean.parseBoolean(getString(R.string.pref_default_smoothing))));
+			RTABMapLib.setDepthBleedingError(nativeApplication, Float.parseFloat(sharedPref.getString(getString(R.string.pref_key_depth_bleeding_error), getString(R.string.pref_default_depth_bleeding_error))));
 			RTABMapLib.setDepthFromMotion(nativeApplication, sharedPref.getBoolean(getString(R.string.pref_key_depth_from_motion), Boolean.parseBoolean(getString(R.string.pref_default_depth_from_motion))));
 			RTABMapLib.setCameraColor(nativeApplication, !sharedPref.getBoolean(getString(R.string.pref_key_fisheye), Boolean.parseBoolean(getString(R.string.pref_default_fisheye))));
 			RTABMapLib.setAppendMode(nativeApplication, sharedPref.getBoolean(getString(R.string.pref_key_append), Boolean.parseBoolean(getString(R.string.pref_default_append))));

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

@@ -73,6 +73,7 @@ public class RTABMapLib
     public static native void setRawScanSaved(long nativeApplication, boolean enabled);
     public static native void setFullResolution(long nativeApplication, boolean enabled);
     public static native void setSmoothing(long nativeApplication, boolean enabled);
+    public static native void setDepthBleedingError(long nativeApplication, float value);
     public static native void setDepthFromMotion(long nativeApplication, boolean enabled);
     public static native void setCameraColor(long nativeApplication, boolean enabled);
     public static native void setAppendMode(long nativeApplication, boolean enabled);

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

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

app/ios/RTABMapApp.xcodeproj/project.pbxproj

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

app/ios/RTABMapApp/NativeWrapper.cpp

@@ -505,6 +505,13 @@ void setSmoothingNative(const void *object, bool enabled)
     else
         UERROR("object is null!");
 }
+void setDepthBleedingErrorNative(const void *object, float value)
+{
+    if(object)
+        native(object)->setDepthBleedingError(value);
+    else
+        UERROR("object is null!");
+}
 void setAppendModeNative(const void *object, bool enabled)
 {
     if(object)

app/ios/RTABMapApp/NativeWrapper.hpp

@@ -101,6 +101,7 @@ void setGraphVisibleNative(const void *object, bool visible);
 void setGridVisibleNative(const void *object, bool visible);
 void setFullResolutionNative(const void *object, bool enabled);
 void setSmoothingNative(const void *object, bool enabled);
+void setDepthBleedingErrorNative(const void *object, float value);
 void setAppendModeNative(const void *object, bool enabled);
 void setUpstreamRelocalizationAccThrNative(const void *object, float value);
 void setMaxCloudDepthNative(const void *object, float value);

app/ios/RTABMapApp/RTABMap.swift

@@ -453,6 +453,9 @@ class RTABMap {
     func setSmoothing(enabled: Bool) {
         setSmoothingNative(native_rtabmap, enabled)
     }
+    func setDepthBleedingError(value: Float) {
+        setDepthBleedingErrorNative(native_rtabmap, value)
+    }
     func setAppendMode(enabled: Bool) {
         setAppendModeNative(native_rtabmap, enabled)
     }

app/ios/RTABMapApp/ViewController.swift

@@ -162,7 +162,6 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
     func resetNoTouchTimer(_ showHud: Bool = false) {
         if(showHud)
         {
-            print("Show HUD")
             mMenuOpened = false
             mHudVisible = true
             setNeedsStatusBarAppearanceUpdate()
@@ -184,7 +183,6 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         }
         else if(mState != .STATE_WELCOME && mState != .STATE_CAMERA && presentedViewController as? UIAlertController == nil && !mMenuOpened)
         {
-            print("Hide HUD")
             self.mHudVisible = false
             self.setNeedsStatusBarAppearanceUpdate()
             self.updateState(state: self.mState)
@@ -840,13 +838,11 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
             self.isPaused = true
             view?.enableSetNeedsDisplay = true
             self.view.setNeedsDisplay()
-            print("enableSetNeedsDisplay")
         }
         else
         {
             view?.enableSetNeedsDisplay = false
             self.isPaused = false
-            print("disableSetNeedsDisplay")
         }
         
         if !self.isPaused {
@@ -934,7 +930,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
@@ -1516,6 +1511,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         rtabmap!.setNodesFiltering(enabled: defaults.bool(forKey: "NodesFiltering"));
         rtabmap!.setFullResolution(enabled: defaults.bool(forKey: "HDMode"));
         rtabmap!.setSmoothing(enabled: defaults.bool(forKey: "Smoothing"));
+        rtabmap!.setDepthBleedingError(value: defaults.float(forKey: "DepthBleedingError"));
         rtabmap!.setAppendMode(enabled: defaults.bool(forKey: "AppendMode"));
         rtabmap!.setUpstreamRelocalizationAccThr(value: defaults.float(forKey: "UpstreamRelocalizationFilteringAccThr"));
         rtabmap!.setExportPointCloudFormat(format: defaults.string(forKey: "ExportPointCloudFormat")!);
@@ -1918,7 +1914,9 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
             })
             
         }))
-
+	
+        let previousState = mState
+        
         updateState(state: .STATE_PROCESSING);
         
         present(alertView, animated: true, completion: {
@@ -1989,8 +1987,11 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
                             self.updateState(state: .STATE_VISUALIZING)
                             
                             self.rtabmap!.postExportation(visualize: true)
-
-                            self.setGLCamera(type: 2)
+							
+                            if previousState != .STATE_VISUALIZING
+                            {
+                                self.setGLCamera(type: 2)
+                            }
 
                             if self.openedDatabasePath == nil
                             {

app/ios/RTABMapApp/install_deps.sh

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

app/ios/Settings.bundle/Mapping.plist

@@ -58,7 +58,7 @@
 			<key>Type</key>
 			<string>PSGroupSpecifier</string>
 			<key>FooterText</key>
-			<string>Choosing between high and low depth confidence depends on the application. If precise geometry is critical, prioritize high depth confidence even if it means fewer points. If you need a dense point cloud for analysis, even in areas with weak LiDAR signal, a lower depth confidence might be suitable at the cost of highly interpolated points (that could give a wavy look of some surfaces). Important: the points with too low confidence are not saved in the db file, so we cannot recover or filter these points in post-processing.</string>
+			<string>Choosing between high and low depth confidence depends on the application. If precise geometry is critical, prioritize high depth confidence even if it means fewer points. If you need a dense point cloud for analysis, even in areas with weak LiDAR signal, a lower depth confidence might be suitable at the cost of highly interpolated points (that could give a wavy look of some surfaces). Note: it is possible to change confidence level in post-processing.</string>
 		</dict>
 		<dict>
 			<key>DefaultValue</key>
@@ -86,7 +86,7 @@
 			<key>Type</key>
 			<string>PSGroupSpecifier</string>
 			<key>FooterText</key>
-			<string>Smooth the point clouds.</string>
+			<string>Smooth the point clouds. Note that this can be changed in post-processing.</string>
 		</dict>
 		<dict>
 			<key>Type</key>
@@ -98,6 +98,44 @@
 			<key>DefaultValue</key>
 			<false/>
 		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+			<key>FooterText</key>
+			<string>Filter interpolated depth pixels between foreground and background objects. Note that values can be changed in post-processing.</string>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSMultiValueSpecifier</string>
+			<key>Title</key>
+			<string>Depth Bleeding Filter Error</string>
+			<key>Key</key>
+			<string>DepthBleedingError</string>
+			<key>DefaultValue</key>
+			<string>0</string>
+			<key>Titles</key>
+			<array>
+				<string>Disabled</string>
+				<string>2.5 cm</string>
+				<string>5 cm</string>
+				<string>10 cm</string>
+				<string>15 cm</string>
+				<string>20 cm</string>
+				<string>25 cm</string>
+				<string>30 cm</string>
+			</array>
+			<key>Values</key>
+			<array>
+				<string>0</string>
+				<string>0.025</string>
+				<string>0.05</string>
+				<string>0.10</string>
+				<string>0.15</string>
+				<string>0.20</string>
+				<string>0.25</string>
+				<string>0.30</string>
+			</array>
+		</dict>
 		<dict>
 			<key>Type</key>
 			<string>PSGroupSpecifier</string>
@@ -590,7 +628,7 @@
 			<key>Type</key>
 			<string>PSGroupSpecifier</string>
 			<key>FooterText</key>
-			<string>BRIEF features are fast to compute but are not rotation invariant like FREAK. Warning: Changing feature type will automatically reset the map!</string>
+			<string>BRIEF, FREAK and ORB are binary features, which are fast to compute and use less memory. SURF and SIFT are high dimensional float descriptors (rotation/scale/shear-invariant), so they use more memory and CPU but they are significantly better to detect loop closures in large environments. Warning: Changing feature type will automatically reset the map!</string>
 		</dict>
 		<dict>
 			<key>Type</key>
@@ -603,13 +641,21 @@
 			<string>6</string>
 			<key>Titles</key>
 			<array>
-				<string>BRIEF</string>
-				<string>FREAK</string>
+				<string>SURF</string>
+				<string>SIFT</string>
+				<string>GFTT-FREAK</string>
+				<string>GFTT-BRIEF</string>
+				<string>GFTT-ORB</string>
+				<string>ORB-OCTREE</string>
 			</array>
 			<key>Values</key>
 			<array>
-				<string>6</string>
+				<string>0</string>
+				<string>1</string>
 				<string>5</string>
+				<string>6</string>
+				<string>8</string>
+				<string>10</string>
 			</array>
 		</dict>
 		<dict>

app/ios/Settings.bundle/Root.plist

@@ -494,7 +494,7 @@
 		</dict>
 		<dict>
 			<key>DefaultValue</key>
-			<string>0.21.11</string>
+			<string>0.22.0</string>
 			<key>Key</key>
 			<string>Version</string>
 			<key>Title</key>

corelib/include/rtabmap/core/Optimizer.h

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

corelib/include/rtabmap/core/Parameters.h

@@ -390,6 +390,7 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(RGBD, MaxOdomCacheSize,             int,  10,      uFormat("Maximum odometry cache size. Used only in localization mode (when %s=false). This is used to get smoother localizations and to verify localization transforms (when %s!=0) to make sure we don't teleport to a location very similar to one we previously localized on. Set 0 to disable caching.", kMemIncrementalMemory().c_str(), kRGBDOptimizeMaxError().c_str()));
     RTABMAP_PARAM(RGBD, LocalizationSmoothing,        bool, true,    uFormat("Adjust localization constraints based on optimized odometry cache poses (when %s>0).",  kRGBDMaxOdomCacheSize().c_str()));
     RTABMAP_PARAM(RGBD, LocalizationPriorError,       double, 0.001, uFormat("The corresponding variance (error x error) set to priors of the map's poses during localization (when %s>0).",  kRGBDMaxOdomCacheSize().c_str()));
+    RTABMAP_PARAM(RGBD, LocalizationSecondTryWithoutProximityLinks,  bool, true, uFormat("When localization is rejected by graph optimization validation, try a second time without proximity links if landmark or loop closure links are also present in odometry cache (see %s). If it succeeds, the proximity links are removed. This assumes that global loop closure and landmark links are more accurate than proximity links.", kRGBDMaxOdomCacheSize().c_str()));
 
     // Local/Proximity loop closure detection
     RTABMAP_PARAM(RGBD, ProximityByTime,              bool, false, "Detection over all locations in STM.");

corelib/include/rtabmap/core/Rtabmap.h

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

corelib/include/rtabmap/core/SensorData.h

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

corelib/include/rtabmap/core/Statistics.h

@@ -81,6 +81,7 @@ class RTABMAP_CORE_EXPORT Statistics
 	RTABMAP_STATS(Loop, Landmark_detected_node_ref,);
 	RTABMAP_STATS(Loop, Visual_inliers_mean_dist,m);
 	RTABMAP_STATS(Loop, Visual_inliers_distribution,);
+	RTABMAP_STATS(Loop, Proximity_links_cleared,);
 	//Odom correction
 	RTABMAP_STATS(Loop, Odom_correction_norm, m);
 	RTABMAP_STATS(Loop, Odom_correction_angle, deg);

corelib/include/rtabmap/core/Transform.h

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

corelib/include/rtabmap/core/util2d.h

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

corelib/include/rtabmap/core/util3d.h

@@ -97,6 +97,15 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT cloudFromDepth(
 		float maxDepth = 0.0f,
 		float minDepth = 0.0f,
 		std::vector<int> * validIndices = 0);
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT cloudFromDepth(
+		const cv::Mat & imageDepth,
+		const cv::Mat & imageDepthConfidence,
+		const CameraModel & model,
+		int decimation = 1,
+		float maxDepth = 0.0f,
+		float minDepth = 0.0f,
+		unsigned char confidenceThr = 0,
+		std::vector<int> * validIndices = 0);
 
 // Use cloudFromDepthRGB with CameraModel interface.
 RTABMAP_DEPRECATED pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_CORE_EXPORT cloudFromDepthRGB(
@@ -116,6 +125,16 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_CORE_EXPORT cloudFromDepthRGB(
 		float maxDepth = 0.0f,
 		float minDepth = 0.0f,
 		std::vector<int> * validIndices = 0);
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_CORE_EXPORT cloudFromDepthRGB(
+	const cv::Mat & imageRgb,
+	const cv::Mat & imageDepth,
+	const cv::Mat & imageDepthConfidence,
+	const CameraModel & model,
+	int decimation = 1,
+	float maxDepth = 0.0f,
+	float minDepth = 0.0f,
+	unsigned char confidenceThr = 0, // 0=low, 100=high
+	std::vector<int> * validIndices = 0);
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT cloudFromDisparity(
 		const cv::Mat & imageDisparity,
@@ -164,7 +183,8 @@ std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> RTABMAP_CORE_EXPORT cloudsFromS
 		float minDepth = 0.0f,
 		std::vector<pcl::IndicesPtr> * validIndices = 0,
 		const ParametersMap & stereoParameters = ParametersMap(),
-		const std::vector<float> & roiRatios = std::vector<float>()); // ignored for stereo
+		const std::vector<float> & roiRatios = std::vector<float>(), // ignored for stereo
+		unsigned char confidenceThr = 0);                            // ignored for stereo
 
 /**
  * Create a XYZ cloud from the images contained in SensorData. If there is only one camera,
@@ -188,7 +208,8 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT cloudFromSensorData(
 		float minDepth = 0.0f,
 		std::vector<int> * validIndices = 0,
 		const ParametersMap & stereoParameters = ParametersMap(),
-		const std::vector<float> & roiRatios = std::vector<float>()); // ignored for stereo
+		const std::vector<float> & roiRatios = std::vector<float>(), // ignored for stereo
+		unsigned char confidenceThr = 0);                                // ignored for stereo
 
 /**
  * Create an RGB cloud from the images contained in SensorData, one for each camera
@@ -210,7 +231,8 @@ std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> RTABMAP_CORE_EXPORT cloudsRG
 		float minDepth = 0.0f,
 		std::vector<pcl::IndicesPtr > * validIndices = 0,
 		const ParametersMap & stereoParameters = ParametersMap(),
-		const std::vector<float> & roiRatios = std::vector<float>()); // ignored for stereo
+		const std::vector<float> & roiRatios = std::vector<float>(), // ignored for stereo
+		unsigned char confidenceThr = 0);                            // ignored for stereo
 
 /**
  * Create an RGB cloud from the images contained in SensorData. If there is only one camera,
@@ -234,7 +256,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_CORE_EXPORT cloudRGBFromSensorDat
 		float minDepth = 0.0f,
 		std::vector<int> * validIndices = 0,
 		const ParametersMap & stereoParameters = ParametersMap(),
-		const std::vector<float> & roiRatios = std::vector<float>()); // ignored for stereo
+		const std::vector<float> & roiRatios = std::vector<float>(), // ignored for stereo
+		unsigned char confidenceThr = 0);                            // ignored for stereo
 
 /**
  * Simulate a laser scan rotating counterclockwise, using middle line of the depth image.
@@ -390,6 +413,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > RTABMAP_CORE_EXPORT
 		const std::map<int, std::vector<CameraModel> > & cameraModels,
 		float maxDistance = 0.0f,
 		float maxAngle = 0.0f,
+		float maxDepthError = 0.0f,
 		const std::vector<float> & roiRatios = std::vector<float>(),
 		const cv::Mat & projMask = cv::Mat(),
 		bool distanceToCamPolicy = false,
@@ -404,6 +428,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > RTABMAP_CORE_EXPORT
 		const std::map<int, std::vector<CameraModel> > & cameraModels,
 		float maxDistance = 0.0f,
 		float maxAngle = 0.0f,
+		float maxDepthError = 0.0f,
 		const std::vector<float> & roiRatios = std::vector<float>(),
 		const cv::Mat & projMask = cv::Mat(),
 		bool distanceToCamPolicy = false,

corelib/src/CMakeLists.txt

@@ -816,6 +816,7 @@ CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/resources/DatabaseSchema.sql.in ${CMA
 
 SET(RESOURCES
 	${CMAKE_CURRENT_SOURCE_DIR}/resources/DatabaseSchema.sql
+	${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_20_0.sql
 	${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_18_3.sql
 	${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_18_0.sql
 	${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_17_0.sql

corelib/src/DBDriverSqlite3.cpp

@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/util3d.h"
 #include "rtabmap/core/Compression.h"
 #include "DatabaseSchema_sql.h"
+#include "DatabaseSchema_0_20_0_sql.h"
 #include "DatabaseSchema_0_18_3_sql.h"
 #include "DatabaseSchema_0_18_0_sql.h"
 #include "DatabaseSchema_0_17_0_sql.h"
@@ -402,6 +403,7 @@ bool DBDriverSqlite3::connectDatabaseQuery(const std::string & url, bool overwri
 			schemas.push_back(std::make_pair("0.17.0", DATABASESCHEMA_0_17_0_SQL));
 			schemas.push_back(std::make_pair("0.18.0", DATABASESCHEMA_0_18_0_SQL));
 			schemas.push_back(std::make_pair("0.18.3", DATABASESCHEMA_0_18_3_SQL));
+			schemas.push_back(std::make_pair("0.20.0", DATABASESCHEMA_0_20_0_SQL));
 			schemas.push_back(std::make_pair(uNumber2Str(RTABMAP_VERSION_MAJOR)+"."+uNumber2Str(RTABMAP_VERSION_MINOR), DATABASESCHEMA_SQL));
 			for(size_t i=0; i<schemas.size(); ++i)
 			{
@@ -1317,7 +1319,15 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 
 			if(images)
 			{
-				fields << "image, depth, calibration";
+				if(uStrNumCmp(_version, "0.22.0") >= 0)
+				{
+					fields << "image, depth, depth_confidence, calibration";
+				}
+				else
+				{
+					fields << "image, depth, calibration";
+				}
+				
 				if(scan || userData || occupancyGrid)
 				{
 					fields << ", ";
@@ -1448,6 +1458,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 
 				cv::Mat imageCompressed;
 				cv::Mat depthOrRightCompressed;
+				cv::Mat depthConfidenceCompressed;
 				std::vector<CameraModel> models;
 				std::vector<StereoCameraModel> stereoModels;
 				Transform localTransform = Transform::getIdentity();
@@ -1472,6 +1483,17 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 						depthOrRightCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
 					}
 
+					if(uStrNumCmp(_version, "0.22.0") >= 0)
+					{
+						//Create the depth image
+						data = sqlite3_column_blob(ppStmt, index);
+						dataSize = sqlite3_column_bytes(ppStmt, index++);
+						if(dataSize>4 && data)
+						{
+							depthConfidenceCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
+						}
+					}
+
 					if(uStrNumCmp(_version, "0.10.0") < 0)
 					{
 						data = sqlite3_column_blob(ppStmt, index); // local transform
@@ -1823,7 +1845,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 				{
 					if(models.size())
 					{
-						(*iter)->sensorData().setRGBDImage(imageCompressed, depthOrRightCompressed, models);
+						(*iter)->sensorData().setRGBDImage(imageCompressed, depthOrRightCompressed, depthConfidenceCompressed, models);
 					}
 					else
 					{
@@ -4478,6 +4500,7 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures)
 			{
 				if(!(*i)->sensorData().imageCompressed().empty() ||
 				   !(*i)->sensorData().depthOrRightCompressed().empty() ||
+				   !(*i)->sensorData().depthConfidenceCompressed().empty() ||
 				   !(*i)->sensorData().laserScanCompressed().isEmpty() ||
 				   !(*i)->sensorData().userDataCompressed().empty() ||
 				   !(*i)->sensorData().cameraModels().empty() ||
@@ -6186,7 +6209,11 @@ void DBDriverSqlite3::stepScanUpdate(sqlite3_stmt * ppStmt, int nodeId, const La
 std::string DBDriverSqlite3::queryStepSensorData() const
 {
 	UASSERT(uStrNumCmp(_version, "0.10.0") >= 0);
-	if(uStrNumCmp(_version, "0.16.0") >= 0)
+	if(uStrNumCmp(_version, "0.22.0") >= 0)
+	{
+		return "INSERT INTO Data(id, image, depth, depth_confidence, calibration, scan_info, scan, user_data, ground_cells, obstacle_cells, empty_cells, cell_size, view_point_x, view_point_y, view_point_z) VALUES(?,?,?,?,?,?,?,?,?,?,?,?,?,?,?);";
+	}
+	else if(uStrNumCmp(_version, "0.16.0") >= 0)
 	{
 		return "INSERT INTO Data(id, image, depth, calibration, scan_info, scan, user_data, ground_cells, obstacle_cells, empty_cells, cell_size, view_point_x, view_point_y, view_point_z) VALUES(?,?,?,?,?,?,?,?,?,?,?,?,?,?);";
 	}
@@ -6250,6 +6277,20 @@ void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt,
 	}
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 
+	//depth confidence
+	if(uStrNumCmp(_version, "0.22.0") >= 0)
+	{
+		if(!sensorData.depthConfidenceCompressed().empty())
+		{
+			rc = sqlite3_bind_blob(ppStmt, index++, sensorData.depthConfidenceCompressed().data, (int)sensorData.depthConfidenceCompressed().cols, SQLITE_STATIC);
+		}
+		else
+		{
+			rc = sqlite3_bind_null(ppStmt, index++);
+		}
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+	}
+
 	// calibration
 	std::vector<unsigned char> calibrationData;
 	std::vector<float> calibration;

corelib/src/DBReader.cpp

@@ -607,6 +607,7 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
 				// update images and local transforms
 				cv::Mat combinedImages;
 				cv::Mat combinedDepthImages;
+				cv::Mat combinedDepthConfidenceImages;
 				std::vector<CameraModel> combinedModels;
 				std::vector<StereoCameraModel> combinedStereoModels;
 				for(size_t i=0; i<_cameraIndices.size(); ++i)
@@ -645,6 +646,18 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
 						fromROI = cv::Mat(data.depthOrRightRaw(), cv::Rect(_cameraIndices[i]*subImageWidth, 0, subImageWidth, data.depthOrRightRaw().rows));
 						toROI = cv::Mat(combinedDepthImages, cv::Rect(addedCameras*subImageWidth, 0, subImageWidth, combinedDepthImages.rows));
 						fromROI.copyTo(toROI);
+
+						if(!data.depthConfidenceRaw().empty())
+						{
+							UASSERT(data.depthConfidenceRaw().size() == data.depthOrRightRaw().size());
+							if(combinedDepthConfidenceImages.empty())
+							{
+								combinedDepthConfidenceImages = cv::Mat(data.depthConfidenceRaw().rows, subImageWidth*(_cameraIndices.size()-i), data.depthConfidenceRaw().type());
+							}
+							fromROI = cv::Mat(data.depthConfidenceRaw(), cv::Rect(_cameraIndices[i]*subImageWidth, 0, subImageWidth, data.depthConfidenceRaw().rows));
+							toROI = cv::Mat(combinedDepthConfidenceImages, cv::Rect(addedCameras*subImageWidth, 0, subImageWidth, combinedDepthConfidenceImages.rows));
+							fromROI.copyTo(toROI);
+						}
 					}
 
 					if(!data.cameraModels().empty())
@@ -671,7 +684,7 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
 				}
 				if(!combinedModels.empty())
 				{
-					data.setRGBDImage(combinedImages, combinedDepthImages, combinedModels);
+					data.setRGBDImage(combinedImages, combinedDepthImages, combinedDepthConfidenceImages, combinedModels);
 				}
 				else
 				{
@@ -728,10 +741,11 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
 			}
 			data.setLandmarks(landmarks);
 
-			UDEBUG("Laser=%d RGB/Left=%d Depth/Right=%d, Grid=%d, UserData=%d, GlobalPose=%d, GPS=%d, IMU=%d",
+			UDEBUG("Laser=%d RGB/Left=%d Depth/Right=%d, Conf=%d, Grid=%d, UserData=%d, GlobalPose=%d, GPS=%d, IMU=%d",
 					data.laserScanRaw().isEmpty()?0:1,
 					data.imageRaw().empty()?0:1,
 					data.depthOrRightRaw().empty()?0:1,
+					data.depthConfidenceRaw().empty()?0:1,
 					data.gridCellSize()==0.0f?0:1,
 					data.userDataRaw().empty()?0:1,
 					globalPose.isNull()?0:1,
@@ -780,13 +794,19 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
 				}
 				else if(!combinedLocalTransforms.empty())
 				{
-					// We are overriding the camra local transforms, let's move 3D words accordingly
+					// We are overriding the camera local transforms, let's move 3D words accordingly
 					UASSERT(dbModels.size() == combinedLocalTransforms.size());
 					std::vector<cv::Point3f> newKeypoints3D;
+					UASSERT(dbModels[0].imageWidth()>0);
+					int subImageWidth = dbModels[0].imageWidth();
 					for(size_t i = 0; i<keypoints3D.size(); ++i)
 					{
-						cv::Point3f pt = util3d::transformPoint(keypoints3D.at(i), dbModels[i].localTransform().inverse());
-						pt = util3d::transformPoint(pt, combinedLocalTransforms[i]);
+						int cameraIndex = int(keypoints.at(i).pt.x / subImageWidth);
+						UASSERT_MSG(cameraIndex >= 0 && cameraIndex < (int)dbModels.size(),
+								uFormat("cameraIndex=%d, db models=%d, kpt.x=%f, image width=%d",
+										cameraIndex, (int)dbModels.size(), keypoints[i].pt.x, subImageWidth).c_str());
+						cv::Point3f pt = util3d::transformPoint(keypoints3D.at(i), dbModels[cameraIndex].localTransform().inverse());
+						pt = util3d::transformPoint(pt, combinedLocalTransforms[cameraIndex]);
 						newKeypoints3D.push_back(pt);
 					}
 					data.setFeatures(keypoints, newKeypoints3D, descriptors);

corelib/src/Graph.cpp

@@ -744,9 +744,8 @@ void calcRelativeErrors (
 		// compute rotational and translational errors
 		Transform pose_delta_gt     = poses_gt[i].inverse()*poses_gt[i+1];
 		Transform pose_delta_result = poses_result[i].inverse()*poses_result[i+1];
-		Transform pose_error        = pose_delta_result.inverse()*pose_delta_gt;
-		float r_err = pose_error.getAngle();
-		float t_err = pose_error.getNorm();
+		float r_err = pose_delta_result.getAngle(pose_delta_gt);
+		float t_err = pose_delta_result.getDistance(pose_delta_gt);
 
 		// write to file
 		err.push_back(errors(i,r_err,t_err,0,0));
@@ -993,15 +992,21 @@ void computeMaxGraphErrors(
 			if(iter->second.type() != Link::kLandmark ||
 				1.0 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) < 9999.0)
 			{
-				float opt_roll,opt_pitch,opt_yaw;
-				float link_roll,link_pitch,link_yaw;
-				t.getEulerAngles(opt_roll, opt_pitch, opt_yaw);
-				linkT.getEulerAngles(link_roll, link_pitch, link_yaw);
-				float angularError = uMax3(
-						force3DoF?0:fabs(opt_roll - link_roll),
-						force3DoF?0:fabs(opt_pitch - link_pitch),
-						fabs(opt_yaw - link_yaw));
-				angularError = angularError>M_PI?2*M_PI-angularError:angularError;
+				float angularError = 0.0f;
+				if(force3DoF)
+				{
+					float opt_roll,opt_pitch,opt_yaw;
+					float link_roll,link_pitch,link_yaw;
+					t.getEulerAngles(opt_roll, opt_pitch, opt_yaw);
+					linkT.getEulerAngles(link_roll, link_pitch, link_yaw);
+					angularError = fabs(opt_yaw - link_yaw);
+					angularError = angularError>M_PI?2*M_PI-angularError:angularError;
+				}
+				else
+				{
+					angularError = t.getAngle(linkT);
+				}
+
 				UASSERT(iter->second.rotVariance(false)>0.0);
 				float stddevAngular = sqrt(iter->second.rotVariance(false));
 				float angularErrorRatio = angularError/stddevAngular;

corelib/src/Memory.cpp

@@ -4893,9 +4893,10 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 					else
 					{
 						decimationDepth = (int)ceil(float(data.depthRaw().rows) / float(targetSize));
+						UASSERT(data.depthConfidenceRaw().empty() || data.depthConfidenceRaw().size() == data.depthRaw().size());
 					}
 				}
-				UDEBUG("decimation rgbOrLeft(rows=%d)=%d, depthOrRight(rows=%d)=%d", data.imageRaw().rows, _imagePreDecimation, data.depthOrRightRaw().rows, decimationDepth);
+				UDEBUG("decimation rgbOrLeft(rows=%d)=%d, depthOrRight(rows=%d)=%d (conf? %d)", data.imageRaw().rows, _imagePreDecimation, data.depthOrRightRaw().rows, decimationDepth, data.depthConfidenceRaw().empty()?0:1);
 
 				std::vector<CameraModel> cameraModels = decimatedData.cameraModels();
 				for(unsigned int i=0; i<cameraModels.size(); ++i)
@@ -4907,6 +4908,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 					decimatedData.setRGBDImage(
 							util2d::decimate(decimatedData.imageRaw(), _imagePreDecimation),
 							util2d::decimate(decimatedData.depthOrRightRaw(), decimationDepth),
+							util2d::decimate(decimatedData.depthConfidenceRaw(), decimationDepth),
 							cameraModels);
 				}
 
@@ -5656,6 +5658,8 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 
 	cv::Mat image = data.imageRaw();
 	cv::Mat depthOrRightImage = data.depthOrRightRaw();
+	cv::Mat depthConfidence = data.depthConfidenceRaw();
+
 	std::vector<CameraModel> cameraModels = data.cameraModels();
 	std::vector<StereoCameraModel> stereoCameraModels = data.stereoCameraModels();
 
@@ -5666,6 +5670,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		{
 			image = decimatedData.imageRaw();
 			depthOrRightImage = decimatedData.depthOrRightRaw();
+			depthConfidence = decimatedData.depthConfidenceRaw();
 			cameraModels = decimatedData.cameraModels();
 			stereoCameraModels = decimatedData.stereoCameraModels();
 		}
@@ -5879,9 +5884,10 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 	Signature * s;
 	if(this->isBinDataKept() && (!isIntermediateNode || _saveIntermediateNodeData))
 	{
-		UDEBUG("Bin data kept: rgb=%d, depth=%d, scan=%d, userData=%d",
+		UDEBUG("Bin data kept: rgb=%d, depth=%d, conf=%d, scan=%d, userData=%d",
 				image.empty()?0:1,
 				depthOrRightImage.empty()?0:1,
+				depthConfidence.empty()?0:1,
 				laserScan.isEmpty()?0:1,
 				data.userDataRaw().empty()?0:1);
 
@@ -5928,12 +5934,14 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 
 		cv::Mat compressedImage;
 		cv::Mat compressedDepth;
+		cv::Mat compressedDepthConfidence;
 		cv::Mat compressedScan;
 		cv::Mat compressedUserData;
 		if(_compressionParallelized)
 		{
 			rtabmap::CompressionThread ctImage(image, _rgbCompressionFormat);
 			rtabmap::CompressionThread ctDepth(depthOrRightImage, depthOrRightImage.type() == CV_32FC1 || depthOrRightImage.type() == CV_16UC1?_depthCompressionFormat:_rgbCompressionFormat);
+			rtabmap::CompressionThread ctDepthConfidence(depthConfidence);
 			rtabmap::CompressionThread ctLaserScan(laserScan.data());
 			rtabmap::CompressionThread ctUserData(data.userDataRaw());
 			if(!image.empty())
@@ -5944,6 +5952,10 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			{
 				ctDepth.start();
 			}
+			if(!depthConfidence.empty())
+			{
+				ctDepthConfidence.start();
+			}
 			if(!laserScan.isEmpty())
 			{
 				ctLaserScan.start();
@@ -5954,11 +5966,13 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			}
 			ctImage.join();
 			ctDepth.join();
+			ctDepthConfidence.join();
 			ctLaserScan.join();
 			ctUserData.join();
 
 			compressedImage = ctImage.getCompressedData();
 			compressedDepth = ctDepth.getCompressedData();
+			compressedDepthConfidence = ctDepthConfidence.getCompressedData();
 			compressedScan = ctLaserScan.getCompressedData();
 			compressedUserData = ctUserData.getCompressedData();
 		}
@@ -5966,6 +5980,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		{
 			compressedImage = compressImage2(image, _rgbCompressionFormat);
 			compressedDepth = compressImage2(depthOrRightImage, depthOrRightImage.type() == CV_32FC1 || depthOrRightImage.type() == CV_16UC1?_depthCompressionFormat:_rgbCompressionFormat);
+			compressedDepthConfidence = compressData2(depthConfidence);
 			compressedScan = compressData2(laserScan.data());
 			compressedUserData = compressData2(data.userDataRaw());
 		}
@@ -6016,6 +6031,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 								laserScan.localTransform()),
 						compressedImage.empty()?data.imageCompressed():compressedImage,
 						compressedDepth.empty()?data.depthOrRightCompressed():compressedDepth,
+						compressedDepthConfidence.empty()?data.depthConfidenceCompressed():compressedDepthConfidence,
 						cameraModels,
 						id,
 						0,
@@ -6113,7 +6129,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 	// set raw data
 	if(!cameraModels.empty())
 	{
-		s->sensorData().setRGBDImage(image, depthOrRightImage, cameraModels, false);
+		s->sensorData().setRGBDImage(image, depthOrRightImage, depthConfidence, cameraModels, false);
 	}
 	else
 	{

corelib/src/OdometryInfo.cpp

@@ -191,9 +191,8 @@ std::map<std::string, float> OdometryInfo::statistics(const Transform & pose)
 	{
 		if(!transform.isNull())
 		{
-			rtabmap::Transform diff = transformGroundTruth.inverse()*transform;
-			stats.insert(std::make_pair("Odometry/TG_error_lin/m", diff.getNorm()));
-			stats.insert(std::make_pair("Odometry/TG_error_ang/deg", diff.getAngle()*180.0/CV_PI));
+			stats.insert(std::make_pair("Odometry/TG_error_lin/m", transformGroundTruth.getDistance(transform)));
+			stats.insert(std::make_pair("Odometry/TG_error_ang/deg", transformGroundTruth.getAngle(transform)*180.0/CV_PI));
 		}
 
 		transformGroundTruth.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);

corelib/src/Optimizer.cpp

@@ -447,7 +447,8 @@ std::map<int, Transform> Optimizer::optimizeBA(
 		const std::map<int, Signature> & signatures,
 		std::map<int, cv::Point3f> & points3DMap,
 		std::map<int, std::map<int, FeatureBA> > & wordReferences,
-		bool rematchFeatures)
+		bool rematchFeatures,
+		const ParametersMap & registrationParameters)
 {
 	UDEBUG("");
 	std::map<int, std::vector<CameraModel> > multiModels;
@@ -497,7 +498,7 @@ std::map<int, Transform> Optimizer::optimizeBA(
 	}
 
 	// compute correspondences
-	this->computeBACorrespondences(poses, links, signatures, points3DMap, wordReferences, rematchFeatures);
+	this->computeBACorrespondences(poses, links, signatures, points3DMap, wordReferences, rematchFeatures, false, registrationParameters);
 
 	return optimizeBA(rootId, poses, links, multiModels, points3DMap, wordReferences);
 }
@@ -507,11 +508,12 @@ std::map<int, Transform> Optimizer::optimizeBA(
 		const std::map<int, Transform> & poses,
 		const std::multimap<int, Link> & links,
 		const std::map<int, Signature> & signatures,
-		bool rematchFeatures)
+		bool rematchFeatures,
+		const ParametersMap & registrationParameters)
 {
 	std::map<int, cv::Point3f> points3DMap;
 	std::map<int, std::map<int, FeatureBA> > wordReferences;
-	return optimizeBA(rootId, poses, links, signatures, points3DMap, wordReferences, rematchFeatures);
+	return optimizeBA(rootId, poses, links, signatures, points3DMap, wordReferences, rematchFeatures, registrationParameters);
 }
 
 Transform Optimizer::optimizeBA(
@@ -557,11 +559,20 @@ void Optimizer::computeBACorrespondences(
 		std::map<int, cv::Point3f> & points3DMap,
 		std::map<int, std::map<int, FeatureBA> > & wordReferences,
 		bool rematchFeatures,
-		bool useLinkTransformAsGuess)
+		bool useLinkTransformAsGuess,
+		ParametersMap registrationParameters)
 {
 	UDEBUG("rematchFeatures=%d", rematchFeatures?1:0);
 	int wordCount = 0;
 	int edgeWithWordsAdded = 0;
+
+	// Some defaults if not provided
+	registrationParameters.insert(ParametersPair(Parameters::kVisEstimationType(), "1"));
+	registrationParameters.insert(ParametersPair(Parameters::kVisPnPReprojError(), "5"));
+	registrationParameters.insert(ParametersPair(Parameters::kVisMinInliers(), "6"));
+	registrationParameters.insert(ParametersPair(Parameters::kVisCorNNDR(), "0.6"));
+	RegistrationVis reg(registrationParameters);
+
 	std::map<int, std::map<cv::KeyPoint, int, KeyPointCompare> > frameToWordMap; // <FrameId, <Keypoint, wordId> >
 	for(std::multimap<int, Link>::const_iterator iter=links.lower_bound(1); iter!=links.end(); ++iter)
 	{
@@ -601,13 +612,6 @@ void Optimizer::computeBACorrespondences(
 					sTo.getWords().size() &&
 					sFrom.getWords3().size())
 				{
-					ParametersMap regParam;
-					regParam.insert(ParametersPair(Parameters::kVisEstimationType(), "1"));
-					regParam.insert(ParametersPair(Parameters::kVisPnPReprojError(), "5"));
-					regParam.insert(ParametersPair(Parameters::kVisMinInliers(), "6"));
-					regParam.insert(ParametersPair(Parameters::kVisCorNNDR(), "0.6"));
-					RegistrationVis reg(regParam);
-
 					if(!rematchFeatures)
 					{
 						sFrom.setWordsDescriptors(cv::Mat());

corelib/src/Rtabmap.cpp

@@ -152,6 +152,7 @@ Rtabmap::Rtabmap() :
 	_maxOdomCacheSize(Parameters::defaultRGBDMaxOdomCacheSize()),
 	_localizationSmoothing(Parameters::defaultRGBDLocalizationSmoothing()),
 	_localizationPriorInf(1.0/(Parameters::defaultRGBDLocalizationPriorError()*Parameters::defaultRGBDLocalizationPriorError())),
+	_localizationSecondTryWithoutProximityLinks(Parameters::defaultRGBDLocalizationSecondTryWithoutProximityLinks()),
 	_createGlobalScanMap(Parameters::defaultRGBDProximityGlobalScanMap()),
 	_markerPriorsLinearVariance(Parameters::defaultMarkerPriorsVarianceLinear()),
 	_markerPriorsAngularVariance(Parameters::defaultMarkerPriorsVarianceAngular()),
@@ -632,6 +633,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRGBDLocalizationPriorError(), localizationPriorError);
 	UASSERT(localizationPriorError>0.0);
 	_localizationPriorInf = 1.0/(localizationPriorError*localizationPriorError);
+	Parameters::parse(parameters, Parameters::kRGBDLocalizationSecondTryWithoutProximityLinks(), _localizationSecondTryWithoutProximityLinks);
 	Parameters::parse(parameters, Parameters::kRGBDProximityGlobalScanMap(), _createGlobalScanMap);
 
 	Parameters::parse(parameters, Parameters::kMarkerPriorsVarianceLinear(), _markerPriorsLinearVariance);
@@ -1699,7 +1701,7 @@ bool Rtabmap::process(
 			{
 				distanceToClosestNodeInTheGraph = sqrt(sqrdDistance);
 				UDEBUG("Last localization pose = %s, closest node=%d (%f m)", newPose.prettyPrint().c_str(), closestNode, distanceToClosestNodeInTheGraph);
-				angleToClosestNodeInTheGraph = (newPose.inverse() * _optimizedPoses.at(closestNode)).getAngle();
+				angleToClosestNodeInTheGraph = newPose.getAngle(_optimizedPoses.at(closestNode));
 			}
 		}
 
@@ -3181,6 +3183,7 @@ bool Rtabmap::process(
 	int optimizationIterations = 0;
 	Transform previousMapCorrection;
 	bool delayedLocalization = false;
+	int odomCacheProximityLinksCleared = 0;
 	UDEBUG("RGB-D SLAM mode: %d", _rgbdSlamMode?1:0);
 	UDEBUG("Incremental: %d", _memory->isIncremental());
 	UDEBUG("Loop hyp: %d", _loopClosureHypothesis.first);
@@ -3299,7 +3302,7 @@ bool Rtabmap::process(
 				if(!posesOut.empty() &&
 				   posesOut.begin()->first < _odomCachePoses.begin()->first)
 				{
-					optPoses = _graphOptimizer->optimize(posesOut.begin()->first, posesOut, edgeConstraintsOut, locOptCovariance);
+					optPoses = _graphOptimizer->optimize(posesOut.begin()->first, posesOut, edgeConstraintsOut, locOptCovariance, 0, &optimizationError, &optimizationIterations);
 				}
 				else
 				{
@@ -3424,7 +3427,8 @@ bool Rtabmap::process(
 				}
 
 				bool hasGlobalLoopClosuresOrLandmarks = false;
-				if(rejectLocalization && !graph::filterLinks(constraints, Link::kLocalSpaceClosure, true).empty())
+				if(rejectLocalization && 
+					(_localizationSecondTryWithoutProximityLinks && !graph::filterLinks(constraints, Link::kLocalSpaceClosure, true).empty()))
 				{
 					// Let's try again without local loop closures
 					localizationLinks = graph::filterLinks(localizationLinks, Link::kLocalSpaceClosure);
@@ -3448,7 +3452,7 @@ bool Rtabmap::process(
 						if(!posesOut.empty() &&
 						   posesOut.begin()->first < _odomCachePoses.begin()->first)
 						{
-							optPoses = _graphOptimizer->optimize(posesOut.begin()->first, posesOut, edgeConstraintsOut, locOptCovariance);
+							optPoses = _graphOptimizer->optimize(posesOut.begin()->first, posesOut, edgeConstraintsOut, locOptCovariance, 0, &optimizationError, &optimizationIterations);
 						}
 						else
 						{
@@ -3584,13 +3588,14 @@ bool Rtabmap::process(
 						_odomCacheConstraints = graph::filterLinks(_odomCacheConstraints, Link::kLocalSpaceClosure);
 						if(before != _odomCacheConstraints.size())
 						{
-							UWARN("Successfully optimized without local loop closures! Clear them from local odometry cache. %ld/%ld have been removed.",
+							UWARN("Successfully optimized without local loop closures! Clearing them from local odometry cache. %ld/%ld have been removed.",
 									before - _odomCacheConstraints.size(), before);
 						}
 						else
 						{
 							UWARN("Successfully optimized without local loop closures!");
 						}
+						odomCacheProximityLinksCleared = before - _odomCacheConstraints.size();
 					}
 
 					// Count how many localization links are in the constraints
@@ -3645,6 +3650,14 @@ bool Rtabmap::process(
 					if(hadAlreadyLocalizationLinks || _maxOdomCacheSize == 0)
 					{
 						UINFO("Update localization");
+
+						// update odomCachePoses with optimized poses (but make sure to put them back in odom frame)
+						Transform mapToOdomCache = signature->getPose() * newOptPoseInv;
+						for(std::map<int, Transform>::iterator iter = _odomCachePoses.begin(); iter!=_odomCachePoses.end(); ++iter)
+						{
+							iter->second = mapToOdomCache * optPoses.at(iter->first);
+						}
+
 						if(_optimizeFromGraphEnd)
 						{
 							// update all previous nodes
@@ -3941,7 +3954,7 @@ bool Rtabmap::process(
 			{
 				distanceToClosestNodeInTheGraph = _lastLocalizationPose.getDistance(_optimizedPoses.at(closestNode));
 				UDEBUG("Last localization pose = %s, updated closest node=%d (%f m)", _lastLocalizationPose.prettyPrint().c_str(), closestNode, distanceToClosestNodeInTheGraph);
-				angleToClosestNodeInTheGraph = (_lastLocalizationPose.inverse() * _optimizedPoses.at(closestNode)).getAngle();
+				angleToClosestNodeInTheGraph = _lastLocalizationPose.getAngle(_optimizedPoses.at(closestNode));
 			}
 		}
 		_lastLocalizationPose = _optimizedPoses.at(signature->id()); // update
@@ -4090,16 +4103,15 @@ bool Rtabmap::process(
 					if(!sLoop->getGroundTruthPose().isNull() && !signature->getGroundTruthPose().isNull())
 					{
 						Transform transformGT = sLoop->getGroundTruthPose().inverse() * signature->getGroundTruthPose();
-						Transform error = loopIter->second.transform().inverse() * transformGT;
-						statistics_.addStatistic(Statistics::kGtLocalization_linear_error(), error.getNorm());
-						statistics_.addStatistic(Statistics::kGtLocalization_angular_error(), error.getAngle(1,0,0)*180/M_PI);
+						statistics_.addStatistic(Statistics::kGtLocalization_linear_error(), loopIter->second.transform().getDistance(transformGT));
+						statistics_.addStatistic(Statistics::kGtLocalization_angular_error(), loopIter->second.transform().getAngle(transformGT)*180/M_PI);
 					}
 				}
 
 				_distanceTravelledSinceLastLocalization = 0.0f;
 
 				statistics_.addStatistic(Statistics::kLoopMapToOdom_norm(), _mapCorrection.getNorm());
-				statistics_.addStatistic(Statistics::kLoopMapToOdom_angle(), _mapCorrection.getAngle()*180.0f/M_PI);
+				statistics_.addStatistic(Statistics::kLoopMapToOdom_angle(), _mapCorrection.getAngle(Transform::getIdentity())*180.0f/M_PI);
 				_mapCorrection.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
 				statistics_.addStatistic(Statistics::kLoopMapToOdom_x(), x);
 				statistics_.addStatistic(Statistics::kLoopMapToOdom_y(), y);
@@ -4113,7 +4125,7 @@ bool Rtabmap::process(
 				{
 					Transform odomCorrection = (previousMapCorrection*odomPose).inverse()*_mapCorrection*odomPose;
 					statistics_.addStatistic(Statistics::kLoopOdom_correction_norm(), odomCorrection.getNorm());
-					statistics_.addStatistic(Statistics::kLoopOdom_correction_angle(), odomCorrection.getAngle()*180.0f/M_PI);
+					statistics_.addStatistic(Statistics::kLoopOdom_correction_angle(), odomCorrection.getAngle(Transform::getIdentity())*180.0f/M_PI);
 					odomCorrection.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
 					statistics_.addStatistic(Statistics::kLoopOdom_correction_x(), x);
 					statistics_.addStatistic(Statistics::kLoopOdom_correction_y(), y);
@@ -4134,6 +4146,10 @@ bool Rtabmap::process(
 				statistics_.addStatistic(Statistics::kLoopMapToBase_yaw(), yaw*180.0f/M_PI);
 				UINFO("Localization pose = %s", _lastLocalizationPose.prettyPrint().c_str());
 
+				if(_localizationSecondTryWithoutProximityLinks) {
+					statistics_.addStatistic(Statistics::kLoopProximity_links_cleared(), (float)odomCacheProximityLinksCleared);
+				}
+
 				if(_localizationCovariance.total()==36)
 				{
 					double varLin = _graphOptimizer->isSlam2d()?

corelib/src/SensorData.cpp

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

corelib/src/Transform.cpp

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

corelib/src/odometry/OdometryDVO.cpp

@@ -217,7 +217,7 @@ Transform OdometryDVO::computeTransform(
 			t = motionFromKeyFrame_.inverse() * t;
 
 			// TODO make parameters?
-			if(currentMotion.getNorm() > 0.01 || currentMotion.getAngle() > 0.01)
+			if(currentMotion.getNorm() > 0.01 || currentMotion.getAngle(Transform::getIdentity()) > 0.01)
 			{
 				if(info)
 				{

corelib/src/python/rtabmap_superpoint.py

@@ -2,7 +2,7 @@
 #
 # Drop this file in the root folder of SuperPoint git: https://github.com/magicleap/SuperPointPretrainedNetwork
 # To use with rtabmap:
-#   --Vis/FeatureType 15 --PyDetector/Path "~/SuperPointPretrainedNetwork/rtabmap_superpoint.py" --PyDetector/Model "~/SuperPointPretrainedNetwork/superpoint_v1.pth"
+#   --Vis/FeatureType 15 --Kp/DetectorStrategy 15 --PyDetector/Path "~/SuperPointPretrainedNetwork/rtabmap_superpoint.py"
 #
 
 import random

corelib/src/resources/DatabaseSchema.sql.in

@@ -32,6 +32,7 @@ CREATE TABLE Data (
 	id INTEGER NOT NULL,
 	image BLOB,               -- compressed image (Grayscale or RGB)
 	depth BLOB,               -- compressed image (Depth or Right image)
+	depth_confidence BLOB,    -- compressed data (low=0 high=100)
 	calibration BLOB,         -- fx, fy, cx, cy, [baseline,] width, height, local_transform
 	
 	scan BLOB,                -- compressed data (Laser scan)

corelib/src/resources/backward_compatibility/DatabaseSchema_0_20_0.sql

@@ -0,0 +1,182 @@
+-- *******************************************************************
+--  DatabaseSchema: Script for creating the database
+--   Usage:
+--       $ sqlite3 LTM.db < DatabaseSchema.sql
+--
+-- *******************************************************************
+
+-- *******************************************************************
+-- CLEAN
+-- *******************************************************************
+/*DROP TABLE Node;*/
+
+-- *******************************************************************
+-- CREATE
+-- *******************************************************************
+CREATE TABLE Node (
+	id INTEGER NOT NULL,
+	map_id INTEGER NOT NULL,
+	weight INTEGER,
+	stamp FLOAT,
+	pose BLOB,                -- 3x4 float
+	ground_truth_pose BLOB,   -- 3x4 float
+	velocity BLOB,            -- 6 float (vx,vy,vz,vroll,vpitch,vyaw) m/s and rad/s
+	label TEXT,
+	gps BLOB,                 -- 1x6 double: stamp, longitude (DD), latitude (DD), altitude (m), accuracy (m), bearing (North 0->360 deg clockwise)
+	env_sensors BLOB,         -- Variable 3xdouble: (sensorId1, value, stamp, sensorId2, value, stamp, ...)
+	time_enter DATE,
+	PRIMARY KEY (id)
+);
+
+CREATE TABLE Data (
+	id INTEGER NOT NULL,
+	image BLOB,               -- compressed image (Grayscale or RGB)
+	depth BLOB,               -- compressed image (Depth or Right image)
+	calibration BLOB,         -- fx, fy, cx, cy, [baseline,] width, height, local_transform
+	
+	scan BLOB,                -- compressed data (Laser scan)
+	scan_info BLOB,           -- scan_max_pts, scan_max_range, scan_format, local_transform
+	
+	ground_cells BLOB,        -- compressed data (occupancy grid)
+	obstacle_cells BLOB,      -- compressed data (occupancy grid)
+	empty_cells BLOB,         -- compressed data (occupancy grid)
+	cell_size FLOAT,
+	view_point_x FLOAT,
+	view_point_y FLOAT,
+	view_point_z FLOAT,
+	
+	user_data BLOB,           -- compressed data (User data)
+	time_enter DATE,
+	PRIMARY KEY (id)
+);
+
+CREATE TABLE Link (
+	from_id INTEGER NOT NULL,
+	to_id INTEGER NOT NULL,
+	type INTEGER NOT NULL,    -- kNeighbor=0, kGlobalClosure=1, kLocalSpaceClosure=2, kLocalTimeClosure=3, kUserClosure=4, kVirtualClosure=5, kNeighborMerged=6, kPosePrior=7, kLandmark=8
+	information_matrix BLOB NOT NULL, -- 6x6 double (inverse covariance)
+	transform BLOB,           -- 3x4 float
+	user_data BLOB,           -- compressed data (User data)
+	FOREIGN KEY (from_id) REFERENCES Node(id),
+	FOREIGN KEY (to_id) REFERENCES Node(id)
+);
+
+-- 
+CREATE TABLE Word (
+	id INTEGER NOT NULL,
+	descriptor_size INTEGER NOT NULL,
+	descriptor BLOB NOT NULL,
+	time_enter DATE,
+	PRIMARY KEY (id)
+);
+
+CREATE TABLE Feature (
+	node_id INTEGER NOT NULL,
+	word_id INTEGER NOT NULL,
+	pos_x FLOAT NOT NULL,
+	pos_y FLOAT NOT NULL,
+	size INTEGER NOT NULL,
+	dir FLOAT NOT NULL,
+	response FLOAT NOT NULL,
+	octave INTEGER NOT NULL,
+	depth_x FLOAT,
+	depth_y FLOAT,
+	depth_z FLOAT,
+	descriptor_size INTEGER,
+	descriptor BLOB,
+	FOREIGN KEY (node_id) REFERENCES Node(id)
+);
+
+CREATE TABLE GlobalDescriptor (
+	node_id INTEGER NOT NULL,
+	type INTEGER NOT NULL,
+	info BLOB,
+	data BLOB NOT NULL,
+	FOREIGN KEY (node_id) REFERENCES Node(id)
+);
+
+--
+
+CREATE TABLE Info (
+	STM_size INTEGER,
+	last_sign_added INTEGER,
+	process_mem_used INTEGER,
+	database_mem_used INTEGER,
+	dictionary_size INTEGER,
+	parameters TEXT,
+	time_enter DATE
+);
+
+CREATE TABLE Statistics (
+	id INTEGER NOT NULL,
+	stamp FLOAT,
+	data BLOB,              -- compressed string
+	wm_state BLOB,	        -- compressed data
+	FOREIGN KEY (id) REFERENCES Node(id)
+);
+
+CREATE TABLE Admin (
+	version TEXT,
+	preview_image BLOB,      -- compressed image
+	
+	opt_cloud BLOB,          -- compressed data
+	opt_ids BLOB,            -- Node ids used to generate the optimized cloud/mesh
+	opt_poses BLOB,          -- compressed N*3x4 float 
+	opt_last_localization BLOB, -- 3x4 float
+	opt_polygons_size INTEGER, -- e.g., 3
+	opt_polygons BLOB,       -- compressed data [length_v0, i0,i1,i3, length_v1, i0,i1,i3]
+	opt_tex_coords BLOB,     -- compressed data [length_v0, u0,v0,u1,v1,u2,v2, length_v1, u0,v0,u1,v1,u2,v2]
+	opt_tex_materials BLOB,  -- compressed image
+	opt_map BLOB,            -- compressed CV_8SC1 occupancy grid
+	opt_map_x_min FLOAT,
+	opt_map_y_min FLOAT, 
+	opt_map_resolution FLOAT, 
+
+	time_enter DATE
+);
+
+-- *******************************************************************
+-- TRIGGERS
+-- *******************************************************************
+CREATE TRIGGER insert_Feature BEFORE INSERT ON Feature 
+WHEN NOT EXISTS (SELECT Node.id FROM Node WHERE Node.id = NEW.node_id)
+BEGIN
+ SELECT RAISE(ABORT, 'Foreign key constraint failed in Feature table');
+END;
+
+ --   Creating a trigger for time_enter
+CREATE TRIGGER insert_Node_timeEnter AFTER INSERT ON Node
+BEGIN
+ UPDATE Node SET time_enter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+CREATE TRIGGER insert_Data_timeEnter AFTER INSERT ON Data
+BEGIN
+ UPDATE Node SET time_enter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+CREATE TRIGGER insert_Word_timeEnter AFTER INSERT ON Word
+BEGIN
+ UPDATE Word SET time_enter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+CREATE TRIGGER insert_Info_timeEnter AFTER INSERT ON Info
+BEGIN
+ UPDATE Info SET time_enter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+-- *******************************************************************
+-- INDEXES
+-- *******************************************************************
+CREATE UNIQUE INDEX IDX_Node_id on Node (id);
+CREATE INDEX IDX_Feature_node_id on Feature (node_id);
+CREATE INDEX IDX_GlobalDescriptor_node_id on GlobalDescriptor (node_id);
+CREATE INDEX IDX_Link_from_id on Link (from_id);
+CREATE UNIQUE INDEX IDX_node_label on Node (label);
+CREATE UNIQUE INDEX IDX_Statistics_id on Statistics (id);
+
+-- *******************************************************************
+-- VERSION
+-- *******************************************************************
+INSERT INTO Admin(version) VALUES('0.20.0');
+

corelib/src/superpoint_torch/SuperPoint.cc

@@ -242,8 +242,7 @@ cv::Mat SPDetector::compute(const std::vector<cv::KeyPoint> &keypoints)
 		auto desc = torch::grid_sampler(desc_, grid, 0, 0, true);  // [1, 256, 1, n_keypoints]
 
 		// normalize to 1
-		desc = torch::nn::functional::normalize(desc.reshape({1, desc_.size(1), -1})); //[1, 256, n_keypoints]
-		desc = desc.squeeze(); //[256, n_keypoints]
+		desc = torch::nn::functional::normalize(desc.reshape({desc_.size(1), -1}), torch::nn::functional::NormalizeFuncOptions().dim(0)); //[256, n_keypoints]
 		desc = desc.transpose(0, 1).contiguous(); //[n_keypoints, 256]
 
 		if(cuda_)

corelib/src/util2d.cpp

@@ -1359,18 +1359,38 @@ cv::Mat interpolate(const cv::Mat & image, int factor, float depthErrorRatio)
 }
 
 // Registration Depth to RGB (return registered depth image)
+cv::Mat registerDepth(
+	const cv::Mat & depth,
+	const cv::Mat & depthK,
+	const cv::Size & colorSize,
+	const cv::Mat & colorK,
+	const rtabmap::Transform & transform)
+{
+	cv::Mat tmp;
+	return registerDepth(
+		depth,
+		cv::Mat(),
+		depthK,
+		colorSize,
+		colorK,
+		transform,
+		tmp);
+}
 cv::Mat registerDepth(
 		const cv::Mat & depth,
+		const cv::Mat & confidence,
 		const cv::Mat & depthK,
 		const cv::Size & colorSize,
 		const cv::Mat & colorK,
-		const rtabmap::Transform & transform)
+		const rtabmap::Transform & transform,
+		cv::Mat & registeredConfidence)
 {
 	UASSERT(!transform.isNull());
 	UASSERT(!depth.empty());
 	UASSERT(depth.type() == CV_16UC1 || depth.type() == CV_32FC1); // mm or m
 	UASSERT(depthK.type() == CV_64FC1 && depthK.cols == 3 && depthK.cols == 3);
 	UASSERT(colorK.type() == CV_64FC1 && colorK.cols == 3 && colorK.cols == 3);
+	UASSERT(confidence.empty() || (confidence.size() == depth.size() && confidence.type()==CV_8UC1));
 
 	float fx = depthK.at<double>(0,0);
 	float fy = depthK.at<double>(1,1);
@@ -1389,10 +1409,19 @@ cv::Mat registerDepth(
 	Eigen::Vector4f P4,P3;
 	P4[3] = 1;
 	cv::Mat registered = cv::Mat::zeros(colorSize, depth.type());
+	registeredConfidence = cv::Mat();
+	if(!confidence.empty())
+	{
+		registeredConfidence = cv::Mat::zeros(colorSize, confidence.type());
+	}
 
 	bool depthInMM = depth.type() == CV_16UC1;
 	for(int y=0; y<depth.rows; ++y)
 	{
+		const unsigned char * confPtr = 0;
+		if(!confidence.empty()) {
+			confPtr = confidence.ptr<unsigned char>(y);
+		}
 		for(int x=0; x<depth.cols; ++x)
 		{
 			//filtering
@@ -1419,6 +1448,9 @@ cv::Mat registerDepth(
 						if(zReg == 0 || z16 < zReg)
 						{
 							zReg = z16;
+							if(confPtr) {
+								registeredConfidence.at<unsigned char>(dy, dx) = confPtr[x];
+							}
 						}
 					}
 					else
@@ -1427,6 +1459,9 @@ cv::Mat registerDepth(
 						if(zReg == 0 || z < zReg)
 						{
 							zReg = z;
+							if(confPtr) {
+								registeredConfidence.at<unsigned char>(dy, dx) = confPtr[x];
+							}
 						}
 					}
 				}
@@ -1931,6 +1966,61 @@ cv::Mat fastBilateralFiltering(const cv::Mat & depth, float sigmaS, float sigmaR
 	return output;
 }
 
+void depthBleedingFiltering(cv::Mat & depth, float maxDepthError)
+{
+	if(depth.empty())
+	{
+		return;
+	}
+	UASSERT(depth.type() == CV_32FC1 || depth.type() == CV_16UC1);
+
+	// ignore border
+	depth.row(0).setTo(cv::Scalar(0));
+	depth.row(depth.rows-1).setTo(cv::Scalar(0));
+	depth.col(0).setTo(cv::Scalar(0));
+	depth.col(depth.cols-1).setTo(cv::Scalar(0));
+
+	if(depth.type() == CV_32FC1)
+	{
+		float * depthPtr = depth.ptr<float>();
+		for(int v=1; v<depth.rows-1; ++v)
+		{
+			for(int u=1; u<depth.cols-1; ++u)
+			{
+				int row = depth.cols*v;
+				float & ref  = depthPtr[row + u];
+				if((fabs(ref - depthPtr[row + u - 1]) > maxDepthError && 
+				    fabs(ref - depthPtr[row + u + 1]) > maxDepthError) || 
+				   (fabs(ref - depthPtr[depth.cols*(v-1) + u]) > maxDepthError && 
+				    fabs(ref - depthPtr[depth.cols*(v+1) + u]) > maxDepthError)) 
+				{
+					ref = 0.0f;
+				} 
+			}
+		}
+	}
+	else if(depth.type() == CV_16UC1)
+	{
+		unsigned short * depthPtr = depth.ptr<unsigned short>();
+		unsigned short maxDepthErrorMM = (unsigned short)(maxDepthError*1000.0f);
+		for(int v=1; v<depth.rows-1; ++v)
+		{
+			for(int u=1; u<depth.cols-1; ++u)
+			{
+				int row = depth.cols*v;
+				unsigned short & ref  = depthPtr[row + u];
+				if((abs((int)ref - (int)depthPtr[row + u - 1]) > maxDepthErrorMM && 
+				    abs((int)ref - (int)depthPtr[row + u + 1]) > maxDepthErrorMM) || 
+				   (abs((int)ref - (int)depthPtr[depth.cols*(v-1) + u]) > maxDepthErrorMM && 
+				    abs((int)ref - (int)depthPtr[depth.cols*(v+1) + u]) > maxDepthErrorMM)) 
+				{
+					ref = 0;
+				} 
+			}
+		}
+	}
+}
+
 /**
  *  \brief Automatic brightness and contrast optimization with optional histogram clipping
  *  \param [in]src Input image GRAY or BGR or BGRA

corelib/src/util3d.cpp

@@ -276,12 +276,33 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
 	return cloudFromDepth(imageDepth, model, decimation, maxDepth, minDepth, validIndices);
 }
 
+pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
+	const cv::Mat & imageDepthIn,
+	const CameraModel & model,
+	int decimation,
+	float maxDepth,
+	float minDepth,
+	std::vector<int> * validIndices)
+{
+	return cloudFromDepth(
+		imageDepthIn,
+		cv::Mat(),
+		model,
+		decimation,
+		maxDepth,
+		minDepth,
+		0,
+		validIndices);
+}
+
 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
 		const cv::Mat & imageDepthIn,
+		const cv::Mat & imageDepthConfidenceIn,
 		const CameraModel & model,
 		int decimation,
 		float maxDepth,
 		float minDepth,
+		unsigned char confidenceThr,
 		std::vector<int> * validIndices)
 {
 	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
@@ -294,8 +315,10 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
 
 	UASSERT(model.isValidForProjection());
 	UASSERT(!imageDepthIn.empty() && (imageDepthIn.type() == CV_16UC1 || imageDepthIn.type() == CV_32FC1));
+	UASSERT(imageDepthConfidenceIn.empty() || confidenceThr == 0 || (imageDepthConfidenceIn.type() == CV_8UC1 && imageDepthConfidenceIn.size() == imageDepthIn.size()));
 
 	cv::Mat imageDepth = imageDepthIn;
+	cv::Mat imageDepthConfidence = confidenceThr==0?cv::Mat():imageDepthConfidenceIn;
 	if(model.imageHeight()>0 && model.imageWidth()>0)
 	{
 		UASSERT(model.imageHeight() % imageDepthIn.rows == 0 && model.imageWidth() % imageDepthIn.cols == 0);
@@ -322,6 +345,9 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
 			{
 				UDEBUG("Depth interpolation factor=%d", targetSize/imageDepthIn.rows);
 				imageDepth = util2d::interpolate(imageDepthIn, targetSize/imageDepthIn.rows);
+				if(!imageDepthConfidence.empty()) {
+					imageDepthConfidence = util2d::interpolate(imageDepthConfidenceIn, targetSize/imageDepthConfidenceIn.rows);
+				}
 				decimation = 1;
 			}
 			else if(targetSize == imageDepthIn.rows)
@@ -373,11 +399,13 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
 	float depthCx = model.cx() * rgbToDepthFactorX;
 	float depthCy = model.cy() * rgbToDepthFactorY;
 
-	UDEBUG("depth=%dx%d fx=%f fy=%f cx=%f cy=%f (depth factors=%f %f) decimation=%d",
+	UDEBUG("depth=%dx%d fx=%f fy=%f cx=%f cy=%f (depth factors=%f %f) has confidence=%d (thr=%d) decimation=%d",
 			imageDepth.cols, imageDepth.rows,
 			model.fx(), model.fy(), model.cx(), model.cy(),
 			rgbToDepthFactorX,
 			rgbToDepthFactorY,
+			imageDepthConfidenceIn.empty()?0:1,
+			(int)confidenceThr,
 			decimation);
 
 	int oi = 0;
@@ -387,21 +415,21 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
 		{
 			pcl::PointXYZ & pt = cloud->at((h/decimation)*cloud->width + (w/decimation));
 
-			pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, depthCx, depthCy, depthFx, depthFy, false);
-			if(pcl::isFinite(ptXYZ) && ptXYZ.z>=minDepth && (maxDepth<=0.0f || ptXYZ.z <= maxDepth))
+			pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
+			if(imageDepthConfidence.empty() || imageDepthConfidence.at<unsigned char>(h,w) >= confidenceThr)
 			{
-				pt.x = ptXYZ.x;
-				pt.y = ptXYZ.y;
-				pt.z = ptXYZ.z;
-				if(validIndices)
+				pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, depthCx, depthCy, depthFx, depthFy, false);
+				if(pcl::isFinite(ptXYZ) && ptXYZ.z>=minDepth && (maxDepth<=0.0f || ptXYZ.z <= maxDepth))
 				{
-					validIndices->at(oi++) = (h/decimation)*cloud->width + (w/decimation);
+					pt.x = ptXYZ.x;
+					pt.y = ptXYZ.y;
+					pt.z = ptXYZ.z;
+					if(validIndices)
+					{
+						validIndices->at(oi++) = (h/decimation)*cloud->width + (w/decimation);
+					}
 				}
 			}
-			else
-			{
-				pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
-			}
 		}
 	}
 
@@ -427,13 +455,36 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
 	return cloudFromDepthRGB(imageRgb, imageDepth, model, decimation, maxDepth, minDepth, validIndices);
 }
 
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
+	const cv::Mat & imageRgb,
+	const cv::Mat & imageDepthIn,
+	const CameraModel & model,
+	int decimation,
+	float maxDepth,
+	float minDepth,
+	std::vector<int> * validIndices)
+{
+	return cloudFromDepthRGB(
+		imageRgb,
+		imageDepthIn,
+		cv::Mat(),
+		model,
+		decimation,
+		maxDepth,
+		minDepth,
+		0,
+		validIndices);
+}
+
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
 		const cv::Mat & imageRgb,
 		const cv::Mat & imageDepthIn,
+		const cv::Mat & imageDepthConfidenceIn,
 		const CameraModel & model,
 		int decimation,
 		float maxDepth,
 		float minDepth,
+		unsigned char confidenceThr,
 		std::vector<int> * validIndices)
 {
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
@@ -449,6 +500,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
 	//UASSERT_MSG(imageRgb.rows % imageDepthIn.rows == 0 && imageRgb.cols % imageDepthIn.cols == 0,
 	//		uFormat("rgb=%dx%d depth=%dx%d", imageRgb.cols, imageRgb.rows, imageDepthIn.cols, imageDepthIn.rows).c_str());
 	UASSERT(!imageDepthIn.empty() && (imageDepthIn.type() == CV_16UC1 || imageDepthIn.type() == CV_32FC1));
+	UASSERT(imageDepthConfidenceIn.empty() || confidenceThr==0 || (imageDepthConfidenceIn.type() == CV_8UC1 && imageDepthConfidenceIn.size() == imageDepthIn.size()));
 	if(decimation < 0)
 	{
 		if(imageRgb.rows % decimation != 0 || imageRgb.cols % decimation != 0)
@@ -491,6 +543,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
 	}
 
 	cv::Mat imageDepth = imageDepthIn;
+	cv::Mat imageDepthConfidence = confidenceThr==0?cv::Mat():imageDepthConfidenceIn;
 	if(decimation < 0)
 	{
 		UDEBUG("Decimation from RGB image (%d)", decimation);
@@ -502,6 +555,9 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
 		{
 			UDEBUG("Depth interpolation factor=%d", targetSize/imageDepthIn.rows);
 			imageDepth = util2d::interpolate(imageDepthIn, targetSize/imageDepthIn.rows);
+			if(!imageDepthConfidence.empty()) {
+				imageDepthConfidence = util2d::interpolate(imageDepthConfidenceIn, targetSize/imageDepthConfidenceIn.rows);
+			}
 			decimation = 1;
 		}
 		else if(targetSize == imageDepthIn.rows)
@@ -546,12 +602,14 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
 	float depthCx = model.cx() / rgbToDepthFactorX;
 	float depthCy = model.cy() / rgbToDepthFactorY;
 
-	UDEBUG("rgb=%dx%d depth=%dx%d fx=%f fy=%f cx=%f cy=%f (depth factors=%f %f) decimation=%d",
+	UDEBUG("rgb=%dx%d depth=%dx%d fx=%f fy=%f cx=%f cy=%f (depth factors=%f %f) has confidence=%d (thr=%d) decimation=%d",
 			imageRgb.cols, imageRgb.rows,
 			imageDepth.cols, imageDepth.rows,
 			model.fx(), model.fy(), model.cx(), model.cy(),
 			rgbToDepthFactorX,
 			rgbToDepthFactorY,
+			imageDepthConfidenceIn.empty()?0:1,
+			(int)confidenceThr,
 			decimation);
 
 	int oi = 0;
@@ -579,21 +637,21 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
 				pt.r = v;
 			}
 
-			pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, depthCx, depthCy, depthFx, depthFy, false);
-			if (pcl::isFinite(ptXYZ) && ptXYZ.z >= minDepth && (maxDepth <= 0.0f || ptXYZ.z <= maxDepth))
+			pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
+			if(imageDepthConfidence.empty() || imageDepthConfidence.at<unsigned char>(h,w) >= confidenceThr)
 			{
-				pt.x = ptXYZ.x;
-				pt.y = ptXYZ.y;
-				pt.z = ptXYZ.z;
-				if (validIndices)
+				pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, depthCx, depthCy, depthFx, depthFy, false);
+				if (pcl::isFinite(ptXYZ) && ptXYZ.z >= minDepth && (maxDepth <= 0.0f || ptXYZ.z <= maxDepth))
 				{
-					validIndices->at(oi) = (h / decimation)*cloud->width + (w / decimation);
+					pt.x = ptXYZ.x;
+					pt.y = ptXYZ.y;
+					pt.z = ptXYZ.z;
+					if (validIndices)
+					{
+						validIndices->at(oi) = (h / decimation)*cloud->width + (w / decimation);
+					}
+					++oi;
 				}
-				++oi;
-			}
-			else
-			{
-				pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
 			}
 		}
 	}
@@ -868,7 +926,8 @@ std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloudsFromSensorData(
 		float minDepth,
 		std::vector<pcl::IndicesPtr> * validIndices,
 		const ParametersMap & stereoParameters,
-		const std::vector<float> & roiRatios)
+		const std::vector<float> & roiRatios,
+		unsigned char confidenceThr)
 {
 	if(decimation == 0)
 	{
@@ -881,6 +940,7 @@ std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloudsFromSensorData(
 	{
 		//depth
 		UASSERT(int((sensorData.depthRaw().cols/sensorData.cameraModels().size())*sensorData.cameraModels().size()) == sensorData.depthRaw().cols);
+		UASSERT(sensorData.depthConfidenceRaw().empty() || confidenceThr==0 || (sensorData.depthConfidenceRaw().type() == CV_8UC1 && sensorData.depthConfidenceRaw().cols == sensorData.depthRaw().cols && sensorData.depthConfidenceRaw().rows == sensorData.depthRaw().rows));
 		int subImageWidth = sensorData.depthRaw().cols/sensorData.cameraModels().size();
 		for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
 		{
@@ -892,6 +952,10 @@ std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloudsFromSensorData(
 			if(sensorData.cameraModels()[i].isValidForProjection())
 			{
 				cv::Mat depth = cv::Mat(sensorData.depthRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.depthRaw().rows));
+				cv::Mat depthConfidence;
+				if(!sensorData.depthConfidenceRaw().empty() && confidenceThr > 0) {
+					depthConfidence = cv::Mat(sensorData.depthConfidenceRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.depthConfidenceRaw().rows));
+				}
 				CameraModel model = sensorData.cameraModels()[i];
 				if( roiRatios.size() == 4 &&
 					(roiRatios[0] > 0.0f ||
@@ -912,6 +976,9 @@ std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloudsFromSensorData(
 							roiRgb.height%decimation==0)))
 					{
 						depth = cv::Mat(depth, roiDepth);
+						if(!depthConfidence.empty()) {
+							depthConfidence = cv::Mat(depthConfidence, roiDepth);
+						}
 						if(model.imageWidth() != 0 && model.imageHeight() != 0)
 						{
 							model = model.roi(util2d::computeRoi(model.imageSize(), roiRatios));
@@ -940,10 +1007,12 @@ std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloudsFromSensorData(
 
 				pcl::PointCloud<pcl::PointXYZ>::Ptr tmp = util3d::cloudFromDepth(
 						depth,
+						depthConfidence,
 						model,
 						decimation,
 						maxDepth,
 						minDepth,
+						confidenceThr,
 						validIndices?validIndices->back().get():0);
 
 				if(tmp->size())
@@ -1064,7 +1133,8 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromSensorData(
 		float minDepth,
 		std::vector<int> * validIndices,
 		const ParametersMap & stereoParameters,
-		const std::vector<float> & roiRatios)
+		const std::vector<float> & roiRatios,
+		unsigned char confidenceThr)
 {
 	std::vector<pcl::IndicesPtr> validIndicesV;
 	std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clouds = cloudsFromSensorData(
@@ -1074,7 +1144,8 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromSensorData(
 		minDepth,
 		validIndices?&validIndicesV:0,
 		stereoParameters,
-		roiRatios);
+		roiRatios,
+		confidenceThr);
 
 	if(validIndices)
 	{
@@ -1117,7 +1188,8 @@ std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> cloudsRGBFromSensorData(
 		float minDepth,
 		std::vector<pcl::IndicesPtr> * validIndices,
 		const ParametersMap & stereoParameters,
-		const std::vector<float> & roiRatios)
+		const std::vector<float> & roiRatios,
+		unsigned char confidenceThr)
 {
 	if(decimation == 0)
 	{
@@ -1136,6 +1208,7 @@ std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> cloudsRGBFromSensorData(
 		//UASSERT_MSG(sensorData.imageRaw().rows % sensorData.depthRaw().rows == 0, uFormat("rgb=%d depth=%d", sensorData.imageRaw().rows, sensorData.depthRaw().rows).c_str());
 		int subRGBWidth = sensorData.imageRaw().cols/sensorData.cameraModels().size();
 		int subDepthWidth = sensorData.depthRaw().cols/sensorData.cameraModels().size();
+		UASSERT(sensorData.depthConfidenceRaw().empty() || confidenceThr==0 || (sensorData.depthConfidenceRaw().type() == CV_8UC1 && sensorData.depthConfidenceRaw().cols == sensorData.depthRaw().cols && sensorData.depthConfidenceRaw().rows == sensorData.depthRaw().rows));
 
 		for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
 		{
@@ -1148,6 +1221,10 @@ std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> cloudsRGBFromSensorData(
 			{
 				cv::Mat rgb(sensorData.imageRaw(), cv::Rect(subRGBWidth*i, 0, subRGBWidth, sensorData.imageRaw().rows));
 				cv::Mat depth(sensorData.depthRaw(), cv::Rect(subDepthWidth*i, 0, subDepthWidth, sensorData.depthRaw().rows));
+				cv::Mat depthConfidence;
+				if(!sensorData.depthConfidenceRaw().empty() && confidenceThr>0) {
+					depthConfidence = cv::Mat(sensorData.depthConfidenceRaw(), cv::Rect(subDepthWidth*i, 0, subDepthWidth, sensorData.depthConfidenceRaw().rows));
+				}
 				CameraModel model = sensorData.cameraModels()[i];
 				if( roiRatios.size() == 4 &&
 					((roiRatios[0] > 0.0f && roiRatios[0] <= 1.0f) ||
@@ -1163,6 +1240,9 @@ std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> cloudsRGBFromSensorData(
 						roiRgb.height%decimation==0)
 					{
 						depth = cv::Mat(depth, roiDepth);
+						if(!depthConfidence.empty()) {
+							depthConfidence = cv::Mat(depthConfidence, roiDepth);
+						}
 						rgb = cv::Mat(rgb, roiRgb);
 						model = model.roi(roiRgb);
 					}
@@ -1186,10 +1266,12 @@ std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> cloudsRGBFromSensorData(
 				pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp = util3d::cloudFromDepthRGB(
 						rgb,
 						depth,
+						depthConfidence,
 						model,
 						decimation,
 						maxDepth,
 						minDepth,
+						confidenceThr,
 						validIndices?validIndices->back().get():0);
 
 				if(tmp->size())
@@ -1292,7 +1374,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGBFromSensorData(
 		float minDepth,
 		std::vector<int> * validIndices,
 		const ParametersMap & stereoParameters,
-		const std::vector<float> & roiRatios)
+		const std::vector<float> & roiRatios,
+		unsigned char confidenceThr)
 {
 	std::vector<pcl::IndicesPtr> validIndicesV;
 	std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds = cloudsRGBFromSensorData(
@@ -1302,7 +1385,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGBFromSensorData(
 		minDepth,
 		validIndices?&validIndicesV:0,
 		stereoParameters,
-		roiRatios);
+		roiRatios,
+		confidenceThr);
 
 	if(validIndices)
 	{
@@ -3149,6 +3233,18 @@ public:
 	float distance;
 };
 
+class RegisteredPoints {
+	public:
+		class Point {
+			public:
+				Point(float distance_, int index_) : distance(distance_), index(index_) {}
+				float distance;
+				int index;
+		};
+		float minDistance;
+		std::vector<Point> points;
+};
+
 /**
  * For each point, return pixel of the best camera (NodeID->CameraIndex)
  * looking at it based on the policy and parameters
@@ -3160,6 +3256,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 		const std::map<int, std::vector<CameraModel> > & cameraModels,
 		float maxDistance,
 		float maxAngle,
+		float maxDepthError,
 		const std::vector<float> & roiRatios,
 		const cv::Mat & projMask,
 		bool distanceToCamPolicy,
@@ -3170,6 +3267,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 	UINFO("cameraModels=%d", (int)cameraModels.size());
 	UINFO("maxDistance=%f", maxDistance);
 	UINFO("maxAngle=%f", maxAngle);
+	UINFO("maxDepthError=%f", maxDepthError);
 	UINFO("distanceToCamPolicy=%s", distanceToCamPolicy?"true":"false");
 	UINFO("roiRatios=%s", roiRatios.size() == 4?uFormat("%f %f %f %f", roiRatios[0], roiRatios[1], roiRatios[2], roiRatios[3]).c_str():"");
 	UINFO("projMask=%dx%d", projMask.cols, projMask.rows);
@@ -3235,8 +3333,9 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 				float cx = cameraMatrixK.at<double>(0,2);
 				float cy = cameraMatrixK.at<double>(1,2);
 
-				// depth: 2 channels UINT: [depthMM, indexPt]
-				cv::Mat registered = cv::Mat::zeros(imageSize, CV_32SC2);
+				// [rows][cols][depth, indexPt]
+				std::vector<std::vector<RegisteredPoints> > registered(
+					imageSize.height, std::vector<RegisteredPoints>(imageSize.width));
 				Transform t = cameraTransform.inverse();
 
 				cv::Rect roi(0,0,imageSize.width, imageSize.height);
@@ -3258,34 +3357,44 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 					if(z > 0.0f && (maxDistance<=0 || z<maxDistance))
 					{
 						float invZ = 1.0f/z;
-						float dx = (fx*ptScan.x)*invZ + cx;
-						float dy = (fy*ptScan.y)*invZ + cy;
-						int dx_low = dx;
-						int dy_low = dy;
-						int dx_high = dx + 0.5f;
-						int dy_high = dy + 0.5f;
-						int zMM = z * 1000;
-						if(uIsInBounds(dx_low, roi.x, roi.x+roi.width) && uIsInBounds(dy_low, roi.y, roi.y+roi.height) &&
-						   (validProjMask.empty() || validProjMask.at<unsigned char>(dy_low, imageSize.width*camIndex+dx_low) > 0))
-						{
-							set = true;
-							cv::Vec2i &zReg = registered.at<cv::Vec2i>(dy_low, dx_low);
-							if(zReg[0] == 0 || zMM < zReg[0])
-							{
-								zReg[0] = zMM;
-								zReg[1] = i;
+						float u = (fx*ptScan.x)*invZ + cx;
+						float v = (fy*ptScan.y)*invZ + cy;
+						int x = u + 0.5f;
+						int y = v + 0.5f;
+
+						if(uIsInBounds(x, roi.x, roi.x+roi.width) && uIsInBounds(y, roi.y, roi.y+roi.height) &&
+						   (validProjMask.empty() || validProjMask.at<unsigned char>(y, imageSize.width*camIndex+x) > 0)) {
+							RegisteredPoints &zReg = registered[y][x];
+							if(zReg.points.empty()) {
+								zReg.minDistance = z;
+								zReg.points.push_back(RegisteredPoints::Point(z, i));
+								set = true;
 							}
-						}
-						if((dx_low != dx_high || dy_low != dy_high) &&
-							uIsInBounds(dx_high, roi.x, roi.x+roi.width) && uIsInBounds(dy_high, roi.y, roi.y+roi.height) &&
-							(validProjMask.empty() || validProjMask.at<unsigned char>(dy_high, imageSize.width*camIndex+dx_high) > 0))
-						{
-							set = true;
-							cv::Vec2i &zReg = registered.at<cv::Vec2i>(dy_high, dx_high);
-							if(zReg[0] == 0 || zMM < zReg[0])
-							{
-								zReg[0] = zMM;
-								zReg[1] = i;
+							else if(z < zReg.minDistance) {
+								zReg.minDistance = z;
+								if(maxDepthError<=0.0f) {
+									// keeping only closest point, just update it
+									zReg.points[0].distance = z;
+									zReg.points[0].index = i;
+								}
+								else {
+									// update the points attached to same pixel based on new closest distance
+									std::vector<RegisteredPoints::Point> reOrderedPts;
+									reOrderedPts.push_back(RegisteredPoints::Point(z, i));
+									for(size_t p=0; p<zReg.points.size(); ++p) {
+										if(zReg.points[p].distance - z < maxDepthError) {
+											reOrderedPts.push_back(zReg.points[p]);
+										}
+									}
+									zReg.points = reOrderedPts;
+								}
+								set = true;
+							}
+							else if(maxDepthError>=0.0f && z - zReg.minDistance < maxDepthError) {
+								// The point is closer than current closest one to camera, 
+								// but still under max depth difference, just append
+								zReg.points.push_back(RegisteredPoints::Point(z, i));
+								set = true;
 							}
 						}
 					}
@@ -3296,19 +3405,19 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 				}
 				if(count == 0)
 				{
-					registered = cv::Mat();
+					registered.clear();
 					UINFO("No points projected in camera %d/%d", pter->first, camIndex);
 				}
 				else
 				{
 					UDEBUG("%d points projected in camera %d/%d", count, pter->first, camIndex);
 				}
-				for(int u=0; u<registered.cols; ++u)
+				for(int u=0; u<imageSize.width; ++u)
 				{
-					for(int v=0; v<registered.rows; ++v)
+					for(int v=0; v<imageSize.height; ++v)
 					{
-						cv::Vec2i &zReg = registered.at<cv::Vec2i>(v, u);
-						if(zReg[0] > 0)
+						RegisteredPoints &zReg = registered[v][u];
+						if(!zReg.points.empty())
 						{
 							ProjectionInfo info;
 							info.nodeID = pter->first;
@@ -3316,36 +3425,40 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 							info.uv.x = float(u)/float(imageSize.width);
 							info.uv.y = float(v)/float(imageSize.height);
 							const Transform & cam = cameraPoses.at(info.nodeID);
-							const PointT & pt = cloud.at(zReg[1]);
-							Eigen::Vector4f camDir(cam.x()-pt.x, cam.y()-pt.y, cam.z()-pt.z, 0);
-							Eigen::Vector4f normal(pt.normal_x, pt.normal_y, pt.normal_z, 0);
-							float angleToCam = maxAngle<=0?0:pcl::getAngle3D(normal, camDir);
-							float distanceToCam = zReg[0]/1000.0f;
-							if( (maxAngle<=0 || (camDir.dot(normal) > 0 && angleToCam < maxAngle)) && // is facing camera? is point normal perpendicular to camera?
-								(maxDistance<=0 || distanceToCam<maxDistance)) // is point not too far from camera?
+							for(size_t p=0; p<zReg.points.size(); ++p)
 							{
-								float vx = info.uv.x-0.5f;
-								float vy = info.uv.y-0.5f;
-
-								float distanceToCenter = vx*vx+vy*vy;
-								float distance = distanceToCenter;
-								if(distanceToCamPolicy)
+								int ptIdx = zReg.points[p].index;
+								const PointT & pt = cloud.at(ptIdx);
+								Eigen::Vector4f camDir(cam.x()-pt.x, cam.y()-pt.y, cam.z()-pt.z, 0);
+								Eigen::Vector4f normal(pt.normal_x, pt.normal_y, pt.normal_z, 0);
+								float angleToCam = maxAngle<=0?0:pcl::getAngle3D(normal, camDir);
+								float distanceToCam = zReg.points[p].distance;
+								if( (maxAngle<=0 || (camDir.dot(normal) > 0 && angleToCam < maxAngle)) && // is facing camera? is point normal perpendicular to camera?
+									(maxDistance<=0 || distanceToCam<maxDistance)) // is point not too far from camera?
 								{
-									distance = distanceToCam;
-								}
+									float vx = info.uv.x-0.5f;
+									float vy = info.uv.y-0.5f;
 
-								info.distance = distance;
-
-								if(invertedIndex[zReg[1]].distance != -1.0f)
-								{
-									if(distance <= invertedIndex[zReg[1]].distance)
+									float distanceToCenter = vx*vx+vy*vy;
+									float distance = distanceToCenter;
+									if(distanceToCamPolicy)
 									{
-										invertedIndex[zReg[1]] = info;
+										distance = distanceToCam;
+									}
+
+									info.distance = distance;
+
+									if(invertedIndex[ptIdx].distance != -1.0f)
+									{
+										if(distance <= invertedIndex[ptIdx].distance)
+										{
+											invertedIndex[ptIdx] = info;
+										}
+									}
+									else
+									{
+										invertedIndex[ptIdx] = info;
 									}
-								}
-								else
-								{
-									invertedIndex[zReg[1]] = info;
 								}
 							}
 						}
@@ -3415,6 +3528,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 		const std::map<int, std::vector<CameraModel> > & cameraModels,
 		float maxDistance,
 		float maxAngle,
+		float maxDepthError,
 		const std::vector<float> & roiRatios,
 		const cv::Mat & projMask,
 		bool distanceToCamPolicy,
@@ -3425,6 +3539,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 			cameraModels,
 			maxDistance,
 			maxAngle,
+			maxDepthError,
 			roiRatios,
 			projMask,
 			distanceToCamPolicy,
@@ -3437,6 +3552,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 		const std::map<int, std::vector<CameraModel> > & cameraModels,
 		float maxDistance,
 		float maxAngle,
+		float maxDepthError,
 		const std::vector<float> & roiRatios,
 		const cv::Mat & projMask,
 		bool distanceToCamPolicy,
@@ -3447,6 +3563,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 			cameraModels,
 			maxDistance,
 			maxAngle,
+			maxDepthError,
 			roiRatios,
 			projMask,
 			distanceToCamPolicy,

corelib/src/util3d_surface.cpp

@@ -781,10 +781,7 @@ pcl::TextureMesh::Ptr createTextureMesh(
 	pcl::TextureMapping<pcl::PointXYZ> tm; // TextureMapping object that will perform the sort
 	tm.setMaxDistance(maxDistance);
 	tm.setMaxAngle(maxAngle);
-	if(maxDepthError > 0.0f)
-	{
-		tm.setMaxDepthError(maxDepthError);
-	}
+	tm.setMaxDepthError(maxDepthError);
 	tm.setMinClusterSize(minClusterSize);
 	if(tm.textureMeshwithMultipleCameras2(*textureMesh, cameras, state, vertexToPixels, distanceToCamPolicy))
 	{

docker/bionic/Dockerfile

@@ -46,7 +46,7 @@ RUN git clone https://github.com/laurentkneip/opengv.git && \
     mkdir build && \
     cd build && \
     ~/cmake -DCMAKE_BUILD_TYPE=Release .. && \
-    make -j$(nproc) && \
+    make -j4 && \
     make install && \
     cd && \
     rm -r opengv
@@ -63,7 +63,7 @@ COPY . /root/rtabmap
 RUN source /ros_entrypoint.sh && \
     cd rtabmap/build && \
     ~/cmake -DWITH_OPENGV=ON .. && \
-    make -j$(nproc) && \
+    make -j4 && \
     make install && \
     cd ../.. && \
     rm -rf rtabmap && \

docker/focal/Dockerfile

@@ -2,22 +2,6 @@
 
 FROM introlab3it/rtabmap:focal-deps
 
-# June 19 2023: moved opengv here so that focal-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 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
 
@@ -28,7 +12,7 @@ COPY . /root/rtabmap
 RUN source /ros_entrypoint.sh && \
     cd rtabmap/build && \
     cmake -DWITH_ALICE_VISION=ON -DWITH_OPENGV=ON .. && \
-    make -j$(nproc) && \
+    make -j4 && \
     make install && \
     cd ../.. && \
     rm -rf rtabmap && \

docker/focal/deps/Dockerfile

@@ -10,6 +10,13 @@ ENV DEBIAN_FRONTEND=noninteractive
 
 WORKDIR /root/
 
+# issue: https://github.com/introlab/rtabmap/issues/1523
+RUN rm /etc/apt/sources.list.d/ros1-latest.list && \
+    apt-get update && apt-get install -y curl && \
+    sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' && \
+    curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add - && \
+    apt-get clean && rm -rf /var/lib/apt/lists/
+
 # Install build dependencies
 RUN if [ "$TARGETPLATFORM" = "linux/arm/v7" ]; then \
     apt-get update && \
@@ -29,7 +36,7 @@ RUN if [ "$TARGETPLATFORM" = "linux/arm/v7" ]; then \
     export CFLAGS="-D_FILE_OFFSET_BITS=64" && \
     export CXXFLAGS="-D_FILE_OFFSET_BITS=64" && \
     ./bootstrap && \
-    make -j$(nproc) && \
+    make -j4 && \
     sudo make install && \
     cd .. && \
     cmake --version && \
@@ -52,7 +59,7 @@ RUN apt-get update && apt-get install -y ros-noetic-librealsense2 && \
 # Taken from https://github.com/microsoft/Azure-Kinect-Sensor-SDK/issues/1190#issuecomment-822772494
 # K4A binaries on 20.04 not released yet, we should take those from 18.04
 RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing k4a..." && \
-    apt-get update && apt-get install -y curl && \
+    apt-get update && \
     echo "Download libk4a1.3_1.3.0_amd64.deb..." && \
     curl -sSL https://packages.microsoft.com/ubuntu/18.04/prod/pool/main/libk/libk4a1.3/libk4a1.3_1.3.0_amd64.deb > /tmp/libk4a1.3_1.3.0_amd64.deb && \
     echo "Download libk4a1.3-dev_1.3.0_amd64.deb..." && \
@@ -84,7 +91,7 @@ RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing libfreenect2
     mkdir build && \
     cd build && \
     cmake .. && \
-    make -j$(nproc) && \
+    make -j4 && \
     make install && \
     cd && \
     rm -r libfreenect2; fi
@@ -98,7 +105,7 @@ RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing zed-open-cap
     mkdir build && \
     cd build && \
     cmake .. && \
-    make -j$(nproc) && \
+    make -j4 && \
     make install && \
     cd && \
     rm -r zed-open-capture; fi
@@ -117,7 +124,7 @@ RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/
     mkdir build && \
     cd build && \
     cmake -DUSE_PYTHON=OFF -DOIIO_BUILD_TESTS=OFF -DOIIO_BUILD_TOOLS=OFF .. && \
-    make -j$(nproc) && \
+    make -j4 && \
     make install && \
     cd && \
     rm -r oiio; fi
@@ -127,7 +134,7 @@ RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/
     mkdir build && \
     cd build && \
     cmake .. && \
-    make -j$(nproc) && \
+    make -j4 && \
     make install && \
     cd && \
     rm -r assimp; fi
@@ -140,7 +147,7 @@ RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/
     git apply geogram_8b2ae61.patch && \
     ./configure.sh && \
     cd build/Linux64-gcc-dynamic-Release && \
-    make -j$(nproc) && \
+    make -j4 && \
     make install && \
     cd && \
     rm -r geogram; fi
@@ -153,7 +160,7 @@ RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/
     mkdir build && \
     cd build && \
     cmake -DALICEVISION_USE_CUDA=OFF -DALICEVISION_USE_APRILTAG=OFF -DALICEVISION_BUILD_SOFTWARE=OFF .. && \
-    make -j$(nproc) && \
+    make -j4 && \
     make install && \
     cd && \
     rm -r AliceVision; fi
@@ -164,11 +171,25 @@ RUN git clone --branch 4.2.0 https://github.com/opencv/opencv.git && \
     mkdir build && \
     cd build && \
     cmake -DWITH_TBB=ON -DWITH_OPENMP=ON -DBUILD_opencv_python3=OFF -DBUILD_opencv_python_bindings_generator=OFF -DBUILD_opencv_python_tests=OFF -DBUILD_PERF_TESTS=OFF -DBUILD_TESTS=OFF -DOPENCV_ENABLE_NONFREE=ON -DWITH_VTK=OFF -DOPENCV_EXTRA_MODULES_PATH=/root/opencv_contrib/modules .. && \
-    make -j$(nproc) && \
+    make -j4 && \
     make install && \
     cd ../.. && \
     rm -rf opencv opencv_contrib
 
+RUN if [ "$TARGETPLATFORM" = "linux/arm64" ]; then apt-get update && apt install wget && apt-get clean && rm -rf /var/lib/apt/lists/; fi
+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 -j4 && \
+    make install && \
+    cd && \
+    rm -r opengv
+
 RUN rm /bin/sh && ln -s /bin/bash /bin/sh
 
 # for jetson (https://github.com/introlab/rtabmap/issues/776)

docker/jammy/Dockerfile

@@ -12,7 +12,7 @@ COPY . /root/rtabmap
 RUN source /ros_entrypoint.sh && \
     cd rtabmap/build && \
     cmake -DWITH_OPENGV=ON .. && \
-    make -j$(nproc) && \
+    make -j4 && \
     make install && \
     cd ../.. && \
     rm -rf rtabmap && \

docker/jammy/deps/Dockerfile

@@ -49,7 +49,7 @@ RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing libfreenect2
     mkdir build && \
     cd build && \
     cmake .. && \
-    make -j$(nproc) && \
+    make -j2 && \
     make install && \
     cd && \
     rm -r libfreenect2; fi
@@ -63,7 +63,7 @@ RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing zed-open-cap
     mkdir build && \
     cd build && \
     cmake .. && \
-    make -j$(nproc) && \
+    make -j2 && \
     make install && \
     cd && \
     rm -r zed-open-capture; fi
@@ -74,7 +74,7 @@ RUN git clone --branch 4.5.4 https://github.com/opencv/opencv.git && \
     mkdir build && \
     cd build && \
     cmake -DWITH_TBB=ON -DWITH_OPENMP=ON -DBUILD_opencv_python3=OFF -DBUILD_opencv_python_bindings_generator=OFF -DBUILD_opencv_python_tests=OFF -DBUILD_PERF_TESTS=OFF -DBUILD_TESTS=OFF -DOPENCV_ENABLE_NONFREE=ON -DOPENCV_EXTRA_MODULES_PATH=/root/opencv_contrib/modules .. && \
-    make -j$(nproc) && \
+    make -j2 && \
     make install && \
     cd ../.. && \
     rm -rf opencv opencv_contrib
@@ -90,14 +90,14 @@ RUN git clone https://github.com/laurentkneip/opengv.git && \
     mkdir build && \
     cd build && \
     cmake -DCMAKE_BUILD_TYPE=Release .. && \
-    make -j$(nproc) && \
+    make -j2 && \
     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
+RUN echo -e '#!/bin/bash\nset -e\n\n# setup ros2 environment\nsource "/opt/ros/humble/setup.bash" --\nexec "$@"' > /ros_entrypoint.sh
 RUN chmod +x /ros_entrypoint.sh
 ENTRYPOINT [ "/ros_entrypoint.sh" ]
 

docker/jammy/deps/ros_entrypoint.sh

@@ -1,6 +0,0 @@
-#!/bin/bash
-set -e
-
-# setup ros2 environment
-source "/opt/ros/humble/setup.bash" --
-exec "$@"

docker/noble-kilted/Dockerfile

@@ -0,0 +1,20 @@
+# Image: introlab3it/rtabmap:noble-kilted
+
+FROM introlab3it/rtabmap:noble-kilted-deps
+
+# Will be used to read/store databases on host
+RUN mkdir -p /root/Documents/RTAB-Map && chmod 777 /root/Documents/RTAB-Map
+
+# Copy current source code
+COPY . /root/rtabmap
+
+# Build RTAB-Map project
+RUN source /ros_entrypoint.sh && \
+    cd rtabmap/build && \
+    cmake -DWITH_OPENGV=ON .. && \
+    make -j4 && \
+    make install && \
+    cd ../.. && \
+    rm -rf rtabmap && \
+    ldconfig
+

docker/noble-kilted/deps/Dockerfile

@@ -0,0 +1,132 @@
+
+# Image: introlab3it/rtabmap:noble-kilted-deps
+
+FROM ubuntu:24.04
+
+ARG TARGETPLATFORM
+ENV TARGETPLATFORM=${TARGETPLATFORM:-linux/amd64}
+RUN echo "I am building for $TARGETPLATFORM"
+
+ENV DEBIAN_FRONTEND=noninteractive
+
+# Install ROS2
+RUN apt update && \
+    apt install software-properties-common -y && \
+    add-apt-repository universe && \
+    apt update && \
+    apt install curl -y && \
+    curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg && \
+    echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | tee /etc/apt/sources.list.d/ros2.list > /dev/null && \
+    apt-get clean && rm -rf /var/lib/apt/lists/
+
+# Install build dependencies
+RUN apt-get update && \
+    apt upgrade -y && \
+    apt-get install -y \
+      git \
+      wget \
+      libtbb-dev \
+      libproj-dev \
+      libpcl-dev \
+      liboctomap-dev \
+      libfreenect-dev \
+      ros-kilted-ros-base \
+      ros-dev-tools \
+      ros-kilted-cv-bridge \
+      ros-kilted-image-geometry \
+      ros-kilted-laser-geometry \
+      ros-kilted-pcl-conversions \
+      ros-kilted-rviz-common \
+      ros-kilted-rviz-rendering \
+      ros-kilted-rviz-default-plugins \
+      ros-kilted-pcl-ros \
+      ros-kilted-imu-filter-madgwick \
+      ros-kilted-velodyne \
+      ros-kilted-image-transport \
+      ros-kilted-octomap-msgs \
+      ros-kilted-libg2o \
+      ros-kilted-libpointmatcher \
+      ros-kilted-qt-gui-cpp \
+      ros-kilted-diagnostic-updater && \
+    apt-get clean && rm -rf /var/lib/apt/lists/
+
+WORKDIR /root/
+
+# GTSAM (we cannot use ros-kilted-gtsam on Noble because it is linked to a different internal Eigen version than system one)
+RUN git clone https://github.com/borglab/gtsam.git && \
+    cd gtsam && \
+    git checkout 4.2.0 && \ 
+    mkdir build && \
+    cd build && \
+    cmake -DCMAKE_BUILD_TYPE=Release -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF -DGTSAM_USE_SYSTEM_EIGEN=ON .. && \
+    make -j4 && \
+    make install && \
+    cd && \
+    rm -r gtsam
+
+# libfreenect2 
+RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing libfreenect2..." && \
+    apt-get update && apt-get install -y mesa-utils xserver-xorg-video-all libusb-1.0-0-dev libturbojpeg0-dev libglfw3-dev && \
+    apt-get clean && rm -rf /var/lib/apt/lists/ && \
+    git clone https://github.com/OpenKinect/libfreenect2 && \
+    cd libfreenect2 && \
+    mkdir build && \
+    cd build && \
+    cmake -DCMAKE_BUILD_TYPE=Release .. && \
+    make -j4 && \
+    make install && \
+    cd && \
+    rm -r libfreenect2; fi
+
+# zed open capture
+RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing zed-open-capture..." && \
+    apt-get update && apt install -y libusb-1.0-0-dev libhidapi-libusb0 libhidapi-dev wget && \
+    apt-get clean && rm -rf /var/lib/apt/lists/ && \
+    git clone https://github.com/stereolabs/zed-open-capture.git && \
+    cd zed-open-capture && \
+    mkdir build && \
+    cd build && \
+    cmake -DCMAKE_BUILD_TYPE=Release .. && \
+    make -j4 && \
+    make install && \
+    cd && \
+    rm -r zed-open-capture; fi
+
+# OpenCV with all modules (same version than distro version to avoid conflicts with cv_bridge ros package)
+RUN git clone --branch 4.6.0 https://github.com/opencv/opencv.git && \
+    git clone --branch 4.6.0 https://github.com/opencv/opencv_contrib.git && \
+    cd opencv && \
+    sed -i '/#include <libavformat\/avformat.h>/i #include <libavcodec/version.h>' modules/videoio/src/ffmpeg_codecs.hpp && \
+    mkdir build && \
+    cd build && \
+    cmake -DCMAKE_BUILD_TYPE=Release -DWITH_TBB=ON -DWITH_ADE=OFF -DWITH_OPENMP=ON -DBUILD_opencv_python3=OFF -DBUILD_opencv_python_bindings_generator=OFF -DBUILD_opencv_python_tests=OFF -DBUILD_PERF_TESTS=OFF -DBUILD_TESTS=OFF -DOPENCV_ENABLE_NONFREE=ON -DOPENCV_EXTRA_MODULES_PATH=/root/opencv_contrib/modules .. && \
+    make -j4 && \
+    make install && \
+    cd ../.. && \
+    rm -rf opencv opencv_contrib
+
+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 -DBUILD_TESTS=OFF .. && \
+    make -j4 && \
+    make install && \
+    cd && \
+    rm -r opengv
+
+RUN rm /bin/sh && ln -s /bin/bash /bin/sh
+
+RUN echo -e '#!/bin/bash\nset -e\n\n# setup ros2 environment\nsource "/opt/ros/kilted/setup.bash" --\nexec "$@"' > /ros_entrypoint.sh
+RUN chmod +x /ros_entrypoint.sh
+ENTRYPOINT [ "/ros_entrypoint.sh" ]
+
+# ros2 seems not sourcing by default its multi-arch folders
+ENV LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/opt/ros/kilted/lib/x86_64-linux-gnu:/opt/ros/kilted/lib/aarch64-linux-gnu
+
+# for jetson (https://github.com/introlab/rtabmap/issues/776)
+ENV LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/lib/aarch64-linux-gnu/tegra
+

docker/noble/Dockerfile

@@ -2,20 +2,6 @@
 
 FROM introlab3it/rtabmap:noble-deps
 
-# OpenGV (issue seg fault on my computer, moved here from noble-deps to be built on ci)
-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 -DBUILD_TESTS=OFF .. && \
-    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
 
@@ -26,7 +12,7 @@ COPY . /root/rtabmap
 RUN source /ros_entrypoint.sh && \
     cd rtabmap/build && \
     cmake -DWITH_OPENGV=ON .. && \
-    make -j$(nproc) && \
+    make -j4 && \
     make install && \
     cd ../.. && \
     rm -rf rtabmap && \

docker/noble/deps/Dockerfile

@@ -58,7 +58,7 @@ RUN git clone https://github.com/borglab/gtsam.git && \
     mkdir build && \
     cd build && \
     cmake -DCMAKE_BUILD_TYPE=Release -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF -DGTSAM_USE_SYSTEM_EIGEN=ON .. && \
-    make -j$(nproc) && \
+    make -j4 && \
     make install && \
     cd && \
     rm -r gtsam
@@ -72,7 +72,7 @@ RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing libfreenect2
     mkdir build && \
     cd build && \
     cmake -DCMAKE_BUILD_TYPE=Release .. && \
-    make -j$(nproc) && \
+    make -j4 && \
     make install && \
     cd && \
     rm -r libfreenect2; fi
@@ -86,29 +86,40 @@ RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing zed-open-cap
     mkdir build && \
     cd build && \
     cmake -DCMAKE_BUILD_TYPE=Release .. && \
-    make -j$(nproc) && \
+    make -j4 && \
     make install && \
     cd && \
     rm -r zed-open-capture; fi
 
 # OpenCV with all modules (same version than distro version to avoid conflicts with cv_bridge ros package)
-COPY ./docker/noble/deps/opencv_4_6_0_ffmpeg.patch .
 RUN git clone --branch 4.6.0 https://github.com/opencv/opencv.git && \
     git clone --branch 4.6.0 https://github.com/opencv/opencv_contrib.git && \
-    mv opencv_4_6_0_ffmpeg.patch opencv/. && \
     cd opencv && \
-    git apply opencv_4_6_0_ffmpeg.patch && \
+    sed -i '/#include <libavformat\/avformat.h>/i #include <libavcodec/version.h>' modules/videoio/src/ffmpeg_codecs.hpp && \
     mkdir build && \
     cd build && \
     cmake -DCMAKE_BUILD_TYPE=Release -DWITH_TBB=ON -DWITH_ADE=OFF -DWITH_OPENMP=ON -DBUILD_opencv_python3=OFF -DBUILD_opencv_python_bindings_generator=OFF -DBUILD_opencv_python_tests=OFF -DBUILD_PERF_TESTS=OFF -DBUILD_TESTS=OFF -DOPENCV_ENABLE_NONFREE=ON -DOPENCV_EXTRA_MODULES_PATH=/root/opencv_contrib/modules .. && \
-    make -j$(nproc) && \
+    make -j4 && \
     make install && \
     cd ../.. && \
     rm -rf opencv opencv_contrib
 
+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 -DBUILD_TESTS=OFF .. && \
+    make -j4 && \
+    make install && \
+    cd && \
+    rm -r opengv
+
 RUN rm /bin/sh && ln -s /bin/bash /bin/sh
 
-COPY ./docker/noble/deps/ros_entrypoint.sh /ros_entrypoint.sh
+RUN echo -e '#!/bin/bash\nset -e\n\n# setup ros2 environment\nsource "/opt/ros/jazzy/setup.bash" --\nexec "$@"' > /ros_entrypoint.sh
 RUN chmod +x /ros_entrypoint.sh
 ENTRYPOINT [ "/ros_entrypoint.sh" ]
 

docker/noble/deps/opencv_4_6_0_ffmpeg.patch

@@ -1,12 +0,0 @@
-diff --git a/modules/videoio/src/ffmpeg_codecs.hpp b/modules/videoio/src/ffmpeg_codecs.hpp
-index 61788e0345..faad2596ed 100644
---- a/modules/videoio/src/ffmpeg_codecs.hpp
-+++ b/modules/videoio/src/ffmpeg_codecs.hpp
-@@ -60,6 +60,7 @@ extern "C" {
- #include <errno.h>
- #endif
- 
-+#include <libavcodec/version.h>
- #include <libavformat/avformat.h>
- 
- #ifdef __cplusplus

docker/noble/deps/ros_entrypoint.sh

@@ -1,6 +0,0 @@
-#!/bin/bash
-set -e
-
-# setup ros2 environment
-source "/opt/ros/jazzy/setup.bash" --
-exec "$@"

examples/RGBDMapping/main.cpp

@@ -256,7 +256,8 @@ int main(int argc, char * argv[])
 				node.sensorData(),
 				4,           // image decimation before creating the clouds
 				4.0f,        // maximum depth of the cloud
-				0.0f);
+				0.0f,
+				0);
 		pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmpNoNaN(new pcl::PointCloud<pcl::PointXYZRGB>);
 		std::vector<int> index;
 		pcl::removeNaNFromPointCloud(*tmp, *tmpNoNaN, index);

guilib/include/rtabmap/gui/DatabaseViewer.h

@@ -65,6 +65,8 @@ class ExportCloudsDialog;
 class EditDepthArea;
 class EditMapArea;
 class LinkRefiningDialog;
+class Registration;
+class RegistrationIcp;
 
 class RTABMAP_GUI_EXPORT DatabaseViewer : public QMainWindow
 {
@@ -202,8 +204,8 @@ private:
 	void updateLoopClosuresSlider(int from = 0, int to = 0);
 	void updateCovariances(const QList<Link> & links);
 	void refineLinks(const QList<Link> & links);
-	void refineConstraint(int from, int to,  bool silent);
-	bool addConstraint(int from, int to, bool silent, bool silentlyUseOptimizedGraphAsGuess = false);
+	void refineConstraint(int from, int to, Registration * reg, RegistrationIcp * regIcp, bool silent);
+	bool addConstraint(int from, int to, Registration * reg, bool silent, bool silentlyUseOptimizedGraphAsGuess = false);
 	void exportPoses(int format);
 	void exportGPS(int format);
 

guilib/include/rtabmap/gui/ImageView.h

@@ -60,6 +60,7 @@ public:
 	QRectF sceneRect() const;
 	bool isImageShown() const;
 	bool isImageDepthShown() const;
+	bool isImageDepthConfidenceShown() const;
 	bool isFeaturesShown() const;
 	bool isLinesShown() const;
 	int getAlpha() const {return _alpha;}
@@ -82,6 +83,7 @@ public:
 	void setFeaturesShown(bool shown);
 	void setImageShown(bool shown);
 	void setImageDepthShown(bool shown);
+	void setImageDepthConfidenceShown(bool shown);
 	void setLinesShown(bool shown);
 	void setGraphicsViewMode(bool on);
 	void setGraphicsViewScaled(bool scaled);
@@ -98,8 +100,8 @@ public:
 	void addFeature(int id, const cv::KeyPoint & kpt, float depth, QColor color);
 	void addLine(float x1, float y1, float x2, float y2, QColor color, const QString & text = QString());
 	void setImage(const QImage & image, const std::vector<CameraModel> & models = std::vector<CameraModel>(), const Transform & pose = Transform());
-	void setImageDepth(const cv::Mat & imageDepth);
-	void setImageDepth(const QImage & image);
+	void setImageDepth(const cv::Mat & imageDepth, const cv::Mat & imageDepthConfidence = cv::Mat());
+	void setImageDepth(const QImage & image, const QImage & imageDepthConfidence = QImage());
 	void setFeatureColor(int id, QColor color);
 	void setFeaturesColor(QColor color);
 	void setAlpha(int alpha);
@@ -147,6 +149,7 @@ private:
 	QMenu * _menu;
 	QAction * _showImage;
 	QAction * _showImageDepth;
+	QAction * _showImageDepthConfidence;
 	QAction * _showFeatures;
 	QAction * _showLines;
 	QAction * _setFeatureColor;
@@ -175,9 +178,12 @@ private:
 	QList<QGraphicsLineItem*> _lines;
 	QGraphicsPixmapItem * _imageItem;
 	QGraphicsPixmapItem * _imageDepthItem;
+	QGraphicsPixmapItem * _imageDepthConfidenceItem;
 	QPixmap _image;
 	QPixmap _imageDepth;
+	QPixmap _imageDepthConfidence;
 	cv::Mat _imageDepthCv;
+	cv::Mat _imageDepthConfidenceCv;
 	std::vector<CameraModel> _models;
 	Transform _pose;
 };

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -208,6 +208,7 @@ public:
 	double getCloudMaxDepth(int index) const;  // 0=map, 1=odom
 	double getCloudMinDepth(int index) const;  // 0=map, 1=odom
 	std::vector<float> getCloudRoiRatios(int index) const; // 0=map, 1=odom
+	unsigned char getCloudConfidenceThr(int index) const; // 0=map, 1=odom
 	int getCloudColorScheme(int index) const;   // 0=map, 1=odom
 	double getCloudOpacity(int index) const;   // 0=map, 1=odom
 	int getCloudPointSize(int index) const;    // 0=map, 1=odom
@@ -466,6 +467,7 @@ private:
 	QVector<QDoubleSpinBox*> _3dRenderingMaxDepth;
 	QVector<QDoubleSpinBox*> _3dRenderingMinDepth;
 	QVector<QLineEdit*> _3dRenderingRoiRatios;
+	QVector<QSpinBox*> _3dRenderingDepthConfidenceThr;
 	QVector<QSpinBox*> _3dRenderingColorScheme;
 	QVector<QDoubleSpinBox*> _3dRenderingOpacity;
 	QVector<QSpinBox*> _3dRenderingPtSize;

guilib/include/rtabmap/utilite/UCv2Qt.h

@@ -39,7 +39,7 @@ enum uCvQtDepthColorMap{
  * depth (float32, uint16) image and RGB/BGR 8bits images.
  * @param image the cv::Mat image (can be 1 channel [CV_8U, CV_16U or CV_32F] or 3 channels [CV_U8])
  * @param isBgr if 3 channels, it is BGR or RGB order.
- * @param colorMap gradient of color to use to visualize depth
+ * @param colorMap gradient of color to use to visualize depth or monochrome images
  * @param depthMin fixed minimum range (m) of the depth gradient (if depthMax<=depthMin, max/min are computed based on data in depth image)
  * @param depthMax fixed maximum range (m) of the depth gradient (if depthMax<=depthMin, max/min are computed based on data in depth image)
  * @return the QImage
@@ -47,7 +47,7 @@ enum uCvQtDepthColorMap{
 inline QImage uCvMat2QImage(
 		const cv::Mat & image,
 		bool isBgr = true,
-		uCvQtDepthColorMap colorMap = uCvQtDepthWhiteToBlack,
+		uCvQtDepthColorMap colorMap = uCvQtDepthBlackToWhite,
 		float depthMin = 0,
 		float depthMax = 0)
 {
@@ -81,9 +81,30 @@ inline QImage uCvMat2QImage(
 		{
 			// mono grayscale
 			qtemp = QImage(image.data, image.cols, image.rows, image.cols, QImage::Format_Indexed8).copy();
+
 			QVector<QRgb> my_table;
-			for(int i = 0; i < 256; i++)
-				my_table.push_back(qRgb(i,i,i));
+			my_table.reserve(256);
+			if(colorMap == uCvQtDepthRedToBlue || colorMap == uCvQtDepthBlueToRed)
+			{
+				if(colorMap == uCvQtDepthBlueToRed) {
+					for(int i = 0; i < 256; i++)
+						my_table.push_back(QColor::fromHsv(255-i, 255, 255, 255).rgb());
+				}
+				else {
+					for(int i = 0; i < 256; i++)
+						my_table.push_back(QColor::fromHsv(i, 255, 255, 255).rgb());
+				}
+			}
+			else if(colorMap == uCvQtDepthBlackToWhite)
+			{
+				for(int i = 0; i < 256; i++)
+					my_table.push_back(qRgb(i,i,i));
+			}
+			else // uCvQtDepthWhiteToBlack
+			{
+				for(int i = 0; i < 256; i++)
+					my_table.push_back(qRgb(255-i,255-i,255-i));
+			}
 			qtemp.setColorTable(my_table);
 		}
 		else

guilib/src/DataRecorder.cpp

@@ -156,8 +156,12 @@ void DataRecorder::addData(const rtabmap::SensorData & data, const Transform & p
 void DataRecorder::showImage(const cv::Mat & image, const cv::Mat & depth)
 {
 	processingImages_ = true;
-	imageView_->setImage(uCvMat2QImage(image));
-	imageView_->setImageDepth(depth);
+	if(!image.empty()) {
+		imageView_->setImage(uCvMat2QImage(image));
+	}
+	if(!depth.empty()) {
+		imageView_->setImageDepth(depth);
+	}
 	label_->setText(tr("Images=%1 (~%2 MB)").arg(count_).arg(totalSizeKB_/1000));
 	processingImages_ = false;
 }

guilib/src/DatabaseViewer.cpp

@@ -427,6 +427,10 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	connect(ui_->checkBox_showDisparityInsteadOfRight, SIGNAL(stateChanged(int)), this, SLOT(update3dView()));
 	connect(ui_->spinBox_decimation, SIGNAL(valueChanged(int)), this, SLOT(updateConstraintView()));
 	connect(ui_->spinBox_decimation, SIGNAL(valueChanged(int)), this, SLOT(update3dView()));
+	connect(ui_->spinBox_depthConfidence, SIGNAL(valueChanged(int)), this, SLOT(updateConstraintView()));
+	connect(ui_->spinBox_depthConfidence, SIGNAL(valueChanged(int)), this, SLOT(update3dView()));
+	connect(ui_->doubleSpinBox_depthEdgeBleedingError, SIGNAL(valueChanged(double)), this, SLOT(updateConstraintView()));
+	connect(ui_->doubleSpinBox_depthEdgeBleedingError, SIGNAL(valueChanged(double)), this, SLOT(update3dView()));
 	connect(ui_->groupBox_posefiltering, SIGNAL(clicked(bool)), this, SLOT(updateGraphView()));
 	connect(ui_->doubleSpinBox_posefilteringRadius, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
 	connect(ui_->doubleSpinBox_posefilteringAngle, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
@@ -457,6 +461,8 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	connect(ui_->checkBox_cameraProjection, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
 	connect(ui_->checkBox_showDisparityInsteadOfRight, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
 	connect(ui_->spinBox_decimation, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
+	connect(ui_->spinBox_depthConfidence, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
+	connect(ui_->doubleSpinBox_depthEdgeBleedingError, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
 	connect(ui_->groupBox_posefiltering, SIGNAL(clicked(bool)), this, SLOT(configModified()));
 	connect(ui_->doubleSpinBox_posefilteringRadius, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
 	connect(ui_->doubleSpinBox_posefilteringAngle, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
@@ -616,6 +622,8 @@ void DatabaseViewer::readSettings()
 	ui_->doubleSpinBox_gainCompensationRadius->setValue(settings.value("gainCompensationRadius", ui_->doubleSpinBox_gainCompensationRadius->value()).toDouble());
 	ui_->doubleSpinBox_voxelSize->setValue(settings.value("voxelSize", ui_->doubleSpinBox_voxelSize->value()).toDouble());
 	ui_->spinBox_decimation->setValue(settings.value("decimation", ui_->spinBox_decimation->value()).toInt());
+	ui_->spinBox_depthConfidence->setValue(settings.value("depth_confidence_thr", ui_->spinBox_depthConfidence->value()).toInt());
+	ui_->doubleSpinBox_depthEdgeBleedingError->setValue(settings.value("depth_bleeding_error", ui_->doubleSpinBox_depthEdgeBleedingError->value()).toDouble());
 	ui_->checkBox_cameraProjection->setChecked(settings.value("camProj", ui_->checkBox_cameraProjection->isChecked()).toBool());
 	ui_->checkBox_showDisparityInsteadOfRight->setChecked(settings.value("showDisp", ui_->checkBox_showDisparityInsteadOfRight->isChecked()).toBool());
 	settings.endGroup();
@@ -707,6 +715,8 @@ void DatabaseViewer::writeSettings()
 	settings.setValue("gainCompensationRadius", ui_->doubleSpinBox_gainCompensationRadius->value());
 	settings.setValue("voxelSize", ui_->doubleSpinBox_voxelSize->value());
 	settings.setValue("decimation", ui_->spinBox_decimation->value());
+	settings.setValue("depth_confidence", ui_->spinBox_depthConfidence->value());
+	settings.setValue("depth_bleeding_error", ui_->doubleSpinBox_depthEdgeBleedingError->value());
 	settings.setValue("camProj", ui_->checkBox_cameraProjection->isChecked());
 	settings.setValue("showDisp", ui_->checkBox_showDisparityInsteadOfRight->isChecked());
 	settings.endGroup();
@@ -801,6 +811,8 @@ void DatabaseViewer::restoreDefaultSettings()
 	ui_->doubleSpinBox_gainCompensationRadius->setValue(0.0);
 	ui_->doubleSpinBox_voxelSize->setValue(0.0);
 	ui_->spinBox_decimation->setValue(1);
+	ui_->spinBox_depthConfidence->setValue(0);
+	ui_->doubleSpinBox_depthEdgeBleedingError->setValue(0.0);
 	ui_->checkBox_cameraProjection->setChecked(false);
 	ui_->checkBox_showDisparityInsteadOfRight->setChecked(false);
 
@@ -1584,6 +1596,11 @@ void DatabaseViewer::extractImages()
 						dir.mkdir(QString("%1/rgb").arg(path));
 						dir.mkdir(QString("%1/depth").arg(path));
 						dir.mkdir(QString("%1/calib").arg(path));
+						if(!data.depthConfidenceRaw().empty())
+						{
+							dir.mkdir(QString("%1/confidence").arg(path));
+							directoriesCreated = true;
+						}
 						directoriesCreated = true;
 					}
 				}
@@ -1668,7 +1685,14 @@ void DatabaseViewer::extractImages()
 							UWARN("Failed saving \"%s\"", QString("%1/rgb/%2.%3").arg(path).arg(id).arg(ext).toStdString().c_str());
 						if(!cv::imwrite(QString("%1/depth/%2.png").arg(path).arg(id).toStdString(), data.depthRaw().type()==CV_32FC1?util2d::cvtDepthFromFloat(data.depthRaw()):data.depthRaw()))
 							UWARN("Failed saving \"%s\"", QString("%1/depth/%2.png").arg(path).arg(id).toStdString().c_str());
-						UINFO(QString("Saved rgb/%1.%2 and depth/%1.png").arg(id).arg(ext).toStdString().c_str());
+						if(data.depthConfidenceRaw().empty()) {
+							UINFO(QString("Saved rgb/%1.%2 and depth/%1.png").arg(id).arg(ext).toStdString().c_str());
+						}
+						else {
+							if(!cv::imwrite(QString("%1/confidence/%2.png").arg(path).arg(id).toStdString(), data.depthConfidenceRaw()))
+								UWARN("Failed saving \"%s\"", QString("%1/confidence/%2.png").arg(path).arg(id).toStdString().c_str());
+							UINFO(QString("Saved rgb/%1.%2, depth/%1.png and confidence/%1.png").arg(id).arg(ext).toStdString().c_str());
+						}
 					}
 					else
 					{
@@ -4263,6 +4287,8 @@ void DatabaseViewer::detectMoreLoopClosures()
 		return;
 	}
 
+	std::shared_ptr<Registration> reg(Registration::create(ui_->parameters_toolbox->getParameters()));
+
 	for(int n=0; n<iterations; ++n)
 	{
 		UINFO("iteration %d/%d", n+1, iterations);
@@ -4310,7 +4336,7 @@ void DatabaseViewer::detectMoreLoopClosures()
 						   delta.getNorm() >= ui_->doubleSpinBox_detectMore_radiusMin->value())
 						{
 							checkedLoopClosures.insert(std::make_pair(from, to));
-							if(addConstraint(from, to, true, useOptimizedGraphAsGuess))
+							if(addConstraint(from, to, reg.get(), true, useOptimizedGraphAsGuess))
 							{
 								UINFO("Added new loop closure between %d and %d.", from, to);
 								++added;
@@ -4568,13 +4594,16 @@ void DatabaseViewer::refineLinks(const QList<Link> & links)
 		progressDialog->setMinimumWidth(800);
 		progressDialog->show();
 
+		RegistrationIcp regProximity(ui_->parameters_toolbox->getParameters());
+		std::shared_ptr<Registration> reg(Registration::create(ui_->parameters_toolbox->getParameters()));
+
 		for(int i=0; i<links.size(); ++i)
 		{
 			int from = links[i].from();
 			int to = links[i].to();
 			if(from > 0 && to > 0)
 			{
-				this->refineConstraint(links[i].from(), links[i].to(), true);
+				this->refineConstraint(links[i].from(), links[i].to(), reg.get(), &regProximity, true);
 				progressDialog->appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(links.size()));
 			}
 			else
@@ -4802,7 +4831,7 @@ void DatabaseViewer::update(int value,
 
 				if(!imgDepth.empty())
 				{
-					view->setImageDepth(imgDepth);
+					view->setImageDepth(imgDepth, data.depthConfidenceRaw());
 					if(img.isNull())
 					{
 						rect.setWidth(imgDepth.cols);
@@ -5261,6 +5290,12 @@ void DatabaseViewer::update(int value,
 					{
 						if(!data.depthOrRightRaw().empty())
 						{
+							if(!data.depthRaw().empty() && ui_->doubleSpinBox_depthEdgeBleedingError->value() > 0.0) {
+								cv::Mat depth = data.depthRaw();
+								util2d::depthBleedingFiltering(depth, ui_->doubleSpinBox_depthEdgeBleedingError->value());
+								data.setRGBDImage(data.imageRaw(), depth, data.depthConfidenceRaw(), data.cameraModels());
+							}
+
 							if(!data.imageRaw().empty())
 							{
 								std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds;
@@ -5276,8 +5311,13 @@ void DatabaseViewer::update(int value,
 									pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudFromDepthRGB(
 											data.imageRaw(),
 											depth,
+											data.depthConfidenceRaw(),
 											data.cameraModels()[0],
-											ui_->spinBox_decimation->value(),0,0,indices.get());
+											ui_->spinBox_decimation->value(),
+											0,
+											0,
+											(unsigned char)ui_->spinBox_depthConfidence->value(),
+											indices.get());
 									if(indices->size())
 									{
 										clouds.push_back(util3d::transformPointCloud(cloud, data.cameraModels()[0].localTransform()));
@@ -5286,7 +5326,14 @@ void DatabaseViewer::update(int value,
 								}
 								else
 								{
-									clouds = util3d::cloudsRGBFromSensorData(data, ui_->spinBox_decimation->value(), 0, 0, &allIndices, ui_->parameters_toolbox->getParameters());
+									clouds = util3d::cloudsRGBFromSensorData(
+										data, 
+										ui_->spinBox_decimation->value(), 
+										0, 
+										0, 
+										&allIndices, 
+										ui_->parameters_toolbox->getParameters(),std::vector<float>(), 
+										(unsigned char)ui_->spinBox_depthConfidence->value());
 								}
 								UASSERT(clouds.size() == allIndices.size());
 								for(size_t i=0; i<allIndices.size(); ++i)
@@ -5361,7 +5408,14 @@ void DatabaseViewer::update(int value,
 								std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clouds;
 								std::vector<pcl::IndicesPtr> allIndices;
 
-								clouds = util3d::cloudsFromSensorData(data, ui_->spinBox_decimation->value(), 0, 0, &allIndices, ui_->parameters_toolbox->getParameters());
+								clouds = util3d::cloudsFromSensorData(
+									data, 
+									ui_->spinBox_decimation->value(), 
+									0, 
+									0, 
+									&allIndices, ui_->parameters_toolbox->getParameters(),
+									std::vector<float>(),
+									(unsigned char)ui_->spinBox_depthConfidence->value());
 								UASSERT(clouds.size() == allIndices.size());
 								for(size_t i=0; i<allIndices.size(); ++i)
 								{
@@ -6353,11 +6407,12 @@ void DatabaseViewer::updateConstraintView(
 			ui_->checkBox_showOptimized->setEnabled(true);
 			Transform topt = iterFrom->second.inverse()*iterTo->second;
 			float diff = topt.getDistance(t);
+
 			Transform v1 = t.rotation()*Transform(1,0,0,0,0,0);
 			Transform v2 = topt.rotation()*Transform(1,0,0,0,0,0);
 			float a = pcl::getAngle3D(Eigen::Vector4f(v1.x(), v1.y(), v1.z(), 0), Eigen::Vector4f(v2.x(), v2.y(), v2.z(), 0));
 			a = (a *180.0f) / CV_PI;
-			ui_->label_constraint_opt->setText(QString("%1\n(error=%2% a=%3)").arg(QString(topt.prettyPrint().c_str()).replace(" ", "\n")).arg((t.getNorm()>0?diff/t.getNorm():0)*100.0f).arg(a));
+			ui_->label_constraint_opt->setText(QString("%1\n(error=%2% a=%3 deg)").arg(QString(topt.prettyPrint().c_str()).replace(" ", "\n")).arg((t.getNorm()>0?diff/t.getNorm():0)*100.0f).arg(a));
 
 			if(ui_->checkBox_showOptimized->isChecked())
 			{
@@ -6488,11 +6543,37 @@ void DatabaseViewer::updateConstraintView(
 			pcl::IndicesPtr indicesTo(new std::vector<int>);
 			if(!dataFrom.imageRaw().empty() && !dataFrom.depthOrRightRaw().empty())
 			{
-				cloudFrom=util3d::cloudRGBFromSensorData(dataFrom, ui_->spinBox_decimation->value(), 0, 0, indicesFrom.get(), ui_->parameters_toolbox->getParameters());
+				if(!dataFrom.depthRaw().empty() && ui_->doubleSpinBox_depthEdgeBleedingError->value() > 0.0) {
+					cv::Mat depth = dataFrom.depthRaw();
+					util2d::depthBleedingFiltering(depth, ui_->doubleSpinBox_depthEdgeBleedingError->value());
+					dataFrom.setRGBDImage(dataFrom.imageRaw(), depth, dataFrom.depthConfidenceRaw(), dataFrom.cameraModels());
+				}
+				cloudFrom=util3d::cloudRGBFromSensorData(
+					dataFrom, 
+					ui_->spinBox_decimation->value(),
+					0, 
+					0, 
+					indicesFrom.get(), 
+					ui_->parameters_toolbox->getParameters(),
+					std::vector<float>(),
+					(unsigned char)ui_->spinBox_depthConfidence->value());
 			}
 			if(!dataTo.imageRaw().empty() && !dataTo.depthOrRightRaw().empty())
 			{
-				cloudTo=util3d::cloudRGBFromSensorData(dataTo, ui_->spinBox_decimation->value(), 0, 0, indicesTo.get(), ui_->parameters_toolbox->getParameters());
+				if(!dataTo.depthRaw().empty() && ui_->doubleSpinBox_depthEdgeBleedingError->value() > 0.0) {
+					cv::Mat depth = dataTo.depthRaw();
+					util2d::depthBleedingFiltering(depth, ui_->doubleSpinBox_depthEdgeBleedingError->value());
+					dataTo.setRGBDImage(dataTo.imageRaw(), depth, dataTo.depthConfidenceRaw(), dataTo.cameraModels());
+				}
+				cloudTo=util3d::cloudRGBFromSensorData(
+					dataTo, 
+					ui_->spinBox_decimation->value(), 
+					0, 
+					0, 
+					indicesTo.get(), 
+					ui_->parameters_toolbox->getParameters(),
+					std::vector<float>(),
+					(unsigned char)ui_->spinBox_depthConfidence->value());
 			}
 
 			if(cloudTo.get() && indicesTo->size())
@@ -7140,7 +7221,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 			}
 			if(!allNodesAreInWM)
 			{
-				ui_->graphViewer->updatePosterior(colors, 1, 1);
+				ui_->graphViewer->updateNodeColorByValue("In WM", colors, 1, false, 1);
 			}
 		}
 		QGraphicsRectItem * rectScaleItem = 0;
@@ -8085,10 +8166,12 @@ void DatabaseViewer::refineConstraint()
 {
 	int from = ids_.at(ui_->horizontalSlider_A->value());
 	int to = ids_.at(ui_->horizontalSlider_B->value());
-	refineConstraint(from, to, false);
+	RegistrationIcp regProximity(ui_->parameters_toolbox->getParameters());
+	std::shared_ptr<Registration> reg(Registration::create(ui_->parameters_toolbox->getParameters()));
+	refineConstraint(from, to, reg.get(), &regProximity, false);
 }
 
-void DatabaseViewer::refineConstraint(int from, int to, bool silent)
+void DatabaseViewer::refineConstraint(int from, int to, Registration * reg, RegistrationIcp * regProximity, bool silent)
 {
 	UDEBUG("%d -> %d", from, to);
 	bool switchedIds = false;
@@ -8361,8 +8444,7 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
 				fromScan.is2d()?Transform(0,0,fromScan.localTransform().z(),0,0,0):Transform::getIdentity()));
 
 		toS = new Signature(assembledData);
-		RegistrationIcp registrationIcp(parameters);
-		transform = registrationIcp.computeTransformationMod(*fromS, *toS, currentLink.transform(), &info);
+		transform = regProximity->computeTransformationMod(*fromS, *toS, currentLink.transform(), &info);
 		if(!transform.isNull())
 		{
 			// local scan matching proximity detection should have higher variance (see Rtabmap::process())
@@ -8380,7 +8462,6 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
 		}
 
 		bool reextractVisualFeatures = uStr2Bool(parameters.at(Parameters::kRGBDLoopClosureReextractFeatures()));
-		Registration * reg = Registration::create(parameters);
 		if( reg->isScanRequired() ||
 			reg->isUserDataRequired() ||
 			reextractVisualFeatures ||
@@ -8477,8 +8558,6 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
 			transform = reg->computeTransformationMod(*toS, *fromS, t.isNull()?t:t.inverse(), &info);
 			switchedIds = true;
 		}
-
-		delete reg;
 	}
 	UINFO("(%d ->%d) Registration time: %f s", currentLink.from(), currentLink.to(), timer.ticks());
 
@@ -8610,11 +8689,14 @@ void DatabaseViewer::addConstraint()
 {
 	int from = ids_.at(ui_->horizontalSlider_A->value());
 	int to = ids_.at(ui_->horizontalSlider_B->value());
-	addConstraint(from, to, false);
+	std::shared_ptr<Registration> reg(Registration::create(ui_->parameters_toolbox->getParameters()));
+	addConstraint(from, to, reg.get(), false);
 }
 
-bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool silentlyUseOptimizedGraphAsGuess)
+bool DatabaseViewer::addConstraint(int from, int to, Registration * reg, bool silent, bool silentlyUseOptimizedGraphAsGuess)
 {
+	UASSERT(reg);
+
 	bool switchedIds = false;
 	if(from == to)
 	{
@@ -8641,7 +8723,6 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool silentlyU
 		UASSERT(!containsLink(linksRefined_, from, to));
 
 		ParametersMap parameters = ui_->parameters_toolbox->getParameters();
-		Registration * reg = Registration::create(parameters);
 
 		bool loopCovLimited = Parameters::defaultRGBDLoopCovLimited();
 		Parameters::parse(parameters, Parameters::kRGBDLoopCovLimited(), loopCovLimited);
@@ -8823,7 +8904,6 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool silentlyU
 		{
 			t = reg->computeTransformationMod(*fromS, *toS, guess, &info);
 		}
-		delete reg;
 		UDEBUG("");
 
 		if(!t.isNull())

guilib/src/ExportBundlerDialog.cpp

@@ -194,17 +194,39 @@ void ExportBundlerDialog::exportBundler(
 			ParametersMap parametersSBA = parameters;
 			uInsert(parametersSBA, std::make_pair(Parameters::kOptimizerIterations(), uNumber2Str(_ui->sba_iterations->value())));
 			uInsert(parametersSBA, std::make_pair(Parameters::kg2oPixelVariance(), uNumber2Str(_ui->sba_variance->value())));
-			Optimizer * sba = Optimizer::create(sbaType, parametersSBA);
+			std::shared_ptr<Optimizer> sba(Optimizer::create(sbaType, parametersSBA));
 			sba->getConnectedGraph(poses.begin()->first, poses, links, posesOut, linksOut);
-			poses = sba->optimizeBA(
-					posesOut.begin()->first,
-					posesOut,
-					linksOut,
+			// set input poses as initial optimization guess
+			for(std::map<int, Transform>::iterator iter=posesOut.begin(); iter!=posesOut.end(); ++iter)
+			{
+				iter->second = poses.at(iter->first);
+			}
+			if(_ui->sba_iterations->value() > 0)
+			{
+				UINFO("Do BA optimization with %d iterations.", _ui->sba_iterations->value());
+				poses = sba->optimizeBA(
+						posesOut.begin()->first,
+						posesOut,
+						linksOut,
+						signatures.toStdMap(),
+						points3DMap,
+						wordReferences,
+						_ui->sba_rematchFeatures->isChecked(),
+						parametersSBA);
+			}
+			else
+			{
+				UINFO("Do not optimize, just compute 3D features and word correspondences.");
+				poses = posesOut;
+				sba->computeBACorrespondences(poses,
+					linksOut, 
 					signatures.toStdMap(),
 					points3DMap,
 					wordReferences,
-					_ui->sba_rematchFeatures->isChecked());
-			delete sba;
+					_ui->sba_rematchFeatures->isChecked(),
+					false,
+					parametersSBA);
+			}
 
 			if(poses.empty())
 			{

guilib/src/ExportCloudsDialog.cpp

@@ -151,6 +151,9 @@ ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) :
 	connect(_ui->lineEdit_distortionModel, SIGNAL(textChanged(const QString &)), this, SIGNAL(configChanged()));
 	connect(_ui->toolButton_distortionModel, SIGNAL(clicked()), this, SLOT(selectDistortionModel()));
 
+	connect(_ui->spinBox_depthConfidence, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_depthEdgeFiltering, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+
 	connect(_ui->checkBox_bilateral, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->checkBox_bilateral, SIGNAL(stateChanged(int)), this, SLOT(updateReconstructionFlavor()));
 	connect(_ui->doubleSpinBox_bilateral_sigmaS, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
@@ -209,6 +212,7 @@ ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) :
 	connect(_ui->spinBox_camProjDecimation, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->doubleSpinBox_camProjMaxDistance, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
 	connect(_ui->doubleSpinBox_camProjMaxAngle, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_camProjMaxDepthError, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
 	connect(_ui->checkBox_camProjDistanceToCamPolicy, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->checkBox_camProjKeepPointsNotSeenByCameras, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->checkBox_camProjRecolorPoints, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
@@ -400,6 +404,8 @@ void ExportCloudsDialog::saveSettings(QSettings & settings, const QString & grou
 	settings.setValue("regenerate_fill_error", _ui->spinBox_fillDepthHolesError->value());
 	settings.setValue("regenerate_roi", _ui->lineEdit_roiRatios->text());
 	settings.setValue("regenerate_distortion_model", _ui->lineEdit_distortionModel->text());
+	settings.setValue("regenerate_min_depth_confidence", _ui->spinBox_depthConfidence->value());
+	settings.setValue("regenerate_edge_bleeding_error", _ui->doubleSpinBox_depthEdgeFiltering->value());
 
 	settings.setValue("bilateral", _ui->checkBox_bilateral->isChecked());
 	settings.setValue("bilateral_sigma_s", _ui->doubleSpinBox_bilateral_sigmaS->value());
@@ -443,6 +449,7 @@ void ExportCloudsDialog::saveSettings(QSettings & settings, const QString & grou
 	settings.setValue("cam_proj_decimation", _ui->spinBox_camProjDecimation->value());
 	settings.setValue("cam_proj_max_distance", _ui->doubleSpinBox_camProjMaxDistance->value());
 	settings.setValue("cam_proj_max_angle", _ui->doubleSpinBox_camProjMaxAngle->value());
+	settings.setValue("cam_proj_max_depth_error", _ui->doubleSpinBox_camProjMaxDepthError->value());
 	settings.setValue("cam_proj_distance_policy", _ui->checkBox_camProjDistanceToCamPolicy->isChecked());
 	settings.setValue("cam_proj_keep_points", _ui->checkBox_camProjKeepPointsNotSeenByCameras->isChecked());
 	settings.setValue("cam_proj_recolor_points", _ui->checkBox_camProjRecolorPoints->isChecked());
@@ -582,6 +589,8 @@ void ExportCloudsDialog::loadSettings(QSettings & settings, const QString & grou
 	_ui->spinBox_fillDepthHolesError->setValue(settings.value("regenerate_fill_error", _ui->spinBox_fillDepthHolesError->value()).toInt());
 	_ui->lineEdit_roiRatios->setText(settings.value("regenerate_roi", _ui->lineEdit_roiRatios->text()).toString());
 	_ui->lineEdit_distortionModel->setText(settings.value("regenerate_distortion_model", _ui->lineEdit_distortionModel->text()).toString());
+	_ui->spinBox_depthConfidence->setValue(settings.value("regenerate_min_depth_confidence", _ui->spinBox_depthConfidence->value()).toInt());
+	_ui->doubleSpinBox_depthEdgeFiltering->setValue(settings.value("regenerate_edge_bleeding_error", _ui->doubleSpinBox_depthEdgeFiltering->value()).toDouble());
 
 	_ui->checkBox_bilateral->setChecked(settings.value("bilateral", _ui->checkBox_bilateral->isChecked()).toBool());
 	_ui->doubleSpinBox_bilateral_sigmaS->setValue(settings.value("bilateral_sigma_s", _ui->doubleSpinBox_bilateral_sigmaS->value()).toDouble());
@@ -628,6 +637,7 @@ void ExportCloudsDialog::loadSettings(QSettings & settings, const QString & grou
 	_ui->spinBox_camProjDecimation->setValue(settings.value("cam_proj_decimation", _ui->spinBox_camProjDecimation->value()).toInt());
 	_ui->doubleSpinBox_camProjMaxDistance->setValue(settings.value("cam_proj_max_distance", _ui->doubleSpinBox_camProjMaxDistance->value()).toDouble());
 	_ui->doubleSpinBox_camProjMaxAngle->setValue(settings.value("cam_proj_max_angle", _ui->doubleSpinBox_camProjMaxAngle->value()).toDouble());
+	_ui->doubleSpinBox_camProjMaxDepthError->setValue(settings.value("cam_proj_max_depth_error", _ui->doubleSpinBox_camProjMaxDepthError->value()).toDouble());
 	_ui->checkBox_camProjDistanceToCamPolicy->setChecked(settings.value("cam_proj_distance_policy", _ui->checkBox_camProjDistanceToCamPolicy->isChecked()).toBool());
 	_ui->checkBox_camProjKeepPointsNotSeenByCameras->setChecked(settings.value("cam_proj_keep_points", _ui->checkBox_camProjKeepPointsNotSeenByCameras->isChecked()).toBool());
 	_ui->checkBox_camProjRecolorPoints->setChecked(settings.value("cam_proj_recolor_points", _ui->checkBox_camProjRecolorPoints->isChecked()).toBool());
@@ -767,6 +777,8 @@ void ExportCloudsDialog::restoreDefaults()
 	_ui->spinBox_fillDepthHolesError->setValue(2);
 	_ui->lineEdit_roiRatios->setText("0.0 0.0 0.0 0.0");
 	_ui->lineEdit_distortionModel->setText("");
+	_ui->spinBox_depthConfidence->setValue(0);
+	_ui->doubleSpinBox_depthEdgeFiltering->setValue(0.0);
 
 	_ui->checkBox_bilateral->setChecked(false);
 	_ui->doubleSpinBox_bilateral_sigmaS->setValue(10.0);
@@ -810,6 +822,7 @@ void ExportCloudsDialog::restoreDefaults()
 	_ui->spinBox_camProjDecimation->setValue(1);
 	_ui->doubleSpinBox_camProjMaxDistance->setValue(0);
 	_ui->doubleSpinBox_camProjMaxAngle->setValue(0);
+	_ui->doubleSpinBox_camProjMaxDepthError->setValue(0);
 	_ui->checkBox_camProjDistanceToCamPolicy->setChecked(true);
 	_ui->checkBox_camProjKeepPointsNotSeenByCameras->setChecked(false);
 	_ui->checkBox_camProjRecolorPoints->setChecked(true);
@@ -2911,6 +2924,7 @@ bool ExportCloudsDialog::getExportedClouds(
 						cameraModelsProj,
 						_ui->doubleSpinBox_camProjMaxDistance->value(),
 						_ui->doubleSpinBox_camProjMaxAngle->value()*M_PI/180.0,
+						_ui->doubleSpinBox_camProjMaxDepthError->value(),
 						roiRatios,
 						projMask,
 						_ui->checkBox_camProjDistanceToCamPolicy->isChecked(),
@@ -3680,20 +3694,30 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
 				{
 					const Signature & s = cachedSignatures.find(iter->first).value();
 					data = s.sensorData();
-					cv::Mat image,depth;
+					cv::Mat image,depth,confidence;
 					data.uncompressData(
 							_ui->checkBox_fromDepth->isChecked()?&image:0,
 							_ui->checkBox_fromDepth->isChecked()?&depth:0,
-							!_ui->checkBox_fromDepth->isChecked()?&scan:0);
+							!_ui->checkBox_fromDepth->isChecked()?&scan:0,
+							0,
+							0,
+							0,
+							0,
+							_ui->checkBox_fromDepth->isChecked()?&confidence:0);
 				}
 				else if(_dbDriver)
 				{
-					cv::Mat image,depth;
+					cv::Mat image,depth,confidence;
 					_dbDriver->getNodeData(iter->first, data, _ui->checkBox_fromDepth->isChecked(), !_ui->checkBox_fromDepth->isChecked(), false, false);
 					data.uncompressData(
 							_ui->checkBox_fromDepth->isChecked()?&image:0,
 							_ui->checkBox_fromDepth->isChecked()?&depth:0,
-							!_ui->checkBox_fromDepth->isChecked()?&scan:0);
+							!_ui->checkBox_fromDepth->isChecked()?&scan:0,
+							0,
+							0,
+							0,
+							0,
+							_ui->checkBox_fromDepth->isChecked()?&confidence:0);
 				}
 
 				if(_ui->checkBox_fromDepth->isChecked() && !data.imageRaw().empty() && !data.depthOrRightRaw().empty())
@@ -3714,6 +3738,12 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
 						model.undistort(depth);
 					}
 
+					// edge bleeding filter
+					if(!depth.empty() && _ui->doubleSpinBox_depthEdgeFiltering->value()>0.0)
+					{
+						util2d::depthBleedingFiltering(depth, _ui->doubleSpinBox_depthEdgeFiltering->value());
+					}
+
 					// bilateral filtering
 					if(!depth.empty() && _ui->checkBox_bilateral->isChecked())
 					{
@@ -3724,7 +3754,7 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
 
 					if(!depth.empty())
 					{
-						data.setRGBDImage(data.imageRaw(), depth, data.cameraModels());
+						data.setRGBDImage(data.imageRaw(), depth, data.depthConfidenceRaw(), data.cameraModels());
 					}
 
 					UASSERT(iter->first == data.id());
@@ -3749,7 +3779,8 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
 							_ui->doubleSpinBox_minDepth->value(),
 							indices.get(),
 							parameters,
-							roiRatios);
+							roiRatios,
+							(unsigned char)_ui->spinBox_depthConfidence->value());
 
 					if(cloudWithoutNormals->size())
 					{

guilib/src/ImageView.cpp

@@ -181,7 +181,8 @@ ImageView::ImageView(QWidget * parent) :
 		_depthColorMapMinRange(0),
 		_depthColorMapMaxRange(0),
 		_imageItem(0),
-		_imageDepthItem(0)
+		_imageDepthItem(0),
+		_imageDepthConfidenceItem(0)
 {
 #if QT_VERSION >= 0x050000
 	_savedFileName = QStandardPaths::writableLocation(QStandardPaths::PicturesLocation);
@@ -209,9 +210,12 @@ ImageView::ImageView(QWidget * parent) :
 	_showImage = _menu->addAction(tr("Show image"));
 	_showImage->setCheckable(true);
 	_showImage->setChecked(true);
-	_showImageDepth = _menu->addAction(tr("Show image depth"));
+	_showImageDepth = _menu->addAction(tr("Show depth image"));
 	_showImageDepth->setCheckable(true);
 	_showImageDepth->setChecked(false);
+	_showImageDepthConfidence = _menu->addAction(tr("Show depth confidence"));
+	_showImageDepthConfidence->setCheckable(true);
+	_showImageDepthConfidence->setChecked(false);
 	_featureMenu = _menu->addMenu("Features");
 	_showFeatures = _featureMenu->addAction(tr("Show features"));
 	_showFeatures->setCheckable(true);
@@ -293,6 +297,7 @@ void ImageView::saveSettings(QSettings & settings, const QString & group) const
 	}
 	settings.setValue("image_shown", this->isImageShown());
 	settings.setValue("depth_shown", this->isImageDepthShown());
+	settings.setValue("confidence_shown", this->isImageDepthConfidenceShown());
 	settings.setValue("features_shown", this->isFeaturesShown());
 	settings.setValue("features_size", this->getFeaturesSize());
 	settings.setValue("lines_shown", this->isLinesShown());
@@ -322,6 +327,7 @@ void ImageView::loadSettings(QSettings & settings, const QString & group)
 	}
 	this->setImageShown(settings.value("image_shown", this->isImageShown()).toBool());
 	this->setImageDepthShown(settings.value("depth_shown", this->isImageDepthShown()).toBool());
+	this->setImageDepthConfidenceShown(settings.value("confidence_shown", this->isImageDepthConfidenceShown()).toBool());
 	this->setFeaturesShown(settings.value("features_shown", this->isFeaturesShown()).toBool());
 	this->setFeaturesSize(settings.value("features_size", this->getFeaturesSize()).toInt());
 	this->setLinesShown(settings.value("lines_shown", this->isLinesShown()).toBool());
@@ -363,6 +369,11 @@ bool ImageView::isImageDepthShown() const
 	return _showImageDepth->isChecked();
 }
 
+bool ImageView::isImageDepthConfidenceShown() const
+{
+	return _showImageDepthConfidence->isChecked();
+}
+
 bool ImageView::isFeaturesShown() const
 {
 	return _showFeatures->isChecked();
@@ -466,6 +477,13 @@ void ImageView::setImageShown(bool shown)
 void ImageView::setImageDepthShown(bool shown)
 {
 	_showImageDepth->setChecked(shown);
+	if(shown)
+	{
+		_showImageDepthConfidence->setChecked(false);
+		if(_imageDepthConfidenceItem) {
+			_imageDepthConfidenceItem->setVisible(false);
+		}
+	}
 	if(_imageDepthItem)
 	{
 		_imageDepthItem->setVisible(_showImageDepth->isChecked());
@@ -478,6 +496,28 @@ void ImageView::setImageDepthShown(bool shown)
 	}
 }
 
+void ImageView::setImageDepthConfidenceShown(bool shown)
+{
+	_showImageDepthConfidence->setChecked(shown);
+	if(shown)
+	{
+		_showImageDepth->setChecked(false);
+		if(_imageDepthItem) {
+			_imageDepthItem->setVisible(false);
+		}
+	}
+	if(_imageDepthConfidenceItem)
+	{
+		_imageDepthConfidenceItem->setVisible(_showImageDepthConfidence->isChecked());
+		this->updateOpacity();
+	}
+
+	if(!_graphicsView->isVisible())
+	{
+		this->update();
+	}
+}
+
 bool ImageView::isLinesShown() const
 {
 	return _showLines->isChecked();
@@ -517,6 +557,8 @@ void ImageView::setGraphicsViewMode(bool on)
 	_graphicsView->setVisible(on);
 	_scaleMenu->setEnabled(on);
 
+	_mouseTracking->setEnabled(!on);
+
 	if(on)
 	{
 		for(QMultiMap<int, KeypointItem*>::iterator iter=_features.begin(); iter!=_features.end(); ++iter)
@@ -550,11 +592,20 @@ void ImageView::setGraphicsViewMode(bool on)
 		{
 			_imageDepthItem->setPixmap(_imageDepth);
 		}
-		else
+		else if(!_imageDepth.isNull())
 		{
 			_imageDepthItem = _graphicsView->scene()->addPixmap(_imageDepth);
 			_imageDepthItem->setVisible(_showImageDepth->isChecked());
 		}
+		if(_imageDepthConfidenceItem)
+		{
+			_imageDepthConfidenceItem->setPixmap(_imageDepthConfidence);
+		}
+		else if(!_imageDepthConfidence.isNull())
+		{
+			_imageDepthConfidenceItem = _graphicsView->scene()->addPixmap(_imageDepthConfidence);
+			_imageDepthConfidenceItem->setVisible(_showImageDepthConfidence->isChecked());
+		}
 		this->updateOpacity();
 
 		if(_graphicsViewScaled->isChecked())
@@ -790,7 +841,8 @@ void ImageView::paintEvent(QPaintEvent *event)
 
 			painter.save();
 			if(_showImage->isChecked() && !_image.isNull() &&
-			   _showImageDepth->isChecked() && !_imageDepth.isNull())
+			   ((_showImageDepth->isChecked() && !_imageDepth.isNull()) || 
+			    (_showImageDepthConfidence->isChecked()&&!_imageDepthConfidence.isNull())))
 			{
 				painter.setOpacity(0.5);
 			}
@@ -804,6 +856,10 @@ void ImageView::paintEvent(QPaintEvent *event)
 			{
 				painter.drawPixmap(QPoint(0,0), _imageDepth);
 			}
+			else if(_showImageDepthConfidence->isChecked() && !_imageDepthConfidence.isNull())
+			{
+				painter.drawPixmap(QPoint(0,0), _imageDepthConfidence);
+			}
 			painter.restore();
 
 			if(_showFeatures->isChecked())
@@ -966,6 +1022,11 @@ void ImageView::contextMenuEvent(QContextMenuEvent * e)
 		this->setImageDepthShown(_showImageDepth->isChecked());
 		Q_EMIT configChanged();
 	}
+	else if(action == _showImageDepthConfidence)
+	{
+		this->setImageDepthConfidenceShown(_showImageDepthConfidence->isChecked());
+		Q_EMIT configChanged();
+	}
 	else if(action == _showLines)
 	{
 		this->setLinesShown(_showLines->isChecked());
@@ -988,8 +1049,9 @@ void ImageView::contextMenuEvent(QContextMenuEvent * e)
 	}
 	else if(action == _colorMapBlackToWhite || action == _colorMapWhiteToBlack || action == _colorMapRedToBlue || action == _colorMapBlueToRed)
 	{
-		if(!_imageDepthCv.empty())
-			this->setImageDepth(_imageDepthCv);
+		if(!_imageDepthCv.empty()) {
+			this->setImageDepth(_imageDepthCv, _imageDepthConfidenceCv);
+		}
 		Q_EMIT configChanged();
 	}
 	else if(action == _colorMapMinRange)
@@ -999,8 +1061,9 @@ void ImageView::contextMenuEvent(QContextMenuEvent * e)
 		if(ok)
 		{
 			this->setDepthColorMapRange(value, _depthColorMapMaxRange);
-			if(!_imageDepthCv.empty())
-				this->setImageDepth(_imageDepthCv);
+			if(!_imageDepthCv.empty()) {
+				this->setImageDepth(_imageDepthCv, _imageDepthConfidenceCv);
+			}
 			Q_EMIT configChanged();
 		}
 	}
@@ -1011,8 +1074,9 @@ void ImageView::contextMenuEvent(QContextMenuEvent * e)
 		if(ok)
 		{
 			this->setDepthColorMapRange(_depthColorMapMinRange, value);
-			if(!_imageDepthCv.empty())
-				this->setImageDepth(_imageDepthCv);
+			if(!_imageDepthCv.empty()) {
+				this->setImageDepth(_imageDepthCv, _imageDepthConfidenceCv);
+			}
 			Q_EMIT configChanged();
 		}
 	}
@@ -1047,7 +1111,7 @@ void ImageView::contextMenuEvent(QContextMenuEvent * e)
 		}
 	}
 
-	if(action == _showImage || action ==_showImageDepth)
+	if(action == _showImage || action ==_showImageDepth || action ==_showImageDepthConfidence)
 	{
 		this->updateOpacity();
 		Q_EMIT configChanged();
@@ -1059,17 +1123,16 @@ void ImageView::mouseMoveEvent(QMouseEvent * event)
 	if(_mouseTracking->isChecked() &&
 		!_graphicsView->scene()->sceneRect().isNull() &&
 		!_image.isNull() &&
-		!_imageDepthCv.empty() &&
-		(_imageDepthCv.type() == CV_16UC1 || _imageDepthCv.type() == CV_32FC1))
+		!_imageDepthCv.empty() &&(_imageDepthCv.type() == CV_16UC1 || _imageDepthCv.type() == CV_32FC1))
 	{
 		float scale, offsetX, offsetY;
 		computeScaleOffsets(this->rect(), scale, offsetX, offsetY);
 		float u = (event->pos().x() - offsetX) / scale;
 		float v = (event->pos().y() - offsetY) / scale;
 		float depthScale = 1;
-		if(_image.width() > _imageDepth.width())
+		if(_image.width() > _imageDepthCv.cols)
 		{
-			depthScale = _imageDepth.width() / _image.width();
+			depthScale = float(_imageDepthCv.cols) / float(_image.width());
 		}
 		int ud = int(u*depthScale);
 		int vd = int(v*depthScale);
@@ -1077,33 +1140,47 @@ void ImageView::mouseMoveEvent(QMouseEvent * event)
 			ud < _imageDepthCv.cols &&
 			vd < _imageDepthCv.rows)
 		{
-			float depth = 0;
-			if(_imageDepthCv.type() == CV_32FC1)
+			if( _showImageDepthConfidence->isChecked() &&
+				!_imageDepthConfidenceCv.empty() && 
+				_imageDepthCv.size() == _imageDepthConfidenceCv.size())
 			{
-				depth = _imageDepthCv.at<float>(vd, ud);
+				unsigned char confidence = _imageDepthConfidenceCv.at<unsigned char>(vd, ud);
+#if QT_VERSION >= QT_VERSION_CHECK(6, 0, 0)
+				QToolTip::showText(event->globalPosition().toPoint(), tr("Confidence=%1%").arg(confidence));
+#else
+				QToolTip::showText(event->globalPos(), tr("Confidence=%1%").arg(confidence));
+#endif
 			}
 			else
 			{
-				depth = float(_imageDepthCv.at<unsigned short>(vd, ud)) / 1000.0f;
-			}
+				float depth = 0;
+				if(_imageDepthCv.type() == CV_32FC1)
+				{
+					depth = _imageDepthCv.at<float>(vd, ud);
+				}
+				else
+				{
+					depth = float(_imageDepthCv.at<unsigned short>(vd, ud)) / 1000.0f;
+				}
 
-			cv::Point3f pt(0,0,0);
-			if(depth>0 && !_models.empty() && !_pose.isNull())
-			{
-				int subImageWidth = _imageDepthCv.cols / _models.size();
-				int subImageIndex = ud / subImageWidth;
-				UASSERT(subImageIndex < (int)_models.size());
-				float x,y,z;
-				_models[subImageIndex].project(u, v, depth, x, y, z);
-				pt = cv::Point3f(x,y,z);
-				pt = util3d::transformPoint(pt, _pose*_models[subImageIndex].localTransform());
-			}
+				cv::Point3f pt(0,0,0);
+				if(depth>0 && !_models.empty() && !_pose.isNull())
+				{
+					int subImageWidth = _imageDepthCv.cols / _models.size();
+					int subImageIndex = ud / subImageWidth;
+					UASSERT(subImageIndex < (int)_models.size());
+					float x,y,z;
+					_models[subImageIndex].project(u, v, depth, x, y, z);
+					pt = cv::Point3f(x,y,z);
+					pt = util3d::transformPoint(pt, _pose*_models[subImageIndex].localTransform());
+				}
 
 #if QT_VERSION >= QT_VERSION_CHECK(6, 0, 0)
-			QToolTip::showText(event->globalPosition().toPoint(), pt.x!=0?tr("Depth=%1m Map=(%2,%3,%4)").arg(depth).arg(pt.x).arg(pt.y).arg(pt.z):depth > 0?tr("Depth=%1m").arg(depth):tr("Depth=NA"));
+				QToolTip::showText(event->globalPosition().toPoint(), pt.x!=0?tr("Depth=%1m Map=(%2,%3,%4)").arg(depth).arg(pt.x).arg(pt.y).arg(pt.z):depth > 0?tr("Depth=%1m").arg(depth):tr("Depth=NA"));
 #else
-			QToolTip::showText(event->globalPos(), pt.x!=0?tr("Depth=%1m Map=(%2,%3,%4)").arg(depth).arg(pt.x).arg(pt.y).arg(pt.z):depth > 0?tr("Depth=%1m").arg(depth):tr("Depth=NA"));
+				QToolTip::showText(event->globalPos(), pt.x!=0?tr("Depth=%1m Map=(%2,%3,%4)").arg(depth).arg(pt.x).arg(pt.y).arg(pt.z):depth > 0?tr("Depth=%1m").arg(depth):tr("Depth=NA"));
 #endif
+			}
 		}
 		else
 		{
@@ -1115,23 +1192,31 @@ void ImageView::mouseMoveEvent(QMouseEvent * event)
 
 void ImageView::updateOpacity()
 {
-	if(_imageItem && _imageDepthItem)
+	if(_imageItem && (_imageDepthItem || _imageDepthConfidenceItem))
 	{
-		if(_imageItem->isVisible() && _imageDepthItem->isVisible())
+		if(_imageItem->isVisible() && 
+			((_imageDepthItem && _imageDepthItem->isVisible()) || 
+			 (_imageDepthConfidenceItem && _imageDepthConfidenceItem->isVisible())))
 		{
-			QGraphicsOpacityEffect * effect = new QGraphicsOpacityEffect();
-			effect->setOpacity(0.5);
-			_imageDepthItem->setGraphicsEffect(effect);
-		}
-		else
-		{
-			_imageDepthItem->setGraphicsEffect(0);
+			if(_imageDepthItem) {
+				QGraphicsOpacityEffect * effect = new QGraphicsOpacityEffect();
+				effect->setOpacity(0.5);
+				_imageDepthItem->setGraphicsEffect(effect);
+			}
+			if(_imageDepthConfidenceItem) {
+				QGraphicsOpacityEffect * effect = new QGraphicsOpacityEffect();
+				effect->setOpacity(0.5);
+				_imageDepthConfidenceItem->setGraphicsEffect(effect);
+			}
+			return;
 		}
 	}
-	else if(_imageDepthItem)
-	{
+	if(_imageDepthItem) {
 		_imageDepthItem->setGraphicsEffect(0);
 	}
+	if(_imageDepthConfidenceItem) {
+		_imageDepthConfidenceItem->setGraphicsEffect(0);
+	}
 }
 
 void ImageView::setFeatures(const std::multimap<int, cv::KeyPoint> & refWords, const cv::Mat & depth, const QColor & color)
@@ -1261,15 +1346,23 @@ void ImageView::setImage(const QImage & image, const std::vector<CameraModel> &
 	}
 }
 
-void ImageView::setImageDepth(const cv::Mat & imageDepth)
+void ImageView::setImageDepth(const cv::Mat & imageDepth, const cv::Mat & imageDepthConfidence)
 {
 	_imageDepthCv = imageDepth;
-	setImageDepth(uCvMat2QImage(_imageDepthCv, true, getDepthColorMap(), _depthColorMapMinRange, _depthColorMapMaxRange));
+	_imageDepthConfidenceCv = imageDepthConfidence;
+	setImageDepth(
+		uCvMat2QImage(_imageDepthCv, true, getDepthColorMap(), _depthColorMapMinRange, _depthColorMapMaxRange),
+		uCvMat2QImage(_imageDepthConfidenceCv, true, getDepthColorMap()));
 }
 
-void ImageView::setImageDepth(const QImage & imageDepth)
+void ImageView::setImageDepth(const QImage & imageDepth, const QImage & imageDepthConfidence)
 {
 	_imageDepth = QPixmap::fromImage(imageDepth);
+	if(!imageDepthConfidence.isNull()) {
+		_imageDepthConfidence = QPixmap::fromImage(imageDepthConfidence);
+	} else {
+		_showImageDepthConfidence->setChecked(false);
+	}
 
 	UASSERT(_imageDepth.width() && _imageDepth.height());
 
@@ -1279,6 +1372,9 @@ void ImageView::setImageDepth(const QImage & imageDepth)
 	{
 		// scale depth to rgb
 		_imageDepth = _imageDepth.scaled(_image.size());
+		if(!_imageDepthConfidence.isNull()) {
+			_imageDepthConfidence = _imageDepthConfidence.scaled(_image.size());
+		}
 	}
 
 	if(_graphicsView->isVisible())
@@ -1286,11 +1382,18 @@ void ImageView::setImageDepth(const QImage & imageDepth)
 		if(_imageDepthItem)
 		{
 			_imageDepthItem->setPixmap(_imageDepth);
+			if(_imageDepthConfidenceItem) {
+				_imageDepthConfidenceItem->setPixmap(_imageDepthConfidence);
+			}
 		}
 		else
 		{
 			_imageDepthItem = _graphicsView->scene()->addPixmap(_imageDepth);
 			_imageDepthItem->setVisible(_showImageDepth->isChecked());
+			if(!_imageDepthConfidence.isNull()) {
+				_imageDepthConfidenceItem = _graphicsView->scene()->addPixmap(_imageDepthConfidence);
+				_imageDepthConfidenceItem->setVisible(_showImageDepthConfidence->isChecked());
+			}
 			this->updateOpacity();
 		}
 	}
@@ -1471,6 +1574,12 @@ void ImageView::clear()
 		delete _imageDepthItem;
 		_imageDepthItem = 0;
 	}
+	if(_imageDepthConfidenceItem)
+	{
+		_graphicsView->scene()->removeItem(_imageDepthConfidenceItem);
+		delete _imageDepthConfidenceItem;
+		_imageDepthConfidenceItem = 0;
+	}
 	_imageDepth = QPixmap();
 
 	_graphicsView->scene()->setSceneRect(QRectF());

guilib/src/MainWindow.cpp

@@ -1225,13 +1225,15 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
 				pcl::IndicesPtr indices(new std::vector<int>);
 
-				cloud = util3d::cloudRGBFromSensorData(*data,
+				cloud = util3d::cloudRGBFromSensorData(
+						*data,
 						_preferencesDialog->getCloudDecimation(1),
 						_preferencesDialog->getCloudMaxDepth(1),
 						_preferencesDialog->getCloudMinDepth(1),
 						indices.get(),
 						_preferencesDialog->getAllParameters(),
-						_preferencesDialog->getCloudRoiRatios(1));
+						_preferencesDialog->getCloudRoiRatios(1),
+						_preferencesDialog->getCloudConfidenceThr(1));
 				if(indices->size())
 				{
 					cloud = util3d::transformPointCloud(cloud, pose);
@@ -1717,7 +1719,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 			_ui->imageView_odometry->setImage(uCvMat2QImage(data->imageRaw()));
 			if(_ui->imageView_odometry->isImageDepthShown() && !data->depthOrRightRaw().empty())
 			{
-				_ui->imageView_odometry->setImageDepth(data->depthOrRightRaw());
+				_ui->imageView_odometry->setImageDepth(data->depthOrRightRaw(), data->depthConfidenceRaw());
 			}
 
 			if( odom.info().type == (int)Odometry::kTypeF2M ||
@@ -1908,9 +1910,8 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 		{
 			if(!odomT.isNull())
 			{
-				rtabmap::Transform diff = odom.info().transformGroundTruth.inverse()*odomT;
-				_ui->statsToolBox->updateStat("Odometry/TG_error_lin/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), diff.getNorm(), _preferencesDialog->isCacheSavedInFigures());
-				_ui->statsToolBox->updateStat("Odometry/TG_error_ang/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), diff.getAngle()*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
+				_ui->statsToolBox->updateStat("Odometry/TG_error_lin/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), odom.info().transformGroundTruth.getDistance(odomT), _preferencesDialog->isCacheSavedInFigures());
+				_ui->statsToolBox->updateStat("Odometry/TG_error_ang/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), odom.info().transformGroundTruth.getAngle(odomT)*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
 			}
 
 			odom.info().transformGroundTruth.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
@@ -2343,7 +2344,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 				}
 				if(!signature.sensorData().depthOrRightRaw().empty())
 				{
-					_ui->imageView_source->setImageDepth(signature.sensorData().depthOrRightRaw());
+					_ui->imageView_source->setImageDepth(signature.sensorData().depthOrRightRaw(), signature.sensorData().depthConfidenceRaw());
 				}
 				if(img.isNull() && signature.sensorData().depthOrRightRaw().empty())
 				{
@@ -2375,7 +2376,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 				}
 				if(!loopSignature.sensorData().depthOrRightRaw().empty())
 				{
-					_ui->imageView_loopClosure->setImageDepth(loopSignature.sensorData().depthOrRightRaw());
+					_ui->imageView_loopClosure->setImageDepth(loopSignature.sensorData().depthOrRightRaw(), loopSignature.sensorData().depthConfidenceRaw());
 				}
 				if(_ui->imageView_loopClosure->sceneRect().isNull())
 				{
@@ -3718,7 +3719,8 @@ std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> MainWindow::c
 				_preferencesDialog->getCloudMinDepth(0),
 				indices.get(),
 				allParameters,
-				_preferencesDialog->getCloudRoiRatios(0));
+				_preferencesDialog->getCloudRoiRatios(0),
+				_preferencesDialog->getCloudConfidenceThr(0));
 
 		// view point
 		Eigen::Vector3f viewPoint(0.0f,0.0f,0.0f);
@@ -6623,6 +6625,8 @@ void MainWindow::postProcessing(
 			odomMaxInf = graph::getMaxOdomInf(_currentLinksMap);
 		}
 
+		std::shared_ptr<Registration> registration(Registration::create(parameters));
+
 		UASSERT(iterations>0);
 		for(int n=0; n<iterations && !_progressCanceled; ++n)
 		{
@@ -6703,7 +6707,6 @@ void MainWindow::postProcessing(
 										{
 											uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "2"));
 										}
-										Registration * registration = Registration::create(parameters);
 
 										if(reextractFeatures)
 										{
@@ -6735,7 +6738,6 @@ void MainWindow::postProcessing(
 													Parameters::kRGBDLoopClosureReextractFeatures().c_str());
 										}
 										transform = registration->computeTransformation(signatureFrom, signatureTo, Transform(), &info);
-										delete registration;
 										if(!transform.isNull())
 										{
 											//optimize the graph to see if the new constraint is globally valid
@@ -7023,7 +7025,13 @@ void MainWindow::postProcessing(
 		uInsert(parametersSBA, std::make_pair(Parameters::kOptimizerIterations(), uNumber2Str(sbaIterations)));
 		uInsert(parametersSBA, std::make_pair(Parameters::kg2oPixelVariance(), uNumber2Str(sbaVariance)));
 		Optimizer * sbaOptimizer = Optimizer::create(sbaType, parametersSBA);
-		std::map<int, Transform>  newPoses = sbaOptimizer->optimizeBA(optimizedPoses.begin()->first, optimizedPoses, linksOut, _cachedSignatures.toStdMap(), sbaRematchFeatures);
+		std::map<int, Transform>  newPoses = sbaOptimizer->optimizeBA(
+			optimizedPoses.begin()->first,
+			optimizedPoses,
+			linksOut,
+			_cachedSignatures.toStdMap(),
+			sbaRematchFeatures,
+			parametersSBA);
 		delete sbaOptimizer;
 		if(newPoses.size())
 		{
@@ -8181,6 +8189,12 @@ void MainWindow::exportImages()
 							QDir dir;
 							dir.mkdir(QString("%1/rgb").arg(path));
 							dir.mkdir(QString("%1/depth").arg(path));
+
+							if(!data.depthConfidenceRaw().empty())
+							{
+								QDir dir;
+								dir.mkdir(QString("%1/confidence").arg(path));
+							}
 						}
 
 						if(data.cameraModels().size() > 1)
@@ -8239,7 +8253,14 @@ void MainWindow::exportImages()
 					UWARN("Failed saving \"%s\"", QString("%1/rgb/%2.%3").arg(path).arg(id).arg(ext).toStdString().c_str());
 				if(!cv::imwrite(QString("%1/depth/%2.png").arg(path).arg(id).toStdString(), data.depthRaw().type()==CV_32FC1?util2d::cvtDepthFromFloat(data.depthRaw()):data.depthRaw()))
 					UWARN("Failed saving \"%s\"", QString("%1/depth/%2.png").arg(path).arg(id).toStdString().c_str());
-				info = tr("Saved rgb/%1.%2 and depth/%1.png.").arg(id).arg(ext);
+				if(data.depthConfidenceRaw().empty()) {
+					info = tr("Saved rgb/%1.%2 and depth/%1.png.").arg(id).arg(ext);
+				}
+				else {
+					if(!cv::imwrite(QString("%1/confidence/%2.png").arg(path).arg(id).toStdString(), data.depthConfidenceRaw()))
+						UWARN("Failed saving \"%s\"", QString("%1/confidence/%2.png").arg(path).arg(id).toStdString().c_str());
+					info = tr("Saved rgb/%1.%2, depth/%1.png and confidence/%1.png.").arg(id).arg(ext);
+				}
 			}
 			else if(!data.imageRaw().empty())
 			{

guilib/src/PreferencesDialog.cpp

@@ -551,6 +551,10 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_3dRenderingRoiRatios[0] = _ui->lineEdit_roiRatios;
 	_3dRenderingRoiRatios[1] = _ui->lineEdit_roiRatios_odom;
 
+	_3dRenderingDepthConfidenceThr.resize(2);
+	_3dRenderingDepthConfidenceThr[0] = _ui->spinBox_depthConf;
+	_3dRenderingDepthConfidenceThr[1] = _ui->spinBox_depthConf_odom;
+
 	_3dRenderingColorScheme.resize(2);
 	_3dRenderingColorScheme[0] = _ui->spinBox_colorScheme;
 	_3dRenderingColorScheme[1] = _ui->spinBox_colorScheme_odom;
@@ -622,6 +626,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 		connect(_3dRenderingMaxDepth[i], SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 		connect(_3dRenderingMinDepth[i], SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 		connect(_3dRenderingRoiRatios[i], SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+		connect(_3dRenderingDepthConfidenceThr[i], SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 		connect(_3dRenderingShowScans[i], SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 		connect(_3dRenderingShowFeatures[i], SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 		connect(_3dRenderingShowFrustums[i], SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
@@ -1256,6 +1261,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->spinBox_maxOdomCacheSize->setObjectName(Parameters::kRGBDMaxOdomCacheSize().c_str());
 	_ui->checkbox_localizationSmoothing->setObjectName(Parameters::kRGBDLocalizationSmoothing().c_str());
 	_ui->doubleSpinBox_localizationPriorError->setObjectName(Parameters::kRGBDLocalizationPriorError().c_str());
+	_ui->checkbox_localizationSecondTryWithoutProximityLinks->setObjectName(Parameters::kRGBDLocalizationSecondTryWithoutProximityLinks().c_str());
 
 	// Registration
 	_ui->reg_repeatOnce->setObjectName(Parameters::kRegRepeatOnce().c_str());
@@ -2029,6 +2035,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 			_3dRenderingMaxDepth[i]->setValue(0.0);
 			_3dRenderingMinDepth[i]->setValue(0.0);
 			_3dRenderingRoiRatios[i]->setText("0.0 0.0 0.0 0.0");
+			_3dRenderingDepthConfidenceThr[i]->setValue(0);
 			_3dRenderingShowScans[i]->setChecked(true);
 			_3dRenderingShowFeatures[i]->setChecked(i==0?false:true);
 			_3dRenderingShowFrustums[i]->setChecked(false);
@@ -2547,6 +2554,7 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
 		_3dRenderingMaxDepth[i]->setValue(settings.value(QString("maxDepth%1").arg(i), _3dRenderingMaxDepth[i]->value()).toDouble());
 		_3dRenderingMinDepth[i]->setValue(settings.value(QString("minDepth%1").arg(i), _3dRenderingMinDepth[i]->value()).toDouble());
 		_3dRenderingRoiRatios[i]->setText(settings.value(QString("roiRatios%1").arg(i), _3dRenderingRoiRatios[i]->text()).toString());
+		_3dRenderingDepthConfidenceThr[i]->setValue(settings.value(QString("depthConf%1").arg(i), _3dRenderingDepthConfidenceThr[i]->value()).toInt());
 		_3dRenderingShowScans[i]->setChecked(settings.value(QString("showScans%1").arg(i), _3dRenderingShowScans[i]->isChecked()).toBool());
 		_3dRenderingShowFeatures[i]->setChecked(settings.value(QString("showFeatures%1").arg(i), _3dRenderingShowFeatures[i]->isChecked()).toBool());
 		_3dRenderingShowFrustums[i]->setChecked(settings.value(QString("showFrustums%1").arg(i), _3dRenderingShowFrustums[i]->isChecked()).toBool());
@@ -3155,6 +3163,7 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const
 		settings.setValue(QString("maxDepth%1").arg(i), _3dRenderingMaxDepth[i]->value());
 		settings.setValue(QString("minDepth%1").arg(i), _3dRenderingMinDepth[i]->value());
 		settings.setValue(QString("roiRatios%1").arg(i), _3dRenderingRoiRatios[i]->text());
+		settings.setValue(QString("depthConf%1").arg(i), _3dRenderingDepthConfidenceThr[i]->value());
 		settings.setValue(QString("showScans%1").arg(i), _3dRenderingShowScans[i]->isChecked());
 		settings.setValue(QString("showFeatures%1").arg(i), _3dRenderingShowFeatures[i]->isChecked());
 		settings.setValue(QString("showFrustums%1").arg(i), _3dRenderingShowFrustums[i]->isChecked());
@@ -6104,6 +6113,11 @@ std::vector<float> PreferencesDialog::getCloudRoiRatios(int index) const
 	}
 	return roiRatios;
 }
+unsigned char PreferencesDialog::getCloudConfidenceThr(int index) const
+{
+	UASSERT(index >= 0 && index <= 1);
+	return (unsigned char)_3dRenderingDepthConfidenceThr[index]->value();
+}
 int PreferencesDialog::getCloudColorScheme(int index) const
 {
 	UASSERT(index >= 0 && index <= 1);

guilib/src/ui/DatabaseViewer.ui

@@ -61,7 +61,7 @@
           <rect>
            <x>0</x>
            <y>0</y>
-           <width>419</width>
+           <width>307</width>
            <height>389</height>
           </rect>
          </property>
@@ -392,7 +392,7 @@
           <rect>
            <x>0</x>
            <y>0</y>
-           <width>418</width>
+           <width>306</width>
            <height>389</height>
           </rect>
          </property>
@@ -1643,7 +1643,7 @@
      <item>
       <widget class="QToolBox" name="toolBox">
        <property name="currentIndex">
-        <number>2</number>
+        <number>1</number>
        </property>
        <widget class="QWidget" name="page_3">
         <property name="geometry">
@@ -1805,9 +1805,9 @@
         <property name="geometry">
          <rect>
           <x>0</x>
-          <y>0</y>
-          <width>276</width>
-          <height>1201</height>
+          <y>-316</y>
+          <width>518</width>
+          <height>1071</height>
          </rect>
         </property>
         <attribute name="label">
@@ -1816,24 +1816,57 @@
         <layout class="QVBoxLayout" name="verticalLayout_16">
          <item>
           <layout class="QGridLayout" name="gridLayout_9" columnstretch="0,0">
-           <item row="6" column="1">
-            <widget class="QLabel" name="label_53">
+           <item row="3" column="1">
+            <widget class="QLabel" name="label_octomap_empty_3">
              <property name="text">
-              <string>OctoMap</string>
-             </property>
-            </widget>
-           </item>
-           <item row="15" column="1">
-            <widget class="QLabel" name="label_57">
-             <property name="text">
-              <string>Crop radius when filtering empty space from 2d occupancy grid.</string>
+              <string>Ground cell color</string>
              </property>
              <property name="wordWrap">
               <bool>true</bool>
              </property>
             </widget>
            </item>
-           <item row="15" column="0">
+           <item row="14" column="1">
+            <widget class="QLabel" name="label_61">
+             <property name="text">
+              <string>Minimum depth confidence.</string>
+             </property>
+             <property name="wordWrap">
+              <bool>true</bool>
+             </property>
+            </widget>
+           </item>
+           <item row="18" column="1">
+            <widget class="QLabel" name="label_59">
+             <property name="text">
+              <string>Show probabilistic occupancy grid in Graph View.</string>
+             </property>
+             <property name="wordWrap">
+              <bool>true</bool>
+             </property>
+            </widget>
+           </item>
+           <item row="11" column="0">
+            <widget class="QCheckBox" name="checkBox_grid_regenerateFromSavedGrid">
+             <property name="text">
+              <string/>
+             </property>
+             <property name="checked">
+              <bool>false</bool>
+             </property>
+            </widget>
+           </item>
+           <item row="0" column="0">
+            <widget class="QCheckBox" name="checkBox_grid_empty">
+             <property name="text">
+              <string/>
+             </property>
+             <property name="checked">
+              <bool>true</bool>
+             </property>
+            </widget>
+           </item>
+           <item row="17" column="0">
             <widget class="QSpinBox" name="spinBox_cropRadius">
              <property name="suffix">
               <string> pixels</string>
@@ -1843,13 +1876,13 @@
              </property>
             </widget>
            </item>
-           <item row="16" column="0">
-            <widget class="QCheckBox" name="checkBox_grid_showProbMap">
+           <item row="20" column="1">
+            <widget class="QLabel" name="label_logger_level_2">
              <property name="text">
-              <string/>
+              <string>For stereo data, show disparity instead of right image in main views.</string>
              </property>
-             <property name="checked">
-              <bool>false</bool>
+             <property name="wordWrap">
+              <bool>true</bool>
              </property>
             </widget>
            </item>
@@ -1869,33 +1902,17 @@
              </property>
             </widget>
            </item>
-           <item row="10" column="0">
-            <widget class="QSpinBox" name="spinBox_grid_depth">
-             <property name="prefix">
-              <string/>
-             </property>
-             <property name="minimum">
-              <number>0</number>
-             </property>
-             <property name="maximum">
-              <number>16</number>
-             </property>
-             <property name="value">
-              <number>16</number>
-             </property>
-            </widget>
-           </item>
-           <item row="3" column="0">
-            <layout class="QHBoxLayout" name="horizontalLayout_9">
+           <item row="5" column="0">
+            <layout class="QHBoxLayout" name="horizontalLayout_17">
              <item>
-              <widget class="QToolButton" name="toolButton_groundColor">
+              <widget class="QToolButton" name="toolButton_frontierColor">
                <property name="text">
                 <string>...</string>
                </property>
               </widget>
              </item>
              <item>
-              <widget class="QLineEdit" name="lineEdit_groundColor">
+              <widget class="QLineEdit" name="lineEdit_frontierColor">
                <property name="readOnly">
                 <bool>true</bool>
                </property>
@@ -1903,18 +1920,18 @@
              </item>
             </layout>
            </item>
-           <item row="9" column="1">
-            <widget class="QLabel" name="label_octomap_cubes">
+           <item row="17" column="1">
+            <widget class="QLabel" name="label_57">
              <property name="text">
-              <string>OctoMap: Rendering type</string>
+              <string>Crop radius when filtering empty space from 2d occupancy grid.</string>
              </property>
              <property name="wordWrap">
               <bool>true</bool>
              </property>
             </widget>
            </item>
-           <item row="1" column="0">
-            <widget class="QCheckBox" name="checkBox_grid_frontiers">
+           <item row="18" column="0">
+            <widget class="QCheckBox" name="checkBox_grid_showProbMap">
              <property name="text">
               <string/>
              </property>
@@ -1923,10 +1940,20 @@
              </property>
             </widget>
            </item>
-           <item row="14" column="1">
-            <widget class="QLabel" name="label_54">
+           <item row="19" column="1">
+            <widget class="QLabel" name="label_60">
              <property name="text">
-              <string>Voxel size (for clouds and scans)</string>
+              <string>Create RGB-D cloud from RGB projection on scan.</string>
+             </property>
+             <property name="wordWrap">
+              <bool>true</bool>
+             </property>
+            </widget>
+           </item>
+           <item row="11" column="1">
+            <widget class="QLabel" name="label_55">
+             <property name="text">
+              <string>Local grid: regenerate from saved grid instead of sensors</string>
              </property>
              <property name="wordWrap">
               <bool>true</bool>
@@ -1943,20 +1970,13 @@
              </property>
             </widget>
            </item>
-           <item row="11" column="0">
-            <widget class="QCheckBox" name="checkBox_grid_regenerateFromSavedGrid">
+           <item row="9" column="1">
+            <widget class="QLabel" name="label_octomap_cubes">
              <property name="text">
-              <string/>
+              <string>OctoMap: Rendering type</string>
              </property>
-             <property name="checked">
-              <bool>false</bool>
-             </property>
-            </widget>
-           </item>
-           <item row="6" column="0">
-            <widget class="QCheckBox" name="checkBox_octomap">
-             <property name="text">
-              <string/>
+             <property name="wordWrap">
+              <bool>true</bool>
              </property>
             </widget>
            </item>
@@ -1980,8 +2000,8 @@
              </property>
             </widget>
            </item>
-           <item row="18" column="0">
-            <widget class="QCheckBox" name="checkBox_showDisparityInsteadOfRight">
+           <item row="1" column="0">
+            <widget class="QCheckBox" name="checkBox_grid_frontiers">
              <property name="text">
               <string/>
              </property>
@@ -1990,105 +2010,7 @@
              </property>
             </widget>
            </item>
-           <item row="16" column="1">
-            <widget class="QLabel" name="label_59">
-             <property name="text">
-              <string>Show probabilistic occupancy grid in Graph View.</string>
-             </property>
-             <property name="wordWrap">
-              <bool>true</bool>
-             </property>
-            </widget>
-           </item>
-           <item row="18" column="1">
-            <widget class="QLabel" name="label_logger_level_2">
-             <property name="text">
-              <string>For stereo data, show disparity instead of right image in main views.</string>
-             </property>
-             <property name="wordWrap">
-              <bool>true</bool>
-             </property>
-            </widget>
-           </item>
-           <item row="3" column="1">
-            <widget class="QLabel" name="label_octomap_empty_3">
-             <property name="text">
-              <string>Ground cell color</string>
-             </property>
-             <property name="wordWrap">
-              <bool>true</bool>
-             </property>
-            </widget>
-           </item>
-           <item row="12" column="0">
-            <widget class="QDoubleSpinBox" name="doubleSpinBox_gainCompensationRadius">
-             <property name="suffix">
-              <string> m</string>
-             </property>
-             <property name="decimals">
-              <number>2</number>
-             </property>
-             <property name="minimum">
-              <double>0.000000000000000</double>
-             </property>
-             <property name="maximum">
-              <double>10.000000000000000</double>
-             </property>
-             <property name="singleStep">
-              <double>0.010000000000000</double>
-             </property>
-             <property name="value">
-              <double>0.000000000000000</double>
-             </property>
-            </widget>
-           </item>
-           <item row="4" column="1">
-            <widget class="QLabel" name="label_octomap_empty_4">
-             <property name="text">
-              <string>Empty cell color</string>
-             </property>
-             <property name="wordWrap">
-              <bool>true</bool>
-             </property>
-            </widget>
-           </item>
-           <item row="4" column="0">
-            <layout class="QHBoxLayout" name="horizontalLayout_12">
-             <item>
-              <widget class="QToolButton" name="toolButton_emptyColor">
-               <property name="text">
-                <string>...</string>
-               </property>
-              </widget>
-             </item>
-             <item>
-              <widget class="QLineEdit" name="lineEdit_emptyColor">
-               <property name="readOnly">
-                <bool>true</bool>
-               </property>
-              </widget>
-             </item>
-            </layout>
-           </item>
-           <item row="2" column="0">
-            <layout class="QHBoxLayout" name="horizontalLayout_11">
-             <item>
-              <widget class="QToolButton" name="toolButton_obstacleColor">
-               <property name="text">
-                <string>...</string>
-               </property>
-              </widget>
-             </item>
-             <item>
-              <widget class="QLineEdit" name="lineEdit_obstacleColor">
-               <property name="readOnly">
-                <bool>true</bool>
-               </property>
-              </widget>
-             </item>
-            </layout>
-           </item>
-           <item row="17" column="0">
+           <item row="19" column="0">
             <widget class="QCheckBox" name="checkBox_cameraProjection">
              <property name="text">
               <string/>
@@ -2098,20 +2020,10 @@
              </property>
             </widget>
            </item>
-           <item row="0" column="0">
-            <widget class="QCheckBox" name="checkBox_grid_empty">
+           <item row="0" column="1">
+            <widget class="QLabel" name="label_octomap_empty">
              <property name="text">
-              <string/>
-             </property>
-             <property name="checked">
-              <bool>true</bool>
-             </property>
-            </widget>
-           </item>
-           <item row="11" column="1">
-            <widget class="QLabel" name="label_55">
-             <property name="text">
-              <string>Local grid: regenerate from saved grid instead of sensors</string>
+              <string>Show empty space</string>
              </property>
              <property name="wordWrap">
               <bool>true</bool>
@@ -2128,20 +2040,17 @@
              </property>
             </widget>
            </item>
-           <item row="17" column="1">
-            <widget class="QLabel" name="label_60">
+           <item row="6" column="1">
+            <widget class="QLabel" name="label_53">
              <property name="text">
-              <string>Create RGB-D cloud from RGB projection on scan.</string>
-             </property>
-             <property name="wordWrap">
-              <bool>true</bool>
+              <string>OctoMap</string>
              </property>
             </widget>
            </item>
-           <item row="0" column="1">
-            <widget class="QLabel" name="label_octomap_empty">
+           <item row="16" column="1">
+            <widget class="QLabel" name="label_54">
              <property name="text">
-              <string>Show empty space</string>
+              <string>Voxel size (for clouds and scans)</string>
              </property>
              <property name="wordWrap">
               <bool>true</bool>
@@ -2167,6 +2076,64 @@
              </item>
             </widget>
            </item>
+           <item row="10" column="0">
+            <widget class="QSpinBox" name="spinBox_grid_depth">
+             <property name="prefix">
+              <string/>
+             </property>
+             <property name="minimum">
+              <number>0</number>
+             </property>
+             <property name="maximum">
+              <number>16</number>
+             </property>
+             <property name="value">
+              <number>16</number>
+             </property>
+            </widget>
+           </item>
+           <item row="12" column="0">
+            <widget class="QDoubleSpinBox" name="doubleSpinBox_gainCompensationRadius">
+             <property name="suffix">
+              <string> m</string>
+             </property>
+             <property name="decimals">
+              <number>2</number>
+             </property>
+             <property name="minimum">
+              <double>0.000000000000000</double>
+             </property>
+             <property name="maximum">
+              <double>10.000000000000000</double>
+             </property>
+             <property name="singleStep">
+              <double>0.010000000000000</double>
+             </property>
+             <property name="value">
+              <double>0.000000000000000</double>
+             </property>
+            </widget>
+           </item>
+           <item row="5" column="1">
+            <widget class="QLabel" name="label_octomap_frontiers_3">
+             <property name="text">
+              <string>Frontier cell color</string>
+             </property>
+             <property name="wordWrap">
+              <bool>true</bool>
+             </property>
+            </widget>
+           </item>
+           <item row="4" column="1">
+            <widget class="QLabel" name="label_octomap_empty_4">
+             <property name="text">
+              <string>Empty cell color</string>
+             </property>
+             <property name="wordWrap">
+              <bool>true</bool>
+             </property>
+            </widget>
+           </item>
            <item row="13" column="1">
             <widget class="QLabel" name="label_56">
              <property name="text">
@@ -2177,7 +2144,32 @@
              </property>
             </widget>
            </item>
-           <item row="14" column="0">
+           <item row="6" column="0">
+            <widget class="QCheckBox" name="checkBox_octomap">
+             <property name="text">
+              <string/>
+             </property>
+            </widget>
+           </item>
+           <item row="3" column="0">
+            <layout class="QHBoxLayout" name="horizontalLayout_9">
+             <item>
+              <widget class="QToolButton" name="toolButton_groundColor">
+               <property name="text">
+                <string>...</string>
+               </property>
+              </widget>
+             </item>
+             <item>
+              <widget class="QLineEdit" name="lineEdit_groundColor">
+               <property name="readOnly">
+                <bool>true</bool>
+               </property>
+              </widget>
+             </item>
+            </layout>
+           </item>
+           <item row="16" column="0">
             <widget class="QDoubleSpinBox" name="doubleSpinBox_voxelSize">
              <property name="suffix">
               <string> m</string>
@@ -2199,27 +2191,17 @@
              </property>
             </widget>
            </item>
-           <item row="5" column="1">
-            <widget class="QLabel" name="label_octomap_frontiers_3">
-             <property name="text">
-              <string>Frontier cell color</string>
-             </property>
-             <property name="wordWrap">
-              <bool>true</bool>
-             </property>
-            </widget>
-           </item>
-           <item row="5" column="0">
-            <layout class="QHBoxLayout" name="horizontalLayout_17">
+           <item row="2" column="0">
+            <layout class="QHBoxLayout" name="horizontalLayout_11">
              <item>
-              <widget class="QToolButton" name="toolButton_frontierColor">
+              <widget class="QToolButton" name="toolButton_obstacleColor">
                <property name="text">
                 <string>...</string>
                </property>
               </widget>
              </item>
              <item>
-              <widget class="QLineEdit" name="lineEdit_frontierColor">
+              <widget class="QLineEdit" name="lineEdit_obstacleColor">
                <property name="readOnly">
                 <bool>true</bool>
                </property>
@@ -2227,6 +2209,82 @@
              </item>
             </layout>
            </item>
+           <item row="20" column="0">
+            <widget class="QCheckBox" name="checkBox_showDisparityInsteadOfRight">
+             <property name="text">
+              <string/>
+             </property>
+             <property name="checked">
+              <bool>false</bool>
+             </property>
+            </widget>
+           </item>
+           <item row="4" column="0">
+            <layout class="QHBoxLayout" name="horizontalLayout_12">
+             <item>
+              <widget class="QToolButton" name="toolButton_emptyColor">
+               <property name="text">
+                <string>...</string>
+               </property>
+              </widget>
+             </item>
+             <item>
+              <widget class="QLineEdit" name="lineEdit_emptyColor">
+               <property name="readOnly">
+                <bool>true</bool>
+               </property>
+              </widget>
+             </item>
+            </layout>
+           </item>
+           <item row="15" column="1">
+            <widget class="QLabel" name="label_62">
+             <property name="text">
+              <string>Depth's edge bleeding filtering error.</string>
+             </property>
+             <property name="wordWrap">
+              <bool>true</bool>
+             </property>
+            </widget>
+           </item>
+           <item row="15" column="0">
+            <widget class="QDoubleSpinBox" name="doubleSpinBox_depthEdgeBleedingError">
+             <property name="suffix">
+              <string> m</string>
+             </property>
+             <property name="decimals">
+              <number>2</number>
+             </property>
+             <property name="minimum">
+              <double>0.000000000000000</double>
+             </property>
+             <property name="maximum">
+              <double>10.000000000000000</double>
+             </property>
+             <property name="singleStep">
+              <double>0.010000000000000</double>
+             </property>
+             <property name="value">
+              <double>0.000000000000000</double>
+             </property>
+            </widget>
+           </item>
+           <item row="14" column="0">
+            <widget class="QSpinBox" name="spinBox_depthConfidence">
+             <property name="suffix">
+              <string> %</string>
+             </property>
+             <property name="minimum">
+              <number>0</number>
+             </property>
+             <property name="maximum">
+              <number>100</number>
+             </property>
+             <property name="value">
+              <number>0</number>
+             </property>
+            </widget>
+           </item>
           </layout>
          </item>
          <item>
@@ -2468,7 +2526,7 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>294</width>
+          <width>296</width>
           <height>272</height>
          </rect>
         </property>
@@ -2642,8 +2700,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>181</width>
-          <height>485</height>
+          <width>428</width>
+          <height>196</height>
          </rect>
         </property>
         <attribute name="label">

guilib/src/ui/exportBundlerDialog.ui

@@ -141,7 +141,7 @@
         <item row="1" column="0">
          <widget class="QSpinBox" name="sba_iterations">
           <property name="minimum">
-           <number>1</number>
+           <number>0</number>
           </property>
           <property name="maximum">
            <number>999999</number>

guilib/src/ui/exportCloudsDialog.ui

@@ -6,7 +6,7 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>919</width>
+    <width>895</width>
     <height>869</height>
    </rect>
   </property>
@@ -23,9 +23,9 @@
       <property name="geometry">
        <rect>
         <x>0</x>
-        <y>-3199</y>
-        <width>885</width>
-        <height>6183</height>
+        <y>-654</y>
+        <width>861</width>
+        <height>6498</height>
        </rect>
       </property>
       <layout class="QVBoxLayout" name="verticalLayout_13">
@@ -710,6 +710,26 @@
          <layout class="QVBoxLayout" name="verticalLayout_14">
           <item>
            <layout class="QGridLayout" name="gridLayout" columnstretch="0,0,1">
+            <item row="3" column="1">
+             <widget class="QLineEdit" name="lineEdit_roiRatios"/>
+            </item>
+            <item row="2" column="2">
+             <widget class="QLabel" name="label_133">
+              <property name="text">
+               <string>3D cloud minimum depth.</string>
+              </property>
+              <property name="wordWrap">
+               <bool>true</bool>
+              </property>
+             </widget>
+            </item>
+            <item row="5" column="1">
+             <widget class="QSpinBox" name="spinBox_fillDepthHolesError">
+              <property name="suffix">
+               <string> %</string>
+              </property>
+             </widget>
+            </item>
             <item row="6" column="2">
              <widget class="QLabel" name="label_134">
               <property name="text">
@@ -720,13 +740,10 @@
               </property>
              </widget>
             </item>
-            <item row="3" column="2">
-             <widget class="QLabel" name="label_135">
-              <property name="text">
-               <string>ROI ratios [left, right, top, bottom] between 0 and 1. Only generate 3D points for pixels inside the region of interest (RGB image).</string>
-              </property>
-              <property name="wordWrap">
-               <bool>true</bool>
+            <item row="4" column="1">
+             <widget class="QSpinBox" name="spinBox_fillDepthHoles">
+              <property name="suffix">
+               <string> pixels</string>
               </property>
              </widget>
             </item>
@@ -749,21 +766,18 @@
               </property>
              </widget>
             </item>
-            <item row="5" column="2">
-             <widget class="QLabel" name="label_198">
+            <item row="3" column="2">
+             <widget class="QLabel" name="label_135">
               <property name="text">
-               <string>Fill depth hole error.</string>
+               <string>ROI ratios [left, right, top, bottom] between 0 and 1. Only generate 3D points for pixels inside the region of interest (RGB image).</string>
               </property>
               <property name="wordWrap">
                <bool>true</bool>
               </property>
-              <property name="textInteractionFlags">
-               <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-              </property>
              </widget>
             </item>
-            <item row="3" column="1">
-             <widget class="QLineEdit" name="lineEdit_roiRatios"/>
+            <item row="6" column="1">
+             <widget class="QLineEdit" name="lineEdit_distortionModel"/>
             </item>
             <item row="2" column="1">
              <widget class="QDoubleSpinBox" name="doubleSpinBox_minDepth">
@@ -794,39 +808,6 @@
               </property>
              </widget>
             </item>
-            <item row="8" column="1">
-             <widget class="QCheckBox" name="checkBox_subtraction">
-              <property name="text">
-               <string/>
-              </property>
-             </widget>
-            </item>
-            <item row="6" column="1">
-             <widget class="QLineEdit" name="lineEdit_distortionModel"/>
-            </item>
-            <item row="8" column="2">
-             <widget class="QLabel" name="label_137">
-              <property name="text">
-               <string>Cloud subtraction. Superposed points from different nodes are filtered.</string>
-              </property>
-              <property name="wordWrap">
-               <bool>true</bool>
-              </property>
-             </widget>
-            </item>
-            <item row="4" column="2">
-             <widget class="QLabel" name="label_197">
-              <property name="text">
-               <string>Fill depth hole size (0 means no fill).</string>
-              </property>
-              <property name="wordWrap">
-               <bool>true</bool>
-              </property>
-              <property name="textInteractionFlags">
-               <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-              </property>
-             </widget>
-            </item>
             <item row="1" column="2">
              <widget class="QLabel" name="label_132">
               <property name="text">
@@ -850,13 +831,6 @@
               </property>
              </widget>
             </item>
-            <item row="5" column="1">
-             <widget class="QSpinBox" name="spinBox_fillDepthHolesError">
-              <property name="suffix">
-               <string> %</string>
-              </property>
-             </widget>
-            </item>
             <item row="6" column="0">
              <widget class="QToolButton" name="toolButton_distortionModel">
               <property name="text">
@@ -864,24 +838,57 @@
               </property>
              </widget>
             </item>
-            <item row="7" column="1">
-             <widget class="QCheckBox" name="checkBox_bilateral">
+            <item row="10" column="2">
+             <widget class="QLabel" name="label_137">
               <property name="text">
-               <string/>
-              </property>
-             </widget>
-            </item>
-            <item row="2" column="2">
-             <widget class="QLabel" name="label_133">
-              <property name="text">
-               <string>3D cloud minimum depth.</string>
+               <string>Cloud subtraction. Superposed points from different nodes are filtered.</string>
               </property>
               <property name="wordWrap">
                <bool>true</bool>
               </property>
              </widget>
             </item>
-            <item row="7" column="2">
+            <item row="5" column="2">
+             <widget class="QLabel" name="label_198">
+              <property name="text">
+               <string>Fill depth hole error.</string>
+              </property>
+              <property name="wordWrap">
+               <bool>true</bool>
+              </property>
+              <property name="textInteractionFlags">
+               <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+              </property>
+             </widget>
+            </item>
+            <item row="10" column="1">
+             <widget class="QCheckBox" name="checkBox_subtraction">
+              <property name="text">
+               <string/>
+              </property>
+             </widget>
+            </item>
+            <item row="8" column="1">
+             <widget class="QCheckBox" name="checkBox_bilateral">
+              <property name="text">
+               <string/>
+              </property>
+             </widget>
+            </item>
+            <item row="4" column="2">
+             <widget class="QLabel" name="label_197">
+              <property name="text">
+               <string>Fill depth hole size (0 means no fill).</string>
+              </property>
+              <property name="wordWrap">
+               <bool>true</bool>
+              </property>
+              <property name="textInteractionFlags">
+               <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+              </property>
+             </widget>
+            </item>
+            <item row="8" column="2">
              <widget class="QLabel" name="label_136">
               <property name="text">
                <string>Bilateral filtering of the depth image. Reduce noise in depth images.</string>
@@ -891,10 +898,52 @@
               </property>
              </widget>
             </item>
-            <item row="4" column="1">
-             <widget class="QSpinBox" name="spinBox_fillDepthHoles">
+            <item row="7" column="2">
+             <widget class="QLabel" name="label_147">
+              <property name="text">
+               <string>Depth's edge bleeding filter error (0 means disabled). Reduce depth noise around edges.</string>
+              </property>
+              <property name="wordWrap">
+               <bool>true</bool>
+              </property>
+             </widget>
+            </item>
+            <item row="7" column="1">
+             <widget class="QDoubleSpinBox" name="doubleSpinBox_depthEdgeFiltering">
               <property name="suffix">
-               <string> pixels</string>
+               <string> m</string>
+              </property>
+              <property name="decimals">
+               <number>2</number>
+              </property>
+              <property name="maximum">
+               <double>100.000000000000000</double>
+              </property>
+              <property name="singleStep">
+               <double>0.100000000000000</double>
+              </property>
+              <property name="value">
+               <double>4.000000000000000</double>
+              </property>
+             </widget>
+            </item>
+            <item row="9" column="2">
+             <widget class="QLabel" name="label_149">
+              <property name="text">
+               <string>Minimum depth confidence: 0% is low confidence, 100% is high confidence. Ignored if no depth confidence images were recorded or if set to 0%.</string>
+              </property>
+              <property name="wordWrap">
+               <bool>true</bool>
+              </property>
+             </widget>
+            </item>
+            <item row="9" column="1">
+             <widget class="QSpinBox" name="spinBox_depthConfidence">
+              <property name="suffix">
+               <string> %</string>
+              </property>
+              <property name="maximum">
+               <number>100</number>
               </property>
              </widget>
             </item>
@@ -2012,100 +2061,30 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
           <bool>false</bool>
          </property>
          <layout class="QGridLayout" name="gridLayout_20" columnstretch="0,0,0,1">
+          <item row="7" column="2" colspan="2">
+           <widget class="QLabel" name="label_meshingTextureSize_14">
+            <property name="text">
+             <string>Distance to camera policy. The closest camera from a point is used to colorize the point. If disabled, the camera for which the point projection is the closest of the image center is used to colorize the point.</string>
+            </property>
+            <property name="wordWrap">
+             <bool>true</bool>
+            </property>
+           </widget>
+          </item>
+          <item row="9" column="2">
+           <widget class="QLabel" name="label_meshingTextureSize_18">
+            <property name="text">
+             <string>Recolor points from camera projection. This would be used to color laser scans with the cameras.</string>
+            </property>
+            <property name="wordWrap">
+             <bool>true</bool>
+            </property>
+           </widget>
+          </item>
           <item row="6" column="1">
-           <widget class="QCheckBox" name="checkBox_camProjDistanceToCamPolicy">
-            <property name="text">
-             <string/>
-            </property>
-           </widget>
-          </item>
-          <item row="4" column="2" colspan="2">
-           <widget class="QLabel" name="label_meshingTextureSize_16">
-            <property name="text">
-             <string>Decimation of camera resolution before projection. This can help to correctly estimate points hidden by other points, in case the point cloud is sparse.</string>
-            </property>
-            <property name="wordWrap">
-             <bool>true</bool>
-            </property>
-           </widget>
-          </item>
-          <item row="9" column="2" colspan="2">
-           <widget class="QLabel" name="label_camProjExportCamera">
-            <property name="text">
-             <string>ID format of the camera selected for each point.</string>
-            </property>
-            <property name="wordWrap">
-             <bool>true</bool>
-            </property>
-           </widget>
-          </item>
-          <item row="2" column="2" colspan="2">
-           <widget class="QLabel" name="label_meshingTextureSize_13">
-            <property name="text">
-             <string>Maximum distance from the camera for points to be colorized by this camera (0 means infinite).</string>
-            </property>
-            <property name="wordWrap">
-             <bool>true</bool>
-            </property>
-           </widget>
-          </item>
-          <item row="4" column="1">
-           <widget class="QSpinBox" name="spinBox_camProjDecimation">
-            <property name="suffix">
-             <string/>
-            </property>
-            <property name="minimum">
-             <number>1</number>
-            </property>
-            <property name="maximum">
-             <number>9999</number>
-            </property>
-            <property name="singleStep">
-             <number>1</number>
-            </property>
-            <property name="value">
-             <number>1</number>
-            </property>
-           </widget>
-          </item>
-          <item row="1" column="2" colspan="2">
-           <widget class="QLabel" name="label_meshingTextureSize_17">
-            <property name="text">
-             <string>ROI ratios [left right top bottom] between 0 and 1. This can be used to ignore black borders of RGB images caused by calibration. </string>
-            </property>
-            <property name="wordWrap">
-             <bool>true</bool>
-            </property>
-           </widget>
-          </item>
-          <item row="3" column="2" colspan="2">
-           <widget class="QLabel" name="label_meshingTextureSize_12">
-            <property name="text">
-             <string>Maximum angle between camera and point's normal to apply color (0 means disabled).</string>
-            </property>
-            <property name="wordWrap">
-             <bool>true</bool>
-            </property>
-           </widget>
-          </item>
-          <item row="8" column="1">
-           <widget class="QCheckBox" name="checkBox_camProjRecolorPoints">
-            <property name="text">
-             <string/>
-            </property>
-           </widget>
-          </item>
-          <item row="5" column="1">
            <widget class="QLineEdit" name="lineEdit_camProjMaskFilePath"/>
           </item>
-          <item row="7" column="1">
-           <widget class="QCheckBox" name="checkBox_camProjKeepPointsNotSeenByCameras">
-            <property name="text">
-             <string/>
-            </property>
-           </widget>
-          </item>
-          <item row="9" column="1">
+          <item row="10" column="1">
            <widget class="QComboBox" name="comboBox_camProjExportCamera">
             <property name="toolTip">
              <string>By Node ID: cameras of same node have same ID
@@ -2134,18 +2113,56 @@ By Node ID and Camera Index: NodeID*10+CameraIndex</string>
             </item>
            </widget>
           </item>
-          <item row="6" column="2" colspan="2">
-           <widget class="QLabel" name="label_meshingTextureSize_14">
+          <item row="9" column="1">
+           <widget class="QCheckBox" name="checkBox_camProjRecolorPoints">
             <property name="text">
-             <string>Distance to camera policy. The closest camera from a point is used to colorize the point. If disabled, the camera for which the point projection is the closest of the image center is used to colorize the point.</string>
+             <string/>
+            </property>
+           </widget>
+          </item>
+          <item row="2" column="2" colspan="2">
+           <widget class="QLabel" name="label_meshingTextureSize_13">
+            <property name="text">
+             <string>Maximum distance from the camera for points to be colorized by this camera (0 means infinite).</string>
             </property>
             <property name="wordWrap">
              <bool>true</bool>
             </property>
            </widget>
           </item>
-          <item row="1" column="1">
-           <widget class="QLineEdit" name="lineEdit_camProjRoiRatios"/>
+          <item row="8" column="1">
+           <widget class="QCheckBox" name="checkBox_camProjKeepPointsNotSeenByCameras">
+            <property name="text">
+             <string/>
+            </property>
+           </widget>
+          </item>
+          <item row="1" column="2" colspan="2">
+           <widget class="QLabel" name="label_meshingTextureSize_17">
+            <property name="text">
+             <string>ROI ratios [left right top bottom] between 0 and 1. This can be used to ignore black borders of RGB images caused by calibration. </string>
+            </property>
+            <property name="wordWrap">
+             <bool>true</bool>
+            </property>
+           </widget>
+          </item>
+          <item row="6" column="0">
+           <widget class="QToolButton" name="toolButton_camProjMaskFilePath">
+            <property name="text">
+             <string>...</string>
+            </property>
+           </widget>
+          </item>
+          <item row="5" column="2" colspan="2">
+           <widget class="QLabel" name="label_meshingTextureSize_16">
+            <property name="text">
+             <string>Decimation of camera resolution before projection. This can help to correctly estimate points hidden by other points, in case the point cloud is sparse.</string>
+            </property>
+            <property name="wordWrap">
+             <bool>true</bool>
+            </property>
+           </widget>
           </item>
           <item row="3" column="1">
            <widget class="QDoubleSpinBox" name="doubleSpinBox_camProjMaxAngle">
@@ -2169,6 +2186,65 @@ By Node ID and Camera Index: NodeID*10+CameraIndex</string>
             </property>
            </widget>
           </item>
+          <item row="1" column="1">
+           <widget class="QLineEdit" name="lineEdit_camProjRoiRatios"/>
+          </item>
+          <item row="5" column="1">
+           <widget class="QSpinBox" name="spinBox_camProjDecimation">
+            <property name="suffix">
+             <string/>
+            </property>
+            <property name="minimum">
+             <number>1</number>
+            </property>
+            <property name="maximum">
+             <number>9999</number>
+            </property>
+            <property name="singleStep">
+             <number>1</number>
+            </property>
+            <property name="value">
+             <number>1</number>
+            </property>
+           </widget>
+          </item>
+          <item row="7" column="1">
+           <widget class="QCheckBox" name="checkBox_camProjDistanceToCamPolicy">
+            <property name="text">
+             <string/>
+            </property>
+           </widget>
+          </item>
+          <item row="6" column="2" colspan="2">
+           <widget class="QLabel" name="label_meshingTextureSize_19">
+            <property name="text">
+             <string>File path for a mask. Format should be 8-bits grayscale. The mask should cover all cameras in case multi-camera is used and have the same resolution.</string>
+            </property>
+            <property name="wordWrap">
+             <bool>true</bool>
+            </property>
+           </widget>
+          </item>
+          <item row="3" column="2" colspan="2">
+           <widget class="QLabel" name="label_meshingTextureSize_12">
+            <property name="text">
+             <string>Maximum angle between camera and point's normal to apply color (0 means disabled).</string>
+            </property>
+            <property name="wordWrap">
+             <bool>true</bool>
+            </property>
+           </widget>
+          </item>
+          <item row="10" column="2" colspan="2">
+           <widget class="QLabel" name="label_camProjExportCamera">
+            <property name="text">
+             <string>ID format of the camera selected for each point.</string>
+            </property>
+            <property name="wordWrap">
+             <bool>true</bool>
+            </property>
+           </widget>
+          </item>
           <item row="2" column="1">
            <widget class="QDoubleSpinBox" name="doubleSpinBox_camProjMaxDistance">
             <property name="suffix">
@@ -2191,7 +2267,7 @@ By Node ID and Camera Index: NodeID*10+CameraIndex</string>
             </property>
            </widget>
           </item>
-          <item row="7" column="2" colspan="2">
+          <item row="8" column="2" colspan="2">
            <widget class="QLabel" name="label_meshingTextureSize_15">
             <property name="text">
              <string>Keep points not seen by the cameras. These points will be set with a pure red color (255,0,0) if the cloud was created from laser scans.</string>
@@ -2201,33 +2277,38 @@ By Node ID and Camera Index: NodeID*10+CameraIndex</string>
             </property>
            </widget>
           </item>
-          <item row="8" column="2">
-           <widget class="QLabel" name="label_meshingTextureSize_18">
-            <property name="text">
-             <string>Recolor points from camera projection. This would be used to color laser scans with the cameras.</string>
+          <item row="4" column="1">
+           <widget class="QDoubleSpinBox" name="doubleSpinBox_camProjMaxDepthError">
+            <property name="suffix">
+             <string> m</string>
             </property>
-            <property name="wordWrap">
-             <bool>true</bool>
+            <property name="decimals">
+             <number>2</number>
+            </property>
+            <property name="minimum">
+             <double>0.000000000000000</double>
+            </property>
+            <property name="maximum">
+             <double>9.990000000000000</double>
+            </property>
+            <property name="singleStep">
+             <double>0.050000000000000</double>
+            </property>
+            <property name="value">
+             <double>0.000000000000000</double>
             </property>
            </widget>
           </item>
-          <item row="5" column="2" colspan="2">
-           <widget class="QLabel" name="label_meshingTextureSize_19">
+          <item row="4" column="2" colspan="2">
+           <widget class="QLabel" name="label_meshingTextureSize_20">
             <property name="text">
-             <string>File path for a mask. Format should be 8-bits grayscale. The mask should cover all cameras in case multi-camera is used and have the same resolution.</string>
+             <string>For all points registered to same pixel, only those close enough to the closest point will be colored with same color (0 means only the closest point of all points registered in same pixel is colored).</string>
             </property>
             <property name="wordWrap">
              <bool>true</bool>
             </property>
            </widget>
           </item>
-          <item row="5" column="0">
-           <widget class="QToolButton" name="toolButton_camProjMaskFilePath">
-            <property name="text">
-             <string>...</string>
-            </property>
-           </widget>
-          </item>
          </layout>
         </widget>
        </item>

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>rtabmap</name>
-  <version>0.21.13</version>
+  <version>0.22.0</version>
   <description>RTAB-Map's standalone library. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
   <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>
@@ -16,6 +16,7 @@
   <depend>cv_bridge</depend>
   <depend>libg2o</depend>
   <depend>gtsam</depend>
+  <depend>tbb</depend> <!-- till tbb is added to gtsam dependencies https://github.com/borglab/gtsam/issues/1759 -->
   <!-- <depend>libopenni2-dev</depend> --> <!-- not available on Jessie -->
   <depend>libpcl-all-dev</depend> <!--  include libvtk-qt -->
   <depend>libpointmatcher</depend> <!-- optional but recommended if lidar is used, also not available on 32 bits system, but rtabmap can be built without it -->

tools/DataRecorder/main.cpp

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/CameraRGBD.h>
 #include <rtabmap/core/CameraStereo.h>
 #include <rtabmap/core/Camera.h>
+#include <rtabmap/core/SensorEvent.h>
 #include <rtabmap/core/DBReader.h>
 #include <rtabmap/core/SensorCaptureThread.h>
 #include <rtabmap/core/SensorCaptureThread.h>
@@ -71,6 +72,31 @@ void sighandler(int sig)
 	}
 }
 
+// Detect when we reached end-of-files
+class StatusHandler: public UEventsHandler{
+protected:
+	virtual bool handleEvent(UEvent * event)
+	{
+		if(event->getClassName().compare("SensorEvent") == 0)
+		{
+			SensorEvent * camEvent = (SensorEvent*)event;
+			if(camEvent->getCode() == SensorEvent::kCodeNoMoreImages)
+			{
+				printf("End of stream reached...\n");
+				if(cam)
+				{
+					cam->join(true);
+				}
+				if(app)
+				{
+					QMetaObject::invokeMethod(app, "quit");
+				}
+			}
+		}
+		return false;
+	}
+};
+
 int main (int argc, char * argv[])
 {
 	ULogger::setType(ULogger::kTypeConsole);
@@ -138,6 +164,10 @@ int main (int argc, char * argv[])
 	signal(SIGTERM, &sighandler);
 	signal(SIGINT, &sighandler);
 
+	// Catch end of stream to close the gui
+	StatusHandler statusHandler;
+	statusHandler.registerToEventsManager();
+
 	rtabmap::Camera * camera = dialog.createCamera();
 	if(camera == 0)
 	{

tools/Export/main.cpp

@@ -97,7 +97,7 @@ void showUsage()
 			"                              considered blurred. 0 means disabled. 50 can be good default.\n"
 			"    --cam_projection      Camera projection on assembled cloud and export node ID on each point (in PointSourceId field).\n"
 			"    --cam_projection_keep_all  Keep not colored points from cameras (node ID will be 0 and color will be red).\n"
-			"    --cam_projection_decimation  Decimate images before projecting the points.\n"
+			"    --cam_projection_decimation # Decimate images before projecting the points.\n"
 			"    --cam_projection_mask \"\"  File path for a mask. Format should be 8-bits grayscale. The mask should\n"
 			"                                    cover all cameras in case multi-camera is used and have the same resolution.\n"
 			"    --opt #               Optimization approach:\n"
@@ -106,9 +106,10 @@ void showUsage()
 			"                              2=Use optimized poses already computed in the database instead\n"
 			"                                of re-computing them (fallback to default if optimized poses don't exist).\n"
 			"                              3=No optimization, use odometry poses directly.\n"
-			"    --poses               Export optimized poses of the robot frame (e.g., base_link).\n"
+			"    --poses               Export optimized poses of the robot frame (e.g., base_link), including landmarks.\n"
 			"    --poses_camera        Export optimized poses of the camera frame (e.g., optical frame).\n"
 			"    --poses_scan          Export optimized poses of the scan frame.\n"
+			"    --poses_landmark      Export optimized poses of landmarks.\n"
 			"    --poses_gt            Export ground truth poses of the robot frame (e.g., base_link).\n"
 			"    --poses_gps           Export GPS poses of the GPS frame in local coordinates.\n"
 			"    --poses_format #      Format used for exported poses (default is 11):\n"
@@ -152,6 +153,8 @@ void showUsage()
 			"    --min_range     #     Minimum range of the created clouds (default 0 m).\n"
 			"    --max_range     #     Maximum range of the created clouds (default 4 m, 0 m with --scan).\n"
 			"    --decimation    #     Depth image decimation before creating the clouds (default 4, 1 with --scan).\n"
+			"    --edge_bleeding_error #  Depth's edge bleeding filtering error (default 0 m).\n"
+			"    --depth_confidence    #  Depth confidence threshold (should be in [0,100]) (default 0=Low).\n"
 			"    --voxel         #     Voxel size of the created clouds (default 0.01 m, 0 m with --scan).\n"
 			"    --ground_normals_up  #  Flip ground normals up if close to -z axis (default 0, 0=disabled, value should be >0 and <1, typical 0.9).\n"
 			"    --noise_radius  #     Noise filtering search radius (default 0, 0=disabled).\n"
@@ -213,6 +216,8 @@ int main(int argc, char * argv[])
 	float poissonSize = 0.03;
 	int maxPolygons = 300000;
 	int decimation = -1;
+	float depthEdgeBleedingFilterError = 0.0f;
+	unsigned char depthConfidenceThr = 0;
 	float minRange = 0.0f;
 	float maxRange = -1.0f;
 	float voxelSize = -1.0f;
@@ -252,6 +257,7 @@ int main(int argc, char * argv[])
 	bool exportPoses = false;
 	bool exportPosesCamera = false;
 	bool exportPosesScan = false;
+	bool exportPosesLandmarks = false;
 	bool exportPosesGt = false;
 	bool exportPosesGps = false;
 	int exportPosesFormat = 11;
@@ -498,6 +504,10 @@ int main(int argc, char * argv[])
 		{
 			exportPosesScan = true;
 		}
+		else if(std::strcmp(argv[i], "--poses_landmark") == 0)
+		{
+			exportPosesLandmarks = true;
+		}
 		else if(std::strcmp(argv[i], "--poses_gt") == 0)
 		{
 			exportPosesGt = true;
@@ -767,6 +777,30 @@ int main(int argc, char * argv[])
 				showUsage();
 			}
 		}
+		else if(std::strcmp(argv[i], "--edge_bleeding_error") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				depthEdgeBleedingFilterError = uStr2Float(argv[i]);
+			}
+			else
+			{
+				showUsage();
+			}
+		}
+		else if(std::strcmp(argv[i], "--depth_confidence") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				depthConfidenceThr = (unsigned char)uStr2Int(argv[i]);
+			}
+			else
+			{
+				showUsage();
+			}
+		}
 		else if(std::strcmp(argv[i], "--voxel") == 0)
 		{
 			++i;
@@ -1079,6 +1113,7 @@ int main(int argc, char * argv[])
 		 exportPoses ||
 		 exportPosesScan ||
 		 exportPosesCamera ||
+		 exportPosesLandmarks ||
 		 exportPosesGt ||
 		 exportPosesGps ||
 		 exportGps>=0 ||
@@ -1142,7 +1177,7 @@ int main(int argc, char * argv[])
 	std::multimap<int, Link> links;
 	dbDriver->getAllOdomPoses(odomPoses, true);
 	dbDriver->getAllLinks(links, true, true);
-	if(optimizationApproach == 3 || !(exportCloud || exportMesh || exportPoses || exportPosesCamera || exportPosesScan))
+	if(optimizationApproach == 3 || !(exportCloud || exportMesh || exportPoses || exportPosesCamera || exportPosesScan || exportPosesLandmarks))
 	{
 		// Just use odometry poses when exporting only images
 		optimizedPoses = odomPoses;
@@ -1346,13 +1381,18 @@ int main(int argc, char * argv[])
 	pcl::PointCloud<pcl::PointXYZI>::Ptr assembledCloudI(new pcl::PointCloud<pcl::PointXYZI>);
 	std::map<int, rtabmap::Transform> robotPoses;
 	std::vector<std::map<int, rtabmap::Transform> > cameraPoses;
+	std::vector<std::map<int, double> > cameraStamps;
 	std::map<int, rtabmap::Transform> scanPoses;
+	std::map<int, double> scanStamps;
+	std::map<int, rtabmap::Transform> landmarkPoses;
+	std::map<int, double> landmarkStamps;
 	std::map<int, rtabmap::Transform> gtPoses;
+	std::map<int, double> gtStamps;
 	std::map<int, rtabmap::Transform> gpsPoses;
 	std::map<int, double> gpsStamps;
 	GPS gpsOrigin;
 	std::map<int, rtabmap::GPS> gpsValues;
-	std::map<int, double> cameraStamps;
+	std::map<int, double> robotStamps;
 	std::map<int, std::vector<rtabmap::CameraModel> > cameraModels;
 	std::map<int, cv::Mat> cameraDepths;
 	int imagesExported = 0;
@@ -1376,7 +1416,10 @@ int main(int argc, char * argv[])
 		{
 			// landmark, just add to list of poses
 			robotPoses.insert(*iter);
-			cameraStamps.insert(std::make_pair(iter->first, 0));
+			robotStamps.insert(std::make_pair(iter->first, 0));
+
+			landmarkPoses.insert(*iter);
+			landmarkStamps.insert(std::make_pair(iter->first, 0));
 			continue;
 		}
 		
@@ -1417,6 +1460,7 @@ int main(int argc, char * argv[])
 			bool densityFiltered = !densityPoses.empty() && densityPoses.find(iter->first) == densityPoses.end();
 			cv::Mat rgb;
 			cv::Mat depth;
+			cv::Mat confidence;
 			pcl::IndicesPtr indices(new std::vector<int>);
 			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
 			pcl::PointCloud<pcl::PointXYZI>::Ptr cloudI;
@@ -1425,7 +1469,7 @@ int main(int argc, char * argv[])
 				if(!densityFiltered && cloudFromScan && (exportCloud || exportMesh))
 				{
 					LaserScan scan;
-					data.uncompressData(exportImages?&rgb:0, (texture||exportImages)&&!data.depthOrRightCompressed().empty()?&depth:0, &scan);
+					data.uncompressData(exportImages?&rgb:0, (texture||exportImages)&&!data.depthOrRightCompressed().empty()?&depth:0, &scan, 0, 0, 0, 0, exportImages?&confidence:0);
 					if(scan.empty())
 					{
 						printf("Node %d doesn't have scan data, empty cloud is created.\n", iter->first);
@@ -1453,7 +1497,7 @@ int main(int argc, char * argv[])
 				}
 				else
 				{
-					data.uncompressData(&rgb, &depth);
+					data.uncompressData(&rgb, &depth, 0, 0, 0, 0, 0, &confidence);
 					if(!densityFiltered && (exportCloud || exportMesh))
 					{
 						if(depth.empty())
@@ -1461,12 +1505,20 @@ int main(int argc, char * argv[])
 							printf("Node %d doesn't have depth or stereo data, empty cloud is "
 									"created (if you want to create point cloud from scan, use --scan option).\n", iter->first);
 						}
+						else if(!data.depthRaw().empty() && depthEdgeBleedingFilterError>0.0f)
+						{
+							util2d::depthBleedingFiltering(depth, depthEdgeBleedingFilterError);
+							data.setRGBDImage(data.imageRaw(), depth, data.depthConfidenceRaw(), data.cameraModels());
+						}
 						cloud = util3d::cloudRGBFromSensorData(
 								data,
 								decimation,      // image decimation before creating the clouds
 								maxRange,        // maximum depth of the cloud
 								minRange,
-								indices.get());
+								indices.get(),
+								ParametersMap(),
+								std::vector<float>(),
+								depthConfidenceThr);
 						if(noiseRadius>0.0f && noiseMinNeighbors>0)
 						{
 							indices = util3d::radiusFiltering(cloud, indices, noiseRadius, noiseMinNeighbors);
@@ -1510,6 +1562,16 @@ int main(int argc, char * argv[])
 					outputPath=dir+"/"+(exportImagesId?uNumber2Str(iter->first):uFormat("%f", stamp))+ext;
 					cv::imwrite(outputPath, depthExported);
 				}
+				if(!confidence.empty())
+				{
+					dir = outputDirectory+"/"+baseName+"_confidence";
+					if(!UDirectory::exists(dir)) {
+						UDirectory::makeDir(dir);
+					}
+
+					outputPath=dir+"/"+(exportImagesId?uNumber2Str(iter->first):uFormat("%f", stamp))+".png";
+					cv::imwrite(outputPath, confidence);
+				}
 
 				// save calibration per image (calibration can change over time, e.g. camera has auto focus)
 				for(size_t i=0; i<models.size(); ++i)
@@ -1628,7 +1690,7 @@ int main(int argc, char * argv[])
 		}
 
 		robotPoses.insert(std::make_pair(iter->first, iter->second));
-		cameraStamps.insert(std::make_pair(iter->first, stamp));
+		robotStamps.insert(std::make_pair(iter->first, stamp));
 		if(models.empty() && weight == -1 && !cameraModels.empty())
 		{
 			// For intermediate nodes, use latest models
@@ -1645,17 +1707,20 @@ int main(int argc, char * argv[])
 				if(cameraPoses.empty())
 				{
 					cameraPoses.resize(models.size());
+					cameraStamps.resize(models.size());
 				}
 				UASSERT_MSG(models.size() == cameraPoses.size(), "Not all nodes have same number of cameras to export camera poses.");
 				for(size_t i=0; i<models.size(); ++i)
 				{
 					cameraPoses[i].insert(std::make_pair(iter->first, iter->second*models[i].localTransform()));
+					cameraStamps[i].insert(std::make_pair(iter->first, stamp));
 				}
 			}
 		}
 		if(exportPosesScan && !data.laserScanCompressed().empty())
 		{
 			scanPoses.insert(std::make_pair(iter->first, iter->second*data.laserScanCompressed().localTransform()));
+			scanStamps.insert(std::make_pair(iter->first, stamp));
 		}
 
 		if(exportPosesGps || exportGps>=0)
@@ -1688,6 +1753,7 @@ int main(int argc, char * argv[])
 		if(exportPosesGt && !gt.isNull())
 		{
 			gtPoses.insert(std::make_pair(iter->first, gt));
+			gtStamps.insert(std::make_pair(iter->first, stamp));
 		}
 
 		if(optimizedPoses.size() >= 500)
@@ -1750,11 +1816,11 @@ int main(int argc, char * argv[])
 					exportPosesFormat,
 					std::map<int, Transform>(robotPoses.lower_bound(1), robotPoses.end()),
 					links,
-					std::map<int, double>(cameraStamps.lower_bound(1), cameraStamps.end()));
+					std::map<int, double>(robotStamps.lower_bound(1), robotStamps.end()));
 			}
 			else
 			{
-				rtabmap::graph::exportPoses(outputPath, exportPosesFormat, robotPoses, links, cameraStamps);
+				rtabmap::graph::exportPoses(outputPath, exportPosesFormat, robotPoses, links, robotStamps);
 			}
 			cv::Vec3f vmin, vmax;
 			graph::computeMinMax(robotPoses, vmin, vmax);
@@ -1773,7 +1839,7 @@ int main(int argc, char * argv[])
 					outputPath = outputDirectory+"/"+baseName+"_camera_poses." + posesExt;
 				else
 					outputPath = outputDirectory+"/"+baseName+"_camera_poses_"+uNumber2Str((int)i)+"." + posesExt;
-				rtabmap::graph::exportPoses(outputPath, exportPosesFormat, cameraPoses[i], std::multimap<int, Link>(), cameraStamps);
+				rtabmap::graph::exportPoses(outputPath, exportPosesFormat, cameraPoses[i], std::multimap<int, Link>(), cameraStamps[i]);
 				cv::Vec3f vmin, vmax;
 				graph::computeMinMax(cameraPoses[i], vmin, vmax);
 				printf("%d camera poses exported to \"%s\". (min=[%f,%f,%f] max=[%f,%f,%f])\n",
@@ -1786,7 +1852,7 @@ int main(int argc, char * argv[])
 		if(exportPosesScan)
 		{
 			std::string outputPath=outputDirectory+"/"+baseName+"_scan_poses." + posesExt;
-			rtabmap::graph::exportPoses(outputPath, exportPosesFormat, scanPoses, std::multimap<int, Link>(), cameraStamps);
+			rtabmap::graph::exportPoses(outputPath, exportPosesFormat, scanPoses, std::multimap<int, Link>(), scanStamps);
 			cv::Vec3f min, max;
 			graph::computeMinMax(scanPoses, min, max);
 			printf("%d scan poses exported to \"%s\". (min=[%f,%f,%f] max=[%f,%f,%f])\n",
@@ -1795,6 +1861,30 @@ int main(int argc, char * argv[])
 					min[0], min[1], min[2],
 					max[0], max[1], max[2]);
 		}
+		if(exportPosesScan)
+		{
+			std::string outputPath=outputDirectory+"/"+baseName+"_scan_poses." + posesExt;
+			rtabmap::graph::exportPoses(outputPath, exportPosesFormat, scanPoses, std::multimap<int, Link>(), scanStamps);
+			cv::Vec3f min, max;
+			graph::computeMinMax(scanPoses, min, max);
+			printf("%d scan poses exported to \"%s\". (min=[%f,%f,%f] max=[%f,%f,%f])\n",
+					(int)scanPoses.size(),
+					outputPath.c_str(),
+					min[0], min[1], min[2],
+					max[0], max[1], max[2]);
+		}
+		if(exportPosesLandmarks)
+		{
+			std::string outputPath=outputDirectory+"/"+baseName+"_landmark_poses." + posesExt;
+			rtabmap::graph::exportPoses(outputPath, exportPosesFormat, landmarkPoses, std::multimap<int, Link>(), landmarkStamps);
+			cv::Vec3f min, max;
+			graph::computeMinMax(landmarkPoses, min, max);
+			printf("%d landmark poses exported to \"%s\". (min=[%f,%f,%f] max=[%f,%f,%f])\n",
+					(int)landmarkPoses.size(),
+					outputPath.c_str(),
+					min[0], min[1], min[2],
+					max[0], max[1], max[2]);
+		}
 		if(exportPosesGps)
 		{
 			std::string outputPath=outputDirectory+"/"+baseName+"_gps_poses." + posesExt;
@@ -1806,7 +1896,7 @@ int main(int argc, char * argv[])
 		if(exportPosesGt)
 		{
 			std::string outputPath=outputDirectory+"/"+baseName+"_gt_poses." + posesExt;
-			rtabmap::graph::exportPoses(outputPath, exportPosesFormat, gtPoses, std::multimap<int, Link>(), cameraStamps);
+			rtabmap::graph::exportPoses(outputPath, exportPosesFormat, gtPoses, std::multimap<int, Link>(), gtStamps);
 			printf("%d scan poses exported to \"%s\".\n",
 					(int)gtPoses.size(),
 					outputPath.c_str());
@@ -1999,7 +2089,7 @@ int main(int argc, char * argv[])
 					}
 
 					depth = rtabmap::util2d::cvtDepthFromFloat(depth);
-					std::string outputPath=dir+"/"+(exportImagesId?uNumber2Str(iter->first):uFormat("%f",cameraStamps.at(iter->first)))+".png";
+					std::string outputPath=dir+"/"+(exportImagesId?uNumber2Str(iter->first):uFormat("%f",robotStamps.at(iter->first)))+".png";
 					cv::imwrite(outputPath, depth);
 				}
 			}
@@ -2027,6 +2117,7 @@ int main(int argc, char * argv[])
 						cameraModelsProj,
 						textureRange,
 						textureAngle,
+						textureDepthError,
 						textureRoiRatios,
 						projMask,
 						distanceToCamPolicy,
@@ -2040,6 +2131,7 @@ int main(int argc, char * argv[])
 						cameraModelsProj,
 						textureRange,
 						textureAngle,
+						textureDepthError,
 						textureRoiRatios,
 						projMask,
 						distanceToCamPolicy,

tools/GlobalBundleAdjustment/main.cpp

@@ -119,7 +119,7 @@ int main(int argc, char * argv[])
 
 	printf("Global bundle adjustment...\n");
 	Optimizer * optimizer = Optimizer::create(Optimizer::kTypeG2O, parameters);
-	optimizedPoses = optimizer->optimizeBA(optimizedPoses.lower_bound(1)->first, optimizedPoses, links, nodes, true);
+	optimizedPoses = optimizer->optimizeBA(optimizedPoses.lower_bound(1)->first, optimizedPoses, links, nodes, true, parameters);
 	delete optimizer;
 	printf("Global bundle adjustment... done (%fs).\n", timer.ticks());
 

tools/Report/main.cpp

@@ -1255,9 +1255,8 @@ int main(int argc, char * argv[])
 												t.r11(), t.r12(), t.r13(), t.x()*bestScale,
 												t.r21(), t.r22(), t.r23(), t.y()*bestScale,
 												t.r31(), t.r32(), t.r33(), t.z()*bestScale);
-										Transform diff = gtLink.inverse()*scaledLink;
-										sumDist += diff.getNorm();
-										sumAngle += diff.getAngle();
+										sumDist += gtLink.getDistance(scaledLink);
+										sumAngle += gtLink.getAngle(scaledLink);
 										++count;
 									}
 								}

tools/Reprocess/main.cpp

@@ -1024,7 +1024,7 @@ int main(int argc, char * argv[])
 
 		UTimer iterationTime;
 		std::string status;
-		if(!odometryIgnored && info.odomPose.isNull())
+		if(!odometryIgnored && info.odomPose.isNull() && incrementalMemory)
 		{
 			printf("Skipping node %d as it doesn't have odometry pose set.\n", data.id());
 		}
@@ -1235,15 +1235,22 @@ int main(int argc, char * argv[])
 				localizationAngleVariations.push_back(stats.data().at(Statistics::kLoopOdom_correction_angle()));
 			}
 
-			if(exportPoses && !info.odomPose.isNull())
+			if(exportPoses)
 			{
-				if(!odomTrajectoryPoses.empty())
+				if(!info.odomPose.isNull())
 				{
-					int previousId = odomTrajectoryPoses.rbegin()->first;
-					odomTrajectoryLinks.insert(std::make_pair(previousId, Link(previousId, refId, Link::kNeighbor, odomTrajectoryPoses.rbegin()->second.inverse()*info.odomPose, info.odomCovariance)));
+					if(!odomTrajectoryPoses.empty())
+					{
+						int previousId = odomTrajectoryPoses.rbegin()->first;
+						odomTrajectoryLinks.insert(std::make_pair(previousId, Link(previousId, refId, Link::kNeighbor, odomTrajectoryPoses.rbegin()->second.inverse()*info.odomPose, info.odomCovariance)));
+					}
+					odomTrajectoryPoses.insert(std::make_pair(refId, info.odomPose));
+					localizationPoses.insert(std::make_pair(refId, stats.mapCorrection()*info.odomPose));
+				}
+				else
+				{
+					localizationPoses.insert(std::make_pair(refId, rtabmap.getLastLocalizationPose()));
 				}
-				odomTrajectoryPoses.insert(std::make_pair(refId, info.odomPose));
-				localizationPoses.insert(std::make_pair(refId, stats.mapCorrection()*info.odomPose));
 			}
 		}
 
@@ -1282,7 +1289,7 @@ int main(int argc, char * argv[])
 		   !info.odomPose.isNull())
 		{
 			float distance = odomPose.getDistance(info.odomPose);
-			float angle = (odomPose.inverse()*info.odomPose).getAngle();
+			float angle = odomPose.getAngle(info.odomPose);
 			odomDistances.push_back(distance);
 			if(distance < linearUpdate && angle <= angularUpdate)
 			{

utilite/include/rtabmap/utilite/Posix/UThreadC.h

@@ -232,10 +232,10 @@ class UThreadC<void>
     typedef THREAD_HANDLE Handle;
     typedef void ( *Handler)();
 
-    virtual ~UThreadC<void>() {}
+    virtual ~UThreadC() {}
 
   protected:
-    UThreadC<void>() {}
+    UThreadC() {}
 
     virtual void ThreadMain() = 0;