merged master to noetic

.devcontainer/android/Dockerfile

@@ -1,24 +0,0 @@
-FROM introlab3it/rtabmap:android-noble-deps
-
-# remove ubuntu user
-RUN touch /var/mail/ubuntu && chown ubuntu /var/mail/ubuntu && userdel -r ubuntu
-
-RUN apt-get update && apt-get install -y sudo && \
-    apt-get clean && rm -rf /var/lib/apt/lists/
-
-ARG USERNAME=vscode
-ARG USER_UID=1000
-ARG USER_GID=1000
-
-RUN set -ex && \
-    groupadd --gid ${USER_GID} ${USERNAME} && \
-    useradd --uid ${USER_UID} --gid ${USER_GID} -m ${USERNAME} && \
-    usermod -a -G sudo ${USERNAME} && \
-    echo "${USERNAME} ALL=(ALL) NOPASSWD:ALL" > /etc/sudoers.d/${USERNAME} && \
-    chmod 0440 /etc/sudoers.d/${USERNAME}
-
-RUN chmod +x /opt/android-sdk/tools/android
-
-RUN echo "source /usr/share/bash-completion/completions/git" >> /home/${USERNAME}/.bashrc
-
-

.devcontainer/android/devcontainer.json

@@ -1,21 +1,9 @@
 {
-    "build": {
+    "image": "introlab3it/rtabmap:android-deps",
-      "dockerfile": "Dockerfile"
-    },
     "customizations": {
         "vscode": {
           "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools", "vscjava.vscode-java-pack"]
         }
       },
-    "workspaceMount": "source=${localWorkspaceFolder},target=/home/vscode/rtabmap,type=bind",
-    "workspaceFolder": "/home/vscode/rtabmap",
-    "postStartCommand": "./.devcontainer/android/init.sh",
-    "settings": {
-      "terminal.integrated.defaultProfile.linux": "bash"
-    },
-    "remoteUser": "vscode",
     "runArgs": ["--privileged", "--network=host"]
 }
-
-
-  

.devcontainer/android/init.sh

@@ -1,17 +0,0 @@
-#!/bin/bash
-set -e
-
-echo "Running post-start initialization..."
-
-# copy required jars
-cp /opt/android/lib/*.jar app/android/libs/.
-
-mkdir -p build_android/arm64-v8a
-
-# resource tool
-cd build_android
-cmake -DANDROID_PREBUILD=ON ..
-make
-
-echo -e "\nTo build the APK, do (adjust API number):"
-echo -e '\nexport ANDROID_API=30 && cd build_android/arm64-v8a && cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=$ANDROID_API -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/arm64-v8a -DCMAKE_FIND_ROOT_PATH="/opt/android/arm64-v8a/bin;/opt/android/arm64-v8a;/opt/android/arm64-v8a/share" -DBUILD_EXAMPLES=OFF -DBUILD_TOOLS=OFF -DOpenCV_DIR=/opt/android/arm64-v8a/sdk/native/jni ../..\nmake -j6\n'

.devcontainer/rolling/Dockerfile

@@ -1,77 +0,0 @@
-
-FROM ubuntu:24.04
-
-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-rolling-ros-base \
-      ros-dev-tools \
-      ros-rolling-cv-bridge \
-      ros-rolling-image-geometry \
-      ros-rolling-laser-geometry \
-      ros-rolling-pcl-conversions \
-      ros-rolling-rviz-common \
-      ros-rolling-rviz-rendering \
-      ros-rolling-rviz-default-plugins \
-      ros-rolling-pcl-ros \
-      ros-rolling-imu-filter-madgwick \
-      ros-rolling-image-transport \
-      ros-rolling-octomap-msgs \
-      ros-rolling-libg2o \
-      ros-rolling-gtsam \
-      ros-rolling-libpointmatcher \
-      ros-rolling-qt-gui-cpp \
-      ros-rolling-diagnostic-updater && \
-    apt-get clean && rm -rf /var/lib/apt/lists/
-
-WORKDIR /root/
-
-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/rolling/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/rolling/lib/x86_64-linux-gnu
-
-# For devcontainer
-# remove ubuntu user
-RUN touch /var/mail/ubuntu && chown ubuntu /var/mail/ubuntu && userdel -r ubuntu
-
-RUN apt-get update && apt-get install -y sudo && \
-    apt-get clean && rm -rf /var/lib/apt/lists/
-
-ARG USERNAME=vscode
-ARG USER_UID=1000
-ARG USER_GID=1000
-
-RUN set -ex && \
-    groupadd --gid ${USER_GID} ${USERNAME} && \
-    useradd --uid ${USER_UID} --gid ${USER_GID} -m ${USERNAME} && \
-    usermod -a -G sudo ${USERNAME} && \
-    echo "${USERNAME} ALL=(ALL) NOPASSWD:ALL" > /etc/sudoers.d/${USERNAME} && \
-    chmod 0440 /etc/sudoers.d/${USERNAME}
-
-RUN echo "source /usr/share/bash-completion/completions/git" >> /home/${USERNAME}/.bashrc
-

.devcontainer/rolling/devcontainer.json

@@ -1,17 +0,0 @@
-{
-    "build": {
-      "dockerfile": "Dockerfile"
-    },
-    "customizations": {
-        "vscode": {
-          "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools"]
-        }
-      },
-    "workspaceMount": "source=${localWorkspaceFolder},target=/home/vscode/rtabmap,type=bind",
-    "workspaceFolder": "/home/vscode/rtabmap",
-    "settings": {
-      "terminal.integrated.defaultProfile.linux": "bash"
-    },
-    "remoteUser": "vscode",
-    "runArgs": ["--privileged", "--network=host"]
-}

.github/workflows/cmake-ros.yml

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

.github/workflows/cmake.yml

@@ -19,26 +19,19 @@ jobs:
       fail-fast: false
       matrix:
         os: [ubuntu-24.04, ubuntu-22.04]
-        include:
-        - os: ubuntu-22.04
-          extra_deps: "libunwind-dev libceres-dev"
-          extra_cmake_def: ""
-        - os: ubuntu-24.04
-          extra_deps: "libg2o-dev libceres-dev"
-          extra_cmake_def: "-DWITH_CERES=ON"
 
     steps:    
     - name: Install dependencies
       run: |
         DEBIAN_FRONTEND=noninteractive
         sudo apt-get update
-        sudo apt-get -y install libopencv-dev libpcl-dev git cmake software-properties-common libyaml-cpp-dev ${{ matrix.extra_deps }}
+        sudo apt-get -y install libopencv-dev libpcl-dev git cmake software-properties-common libyaml-cpp-dev
 
     - uses: actions/checkout@v4
 
     - name: Configure CMake
       run: |
-        cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} ${{ matrix.extra_cmake_def }}
+        cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}}
 
     - name: Build
       run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}}

.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, noble-deps, noble-kilted-deps]
+        docker_tag: [focal-deps, jammy-deps, jammy-iron-deps, noble-deps]
         include:
         - docker_tag: focal-deps
           docker_tags: |
@@ -30,6 +30,7 @@ jobs:
           docker_platforms: |
             linux/amd64
             linux/arm64
+            linux/arm/v7
           docker_path: 'focal/deps'
         - docker_tag: jammy-deps
           docker_tags: |
@@ -38,6 +39,12 @@ 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
@@ -45,13 +52,6 @@ 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, noble, noble-kilted, android23, android24, android26, android30]
+        docker_tag: [bionic, focal, jammy, jammy-iron, noble, android23, android24, android26, android30]
         include:
         - docker_tag: bionic
           docker_tags: |
@@ -112,6 +112,7 @@ jobs:
           docker_platforms: |
             linux/amd64
             linux/arm64
+            linux/arm/v7
           docker_path: 'focal'
         - docker_tag: jammy
           docker_tags: |
@@ -123,6 +124,14 @@ 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
@@ -133,15 +142,6 @@ 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
@@ -150,7 +150,7 @@ jobs:
             API_VERSION=23
           docker_platforms: |
             linux/amd64
-          docker_path: 'noble/android/rtabmap_apiXX'
+          docker_path: 'bionic/android/rtabmap_apiXX'
         - docker_tag: android24
           docker_tags: |
             introlab3it/rtabmap:android24
@@ -158,7 +158,7 @@ jobs:
             API_VERSION=24
           docker_platforms: |
             linux/amd64
-          docker_path: 'noble/android/rtabmap_apiXX'
+          docker_path: 'bionic/android/rtabmap_apiXX'
         - docker_tag: android26
           docker_tags: |
             introlab3it/rtabmap:android26
@@ -166,7 +166,7 @@ jobs:
             API_VERSION=26
           docker_platforms: |
             linux/amd64
-          docker_path: 'noble/android/rtabmap_apiXX'
+          docker_path: 'bionic/android/rtabmap_apiXX'
         - docker_tag: android30
           docker_tags: |
             introlab3it/rtabmap:android30
@@ -174,7 +174,7 @@ jobs:
             API_VERSION=30
           docker_platforms: |
             linux/amd64
-          docker_path: 'noble/android/rtabmap_apiXX'
+          docker_path: 'bionic/android/rtabmap_apiXX'
     
     steps:
       -

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 22)
+SET(RTABMAP_MINOR_VERSION 21)
-SET(RTABMAP_PATCH_VERSION 1)
+SET(RTABMAP_PATCH_VERSION 13)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   
@@ -501,30 +501,21 @@ IF(WITH_G2O)
          get_target_property(G2O_INCLUDES g2o::core INTERFACE_INCLUDE_DIRECTORIES)
          MESSAGE(STATUS "g2o include dir: ${G2O_INCLUDES}")
          FIND_FILE(G2O_FACTORY_FILE g2o/core/factory.h 
-            PATHS ${G2O_INCLUDES}
+         PATHS ${G2O_INCLUDES}
-            NO_DEFAULT_PATH)
+         NO_DEFAULT_PATH)
-         FILE(READ ${G2O_FACTORY_FILE} TMPTXT)
+       FILE(READ ${G2O_FACTORY_FILE} TMPTXT)
-         STRING(FIND "${TMPTXT}" "shared_ptr" matchres)
+       STRING(FIND "${TMPTXT}" "shared_ptr" matchres)
-         IF(${matchres} EQUAL -1)
+       IF(${matchres} EQUAL -1)
-            MESSAGE(STATUS "Old g2o factory version detected without shared ptr (factory file: ${G2O_FACTORY_FILE}).")
+           MESSAGE(STATUS "Old g2o factory version detected without shared ptr (factory file: ${G2O_FACTORY_FILE}).")
-            SET(G2O_CPP11 2)
+           SET(G2O_CPP11 2)
-         ELSE()
+       ELSE()
-            MESSAGE(STATUS "Latest g2o factory version detected with shared ptr (factory file: ${G2O_FACTORY_FILE}).")
+           MESSAGE(STATUS "Latest g2o factory version detected with shared ptr (factory file: ${G2O_FACTORY_FILE}).")
-            SET(G2O_CPP11 1)
+           SET(G2O_CPP11 1)
-         ENDIF()
+       ENDIF()
     ELSE()
         FIND_PACKAGE(G2O QUIET)
         IF(G2O_FOUND)
             MESSAGE(STATUS "Found g2o: ${G2O_INCLUDE_DIRS}")
-            FIND_FILE(G2O_FACTORY_FILE g2o/core/factory.h 
-               PATHS ${G2O_INCLUDES}
-               NO_DEFAULT_PATH)
-            FILE(READ ${G2O_FACTORY_FILE} TMPTXT)
-            STRING(FIND "${TMPTXT}" "shared_ptr" matchres)
-            IF(NOT ${matchres} EQUAL -1)
-               MESSAGE(STATUS "Latest g2o factory version detected with shared ptr (factory file: ${G2O_FACTORY_FILE}).")
-               SET(G2O_CPP11 1)
-            ENDIF()
         ENDIF(G2O_FOUND)
     ENDIF()
 ENDIF(WITH_G2O)
@@ -1419,11 +1410,11 @@ MESSAGE(STATUS "  With ORB OcTree           = NO (WITH_ORB_OCTREE=OFF)")
 ENDIF()
 
 IF(TORCH_FOUND)
-MESSAGE(STATUS "  With SuperPoint          = YES (License: GPLv3) libtorch=${Torch_VERSION}")
+MESSAGE(STATUS "  With SupertPoint          = YES (License: GPLv3) libtorch=${Torch_VERSION}")
 ELSEIF(NOT WITH_TORCH)
-MESSAGE(STATUS "  With SuperPoint          = NO (WITH_TORCH=OFF)")
+MESSAGE(STATUS "  With SupertPoint          = NO (WITH_TORCH=OFF)")
 ELSE()
-MESSAGE(STATUS "  With SuperPoint          = NO (libtorch not found)")
+MESSAGE(STATUS "  With SupertPoint          = NO (libtorch not found)")
 ENDIF()
 
 IF(WITH_PYTHON AND Python3_FOUND)

app/android/jni/CameraARCore.cpp

@@ -37,8 +37,8 @@ namespace rtabmap {
 //////////////////////////////
 // CameraARCore
 //////////////////////////////
-CameraARCore::CameraARCore(void* env, void* context, void* activity, bool depthFromMotion, float upstreamRelocalizationAccThr):
+CameraARCore::CameraARCore(void* env, void* context, void* activity, bool depthFromMotion, bool smoothing, float upstreamRelocalizationAccThr):
-	CameraMobile(upstreamRelocalizationAccThr),
+	CameraMobile(smoothing, 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, float upstreamRelocalizationAccThr = 0.0f);
+	CameraARCore(void* env, void* context, void* activity, bool depthFromMotion = false, bool smoothing = 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, float upstreamRelocalizationAccThr):
+CameraAREngine::CameraAREngine(void* env, void* context, void* activity, bool smoothing, float upstreamRelocalizationAccThr):
-	CameraMobile(upstreamRelocalizationAccThr),
+	CameraMobile(smoothing, 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, float upstreamRelocalizationAccThr = 0.0f);
+	CameraAREngine(void* env, void* context, void* activity, bool smoothing = false, float upstreamRelocalizationAccThr = 0.0f);
 	virtual ~CameraAREngine();
 
 	virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height);

app/android/jni/CameraMobile.cpp

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

app/android/jni/CameraTango.cpp

@@ -114,7 +114,8 @@ void onTangoEventAvailableRouter(void* context, const TangoEvent* event)
 //////////////////////////////
 // CameraTango
 //////////////////////////////
-CameraTango::CameraTango(bool colorCamera, int decimation, bool publishRawScan) :
+CameraTango::CameraTango(bool colorCamera, int decimation, bool publishRawScan, bool smoothing) :
+		CameraMobile(smoothing),
 		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);
+	CameraTango(bool colorCamera, int decimation, bool publishRawScan, bool smoothing);
 	virtual ~CameraTango();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");

app/android/jni/RTABMapApp.cpp

@@ -84,9 +84,6 @@ 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;
@@ -233,7 +230,6 @@ RTABMapApp::RTABMapApp() :
 		trajectoryMode_(false),
 		rawScanSaved_(false),
 		smoothing_(true),
-		depthBleedingError_(0.0f),
 		depthFromMotion_(false),
 		cameraColor_(true),
 		fullResolution_(false),
@@ -249,11 +245,7 @@ RTABMapApp::RTABMapApp() :
 		maxGainRadius_(0.02f),
 		renderingTextureDecimation_(4),
 		backgroundColor_(0.2f),
-#ifndef RTABMAP_ARCORE
+        depthConfidence_(2),
-        depthConfidence_(100), // iOS
-#else
-		depthConfidence_(0),
-#endif
         upstreamRelocalizationMaxAcc_(0.0f),
 		exportPointCloudFormat_("ply"),
 		dataRecorderMode_(false),
@@ -274,20 +266,20 @@ RTABMapApp::RTABMapApp() :
 		lastPoseEventTime_(0.0),
 		visualizingMesh_(false),
 		exportedMeshUpdated_(false),
-		optTextureMesh_(new pcl::TextureMesh),
-		optRefId_(0),
-		optRefPose_(0),
 		measuresUpdated_(false),
-		metricSystem_(true),
+        targetPoint_(new pcl::PointCloud<pcl::PointXYZRGB>),
+        quadSample_(new pcl::PointCloud<pcl::PointXYZ>),
+        quadSamplePolygons_(2),
+        metricSystem_(true),
         measuringTextSize_(0.05f),
 		snapAxisThr_(0.95),
         measuringMode_(0),
         addMeasureClicked_(false),
         teleportClicked_(false),
         removeMeasureClicked_(false),
-		targetPoint_(new pcl::PointCloud<pcl::PointXYZRGB>),
+		optTextureMesh_(new pcl::TextureMesh),
-        quadSample_(new pcl::PointCloud<pcl::PointXYZ>),
+		optRefId_(0),
-        quadSamplePolygons_(2),
+		optRefPose_(0),
 		mapToOdom_(rtabmap::Transform::getIdentity())
 
 {
@@ -469,13 +461,7 @@ 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();
-                if(textures.rows > 4096) // Limitation iOS, just for rendering on device
+				optTexture_ = textures;
-                {
-                    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);
@@ -583,8 +569,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, tmpB, tmpC;
+						cv::Mat tmpA, depth;
-						data.uncompressData(&tmpA, &tmpB, 0, 0, 0, 0, 0, depthConfidence_>0?&tmpC:0);
+						data.uncompressData(&tmpA, &depth);
 
 						if(!(!data.imageRaw().empty() && !data.depthRaw().empty()) && !data.laserScanCompressed().isEmpty())
 						{
@@ -600,30 +586,8 @@ 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);
-
+                                
-								if(smoothing_ || depthBleedingError_>0.0f)
+								cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get());
-								{
-									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
 							{
@@ -911,11 +875,11 @@ int RTABMapApp::updateMeshDecimation(int width, int height)
         {
             meshDecimation = 5;
         }
-        else if(width % 3 == 0 && height % 3 == 0)
+        else if(width % 3 == 0 && width % 3 == 0)
         {
             meshDecimation = 3;
         }
-        else if(width % 2 == 0 && height % 2 == 0)
+        else if(width % 2 == 0 && width % 2 == 0)
         {
             meshDecimation = 2;
         }
@@ -982,7 +946,7 @@ bool RTABMapApp::startCamera()
 	if(cameraDriver_ == 0) // Tango
 	{
 #ifdef RTABMAP_TANGO
-		camera_ = new rtabmap::CameraTango(cameraColor_, !cameraColor_ || fullResolution_?1:2, rawScanSaved_);
+		camera_ = new rtabmap::CameraTango(cameraColor_, !cameraColor_ || fullResolution_?1:2, rawScanSaved_, smoothing_);
 
 		if (TangoService_setBinder(env, iBinder) != TANGO_SUCCESS) {
 		    UERROR("TangoHandler::ConnectTango, TangoService_setBinder error");
@@ -997,7 +961,7 @@ bool RTABMapApp::startCamera()
 	else if(cameraDriver_ == 1)
 	{
 #ifdef RTABMAP_ARCORE
-		camera_ = new rtabmap::CameraARCore(env, context, activity, depthFromMotion_, upstreamRelocalizationMaxAcc_);
+		camera_ = new rtabmap::CameraARCore(env, context, activity, depthFromMotion_, smoothing_, upstreamRelocalizationMaxAcc_);
 #else
 		UERROR("RTAB-Map is not built with ARCore support!");
 #endif
@@ -1005,14 +969,14 @@ bool RTABMapApp::startCamera()
 	else if(cameraDriver_ == 2)
 	{
 #ifdef RTABMAP_ARENGINE
-		camera_ = new rtabmap::CameraAREngine(env, context, activity, upstreamRelocalizationMaxAcc_);
+		camera_ = new rtabmap::CameraAREngine(env, context, activity, smoothing_, upstreamRelocalizationMaxAcc_);
 #else
 		UERROR("RTAB-Map is not built with AREngine support!");
 #endif
 	}
 	else if(cameraDriver_ == 3)
 	{
-		camera_ = new rtabmap::CameraMobile(upstreamRelocalizationMaxAcc_);
+		camera_ = new rtabmap::CameraMobile(smoothing_, upstreamRelocalizationMaxAcc_);
 	}
 
 	if(camera_ == 0)
@@ -1039,11 +1003,7 @@ 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());
@@ -1394,8 +1354,8 @@ int RTABMapApp::Render()
 #ifdef DEBUG_RENDERING_PERFORMANCE
 				LOGD("Camera updateOnRender %fs", time.ticks());
 #endif
-                // We check if we are in measuring mode: not visualizing mesh or rtabmap is not started (localization mode)
+                // We detect if we are in measuring mode if rtabmap is not running
-				if(main_scene_.background_renderer_ == 0 && camera_->getTextureId() != 0 && (!visualizingMesh_ || !(rtabmapThread_ == 0 || !rtabmapThread_->isRunning())))
+				if(main_scene_.background_renderer_ == 0 && camera_->getTextureId() != 0 && !(rtabmapThread_ == 0 || !rtabmapThread_->isRunning()))
 				{
 					main_scene_.background_renderer_ = new BackgroundRenderer();
 					main_scene_.background_renderer_->InitializeGlContent(((rtabmap::CameraMobile*)camera_)->getTextureId(), cameraDriver_ <= 2);
@@ -1576,6 +1536,7 @@ int RTABMapApp::Render()
                 int textId = 0;
                 int quadId = 0;
                 int circleId = 0;
+                float sphereRadius = 0.02f;
                 float quadSize=0.05f;
                 float quadAlpha = 0.3f;
 
@@ -2049,8 +2010,8 @@ int RTABMapApp::Render()
 								{
 									rtabmap::SensorData data = bufferedSensorData.at(id);
 
-									cv::Mat tmpA, tmpB, tmpC;
+									cv::Mat tmpA, depth;
-									data.uncompressData(&tmpA, &tmpB, 0, 0, 0, 0, 0, depthConfidence_>0?&tmpC:0);
+									data.uncompressData(&tmpA, &depth);
 									if(!(!data.imageRaw().empty() && !data.depthRaw().empty()) && !data.laserScanCompressed().isEmpty())
 									{
 										rtabmap::LaserScan scan;
@@ -2068,20 +2029,7 @@ int RTABMapApp::Render()
 										if(!data.imageRaw().empty() && !data.depthRaw().empty() && (!useExternalLidar_ || data.laserScanRaw().isEmpty()))
 										{
                                             int meshDecimation = updateMeshDecimation(data.depthRaw().cols, data.depthRaw().rows);
-											if(smoothing_ || depthBleedingError_>0.0f)
+											cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get());
-											{
-												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
 										{
@@ -2297,7 +2245,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(), rtabmap::ParametersMap(), std::vector<float>(), depthConfidence_);
+								cloud = rtabmap::util3d::cloudRGBFromSensorData(sensorEvent.data(), meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get());
 							}
 							else
 							{
@@ -3067,11 +3015,6 @@ void RTABMapApp::setSmoothing(bool enabled)
 	}
 }
 
-void RTABMapApp::setDepthBleedingError(float value)
-{
-	depthBleedingError_ = value;
-}
-
 void RTABMapApp::setDepthFromMotion(bool enabled)
 {
 	if(depthFromMotion_ != enabled)
@@ -3164,10 +3107,10 @@ void RTABMapApp::setBackgroundColor(float gray)
 
 void RTABMapApp::setDepthConfidence(int value)
 {
-    depthConfidence_ = value*50; // [0,2] -> [0,100]
+    depthConfidence_ = value;
-    if(depthConfidence_>100)
+    if(depthConfidence_>2)
     {
-        depthConfidence_ = 100;
+        depthConfidence_ = 2;
     }
 }
 
@@ -3467,19 +3410,6 @@ 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();
@@ -3747,20 +3677,7 @@ 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)
+								cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation, maxCloudDepth_, minCloudDepth_);
-								{
-									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_);
 							}
 						}
@@ -4018,20 +3935,7 @@ bool RTABMapApp::exportMesh(
 					if(!data.imageRaw().empty() && !data.depthRaw().empty())
 					{
 						// full resolution
-						if(smoothing_ || depthBleedingError_>0.0f)
+						cloud = rtabmap::util3d::cloudRGBFromSensorData(data, 1, maxCloudDepth_, minCloudDepth_, indices.get());
-						{
-							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())
                     {
@@ -4061,20 +3965,7 @@ bool RTABMapApp::exportMesh(
 						if(!data.imageRaw().empty() && !data.depthRaw().empty())
 						{
                             int meshDecimation = updateMeshDecimation(data.depthRaw().cols, data.depthRaw().rows);
-							if(smoothing_ || depthBleedingError_>0.0f)
+							cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get());
-							{
-								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())
                         {
@@ -4226,13 +4117,7 @@ bool RTABMapApp::postExportation(bool visualize)
 				LOGI("postExportation: Found optimized mesh! Visualizing it.");
 				optTextureMesh_ = rtabmap::util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, true);
                 optMesh_ = rtabmap::Mesh();
-                if(textures.rows > 4096) // Limitation iOS, just for rendering on device
+                optTexture_ = textures;
-                {
-                    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;
@@ -4639,7 +4524,6 @@ void RTABMapApp::postOdometryEvent(
 
 
 				cv::Mat outputDepth;
-				cv::Mat outputDepthConfidence;
 				if(depth && depthHeight>0 && depthWidth>0)
 				{
 #ifndef DISABLE_LOG
@@ -4649,21 +4533,32 @@ void RTABMapApp::postOdometryEvent(
                     {
                         // IOS
                         outputDepth = cv::Mat(depthHeight, depthWidth, CV_32FC1, (void*)depth).clone();
-                        if(conf && confWidth == depthWidth && confHeight == depthHeight && confFormat == 1278226488)
+                        if(conf && confWidth == depthWidth && confHeight == depthHeight && confFormat == 1278226488 && depthConfidence_>0)
                         {
-							// https://developer.apple.com/documentation/arkit/arconfidencelevel
+                            const unsigned char * confPtr = (const unsigned char *)conf;
-							// 0 = low
+                            float * depthPtr = outputDepth.ptr<float>();
-							// 1 = medium
+                            int i=0;
-							// 2 = high
+                            for (int y = 0; y < outputDepth.rows; ++y)
-							// Re-scale confidence from [0,2] to [0,100]
+                            {
-							cv::Mat(depthHeight, depthWidth, CV_8UC1, (void*)conf).convertTo(outputDepthConfidence, CV_8UC1, 50, 0);
+                                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;
+                                    }
+                                }
+                            }
                         }
                     }
                     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)
                         {
@@ -4672,13 +4567,6 @@ 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;
                             }
                         }
                     }
@@ -4738,9 +4626,7 @@ void RTABMapApp::postOdometryEvent(
 								depth_fx, 0, depth_cx,
 								0, depth_fy, depth_cy,
 								0, 0, 1);
-						cv::Mat regConfidence;
+						outputDepth = rtabmap::util2d::registerDepth(outputDepth, depthK, outputDepth.size(), colorK, rgbToDepth);
-						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
@@ -4780,8 +4666,8 @@ void RTABMapApp::postOdometryEvent(
                         depthModel.setLocalTransform(pose*model.localTransform());
                         camera_->setOcclusionImage(outputDepth, depthModel);
                     }
-
+                    
-					rtabmap::SensorData data(scan, outputRGB, outputDepth, outputDepthConfidence, model, 0, stamp);
+					rtabmap::SensorData data(scan, outputRGB, outputDepth, model, 0, stamp);
 					data.setFeatures(kpts,  kpts3, cv::Mat());
                     glm::mat4 projectionMatrix(0);
                     projectionMatrix[0][0] = p00;

app/android/jni/RTABMapApp.h

@@ -139,7 +139,6 @@ 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);
@@ -236,7 +235,6 @@ class RTABMapApp : public UEventsHandler {
   bool trajectoryMode_;
   bool rawScanSaved_;
   bool smoothing_;
-  float depthBleedingError_;
   bool depthFromMotion_;
   bool cameraColor_;
   bool fullResolution_;
@@ -252,7 +250,7 @@ class RTABMapApp : public UEventsHandler {
   float maxGainRadius_;
   int renderingTextureDecimation_;
   float backgroundColor_;
-  unsigned char depthConfidence_;
+  int depthConfidence_;
   float upstreamRelocalizationMaxAcc_;
   std::string exportPointCloudFormat_;
 

app/android/jni/jni_interface.cpp

@@ -512,19 +512,6 @@ 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,13 +114,6 @@
 		        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,8 +76,6 @@
     <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>
     
@@ -339,8 +337,6 @@
     <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>
@@ -440,26 +436,6 @@
         <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,7 +1140,6 @@ 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,7 +73,6 @@ 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,8 +222,7 @@ 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_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_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));
@@ -286,8 +285,7 @@ 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_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_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,90 +8,49 @@
 
 /* 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 */; };
@@ -119,8 +78,80 @@
 		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 */; };
@@ -129,42 +160,6 @@
 /* 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>"; };
@@ -175,15 +170,6 @@
 		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>"; };
@@ -370,79 +356,110 @@
 			isa = PBXFrameworksBuildPhase;
 			buildActionMask = 2147483647;
 			files = (
-				4E8B155426273A580037FC53 /* Zip in Frameworks */,
+				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 */,
 				4EFD0EAB259D581800575D88 /* OpenGLES.framework in Frameworks */,
-				4E10D89C2DE409870015926D /* librtabmap_core.a in Frameworks */,
+				4EFD0B36259D4DE900575D88 /* libboost_filesystem.a in Frameworks */,
-				4E10D89D2DE409870015926D /* librtabmap_utilite.a in Frameworks */,
+				4EFD0B37259D4DE900575D88 /* libboost_program_options.a in Frameworks */,
-				4E10D89E2DE409A60015926D /* libopencv_xfeatures2d.a in Frameworks */,
+				4EFD0B38259D4DE900575D88 /* libboost_regex.a in Frameworks */,
-				4E10D89F2DE409B20015926D /* libopencv_video.a in Frameworks */,
+				4EFD0B39259D4DE900575D88 /* libboost_iostreams.a in Frameworks */,
-				4E10D8A02DE409BB0015926D /* libopencv_tracking.a in Frameworks */,
+				4EFD0B3A259D4DE900575D88 /* libboost_date_time.a in Frameworks */,
-				4E10D8A12DE409C00015926D /* libopencv_optflow.a in Frameworks */,
+				4EFD0B3B259D4DE900575D88 /* libboost_serialization.a in Frameworks */,
-				4E10D8A22DE409C70015926D /* libopencv_imgproc.a in Frameworks */,
+				4EFD0B3C259D4DE900575D88 /* libboost_system.a in Frameworks */,
-				4E10D8A32DE409CA0015926D /* libopencv_features2d.a in Frameworks */,
+				4EFD0B3D259D4DE900575D88 /* libboost_signals.a in Frameworks */,
-				4E10D8A42DE409CE0015926D /* libopencv_core.a in Frameworks */,
+				4EFD0B3E259D4DE900575D88 /* libboost_thread.a in Frameworks */,
-				4E10D8A52DE409D10015926D /* libopencv_calib3d.a in Frameworks */,
+				4EFD0B3F259D4DE900575D88 /* libboost_timer.a in Frameworks */,
-				4E10D8A62DE409D50015926D /* libopencv_aruco.a in Frameworks */,
+				4EFD0B40259D4DE900575D88 /* libboost_chrono.a in Frameworks */,
-				4E10D8A72DE409F40015926D /* libopencv_flann.a in Frameworks */,
+				4EFD0B51259D4DF100575D88 /* libpcl_stereo.a in Frameworks */,
-				4E10D8A82DE40A1B0015926D /* libopencv_ximgproc.a in Frameworks */,
+				4EFD0B52259D4DF100575D88 /* libpcl_registration.a in Frameworks */,
-				4E10D8A92DE40A3A0015926D /* libopencv_photo.a in Frameworks */,
+				4EFD0B53259D4DF100575D88 /* libpcl_features.a in Frameworks */,
-				4E10D8AA2DE40A6C0015926D /* libopencv_objdetect.a in Frameworks */,
+				4EFD0B54259D4DF100575D88 /* libpcl_filters.a in Frameworks */,
-				4E10D8AB2DE40A8C0015926D /* libopencv_stitching.a in Frameworks */,
+				4EFD0B55259D4DF100575D88 /* libpcl_sample_consensus.a in Frameworks */,
-				4E10D8AC2DE40AA20015926D /* libopencv_imgcodecs.a in Frameworks */,
+				4EFD0B56259D4DF100575D88 /* libpcl_kdtree.a in Frameworks */,
-				4E3525672DE4C72D00F39D8F /* libamd.a in Frameworks */,
+				4EFD0B57259D4DF100575D88 /* libpcl_search.a in Frameworks */,
-				4E3525642DE4C71C00F39D8F /* libsuitesparseconfig.a in Frameworks */,
+				4EFD0B58259D4DF100575D88 /* libpcl_surface.a in Frameworks */,
-				4E3525652DE4C71C00F39D8F /* libspqr.a in Frameworks */,
+				4EFD0B59259D4DF100575D88 /* libpcl_keypoints.a in Frameworks */,
-				4E35255E2DE4C71000F39D8F /* libcxsparse.a in Frameworks */,
+				4EFD0B5A259D4DF100575D88 /* libpcl_recognition.a in Frameworks */,
-				4E35255F2DE4C71000F39D8F /* libcolamd.a in Frameworks */,
+				4EFD0B5B259D4DF100575D88 /* libpcl_octree.a in Frameworks */,
-				4E3525602DE4C71000F39D8F /* libcamd.a in Frameworks */,
+				4EFD0B5C259D4DF100575D88 /* libpcl_io.a in Frameworks */,
-				4E3525612DE4C71000F39D8F /* libccolamd.a in Frameworks */,
+				4EFD0B5D259D4DF100575D88 /* libpcl_common.a in Frameworks */,
-				4E3525592DE4C6F600F39D8F /* libcholmod.a in Frameworks */,
+				4EFD0B5E259D4DF100575D88 /* libpcl_io_ply.a in Frameworks */,
-				4E10D8AD2DE40E990015926D /* libg2o_core.a in Frameworks */,
+				4EFD0B5F259D4DF100575D88 /* libpcl_ml.a in Frameworks */,
-				4E3525572DE4C6E500F39D8F /* libg2o_solver_cholmod.a in Frameworks */,
+				4EFD0B60259D4DF100575D88 /* libpcl_segmentation.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;
 		};
@@ -492,51 +509,6 @@
 		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 */,
@@ -917,7 +889,7 @@
 				ALWAYS_SEARCH_USER_PATHS = NO;
 				CLANG_ANALYZER_NONNULL = YES;
 				CLANG_ANALYZER_NUMBER_OBJECT_CONVERSION = YES_AGGRESSIVE;
-				CLANG_CXX_LANGUAGE_STANDARD = "gnu++17";
+				CLANG_CXX_LANGUAGE_STANDARD = "gnu++14";
 				CLANG_CXX_LIBRARY = "libc++";
 				CLANG_ENABLE_MODULES = YES;
 				CLANG_ENABLE_OBJC_ARC = YES;
@@ -948,7 +920,7 @@
 				DEBUG_INFORMATION_FORMAT = dwarf;
 				ENABLE_STRICT_OBJC_MSGSEND = YES;
 				ENABLE_TESTABILITY = YES;
-				GCC_C_LANGUAGE_STANDARD = gnu17;
+				GCC_C_LANGUAGE_STANDARD = gnu11;
 				GCC_DYNAMIC_NO_PIC = NO;
 				GCC_NO_COMMON_BLOCKS = YES;
 				GCC_OPTIMIZATION_LEVEL = 0;
@@ -978,7 +950,7 @@
 				ALWAYS_SEARCH_USER_PATHS = NO;
 				CLANG_ANALYZER_NONNULL = YES;
 				CLANG_ANALYZER_NUMBER_OBJECT_CONVERSION = YES_AGGRESSIVE;
-				CLANG_CXX_LANGUAGE_STANDARD = "gnu++17";
+				CLANG_CXX_LANGUAGE_STANDARD = "gnu++14";
 				CLANG_CXX_LIBRARY = "libc++";
 				CLANG_ENABLE_MODULES = YES;
 				CLANG_ENABLE_OBJC_ARC = YES;
@@ -1009,7 +981,7 @@
 				DEBUG_INFORMATION_FORMAT = "dwarf-with-dsym";
 				ENABLE_NS_ASSERTIONS = NO;
 				ENABLE_STRICT_OBJC_MSGSEND = YES;
-				GCC_C_LANGUAGE_STANDARD = gnu17;
+				GCC_C_LANGUAGE_STANDARD = gnu11;
 				GCC_NO_COMMON_BLOCKS = YES;
 				GCC_PREPROCESSOR_DEFINITIONS = "";
 				GCC_WARN_64_TO_32_BIT_CONVERSION = YES;
@@ -1063,9 +1035,8 @@
 					"$(inherited)",
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/lib",
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/share/OpenCV/3rdparty/lib",
-					"$(PROJECT_DIR)/RTABMapApp/Libraries/lib/opencv4/3rdparty",
 				);
-				MARKETING_VERSION = 0.22.0;
+				MARKETING_VERSION = 0.21.11;
 				OTHER_CFLAGS = "";
 				PRODUCT_BUNDLE_IDENTIFIER = com.introlab.rtabmap;
 				PRODUCT_NAME = "$(TARGET_NAME)";
@@ -1077,12 +1048,11 @@
 				SYSTEM_HEADER_SEARCH_PATHS = (
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include/eigen3\"",
-					"\"$(SRCROOT)/RTABMapApp/Libraries/include/pcl-1.15\"",
+					"\"$(SRCROOT)/RTABMapApp/Libraries/include/pcl-1.11\"",
-					"\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.22\"",
+					"\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.21\"",
 					"\"$(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;
@@ -1123,9 +1093,8 @@
 					"$(inherited)",
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/lib",
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/share/OpenCV/3rdparty/lib",
-					"$(PROJECT_DIR)/RTABMapApp/Libraries/lib/opencv4/3rdparty",
 				);
-				MARKETING_VERSION = 0.22.0;
+				MARKETING_VERSION = 0.21.11;
 				ONLY_ACTIVE_ARCH = YES;
 				OTHER_CFLAGS = "";
 				PRODUCT_BUNDLE_IDENTIFIER = com.introlab.rtabmap;
@@ -1138,12 +1107,11 @@
 				SYSTEM_HEADER_SEARCH_PATHS = (
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include/eigen3\"",
-					"\"$(SRCROOT)/RTABMapApp/Libraries/include/pcl-1.15\"",
+					"\"$(SRCROOT)/RTABMapApp/Libraries/include/pcl-1.11\"",
-					"\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.22\"",
+					"\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.21\"",
 					"\"$(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,13 +505,6 @@ 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,7 +101,6 @@ 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,9 +453,6 @@ 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,6 +162,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
     func resetNoTouchTimer(_ showHud: Bool = false) {
         if(showHud)
         {
+            print("Show HUD")
             mMenuOpened = false
             mHudVisible = true
             setNeedsStatusBarAppearanceUpdate()
@@ -183,6 +184,7 @@ 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)
@@ -838,11 +840,13 @@ 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 {
@@ -930,6 +934,7 @@ 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
@@ -1511,7 +1516,6 @@ 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")!);
@@ -1914,9 +1918,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
             })
             
         }))
-	
+
-        let previousState = mState
-        
         updateState(state: .STATE_PROCESSING);
         
         present(alertView, animated: true, completion: {
@@ -1987,11 +1989,8 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
                             self.updateState(state: .STATE_VISUALIZING)
                             
                             self.rtabmap!.postExportation(visualize: true)
-							
+
-                            if previousState != .STATE_VISUALIZING
+                            self.setGLCamera(type: 2)
-                            {
-                                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 4.0.2, XCode 16.3.
+# Tested on Apple Silicon Mac, with cmake 3.19.2.
 
 mkdir -p Libraries
 cd Libraries
@@ -28,65 +28,41 @@ sysroot=iphoneos
 # Boost
 if [ ! -e $prefix/include/boost ]
 then
-if [ ! -e boost-1.88.0 ]
+if [ ! -e boost_1_59_0 ]
 then
   echo "wget boost..."
-  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
+  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.88.0-cmake.tar.gz
+  tar -xzf boost_1_59_0.tar.gz
 fi
-cd boost-1.88.0
+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
 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 -DBOOST_INCOMPATIBLE_LIBRARIES="process;context;coroutine;fiber;fiber_numa;log_setup;log;cobalt" -DBOOST_IOSTREAMS_ENABLE_ZLIB=OFF -DBOOST_IOSTREAMS_ENABLE_BZIP2=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 ..
 cmake --build . --config Release
 cmake --build . --config Release --target install
 cd $pwd
-#rm -r boost-1.88.0-cmake.tar.gz boost-1.88.0
+#rm -r boost_1_59_0.tar.gz boost_1_59_0
 fi
 
 # eigen
 if [ ! -e $prefix/include/eigen3 ]
 then
-if [ ! -e eigen-3.4.0 ]
+if [ ! -e eigen-3.3.9 ]
 then
   echo "wget eigen..."
-  curl -L https://gitlab.com/libeigen/eigen/-/archive/3.4.0/eigen-3.4.0.tar.gz -o 3.4.0.tar.gz
+  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.4.0.tar.gz
+  tar -xzf 3.3.9.tar.gz
 fi
-cd eigen-3.4.0
+cd eigen-3.3.9
 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.4.0.tar.gz eigen-3.4.0
+#rm -r 3.3.9.tar.gz eigen-3.3.9
-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
@@ -95,17 +71,19 @@ then
 if [ ! -e flann ]
 then
   echo "wget flann..."
-  git clone https://github.com/flann-lib/flann.git -b 1.9.2
+  git clone https://github.com/flann-lib/flann.git -b 1.8.4
 fi
 cd flann
-if [ ! -e flann_ios_lz4.patch ]
+if [ ! -e flann_ios.patch ]
 then
-  curl -L https://gist.githubusercontent.com/matlabbe/c858ba36fb85d5e44d8667dfb3543e12/raw/2586a356dec2b11440ec3c1bb113e709e1266d97/flann_ios_lz4.patch  -o flann_ios_lz4.patch
+  curl -L https://gist.githubusercontent.com/matlabbe/c858ba36fb85d5e44d8667dfb3543e12/raw/8fc40aa9bc3267604869444020476a49f14ab424/flann_ios.patch  -o flann_ios.patch
-  git apply flann_ios_lz4.patch
+  git apply flann_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_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  ..
+# 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 --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
@@ -117,14 +95,17 @@ if [ ! -e $prefix/include/gtsam ]
 then
 if [ ! -e gtsam ]
 then
-  git clone https://github.com/borglab/gtsam.git -b 4.2
+  git clone https://bitbucket.org/gtborg/gtsam.git
+  cd gtsam
+  git checkout fbb9d3bdda8b88df51896bc401bfd170573e66f5
+else
+  cd gtsam
 fi
-cd gtsam
 # patch
-if [ ! -e gtsam_4_2_ios.patch ]
+if [ ! -e gtsam_GKlib_ios_fix.patch ]
 then
-  curl -L https://gist.githubusercontent.com/matlabbe/76d658dddb841b3355ae3a6e32850cd8/raw/e7355348c2d536ec50f41effa775ed251ae4e045/gtsam_4_2_ios.patch -o gtsam_4_2_ios.patch
+  curl -L https://gist.github.com/matlabbe/76d658dddb841b3355ae3a6e32850cd8/raw/7033cba1c89097b0c830651d7277c04dc92cbdd9/gtsam_GKlib_ios_fix.patch -o gtsam_GKlib_ios_fix.patch
-  git apply gtsam_4_2_ios.patch
+  git apply gtsam_GKlib_ios_fix.patch
 fi
 mkdir -p build
 cd build
@@ -135,36 +116,22 @@ 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 -b 20241228_git
+  git clone https://github.com/RainerKuemmerle/g2o.git
+  cd g2o
+  git checkout a3f7706bdbb849b2808dc3e1b7aee189f63b498e
+else
+  cd g2o
 fi
-cd g2o
 # patch
-if [ ! -e g2o_20241228_ios.patch ]
+if [ ! -e g2o_ios_fix.patch ]
 then
-ls
+  curl -L https://gist.github.com/matlabbe/b9ccfeae8f0744b275cab23510872680/raw/a58e06accba3976420d4b61341685c123193810e/g2o_ios_fix.patch -o g2o_ios_fix.patch
-  curl -L https://gist.githubusercontent.com/matlabbe/b9ccfeae8f0744b275cab23510872680/raw/6fe2ffe5ba8fba59171adbd2f38f9c3999c61f75/g2o_20241228_ios.patch -o g2o_20241228_ios.patch
+  git apply g2o_ios_fix.patch
-  git apply g2o_20241228_ios.patch
 fi
 mkdir -p build
 cd build
@@ -182,10 +149,11 @@ if [ ! -e VTK ]
 then
   git clone https://github.com/Kitware/VTK.git
   cd VTK
-  git checkout tags/v9.5.0.rc1
+  git checkout tags/v8.2.0
 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 ..
@@ -195,28 +163,26 @@ cd $pwd
 #rm -rf VTK
 fi
 
-
-
 # PCL
-if [ ! -e $prefix/include/pcl-1.15 ]
+if [ ! -e $prefix/include/pcl-1.11 ]
 then
 if [ ! -e pcl ]
 then
   git clone https://github.com/PointCloudLibrary/pcl.git
   cd pcl
-  git checkout tags/pcl-1.15.0
+  git checkout tags/pcl-1.11.1
 else
   cd pcl
 fi
 # patch
-if [ ! -e pcl_1_15_0_ios.patch ]
+if [ ! -e pcl_1_11_1_vtk_ios_support.patch ]
 then
-  curl -L https://gist.githubusercontent.com/matlabbe/f3ba9366eb91e1b855dadd2ddce5746d/raw/7231688d7fb9e86df72ca7c5f355d6b9727205d5/pcl_1_15_0_ios.patch -o pcl_1_15_0_ios.patch
+  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_15_0_ios.patch
+  git apply pcl_1_11_1_vtk_ios_support.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_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 -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 --build . --config Release
 cmake --build . --config Release --target install
 cd $pwd
@@ -224,21 +190,32 @@ cd $pwd
 fi
 
 # OpenCV
-if [ ! -e $prefix/include/opencv4 ]
+if [ ! -e $prefix/include/opencv2 ]
 then
 if [ ! -e opencv_contrib ]
 then
-  git clone https://github.com/opencv/opencv_contrib.git -b 4.11.0
+  git clone https://github.com/opencv/opencv_contrib.git
+  cd opencv_contrib
+  git checkout tags/3.4.2
 fi
 cd $pwd
 if [ ! -e opencv ]
 then
-  git clone https://github.com/opencv/opencv.git -b 4.11.0
+  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
 fi
-cd opencv
 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 -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 ..
+# 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 --build . --config Release
 cmake --build . --config Release --target install
 cd $pwd
@@ -251,10 +228,12 @@ if [ ! -e $prefix/include/laszip ]
 then
 if [ ! -e LASzip ]
 then
-  git clone https://github.com/LASzip/LASzip.git -b 2.0.1
+  git clone https://github.com/LASzip/LASzip.git
+  cd LASzip
+  git checkout 2.0.1
+else
+  cd LASzip
 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
@@ -272,7 +251,6 @@ 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
@@ -288,6 +266,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=ON  -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=OFF  -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). Note: it is possible to change confidence level 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). 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>
 		</dict>
 		<dict>
 			<key>DefaultValue</key>
@@ -86,7 +86,7 @@
 			<key>Type</key>
 			<string>PSGroupSpecifier</string>
 			<key>FooterText</key>
-			<string>Smooth the point clouds. Note that this can be changed in post-processing.</string>
+			<string>Smooth the point clouds.</string>
 		</dict>
 		<dict>
 			<key>Type</key>
@@ -98,44 +98,6 @@
 			<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>
@@ -628,7 +590,7 @@
 			<key>Type</key>
 			<string>PSGroupSpecifier</string>
 			<key>FooterText</key>
-			<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>
+			<string>BRIEF features are fast to compute but are not rotation invariant like FREAK. Warning: Changing feature type will automatically reset the map!</string>
 		</dict>
 		<dict>
 			<key>Type</key>
@@ -641,21 +603,13 @@
 			<string>6</string>
 			<key>Titles</key>
 			<array>
-				<string>SURF</string>
+				<string>BRIEF</string>
-				<string>SIFT</string>
+				<string>FREAK</string>
-				<string>GFTT-FREAK</string>
-				<string>GFTT-BRIEF</string>
-				<string>GFTT-ORB</string>
-				<string>ORB-OCTREE</string>
 			</array>
 			<key>Values</key>
 			<array>
-				<string>0</string>
-				<string>1</string>
-				<string>5</string>
 				<string>6</string>
-				<string>8</string>
+				<string>5</string>
-				<string>10</string>
 			</array>
 		</dict>
 		<dict>

app/ios/Settings.bundle/Root.plist

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

corelib/include/rtabmap/core/OdometryThread.h

@@ -67,8 +67,8 @@ private:
 	unsigned int _dataBufferMaxSize;
 	bool _resetOdometry;
 	Transform _resetPose;
-	double _oldestAsyncImuStamp;
+	double _lastImuStamp;
-	double _newestAsyncImuStamp;
+	double _imuEstimatedDelay;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/Optimizer.h

@@ -149,16 +149,14 @@ 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,
@@ -174,8 +172,7 @@ 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,7 +390,6 @@ 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,7 +333,6 @@ private:
 	int _maxOdomCacheSize;
 	bool _localizationSmoothing;
 	double _localizationPriorInf;
-	bool _localizationSecondTryWithoutProximityLinks;
 	bool _createGlobalScanMap;
 	float _markerPriorsLinearVariance;
 	float _markerPriorsAngularVariance;

corelib/include/rtabmap/core/SensorData.h

@@ -78,16 +78,6 @@ 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,
@@ -97,17 +87,6 @@ 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(
@@ -117,16 +96,6 @@ 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(
@@ -138,17 +107,6 @@ 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,
@@ -202,8 +160,6 @@ public:
 			_imageCompressed.empty() &&
 			_depthOrRightRaw.empty() &&
 			_depthOrRightCompressed.empty() &&
-			_depthConfidenceRaw.empty() &&
-			_depthConfidenceCompressed.empty() &&
 			_laserScanRaw.isEmpty() &&
 			_laserScanCompressed.isEmpty() &&
 			_cameraModels.empty() &&
@@ -222,12 +178,10 @@ 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;}
 
 	/**
@@ -236,9 +190,7 @@ 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);
 
@@ -275,8 +227,7 @@ 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,
@@ -284,8 +235,7 @@ public:
 			cv::Mat * userDataRaw = 0,
 			cv::Mat * groundCellsRaw = 0,
 			cv::Mat * obstacleCellsRaw = 0,
-			cv::Mat * emptyCellsRaw = 0,
+			cv::Mat * emptyCellsRaw = 0) const;
-			cv::Mat * depthConfidenceRaw = 0) const;
 
 	const std::vector<CameraModel> & cameraModels() const {return _cameraModels;}
 	const std::vector<StereoCameraModel> & stereoCameraModels() const {return _stereoCameraModels;}
@@ -377,12 +327,10 @@ 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,7 +81,6 @@ 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(const Transform & t) const;
+	float getAngle(float x=1.0f, float y=0.0f, float z=0.0f) const;
 	float getNorm() const;
 	float getNormSquared() const;
 	float getDistance(const Transform & t) const;

corelib/include/rtabmap/core/util2d.h

@@ -126,14 +126,6 @@ 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,
@@ -152,10 +144,6 @@ 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,15 +97,6 @@ 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(
@@ -125,16 +116,6 @@ 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,
@@ -183,8 +164,7 @@ 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,
@@ -208,8 +188,7 @@ 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
@@ -231,8 +210,7 @@ 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,
@@ -256,8 +234,7 @@ 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.
@@ -413,7 +390,6 @@ 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,
@@ -428,7 +404,6 @@ 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,7 +816,6 @@ 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,7 +34,6 @@ 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"
@@ -403,7 +402,6 @@ 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)
 			{
@@ -1319,15 +1317,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 
 			if(images)
 			{
-				if(uStrNumCmp(_version, "0.22.0") >= 0)
+				fields << "image, depth, calibration";
-				{
-					fields << "image, depth, depth_confidence, calibration";
-				}
-				else
-				{
-					fields << "image, depth, calibration";
-				}
-				
 				if(scan || userData || occupancyGrid)
 				{
 					fields << ", ";
@@ -1458,7 +1448,6 @@ 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();
@@ -1483,17 +1472,6 @@ 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
@@ -1845,7 +1823,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 				{
 					if(models.size())
 					{
-						(*iter)->sensorData().setRGBDImage(imageCompressed, depthOrRightCompressed, depthConfidenceCompressed, models);
+						(*iter)->sensorData().setRGBDImage(imageCompressed, depthOrRightCompressed, models);
 					}
 					else
 					{
@@ -4500,7 +4478,6 @@ 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() ||
@@ -5526,7 +5503,7 @@ cv::Mat DBDriverSqlite3::loadOptimizedMeshQuery(
 								materialPolygons[p][i] = serializedPolygons.at<int>(t + p*polygonSize + i);
 							}
 						}
-						t+=materialPolygons.size()*polygonSize-1;
+						t+=materialPolygons.size()*polygonSize;
 						polygons->push_back(materialPolygons);
 					}
 				}
@@ -5543,7 +5520,7 @@ cv::Mat DBDriverSqlite3::loadOptimizedMeshQuery(
 						UASSERT(serializedTexCoords.total());
 						for(int t=0; t<serializedTexCoords.cols; ++t)
 						{
-							UASSERT_MSG(int(serializedTexCoords.at<float>(t)) > 0, uFormat("serializedTexCoords.at<float>(%d)=%f", t, serializedTexCoords.at<float>(t)).c_str());
+							UASSERT(int(serializedTexCoords.at<float>(t)) > 0);
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 							std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > materialtexCoords(int(serializedTexCoords.at<float>(t)));
 #else
@@ -5556,10 +5533,8 @@ cv::Mat DBDriverSqlite3::loadOptimizedMeshQuery(
 							{
 								materialtexCoords[p][0] = serializedTexCoords.at<float>(t + p*2);
 								materialtexCoords[p][1] = serializedTexCoords.at<float>(t + p*2 + 1);
-								UASSERT(materialtexCoords[p][0]>=0.0f && materialtexCoords[p][0] <= 1.0f);
-								UASSERT(materialtexCoords[p][1]>=0.0f && materialtexCoords[p][1] <= 1.0f);
 							}
-							t+=materialtexCoords.size()*2-1;
+							t+=materialtexCoords.size()*2;
 							texCoords->push_back(materialtexCoords);
 						}
 					}
@@ -6211,11 +6186,7 @@ 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.22.0") >= 0)
+	if(uStrNumCmp(_version, "0.16.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(?,?,?,?,?,?,?,?,?,?,?,?,?,?);";
 	}
@@ -6279,20 +6250,6 @@ 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,7 +607,6 @@ 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)
@@ -646,18 +645,6 @@ 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())
@@ -684,7 +671,7 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
 				}
 				if(!combinedModels.empty())
 				{
-					data.setRGBDImage(combinedImages, combinedDepthImages, combinedDepthConfidenceImages, combinedModels);
+					data.setRGBDImage(combinedImages, combinedDepthImages, combinedModels);
 				}
 				else
 				{
@@ -741,11 +728,10 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
 			}
 			data.setLandmarks(landmarks);
 
-			UDEBUG("Laser=%d RGB/Left=%d Depth/Right=%d, Conf=%d, Grid=%d, UserData=%d, GlobalPose=%d, GPS=%d, IMU=%d",
+			UDEBUG("Laser=%d RGB/Left=%d Depth/Right=%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,
@@ -794,19 +780,13 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
 				}
 				else if(!combinedLocalTransforms.empty())
 				{
-					// We are overriding the camera local transforms, let's move 3D words accordingly
+					// We are overriding the camra local transforms, let's move 3D words accordingly
 					UASSERT(dbModels.size() == combinedLocalTransforms.size());
 					std::vector<cv::Point3f> newKeypoints3D;
-					UASSERT(dbModels[0].imageWidth()>0);
-					int subImageWidth = dbModels[0].imageWidth();
 					for(size_t i = 0; i<keypoints3D.size(); ++i)
 					{
-						int cameraIndex = int(keypoints.at(i).pt.x / subImageWidth);
+						cv::Point3f pt = util3d::transformPoint(keypoints3D.at(i), dbModels[i].localTransform().inverse());
-						UASSERT_MSG(cameraIndex >= 0 && cameraIndex < (int)dbModels.size(),
+						pt = util3d::transformPoint(pt, combinedLocalTransforms[i]);
-								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/Features2d.cpp

@@ -688,7 +688,7 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame
 #ifndef RTABMAP_TORCH
 	if(type == Feature2D::kFeatureSuperPointTorch)
 	{
-		UWARN("SuperPoint Torch feature cannot be used as RTAB-Map is not built with the option enabled. GFTT/ORB is used instead.");
+		UWARN("SupertPoint Torch feature cannot be used as RTAB-Map is not built with the option enabled. GFTT/ORB is used instead.");
 		type = Feature2D::kFeatureGfttOrb;
 	}
 #endif

corelib/src/Graph.cpp

@@ -445,10 +445,6 @@ bool importPoses(
 				std::list<std::string> strList = uSplit(str);
 				if((strList.size() >=  8 && format!=11) || (strList.size() ==  9 && format==11))
 				{
-					if(!uIsNumber(strList.front())) {
-						UWARN("Skipping \"%s\"", str.c_str());
-						continue;
-					}
 					double stamp = uStr2Double(strList.front());
 					strList.pop_front();
 					if(format==11)
@@ -748,8 +744,9 @@ 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];
-		float r_err = pose_delta_result.getAngle(pose_delta_gt);
+		Transform pose_error        = pose_delta_result.inverse()*pose_delta_gt;
-		float t_err = pose_delta_result.getDistance(pose_delta_gt);
+		float r_err = pose_error.getAngle();
+		float t_err = pose_error.getNorm();
 
 		// write to file
 		err.push_back(errors(i,r_err,t_err,0,0));
@@ -996,21 +993,15 @@ void computeMaxGraphErrors(
 			if(iter->second.type() != Link::kLandmark ||
 				1.0 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) < 9999.0)
 			{
-				float angularError = 0.0f;
+				float opt_roll,opt_pitch,opt_yaw;
-				if(force3DoF)
+				float link_roll,link_pitch,link_yaw;
-				{
+				t.getEulerAngles(opt_roll, opt_pitch, opt_yaw);
-					float opt_roll,opt_pitch,opt_yaw;
+				linkT.getEulerAngles(link_roll, link_pitch, link_yaw);
-					float link_roll,link_pitch,link_yaw;
+				float angularError = uMax3(
-					t.getEulerAngles(opt_roll, opt_pitch, opt_yaw);
+						force3DoF?0:fabs(opt_roll - link_roll),
-					linkT.getEulerAngles(link_roll, link_pitch, link_yaw);
+						force3DoF?0:fabs(opt_pitch - link_pitch),
-					angularError = fabs(opt_yaw - link_yaw);
+						fabs(opt_yaw - link_yaw));
-					angularError = angularError>M_PI?2*M_PI-angularError:angularError;
+				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,10 +4893,9 @@ 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 (conf? %d)", data.imageRaw().rows, _imagePreDecimation, data.depthOrRightRaw().rows, decimationDepth, data.depthConfidenceRaw().empty()?0:1);
+				UDEBUG("decimation rgbOrLeft(rows=%d)=%d, depthOrRight(rows=%d)=%d", data.imageRaw().rows, _imagePreDecimation, data.depthOrRightRaw().rows, decimationDepth);
 
 				std::vector<CameraModel> cameraModels = decimatedData.cameraModels();
 				for(unsigned int i=0; i<cameraModels.size(); ++i)
@@ -4908,7 +4907,6 @@ 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);
 				}
 
@@ -5658,8 +5656,6 @@ 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();
 
@@ -5670,7 +5666,6 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		{
 			image = decimatedData.imageRaw();
 			depthOrRightImage = decimatedData.depthOrRightRaw();
-			depthConfidence = decimatedData.depthConfidenceRaw();
 			cameraModels = decimatedData.cameraModels();
 			stereoCameraModels = decimatedData.stereoCameraModels();
 		}
@@ -5884,10 +5879,9 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 	Signature * s;
 	if(this->isBinDataKept() && (!isIntermediateNode || _saveIntermediateNodeData))
 	{
-		UDEBUG("Bin data kept: rgb=%d, depth=%d, conf=%d, scan=%d, userData=%d",
+		UDEBUG("Bin data kept: rgb=%d, depth=%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);
 
@@ -5934,14 +5928,12 @@ 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())
@@ -5952,10 +5944,6 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			{
 				ctDepth.start();
 			}
-			if(!depthConfidence.empty())
-			{
-				ctDepthConfidence.start();
-			}
 			if(!laserScan.isEmpty())
 			{
 				ctLaserScan.start();
@@ -5966,13 +5954,11 @@ 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();
 		}
@@ -5980,7 +5966,6 @@ 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());
 		}
@@ -6031,7 +6016,6 @@ 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,
@@ -6129,7 +6113,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 	// set raw data
 	if(!cameraModels.empty())
 	{
-		s->sensorData().setRGBDImage(image, depthOrRightImage, depthConfidence, cameraModels, false);
+		s->sensorData().setRGBDImage(image, depthOrRightImage, cameraModels, false);
 	}
 	else
 	{

corelib/src/Odometry.cpp

@@ -322,9 +322,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 				Transform previous = this->getPose();
 				Transform newFramePose = Transform(previous.x(), previous.y(), previous.z(), imuQuat.x(), imuQuat.y(), imuQuat.z(), imuQuat.w());
 				UWARN("Updated initial pose from %s to %s with IMU orientation", previous.prettyPrint().c_str(), newFramePose.prettyPrint().c_str());
-				std::map<double, rtabmap::Transform> imus = imus_;
 				this->reset(newFramePose);
-				imus_ = imus;
 			}
 
 			imus_.insert(std::make_pair(data.stamp(), imuT));

corelib/src/OdometryInfo.cpp

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

corelib/src/OdometryThread.cpp

@@ -40,8 +40,8 @@ OdometryThread::OdometryThread(Odometry * odometry, unsigned int dataBufferMaxSi
 	_dataBufferMaxSize(dataBufferMaxSize),
 	_resetOdometry(false),
 	_resetPose(Transform::getIdentity()),
-	_oldestAsyncImuStamp(0.0),
+	_lastImuStamp(0.0),
-	_newestAsyncImuStamp(0.0)
+	_imuEstimatedDelay(0.0)
 {
 	UASSERT(_odometry != 0);
 }
@@ -110,8 +110,7 @@ void OdometryThread::mainLoop()
 		UScopeMutex lock(_dataMutex);
 		_dataBuffer.clear();
 		_imuBuffer.clear();
-		_oldestAsyncImuStamp = 0.0;
+		_lastImuStamp = 0.0f;
-		_newestAsyncImuStamp = 0.0;
 	}
 
 	SensorData data;
@@ -162,39 +161,22 @@ void OdometryThread::addData(const SensorData & data)
 			!data.laserScanCompressed().empty() || 
 			data.imu().empty())
 		{
-			if(_oldestAsyncImuStamp > 0.0 && data.stamp() < _oldestAsyncImuStamp) {
+			_dataBuffer.push_back(data);
-				UWARN("Received image/lidar with stamp (%f) older than oldest received imu "
+			while(_dataBufferMaxSize > 0 && _dataBuffer.size() > _dataBufferMaxSize)
-					"(%f), skipping that frame (imu buffer size=%ld). "
+			{
-					"When using async IMU, make sure IMU is published faster "
+				UDEBUG("Data buffer is full, the oldest data is removed to add the new one.");
-					"than camera/lidar (assuming IMU latency is very small compared to camera/lidar).",
+				_dataBuffer.erase(_dataBuffer.begin());
-					data.stamp(), _oldestAsyncImuStamp, _imuBuffer.size());
 				notify = false;
 			}
-			else if(_newestAsyncImuStamp > 0.0 && data.stamp()>=_newestAsyncImuStamp) {
-				UWARN("Received image/lidar with stamp (%f) newer than latest received imu "
-					"(%f), skipping that frame (imu buffer size=%ld). "
-					"When using async IMU, make sure IMU is published faster "
-					"than camera/lidar (assuming IMU latency is very small compared to camera/lidar).",
-					data.stamp(), _newestAsyncImuStamp, _imuBuffer.size());
-				notify = false;
-			}
-			else {
-				_dataBuffer.push_back(data);
-				while(_dataBufferMaxSize > 0 && _dataBuffer.size() > _dataBufferMaxSize)
-				{
-					UDEBUG("Data buffer is full, the oldest data is removed to add the new one.");
-					_dataBuffer.erase(_dataBuffer.begin());
-					notify = false;
-				}
-			}
 		}
 		else
 		{
 			_imuBuffer.push_back(data);
-			if(_oldestAsyncImuStamp == 0) {
+			if(_lastImuStamp != 0.0 && data.stamp() > _lastImuStamp)
-				_oldestAsyncImuStamp = data.stamp();
+			{
+				_imuEstimatedDelay = data.stamp() - _lastImuStamp;
 			}
-			_newestAsyncImuStamp = data.stamp();
+			_lastImuStamp = data.stamp();
 		}
 	}
 	_dataMutex.unlock();
@@ -213,14 +195,18 @@ bool OdometryThread::getData(SensorData & data)
 	{
 		if(!_dataBuffer.empty())
 		{
-			// Send IMU up to stamp greater than image (OpenVINS needs this).
+			if(!_imuBuffer.empty())
-			while(!_imuBuffer.empty())
 			{
-				_odometry->process(_imuBuffer.front());
+				// Send IMU up to stamp greater than image (OpenVINS needs this).
-				double stamp =_imuBuffer.front().stamp();
+				while(!_imuBuffer.empty())
-				_imuBuffer.pop_front();
+				{
-				if(stamp > _dataBuffer.front().stamp()) {
+					_odometry->process(_imuBuffer.front());
-					break;
+					double stamp = _imuBuffer.front().stamp();
+					_imuBuffer.pop_front();
+					if(stamp > _dataBuffer.front().stamp())
+					{
+						break;
+					}
 				}
 			}
 

corelib/src/Optimizer.cpp

@@ -447,8 +447,7 @@ 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;
@@ -498,7 +497,7 @@ std::map<int, Transform> Optimizer::optimizeBA(
 	}
 
 	// compute correspondences
-	this->computeBACorrespondences(poses, links, signatures, points3DMap, wordReferences, rematchFeatures, false, registrationParameters);
+	this->computeBACorrespondences(poses, links, signatures, points3DMap, wordReferences, rematchFeatures);
 
 	return optimizeBA(rootId, poses, links, multiModels, points3DMap, wordReferences);
 }
@@ -508,12 +507,11 @@ 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, registrationParameters);
+	return optimizeBA(rootId, poses, links, signatures, points3DMap, wordReferences, rematchFeatures);
 }
 
 Transform Optimizer::optimizeBA(
@@ -559,20 +557,11 @@ 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)
 	{
@@ -612,6 +601,13 @@ 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,7 +152,6 @@ 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()),
@@ -633,7 +632,6 @@ 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);
@@ -1701,7 +1699,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.getAngle(_optimizedPoses.at(closestNode));
+				angleToClosestNodeInTheGraph = (newPose.inverse() * _optimizedPoses.at(closestNode)).getAngle();
 			}
 		}
 
@@ -3183,7 +3181,6 @@ 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);
@@ -3302,7 +3299,7 @@ bool Rtabmap::process(
 				if(!posesOut.empty() &&
 				   posesOut.begin()->first < _odomCachePoses.begin()->first)
 				{
-					optPoses = _graphOptimizer->optimize(posesOut.begin()->first, posesOut, edgeConstraintsOut, locOptCovariance, 0, &optimizationError, &optimizationIterations);
+					optPoses = _graphOptimizer->optimize(posesOut.begin()->first, posesOut, edgeConstraintsOut, locOptCovariance);
 				}
 				else
 				{
@@ -3427,8 +3424,7 @@ bool Rtabmap::process(
 				}
 
 				bool hasGlobalLoopClosuresOrLandmarks = false;
-				if(rejectLocalization && 
+				if(rejectLocalization && !graph::filterLinks(constraints, Link::kLocalSpaceClosure, true).empty())
-					(_localizationSecondTryWithoutProximityLinks && !graph::filterLinks(constraints, Link::kLocalSpaceClosure, true).empty()))
 				{
 					// Let's try again without local loop closures
 					localizationLinks = graph::filterLinks(localizationLinks, Link::kLocalSpaceClosure);
@@ -3452,7 +3448,7 @@ bool Rtabmap::process(
 						if(!posesOut.empty() &&
 						   posesOut.begin()->first < _odomCachePoses.begin()->first)
 						{
-							optPoses = _graphOptimizer->optimize(posesOut.begin()->first, posesOut, edgeConstraintsOut, locOptCovariance, 0, &optimizationError, &optimizationIterations);
+							optPoses = _graphOptimizer->optimize(posesOut.begin()->first, posesOut, edgeConstraintsOut, locOptCovariance);
 						}
 						else
 						{
@@ -3588,14 +3584,13 @@ bool Rtabmap::process(
 						_odomCacheConstraints = graph::filterLinks(_odomCacheConstraints, Link::kLocalSpaceClosure);
 						if(before != _odomCacheConstraints.size())
 						{
-							UWARN("Successfully optimized without local loop closures! Clearing them from local odometry cache. %ld/%ld have been removed.",
+							UWARN("Successfully optimized without local loop closures! Clear 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
@@ -3650,14 +3645,6 @@ 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
@@ -3954,7 +3941,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.getAngle(_optimizedPoses.at(closestNode));
+				angleToClosestNodeInTheGraph = (_lastLocalizationPose.inverse() * _optimizedPoses.at(closestNode)).getAngle();
 			}
 		}
 		_lastLocalizationPose = _optimizedPoses.at(signature->id()); // update
@@ -4103,15 +4090,16 @@ bool Rtabmap::process(
 					if(!sLoop->getGroundTruthPose().isNull() && !signature->getGroundTruthPose().isNull())
 					{
 						Transform transformGT = sLoop->getGroundTruthPose().inverse() * signature->getGroundTruthPose();
-						statistics_.addStatistic(Statistics::kGtLocalization_linear_error(), loopIter->second.transform().getDistance(transformGT));
+						Transform error = loopIter->second.transform().inverse() * transformGT;
-						statistics_.addStatistic(Statistics::kGtLocalization_angular_error(), loopIter->second.transform().getAngle(transformGT)*180/M_PI);
+						statistics_.addStatistic(Statistics::kGtLocalization_linear_error(), error.getNorm());
+						statistics_.addStatistic(Statistics::kGtLocalization_angular_error(), error.getAngle(1,0,0)*180/M_PI);
 					}
 				}
 
 				_distanceTravelledSinceLastLocalization = 0.0f;
 
 				statistics_.addStatistic(Statistics::kLoopMapToOdom_norm(), _mapCorrection.getNorm());
-				statistics_.addStatistic(Statistics::kLoopMapToOdom_angle(), _mapCorrection.getAngle(Transform::getIdentity())*180.0f/M_PI);
+				statistics_.addStatistic(Statistics::kLoopMapToOdom_angle(), _mapCorrection.getAngle()*180.0f/M_PI);
 				_mapCorrection.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
 				statistics_.addStatistic(Statistics::kLoopMapToOdom_x(), x);
 				statistics_.addStatistic(Statistics::kLoopMapToOdom_y(), y);
@@ -4125,7 +4113,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(Transform::getIdentity())*180.0f/M_PI);
+					statistics_.addStatistic(Statistics::kLoopOdom_correction_angle(), odomCorrection.getAngle()*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);
@@ -4146,10 +4134,6 @@ 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,25 +107,6 @@ 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,
@@ -142,23 +123,6 @@ 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,
@@ -177,25 +141,6 @@ 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,
@@ -289,29 +234,9 @@ 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)
 {
@@ -375,30 +300,6 @@ 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,
@@ -627,7 +528,7 @@ void SensorData::setOccupancyGrid(
 
 void SensorData::uncompressData()
 {
-	cv::Mat tmpA, tmpB, tmpD, tmpE, tmpF, tmpG, tmpH;
+	cv::Mat tmpA, tmpB, tmpD, tmpE, tmpF, tmpG;
 	LaserScan tmpC;
 	uncompressData(_imageCompressed.empty()?0:&tmpA,
 				_depthOrRightCompressed.empty()?0:&tmpB,
@@ -635,8 +536,7 @@ 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(
@@ -646,27 +546,16 @@ 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,%d)", 
+	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);
-		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;
 	}
@@ -677,8 +566,7 @@ void SensorData::uncompressData(
 			userDataRaw,
 			groundCellsRaw,
 			obstacleCellsRaw,
-			emptyCellsRaw,
+			emptyCellsRaw);
-			depthConfidenceRaw);
 
 	if(imageRaw && !imageRaw->empty() && _imageRaw.empty())
 	{
@@ -700,10 +588,6 @@ void SensorData::uncompressData(
 	{
 		_depthOrRightRaw = *depthRaw;
 	}
-	if(depthConfidenceRaw && !depthConfidenceRaw->empty() && _depthConfidenceRaw.empty())
-	{
-		_depthConfidenceRaw = *depthConfidenceRaw;
-	}
 	if(laserScanRaw && !laserScanRaw->isEmpty() && _laserScanRaw.isEmpty())
 	{
 		_laserScanRaw = *laserScanRaw;
@@ -744,8 +628,7 @@ void SensorData::uncompressDataConst(
 		cv::Mat * userDataRaw,
 		cv::Mat * groundCellsRaw,
 		cv::Mat * obstacleCellsRaw,
-		cv::Mat * emptyCellsRaw,
+		cv::Mat * emptyCellsRaw) const
-		cv::Mat * depthConfidenceRaw) const
 {
 	if(imageRaw)
 	{
@@ -755,10 +638,6 @@ void SensorData::uncompressDataConst(
 	{
 		*depthRaw = _depthOrRightRaw;
 	}
-	if(depthConfidenceRaw)
-	{
-		*depthConfidenceRaw = _depthConfidenceRaw;
-	}
 	if(laserScanRaw)
 	{
 		*laserScanRaw = _laserScanRaw;
@@ -781,7 +660,6 @@ void SensorData::uncompressDataConst(
 	}
 	if( (imageRaw && imageRaw->empty()) ||
 		(depthRaw && depthRaw->empty()) ||
-		(depthConfidenceRaw && depthConfidenceRaw->empty()) ||
 		(laserScanRaw && laserScanRaw->isEmpty()) ||
 		(userDataRaw && userDataRaw->empty()) ||
 		(groundCellsRaw && groundCellsRaw->empty()) ||
@@ -790,7 +668,6 @@ 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);
@@ -806,11 +683,6 @@ 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());
@@ -838,7 +710,6 @@ void SensorData::uncompressDataConst(
 		}
 		ctImage.join();
 		ctDepth.join();
-		ctDepthConfidence.join();
 		ctLaserScan.join();
 		ctUserData.join();
 		ctGroundCells.join();
@@ -875,21 +746,6 @@ 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)
@@ -959,8 +815,6 @@ 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()) +
@@ -982,7 +836,6 @@ void SensorData::clearCompressedData(bool images, bool scan, bool userData)
 	{
 		_imageCompressed=cv::Mat();
 		_depthOrRightCompressed=cv::Mat();
-		_depthConfidenceCompressed=cv::Mat();
 	}
 	if(scan)
 	{
@@ -999,7 +852,6 @@ 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,9 +273,11 @@ void Transform::getTranslation(float & x, float & y, float & z) const
 	z = this->z();
 }
 
-float Transform::getAngle(const Transform & t) const
+float Transform::getAngle(float x, float y, float z) const
 {
-	return getQuaternionf().angularDistance(t.getQuaternionf());
+	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));
 }
 
 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(Transform::getIdentity()) > 0.01)
+			if(currentMotion.getNorm() > 0.01 || currentMotion.getAngle() > 0.01)
 			{
 				if(info)
 				{

corelib/src/optimizer/OptimizerCeres.cpp

@@ -197,7 +197,7 @@ std::map<int, Transform> OptimizerCeres::optimize(
 
 					if(angle_local_manifold == NULL)
 					{
-						angle_local_manifold = ceres::examples::AngleManifold::Create();
+						angle_local_manifold = ceres::examples::AngleManfold::Create();
 					}
 					SetCeresProblemManifold(problem, &pose_begin_iter->second.yaw_radians, angle_local_manifold);
 					SetCeresProblemManifold(problem, &pose_end_iter->second.yaw_radians, angle_local_manifold);

corelib/src/optimizer/gtsam/GravityFactor.h

@@ -74,8 +74,8 @@ public:
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-    /*ar & boost::serialization::make_nvp("nZ_", const_cast<Unit3&>(nZ_));
+    ar & boost::serialization::make_nvp("nZ_", const_cast<Unit3&>(nZ_));
-    ar & boost::serialization::make_nvp("bRef_", const_cast<Unit3&>(bRef_));*/
+    ar & boost::serialization::make_nvp("bRef_", const_cast<Unit3&>(bRef_));
   }
 #endif
 };
@@ -158,10 +158,10 @@ private:
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-    /*ar & boost::serialization::make_nvp("NoiseModelFactor1",
+    ar & boost::serialization::make_nvp("NoiseModelFactor1",
         boost::serialization::base_object<Base>(*this));
     ar & boost::serialization::make_nvp("GravityFactor",
-        boost::serialization::base_object<GravityFactor>(*this));*/
+        boost::serialization::base_object<GravityFactor>(*this));
   }
 #endif
 
@@ -254,10 +254,10 @@ private:
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-    /*ar & boost::serialization::make_nvp("NoiseModelFactor1",
+    ar & boost::serialization::make_nvp("NoiseModelFactor1",
         boost::serialization::base_object<Base>(*this));
     ar & boost::serialization::make_nvp("GravityFactor",
-        boost::serialization::base_object<GravityFactor>(*this));*/
+        boost::serialization::base_object<GravityFactor>(*this));
   }
 #endif
 public:

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 --Kp/DetectorStrategy 15 --PyDetector/Path "~/SuperPointPretrainedNetwork/rtabmap_superpoint.py"
+#   --Vis/FeatureType 15 --PyDetector/Path "~/SuperPointPretrainedNetwork/rtabmap_superpoint.py" --PyDetector/Model "~/SuperPointPretrainedNetwork/superpoint_v1.pth"
 #
 
 import random

corelib/src/resources/DatabaseSchema.sql.in

@@ -32,7 +32,6 @@ 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

@@ -1,182 +0,0 @@
--- *******************************************************************
---  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,7 +242,8 @@ 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({desc_.size(1), -1}), torch::nn::functional::NormalizeFuncOptions().dim(0)); //[256, n_keypoints]
+		desc = torch::nn::functional::normalize(desc.reshape({1, desc_.size(1), -1})); //[1, 256, n_keypoints]
+		desc = desc.squeeze(); //[256, n_keypoints]
 		desc = desc.transpose(0, 1).contiguous(); //[n_keypoints, 256]
 
 		if(cuda_)

corelib/src/util2d.cpp

@@ -1359,38 +1359,18 @@ 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);
@@ -1409,19 +1389,10 @@ 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
@@ -1448,9 +1419,6 @@ cv::Mat registerDepth(
 						if(zReg == 0 || z16 < zReg)
 						{
 							zReg = z16;
-							if(confPtr) {
-								registeredConfidence.at<unsigned char>(dy, dx) = confPtr[x];
-							}
 						}
 					}
 					else
@@ -1459,9 +1427,6 @@ cv::Mat registerDepth(
 						if(zReg == 0 || z < zReg)
 						{
 							zReg = z;
-							if(confPtr) {
-								registeredConfidence.at<unsigned char>(dy, dx) = confPtr[x];
-							}
 						}
 					}
 				}
@@ -1966,61 +1931,6 @@ 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,33 +276,12 @@ 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>);
@@ -315,10 +294,8 @@ 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);
@@ -345,9 +322,6 @@ 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)
@@ -399,13 +373,11 @@ 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) has confidence=%d (thr=%d) decimation=%d",
+	UDEBUG("depth=%dx%d fx=%f fy=%f cx=%f cy=%f (depth factors=%f %f) 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;
@@ -415,21 +387,21 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
 		{
 			pcl::PointXYZ & pt = cloud->at((h/decimation)*cloud->width + (w/decimation));
 
-			pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
+			pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, depthCx, depthCy, depthFx, depthFy, false);
-			if(imageDepthConfidence.empty() || imageDepthConfidence.at<unsigned char>(h,w) >= confidenceThr)
+			if(pcl::isFinite(ptXYZ) && ptXYZ.z>=minDepth && (maxDepth<=0.0f || ptXYZ.z <= maxDepth))
 			{
-				pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, depthCx, depthCy, depthFx, depthFy, false);
+				pt.x = ptXYZ.x;
-				if(pcl::isFinite(ptXYZ) && ptXYZ.z>=minDepth && (maxDepth<=0.0f || ptXYZ.z <= maxDepth))
+				pt.y = ptXYZ.y;
+				pt.z = ptXYZ.z;
+				if(validIndices)
 				{
-					pt.x = ptXYZ.x;
+					validIndices->at(oi++) = (h/decimation)*cloud->width + (w/decimation);
-					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();
+			}
 		}
 	}
 
@@ -455,36 +427,13 @@ 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>);
@@ -500,7 +449,6 @@ 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)
@@ -543,7 +491,6 @@ 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);
@@ -555,9 +502,6 @@ 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)
@@ -602,14 +546,12 @@ 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) has confidence=%d (thr=%d) decimation=%d",
+	UDEBUG("rgb=%dx%d depth=%dx%d fx=%f fy=%f cx=%f cy=%f (depth factors=%f %f) 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;
@@ -637,21 +579,21 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
 				pt.r = v;
 			}
 
-			pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
+			pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, depthCx, depthCy, depthFx, depthFy, false);
-			if(imageDepthConfidence.empty() || imageDepthConfidence.at<unsigned char>(h,w) >= confidenceThr)
+			if (pcl::isFinite(ptXYZ) && ptXYZ.z >= minDepth && (maxDepth <= 0.0f || ptXYZ.z <= maxDepth))
 			{
-				pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, depthCx, depthCy, depthFx, depthFy, false);
+				pt.x = ptXYZ.x;
-				if (pcl::isFinite(ptXYZ) && ptXYZ.z >= minDepth && (maxDepth <= 0.0f || ptXYZ.z <= maxDepth))
+				pt.y = ptXYZ.y;
+				pt.z = ptXYZ.z;
+				if (validIndices)
 				{
-					pt.x = ptXYZ.x;
+					validIndices->at(oi) = (h / decimation)*cloud->width + (w / decimation);
-					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();
 			}
 		}
 	}
@@ -926,8 +868,7 @@ 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)
 	{
@@ -940,7 +881,6 @@ 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)
 		{
@@ -952,10 +892,6 @@ 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 ||
@@ -976,9 +912,6 @@ 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));
@@ -1007,12 +940,10 @@ 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())
@@ -1133,8 +1064,7 @@ 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(
@@ -1144,8 +1074,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromSensorData(
 		minDepth,
 		validIndices?&validIndicesV:0,
 		stereoParameters,
-		roiRatios,
+		roiRatios);
-		confidenceThr);
 
 	if(validIndices)
 	{
@@ -1188,8 +1117,7 @@ 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)
 	{
@@ -1208,7 +1136,6 @@ 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)
 		{
@@ -1221,10 +1148,6 @@ 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) ||
@@ -1240,9 +1163,6 @@ 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);
 					}
@@ -1266,12 +1186,10 @@ 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())
@@ -1374,8 +1292,7 @@ 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(
@@ -1385,8 +1302,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGBFromSensorData(
 		minDepth,
 		validIndices?&validIndicesV:0,
 		stereoParameters,
-		roiRatios,
+		roiRatios);
-		confidenceThr);
 
 	if(validIndices)
 	{
@@ -3233,18 +3149,6 @@ 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
@@ -3256,7 +3160,6 @@ 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,
@@ -3267,7 +3170,6 @@ 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);
@@ -3333,9 +3235,8 @@ 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);
 
-				// [rows][cols][depth, indexPt]
+				// depth: 2 channels UINT: [depthMM, indexPt]
-				std::vector<std::vector<RegisteredPoints> > registered(
+				cv::Mat registered = cv::Mat::zeros(imageSize, CV_32SC2);
-					imageSize.height, std::vector<RegisteredPoints>(imageSize.width));
 				Transform t = cameraTransform.inverse();
 
 				cv::Rect roi(0,0,imageSize.width, imageSize.height);
@@ -3357,44 +3258,34 @@ 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 u = (fx*ptScan.x)*invZ + cx;
+						float dx = (fx*ptScan.x)*invZ + cx;
-						float v = (fy*ptScan.y)*invZ + cy;
+						float dy = (fy*ptScan.y)*invZ + cy;
-						int x = u + 0.5f;
+						int dx_low = dx;
-						int y = v + 0.5f;
+						int dy_low = dy;
-
+						int dx_high = dx + 0.5f;
-						if(uIsInBounds(x, roi.x, roi.x+roi.width) && uIsInBounds(y, roi.y, roi.y+roi.height) &&
+						int dy_high = dy + 0.5f;
-						   (validProjMask.empty() || validProjMask.at<unsigned char>(y, imageSize.width*camIndex+x) > 0)) {
+						int zMM = z * 1000;
-							RegisteredPoints &zReg = registered[y][x];
+						if(uIsInBounds(dx_low, roi.x, roi.x+roi.width) && uIsInBounds(dy_low, roi.y, roi.y+roi.height) &&
-							if(zReg.points.empty()) {
+						   (validProjMask.empty() || validProjMask.at<unsigned char>(dy_low, imageSize.width*camIndex+dx_low) > 0))
-								zReg.minDistance = z;
+						{
-								zReg.points.push_back(RegisteredPoints::Point(z, i));
+							set = true;
-								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;
 							}
-							else if(z < zReg.minDistance) {
+						}
-								zReg.minDistance = z;
+						if((dx_low != dx_high || dy_low != dy_high) &&
-								if(maxDepthError<=0.0f) {
+							uIsInBounds(dx_high, roi.x, roi.x+roi.width) && uIsInBounds(dy_high, roi.y, roi.y+roi.height) &&
-									// keeping only closest point, just update it
+							(validProjMask.empty() || validProjMask.at<unsigned char>(dy_high, imageSize.width*camIndex+dx_high) > 0))
-									zReg.points[0].distance = z;
+						{
-									zReg.points[0].index = i;
+							set = true;
-								}
+							cv::Vec2i &zReg = registered.at<cv::Vec2i>(dy_high, dx_high);
-								else {
+							if(zReg[0] == 0 || zMM < zReg[0])
-									// update the points attached to same pixel based on new closest distance
+							{
-									std::vector<RegisteredPoints::Point> reOrderedPts;
+								zReg[0] = zMM;
-									reOrderedPts.push_back(RegisteredPoints::Point(z, i));
+								zReg[1] = 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;
 							}
 						}
 					}
@@ -3405,19 +3296,19 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 				}
 				if(count == 0)
 				{
-					registered.clear();
+					registered = cv::Mat();
 					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<imageSize.width; ++u)
+				for(int u=0; u<registered.cols; ++u)
 				{
-					for(int v=0; v<imageSize.height; ++v)
+					for(int v=0; v<registered.rows; ++v)
 					{
-						RegisteredPoints &zReg = registered[v][u];
+						cv::Vec2i &zReg = registered.at<cv::Vec2i>(v, u);
-						if(!zReg.points.empty())
+						if(zReg[0] > 0)
 						{
 							ProjectionInfo info;
 							info.nodeID = pter->first;
@@ -3425,41 +3316,37 @@ 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);
-							for(size_t p=0; p<zReg.points.size(); ++p)
+							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?
 							{
-								int ptIdx = zReg.points[p].index;
+								float vx = info.uv.x-0.5f;
-								const PointT & pt = cloud.at(ptIdx);
+								float vy = info.uv.y-0.5f;
-								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 distanceToCenter = vx*vx+vy*vy;
-								float angleToCam = maxAngle<=0?0:pcl::getAngle3D(normal, camDir);
+								float distance = distanceToCenter;
-								float distanceToCam = zReg.points[p].distance;
+								if(distanceToCamPolicy)
-								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?
 								{
-									float vx = info.uv.x-0.5f;
+									distance = distanceToCam;
-									float vy = info.uv.y-0.5f;
+								}
 
-									float distanceToCenter = vx*vx+vy*vy;
+								info.distance = distance;
-									float distance = distanceToCenter;
-									if(distanceToCamPolicy)
-									{
-										distance = distanceToCam;
-									}
 
-									info.distance = distance;
+								if(invertedIndex[zReg[1]].distance != -1.0f)
-
+								{
-									if(invertedIndex[ptIdx].distance != -1.0f)
+									if(distance <= invertedIndex[zReg[1]].distance)
 									{
-										if(distance <= invertedIndex[ptIdx].distance)
+										invertedIndex[zReg[1]] = info;
-										{
-											invertedIndex[ptIdx] = info;
-										}
-									}
-									else
-									{
-										invertedIndex[ptIdx] = info;
 									}
 								}
+								else
+								{
+									invertedIndex[zReg[1]] = info;
+								}
 							}
 						}
 					}
@@ -3528,7 +3415,6 @@ 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,
@@ -3539,7 +3425,6 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 			cameraModels,
 			maxDistance,
 			maxAngle,
-			maxDepthError,
 			roiRatios,
 			projMask,
 			distanceToCamPolicy,
@@ -3552,7 +3437,6 @@ 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,
@@ -3563,7 +3447,6 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 			cameraModels,
 			maxDistance,
 			maxAngle,
-			maxDepthError,
 			roiRatios,
 			projMask,
 			distanceToCamPolicy,

corelib/src/util3d_surface.cpp

@@ -781,7 +781,10 @@ pcl::TextureMesh::Ptr createTextureMesh(
 	pcl::TextureMapping<pcl::PointXYZ> tm; // TextureMapping object that will perform the sort
 	tm.setMaxDistance(maxDistance);
 	tm.setMaxAngle(maxAngle);
-	tm.setMaxDepthError(maxDepthError);
+	if(maxDepthError > 0.0f)
+	{
+		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 -j4 && \
+    make -j$(nproc) && \
     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 -j4 && \
+    make -j$(nproc) && \
     make install && \
     cd ../.. && \
     rm -rf rtabmap && \

docker/focal/Dockerfile

@@ -2,6 +2,22 @@
 
 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
 
@@ -12,7 +28,7 @@ COPY . /root/rtabmap
 RUN source /ros_entrypoint.sh && \
     cd rtabmap/build && \
     cmake -DWITH_ALICE_VISION=ON -DWITH_OPENGV=ON .. && \
-    make -j4 && \
+    make -j$(nproc) && \
     make install && \
     cd ../.. && \
     rm -rf rtabmap && \

docker/focal/deps/Dockerfile

@@ -10,13 +10,6 @@ ENV DEBIAN_FRONTEND=noninteractive
 
 WORKDIR /root/
 
-# issue: https://github.com/introlab/rtabmap/issues/1523
-RUN rm /etc/apt/sources.list.d/ros1-latest.list || true && \
-    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 && \
@@ -36,7 +29,7 @@ RUN if [ "$TARGETPLATFORM" = "linux/arm/v7" ]; then \
     export CFLAGS="-D_FILE_OFFSET_BITS=64" && \
     export CXXFLAGS="-D_FILE_OFFSET_BITS=64" && \
     ./bootstrap && \
-    make -j4 && \
+    make -j$(nproc) && \
     sudo make install && \
     cd .. && \
     cmake --version && \
@@ -59,7 +52,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 update && apt-get install -y curl && \
     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..." && \
@@ -91,7 +84,7 @@ RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing libfreenect2
     mkdir build && \
     cd build && \
     cmake .. && \
-    make -j4 && \
+    make -j$(nproc) && \
     make install && \
     cd && \
     rm -r libfreenect2; fi
@@ -105,7 +98,7 @@ RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing zed-open-cap
     mkdir build && \
     cd build && \
     cmake .. && \
-    make -j4 && \
+    make -j$(nproc) && \
     make install && \
     cd && \
     rm -r zed-open-capture; fi
@@ -124,7 +117,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 -j4 && \
+    make -j$(nproc) && \
     make install && \
     cd && \
     rm -r oiio; fi
@@ -134,7 +127,7 @@ RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/
     mkdir build && \
     cd build && \
     cmake .. && \
-    make -j4 && \
+    make -j$(nproc) && \
     make install && \
     cd && \
     rm -r assimp; fi
@@ -147,7 +140,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 -j4 && \
+    make -j$(nproc) && \
     make install && \
     cd && \
     rm -r geogram; fi
@@ -155,12 +148,12 @@ RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/
     cd AliceVision && \
     git checkout 0f6115b6af6183c524aa7fcf26141337c1cf3872 && \
     git submodule update -i && \
-    wget https://gist.githubusercontent.com/matlabbe/1df724465106c056ca4cc195c81d8cf0/raw/5e3437cf6229c8d534bbaee475ed9bcb92eb84a1/alicevision_0f6115b.patch && \
+    wget https://gist.githubusercontent.com/matlabbe/1df724465106c056ca4cc195c81d8cf0/raw/b3ed4cb8f9b270833a40d57d870a259eabfa4415/alicevision_0f6115b.patch && \
     git apply alicevision_0f6115b.patch && \
     mkdir build && \
     cd build && \
     cmake -DALICEVISION_USE_CUDA=OFF -DALICEVISION_USE_APRILTAG=OFF -DALICEVISION_BUILD_SOFTWARE=OFF .. && \
-    make -j4 && \
+    make -j$(nproc) && \
     make install && \
     cd && \
     rm -r AliceVision; fi
@@ -171,25 +164,11 @@ 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 -j4 && \
+    make -j$(nproc) && \
     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 -j4 && \
+    make -j$(nproc) && \
     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 -j2 && \
+    make -j$(nproc) && \
     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 -j2 && \
+    make -j$(nproc) && \
     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 -j2 && \
+    make -j$(nproc) && \
     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 -j2 && \
+    make -j$(nproc) && \
     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/humble/setup.bash" --\nexec "$@"' > /ros_entrypoint.sh
+COPY ./docker/jammy/deps/ros_entrypoint.sh /ros_entrypoint.sh
 RUN chmod +x /ros_entrypoint.sh
 ENTRYPOINT [ "/ros_entrypoint.sh" ]
 

docker/jammy/deps/ros_entrypoint.sh

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

docker/noble-kilted/Dockerfile

@@ -1,20 +0,0 @@
-# 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

@@ -1,132 +0,0 @@
-
-# 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,6 +2,20 @@
 
 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
 
@@ -12,7 +26,7 @@ COPY . /root/rtabmap
 RUN source /ros_entrypoint.sh && \
     cd rtabmap/build && \
     cmake -DWITH_OPENGV=ON .. && \
-    make -j4 && \
+    make -j$(nproc) && \
     make install && \
     cd ../.. && \
     rm -rf rtabmap && \

docker/noble/android/Dockerfile

@@ -1,206 +0,0 @@
-# Image: introlab3it/rtabmap:android-noble-deps
-
-FROM ubuntu:24.04
-
-# Install build dependencies
-RUN apt-get update && apt-get install -y --no-install-recommends apt-utils
-RUN apt-get update && apt-get install -y \
-      git unzip wget ant cmake \
-      g++ lib32stdc++6 lib32z1 \
-      software-properties-common \
-      freeglut3-dev \
-      openjdk-8-jdk openjdk-8-jre \
-      curl
-
-ENV ANDROID_HOME=/opt/android-sdk
-ENV PATH=$PATH:/opt/android-sdk/cmdline-tools/latest/bin:/opt/android-sdk/tools:/opt/android-sdk/platform-tools:/opt/android-sdk/ndk/21.4.7075529
-ENV ANDROID_NDK=/opt/android-sdk/ndk/21.4.7075529
-ENV JAVA_HOME=/usr/lib/jvm/java-8-openjdk-amd64
-
-WORKDIR /root/
-
-# Setup android sdk
-RUN wget -nv https://dl.google.com/android/repository/commandlinetools-linux-7583922_latest.zip && \
-    unzip -qq commandlinetools-linux-7583922_latest.zip && \
-    rm commandlinetools-linux-7583922_latest.zip && \
-    mkdir $ANDROID_HOME && \
-    mkdir $ANDROID_HOME/cmdline-tools && \
-    mv cmdline-tools $ANDROID_HOME/cmdline-tools/latest
-# We should use build-tools <=30 to avoid dx missing error
-RUN echo y | sdkmanager --install "platform-tools" "platforms;android-23" "platforms;android-24" "platforms;android-26" "platforms;android-30" "build-tools;30.0.3" "ndk;21.4.7075529"
-
-# we need <=r25 tools to use "android" command (now deprecated)
-RUN wget -nv http://dl-ssl.google.com/android/repository/tools_r25.2.5-linux.zip && \
-    unzip -qq tools_r25.2.5-linux.zip && \
-    mv tools $ANDROID_HOME/. && \
-    rm tools_r25.2.5-linux.zip
-
-##############
-# Dependencies (took from docker/bionic/android/deps.bash)
-##############
-
-# Install directory for all dependencies
-RUN mkdir -p /opt/android/arm64-v8a
-
-# Boost
-RUN echo "Install boost..." && \
-    wget -nv https://downloads.sourceforge.net/project/boost/boost/1.59.0/boost_1_59_0.tar.gz && \
-    tar -xzf boost_1_59_0.tar.gz && \
-    cd boost_1_59_0 && \
-    wget -nv https://gist.github.com/matlabbe/0bce8feeb73a499a76afbbcc5c687221/raw/1733253195bc4d4d9b7f9eda1e60628dc1e51429/BoostConfig.cmake.in && \
-    wget -nv https://gist.github.com/matlabbe/0bce8feeb73a499a76afbbcc5c687221/raw/1733253195bc4d4d9b7f9eda1e60628dc1e51429/CMakeLists.txt && \
-    mkdir build && \
-    cd build && \
-    cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=23 -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/arm64-v8a -DCMAKE_FIND_ROOT_PATH="/opt/android/arm64-v8a/bin;/opt/android/arm64-v8a;/opt/android/arm64-v8a/share" ..  && \
-    make -j4 && \
-    make install && \
-    cd /root  && \
-    rm -r boost_1_59_0.tar.gz boost_1_59_0
-
-# eigen
-RUN echo "Install 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 && \
-    cd eigen-3.3.9 && \
-    mkdir build && \
-    cd build && \
-    cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=23 -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/arm64-v8a -DCMAKE_FIND_ROOT_PATH="/opt/android/arm64-v8a/bin;/opt/android/arm64-v8a;/opt/android/arm64-v8a/share" ..  && \
-    make -j4 && \
-    make install && \
-    cd /root && \
-    rm -r 3.3.9.tar.gz eigen-3.3.9
-
-# FLANN
-RUN echo "Install flann..." && \
-    git clone -b 1.8.4 https://github.com/mariusmuja/flann.git && \
-    cd flann && \
-    wget -nv https://gist.githubusercontent.com/matlabbe/cacff9f8271d0c42acd622939a26cab4/raw/85baf4927b32844ebd7f8eccce421bde181cd190/flann_1_8_4_android_fix.patch && \
-    git apply flann_1_8_4_android_fix.patch && \
-    mkdir build && \
-    cd build && \
-    cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=23 -DBUILD_SHARED_LIBS=OFF -DBUILD_PYTHON_BINDINGS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/arm64-v8a -DCMAKE_FIND_ROOT_PATH="/opt/android/arm64-v8a/bin;/opt/android/arm64-v8a;/opt/android/arm64-v8a/share" .. && \
-    make -j4 && \
-    make install && \
-    cd /root && \
-    rm -rf flann
-
-# GTSAM
-RUN echo "Install gtsam..." && \
-    git clone https://bitbucket.org/gtborg/gtsam.git && \
-    cd gtsam && \
-    git checkout fbb9d3bdda8b88df51896bc401bfd170573e66f5 && \
-    wget -nv https://gist.github.com/matlabbe/726b490c658afd3293f4b3f2f501b863/raw/df09fc8e238a495d66b062d92dc1c1fb20a581e8/gtsam_GKlib_android_fix.patch && \
-    git apply gtsam_GKlib_android_fix.patch && \
-    mkdir build && \
-    cd build && \
-    cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=23 -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/arm64-v8a -DCMAKE_FIND_ROOT_PATH="/opt/android/arm64-v8a/bin;/opt/android/arm64-v8a;/opt/android/arm64-v8a/share" -DMETIS_SHARED=OFF -DGTSAM_BUILD_STATIC_LIBRARY=ON -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DGTSAM_USE_SYSTEM_EIGEN=ON .. && \
-    make -j4 && \
-    make install && \
-    cd /root && \
-    rm -rf gtsam
-
-# g2o
-RUN echo "Install g2o..." && \
-    git clone https://github.com/RainerKuemmerle/g2o.git && \
-    cd g2o && \
-    git checkout a3f7706bdbb849b2808dc3e1b7aee189f63b498e && \
-    mkdir build && \
-    cd build && \
-    cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=23 -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/arm64-v8a -DCMAKE_FIND_ROOT_PATH="/opt/android/arm64-v8a/bin;/opt/android/arm64-v8a;/opt/android/arm64-v8a/share" -DBUILD_LGPL_SHARED_LIBS=OFF -DG2O_BUILD_APPS=OFF -DG2O_BUILD_EXAMPLES=OFF -DG2O_USE_OPENGL=OFF .. && \
-    make -j4 && \
-    make install && \
-    cd /root && \
-    rm -rf g2o
-
-# VTK
-RUN echo "Install VTK..." && \
-    git clone https://github.com/Kitware/VTK.git && \
-    cd VTK && \
-    git checkout tags/v8.2.0 && \
-    wget https://gist.github.com/matlabbe/e217259fb8ece9ee6daf5a8f70e896a0/raw/2214b503a537d6431d764526b5b780f07d6f168d/vtk_8_2_0_android_r21_fix.patch && \
-    git apply vtk_8_2_0_android_r21_fix.patch && \
-    mkdir build && \
-    cd build && \
-    cmake -DBUILD_EXAMPLES=OFF -DBUILD_TESTING=OFF -DVTK_ANDROID_BUILD=ON -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/arm64-v8a -DANDROID_ARCH_ABI=arm64-v8a -DCMAKE_FIND_ROOT_PATH="/opt/android/arm64-v8a/bin;/opt/android/arm64-v8a;/opt/android/arm64-v8a/share" .. && \
-    make -j4 && \
-    cp -r CMakeExternals/Install/vtk-android/* /opt/android/arm64-v8a/. && \
-    cd /root && \
-    rm -rf VTK
-
-# PCL
-RUN echo "Install pcl..." && \
-    git clone https://github.com/PointCloudLibrary/pcl.git && \
-    cd pcl && \
-    git checkout tags/pcl-1.8.0 && \
-    wget https://gist.github.com/matlabbe/41812e50e459b2f27b331a2343569e5d/raw/b2fc0c4d1cfffb3a9f2811abae782e317c539bfb/pcl_1_8_0_vtk_android_support.patch && \
-    git apply pcl_1_8_0_vtk_android_support.patch && \
-    mkdir build && \
-    cd build && \
-    # do it 2 times because there is a cmake error on the first time and not the second time!?
-    cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=23 -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/arm64-v8a -DCMAKE_FIND_ROOT_PATH="/opt/android/arm64-v8a/bin;/opt/android/arm64-v8a;/opt/android/arm64-v8a/share" -DBUILD_apps=OFF -DBUILD_examples=OFF -DBUILD_tools=OFF -DBUILD_visualization=OFF -DBUILD_tracking=OFF -DBUILD_people=OFF -DBUILD_tools=OFF -DBUILD_global_tests=OFF -DWITH_QT=OFF -DWITH_OPENGL=OFF -DWITH_VTK=ON -DPCL_SHARED_LIBS=OFF .. || true && \
-    cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=23 -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/arm64-v8a -DCMAKE_FIND_ROOT_PATH="/opt/android/arm64-v8a/bin;/opt/android/arm64-v8a;/opt/android/arm64-v8a/share" -DBUILD_apps=OFF -DBUILD_examples=OFF -DBUILD_tools=OFF -DBUILD_visualization=OFF -DBUILD_tracking=OFF -DBUILD_people=OFF -DBUILD_tools=OFF -DBUILD_global_tests=OFF -DWITH_QT=OFF -DWITH_OPENGL=OFF -DWITH_VTK=ON -DPCL_SHARED_LIBS=OFF .. && \
-    make -j4 && \
-    make install && \
-    cd /root && \
-    rm -rf pcl
-
-# make sure opencv is using the shared version of zlib
-# see https://github.com/android/ndk/issues/1179
-RUN mv $ANDROID_NDK/toolchains/llvm/prebuilt/linux-x86_64/sysroot/usr/lib/aarch64-linux-android/libz.a $ANDROID_NDK/toolchains/llvm/prebuilt/linux-x86_64/sysroot/usr/lib/aarch64-linux-android/libz.a.back
-
-# OpenCV
-RUN echo "Install OpenCV..." && \
-    git clone https://github.com/opencv/opencv_contrib.git && \
-    git clone https://github.com/opencv/opencv.git && \
-    cd opencv_contrib && \
-    git checkout tags/4.5.5 && \
-    cd /root && \
-    cd opencv && \
-    git checkout tags/4.5.5 && \
-    mkdir build && \
-    cd build && \
-    cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=23 -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/arm64-v8a -DCMAKE_FIND_ROOT_PATH="/opt/android/arm64-v8a/bin;/opt/android/arm64-v8a;/opt/android/arm64-v8a/share" -DOPENCV_EXTRA_MODULES_PATH=/root/opencv_contrib/modules -DBUILD_TESTS=OFF -DBUILD_PERF_TESTS=OFF -DWITH_CUDA=OFF -DBUILD_opencv_structured_light=OFF -DBUILD_ANDROID_PROJECTS=OFF -DOPENCV_ENABLE_NONFREE=ON -DBUILD_ANDROID_EXAMPLES=OFF -DWITH_PROTOBUF=OFF -DBUILD_opencv_stereo=OFF -DBUILD_JAVA=OFF -DWITH_QUIRC=OFF -DBUILD_opencv_js_bindings_generator=OFF -DBUILD_opencv_objc_bindings_generator=OFF -DBUILD_opencv_objdetect=OFF -DBUILD_opencv_xobjdetect=OFF .. && \
-    make -j4 && \
-    make install && \
-    cd /root && \
-    rm -rf opencv opencv_contrib
-
-RUN echo "Strip libraries..." && \
-    $ANDROID_NDK/toolchains/llvm/prebuilt/linux-x86_64/bin/aarch64-linux-android-strip -g -S -d --strip-debug --verbose /opt/android/arm64-v8a/lib/*.a && \
-    $ANDROID_NDK/toolchains/llvm/prebuilt/linux-x86_64/bin/aarch64-linux-android-strip -g -S -d --strip-debug --verbose /opt/android/arm64-v8a/sdk/native/staticlibs/arm64-v8a/*.a
-
-RUN mkdir /opt/android/lib
-
-# tango
-RUN wget 'https://docs.google.com/uc?authuser=0&id=12rHHkYM5k-UnQn-xGXs9JqYWhSXrgJr3&export=download' -O TangoSDK_Ikariotikos_C.zip && \
-    unzip -qq TangoSDK_Ikariotikos_C.zip && \
-    rm TangoSDK_Ikariotikos_C.zip && \
-    cp -r lib_tango_client_api/include/* /opt/android/arm64-v8a/include/. && \
-    cp -r lib_tango_client_api/lib/arm64-v8a/* /opt/android/arm64-v8a/lib/. && \
-    rm -r lib_tango_client_api && \
-    wget 'https://docs.google.com/uc?authuser=0&id=1AqVuEVu5284X6OgrGWu12VTrx4pY99Jb&export=download' -O TangoSupport_Ikariotikos_C.zip && \
-    unzip -qq TangoSupport_Ikariotikos_C.zip && \
-    rm TangoSupport_Ikariotikos_C.zip && \
-    cp -r lib_tango_support_api/include/* /opt/android/arm64-v8a/include/. && \
-    cp -r lib_tango_support_api/lib/arm64-v8a/* /opt/android/arm64-v8a/lib/. && \
-    rm -r lib_tango_support_api && \
-    wget 'https://docs.google.com/uc?authuser=0&id=1s5iPJ7xiridj9Jj--gCy2XiQFniheVm6&export=download' -O TangoSDK_Ikariotikos_Java.jar && \
-    mv TangoSDK_Ikariotikos_Java.jar /opt/android/lib/
-
-# ARCore
-RUN wget 'https://docs.google.com/uc?authuser=0&id=1VsibeqRYpS5pjmrG-vYTXyiPg8kbIfVN&export=download' -O arcore.zip && \
-    unzip -qq arcore.zip && \
-    rm arcore.zip && \
-    cp -r arcore1_18/include/* /opt/android/arm64-v8a/include/. && \
-    cp -r arcore1_18/arm64-v8a/* /opt/android/arm64-v8a/lib/. && \
-    cp arcore1_18/*.jar /opt/android/lib/ && \
-    rm -r arcore1_18
-
-# AREngine
-RUN wget 'https://docs.google.com/uc?authuser=0&id=1rdaD2Z1QBv-SUeUy0oBmg3C2odfxTHgR&export=download' -O arengine.zip && \
-    unzip -qq arengine.zip && \
-    rm arengine.zip && \
-    cp -r arengine/include/* /opt/android/arm64-v8a/include/. && \
-    cp -r arengine/arm64-v8a/* /opt/android/arm64-v8a/lib/. && \
-    cp arengine/*.jar /opt/android/lib/ && \
-    rm -r arengine

docker/noble/android/rtabmap_apiXX/Dockerfile

@@ -1,13 +0,0 @@
-# Image: introlab3it/rtabmap:androidXX
-
-FROM introlab3it/rtabmap:android-noble-deps
-
-ARG API_VERSION=23
-
-# Copy current source code
-COPY . /root/rtabmap-tango
-
-WORKDIR /root/rtabmap-tango
-
-RUN /bin/bash -c "./docker/noble/android/rtabmap_apiXX/rtabmap.bash /opt/android $API_VERSION"
-

docker/noble/android/rtabmap_apiXX/rtabmap.bash

@@ -1,29 +0,0 @@
-#!/bin/bash
-
-set -e
-
-if [ $# -ne 2 ]; then
-    echo "rtabmap.bash android_install_prefix api_level (23 for tango, 24 for arengine)   # Example: build.bash /opt/android 24"
-    exit 1
-fi
-
-prefix=$1
-api=$2
-
-# copy required jars
-cp /opt/android/lib/*.jar app/android/libs/.
-
-# resource tool
-cd build
-cmake -DANDROID_PREBUILD=ON ..
-make
-
-# rtabmap
-mkdir arm64-v8a
-cd arm64-v8a
-cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=$api -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a -DCMAKE_FIND_ROOT_PATH="$prefix/arm64-v8a/bin;$prefix/arm64-v8a;$prefix/arm64-v8a/share" -DBUILD_EXAMPLES=OFF -DBUILD_TOOLS=OFF -DOpenCV_DIR=$prefix/arm64-v8a/sdk/native/jni ../..
-make
-make clean
-
-
-

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 -j4 && \
+    make -j$(nproc) && \
     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 -j4 && \
+    make -j$(nproc) && \
     make install && \
     cd && \
     rm -r libfreenect2; fi
@@ -86,40 +86,29 @@ RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing zed-open-cap
     mkdir build && \
     cd build && \
     cmake -DCMAKE_BUILD_TYPE=Release .. && \
-    make -j4 && \
+    make -j$(nproc) && \
     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 && \
-    sed -i '/#include <libavformat\/avformat.h>/i #include <libavcodec/version.h>' modules/videoio/src/ffmpeg_codecs.hpp && \
+    git apply opencv_4_6_0_ffmpeg.patch && \
     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 -j$(nproc) && \
     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/jazzy/setup.bash" --\nexec "$@"' > /ros_entrypoint.sh
+COPY ./docker/noble/deps/ros_entrypoint.sh /ros_entrypoint.sh
 RUN chmod +x /ros_entrypoint.sh
 ENTRYPOINT [ "/ros_entrypoint.sh" ]
 

docker/noble/deps/opencv_4_6_0_ffmpeg.patch

@@ -0,0 +1,12 @@
+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

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

examples/RGBDMapping/main.cpp

@@ -256,8 +256,7 @@ 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/CloudViewer.h

@@ -334,9 +334,6 @@ public:
 
 	bool getPose(const std::string & id, Transform & pose); //including meshes
 	bool getCloudVisibility(const std::string & id);
-	int getCloudColorIndex(const std::string & id) const;
-	double getCloudOpacity(const std::string & id) const;
-	int getCloudPointSize(const std::string & id) const;
 
 	const QMap<std::string, Transform> & getAddedClouds() const {return _addedClouds;} //including meshes
 	const QColor & getDefaultBackgroundColor() const;
@@ -402,12 +399,6 @@ public:
 	void setIntensityRedColormap(bool value);
 	void setIntensityRainbowColormap(bool value);
 	void setIntensityMax(float value);
-	float getCloudColorRangeMin() const;
-	float getCloudColorRangeMax() const;
-	bool isCloudColorRangeInverted() const;
-	void setCloudColorRangeMin(float value);
-	void setCloudColorRangeMax(float value);
-	void setCloudColorRangeInverted(bool enabled);
 	void buildPickingLocator(bool enable);
 	const std::map<std::string, vtkSmartPointer<vtkOBBTree> > & getLocators() const {return _locators;}
 
@@ -463,10 +454,6 @@ private:
 	QAction * _aSetIntensityRedColormap;
 	QAction * _aSetIntensityRainbowColormap;
 	QAction * _aSetIntensityMaximum;
-	QAction * _aSetCloudColorRangeMin;
-	QAction * _aSetCloudColorRangeMax;
-	QAction * _aCloudColorRangeInverted;
-	QAction * _aClearCloudColorRanges;
     QAction * _aSetBackgroundColor;
     QAction * _aSetRenderingRate;
     QAction * _aSetEDLShading;
@@ -507,8 +494,6 @@ private:
     double _renderingRate;
     vtkProp * _octomapActor;
     float _intensityAbsMax;
-	float _cloudColorRangeMin;
-	float _cloudColorRangeMax;
     double  _coordinateFrameScale;
 };
 

guilib/include/rtabmap/gui/DatabaseViewer.h

@@ -65,8 +65,6 @@ class ExportCloudsDialog;
 class EditDepthArea;
 class EditMapArea;
 class LinkRefiningDialog;
-class Registration;
-class RegistrationIcp;
 
 class RTABMAP_GUI_EXPORT DatabaseViewer : public QMainWindow
 {
@@ -204,8 +202,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, Registration * reg, RegistrationIcp * regIcp, bool silent);
+	void refineConstraint(int from, int to,  bool silent);
-	bool addConstraint(int from, int to, Registration * reg, bool silent, bool silentlyUseOptimizedGraphAsGuess = false);
+	bool addConstraint(int from, int to, bool silent, bool silentlyUseOptimizedGraphAsGuess = false);
 	void exportPoses(int format);
 	void exportGPS(int format);
 

guilib/include/rtabmap/gui/GraphViewer.h

@@ -80,7 +80,6 @@ public:
 	void updateLocalPath(const std::vector<int> & localPath);
 	void setGlobalPath(const std::vector<std::pair<int, Transform> > & globalPath);
 	void setCurrentGoalID(int id, const Transform & pose = Transform());
-	void setNodeInfo(int id, const QString & info);
 	void setLocalRadius(float radius);
 	void highlightNode(int nodeId, int highlightIndex);
 	void clearGraph();

guilib/include/rtabmap/gui/ImageView.h

@@ -60,7 +60,6 @@ public:
 	QRectF sceneRect() const;
 	bool isImageShown() const;
 	bool isImageDepthShown() const;
-	bool isImageDepthConfidenceShown() const;
 	bool isFeaturesShown() const;
 	bool isLinesShown() const;
 	int getAlpha() const {return _alpha;}
@@ -77,14 +76,12 @@ public:
 	float getDepthColorMapMinRange() const;
 	float getDepthColorMapMaxRange() const;
 	uCvQtDepthColorMap getDepthColorMap() const;
-	bool isDepthColorMapInCameraFrame() const;
 
 	float viewScale() const;
 
 	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);
@@ -95,15 +92,14 @@ public:
 	void setDefaultMatchingLineColor(const QColor & color);
 	void setBackgroundColor(const QColor & color);
 	void setDepthColorMapRange(float min, float max);
-	void setDepthColorMapInCameraFrame(bool enabled);
 
 	void setFeatures(const std::multimap<int, cv::KeyPoint> & refWords, const cv::Mat & depth = cv::Mat(), const QColor & color = Qt::yellow);
 	void setFeatures(const std::vector<cv::KeyPoint> & features, const cv::Mat & depth = cv::Mat(), const QColor & color = Qt::yellow);
 	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, const cv::Mat & imageDepthConfidence = cv::Mat());
+	void setImageDepth(const cv::Mat & imageDepth);
-	void setImageDepth(const QImage & image, const QImage & imageDepthConfidence = QImage());
+	void setImageDepth(const QImage & image);
 	void setFeatureColor(int id, QColor color);
 	void setFeaturesColor(QColor color);
 	void setAlpha(int alpha);
@@ -151,7 +147,6 @@ private:
 	QMenu * _menu;
 	QAction * _showImage;
 	QAction * _showImageDepth;
-	QAction * _showImageDepthConfidence;
 	QAction * _showFeatures;
 	QAction * _showLines;
 	QAction * _setFeatureColor;
@@ -169,7 +164,6 @@ private:
 	QAction * _colorMapBlackToWhite;
 	QAction * _colorMapRedToBlue;
 	QAction * _colorMapBlueToRed;
-	QAction * _colorMapInCameraFrame;
 	QAction * _colorMapMinRange;
 	QAction * _colorMapMaxRange;
 	QAction * _mouseTracking;
@@ -181,12 +175,9 @@ 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/PointCloudColorHandlerIntensityField.h

@@ -1,186 +0,0 @@
-/*
-Copyright (c) 2010-2025, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without
-modification, are permitted provided that the following conditions are met:
-	* Redistributions of source code must retain the above copyright
-	  notice, this list of conditions and the following disclaimer.
-	* Redistributions in binary form must reproduce the above copyright
-	  notice, this list of conditions and the following disclaimer in the
-	  documentation and/or other materials provided with the distribution.
-	* Neither the name of the Universite de Sherbrooke nor the
-	  names of its contributors may be used to endorse or promote products
-	  derived from this software without specific prior written permission.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
-ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
-WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
-DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
-ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-*/
-
-#ifndef GUILIB_SRC_POINTCLOUDCOLORHANDLEINTENSITYFIELD_H_
-#define GUILIB_SRC_POINTCLOUDCOLORHANDLEINTENSITYFIELD_H_
-
-#include <pcl/visualization/point_cloud_color_handlers.h>
-#include <pcl/pcl_config.h>
-#include <rtabmap/core/util2d.h>
-#include <rtabmap/utilite/UMath.h>
-
-namespace rtabmap
-{
-
-class PointCloudColorHandlerIntensityField : public pcl::visualization::PointCloudColorHandler<pcl::PCLPointCloud2>
-{
-	typedef pcl::visualization::PointCloudColorHandler<pcl::PCLPointCloud2>::PointCloud PointCloud;
-	typedef PointCloud::Ptr PointCloudPtr;
-	typedef PointCloud::ConstPtr PointCloudConstPtr;
-
-public:
-	/** \brief Constructor. */
-	PointCloudColorHandlerIntensityField(const PointCloudConstPtr &cloud, float maxAbsIntensity = 0.0f, int colorMap = 0) : pcl::visualization::PointCloudColorHandler<pcl::PCLPointCloud2>::PointCloudColorHandler(cloud),
-																															maxAbsIntensity_(maxAbsIntensity),
-																															colormap_(colorMap)
-	{
-		field_idx_ = pcl::getFieldIndex(*cloud, "intensity");
-		if (field_idx_ != -1)
-			capable_ = true;
-		else
-			capable_ = false;
-	}
-
-	/** \brief Empty destructor */
-	virtual ~PointCloudColorHandlerIntensityField() {}
-
-	/** \brief Obtain the actual color for the input dataset as vtk scalars.
-	 * \param[out] scalars the output scalars containing the color for the dataset
-	 * \return true if the operation was successful (the handler is capable and
-	 * the input cloud was given as a valid pointer), false otherwise
-	 */
-#if PCL_VERSION_COMPARE(>, 1, 11, 1)
-	virtual vtkSmartPointer<vtkDataArray> getColor() const
-	{
-		vtkSmartPointer<vtkDataArray> scalars;
-		if (!capable_ || !cloud_)
-			return scalars;
-#else
-	virtual bool getColor(vtkSmartPointer<vtkDataArray> &scalars) const
-	{
-		if (!capable_ || !cloud_)
-			return (false);
-#endif
-		if (!scalars)
-			scalars = vtkSmartPointer<vtkUnsignedCharArray>::New();
-		scalars->SetNumberOfComponents(3);
-
-		vtkIdType nr_points = cloud_->width * cloud_->height;
-		// Allocate enough memory to hold all colors
-		float *intensities = new float[nr_points];
-		float intensity;
-		size_t point_offset = cloud_->fields[field_idx_].offset;
-		size_t j = 0;
-
-		// If XYZ present, check if the points are invalid
-		int x_idx = pcl::getFieldIndex(*cloud_, "x");
-		if (x_idx != -1)
-		{
-			float x_data, y_data, z_data;
-			size_t x_point_offset = cloud_->fields[x_idx].offset;
-
-			// Color every point
-			for (vtkIdType cp = 0; cp < nr_points; ++cp,
-							point_offset += cloud_->point_step,
-							x_point_offset += cloud_->point_step)
-			{
-				// Copy the value at the specified field
-				memcpy(&intensity, &cloud_->data[point_offset], sizeof(float));
-
-				memcpy(&x_data, &cloud_->data[x_point_offset], sizeof(float));
-				memcpy(&y_data, &cloud_->data[x_point_offset + sizeof(float)], sizeof(float));
-				memcpy(&z_data, &cloud_->data[x_point_offset + 2 * sizeof(float)], sizeof(float));
-
-				if (!std::isfinite(x_data) || !std::isfinite(y_data) || !std::isfinite(z_data))
-					continue;
-
-				intensities[j++] = intensity;
-			}
-		}
-		// No XYZ data checks
-		else
-		{
-			// Color every point
-			for (vtkIdType cp = 0; cp < nr_points; ++cp, point_offset += cloud_->point_step)
-			{
-				// Copy the value at the specified field
-				memcpy(&intensity, &cloud_->data[point_offset], sizeof(float));
-
-				intensities[j++] = intensity;
-			}
-		}
-		if (j != 0)
-		{
-			// Allocate enough memory to hold all colors
-			unsigned char *colors = new unsigned char[j * 3];
-			float min, max;
-			if (maxAbsIntensity_ > 0.0f)
-			{
-				max = maxAbsIntensity_;
-			}
-			else
-			{
-				uMinMax(intensities, j, min, max);
-			}
-			for (size_t k = 0; k < j; ++k)
-			{
-				colors[k * 3 + 0] = colors[k * 3 + 1] = colors[k * 3 + 2] = max > 0 ? (unsigned char)(std::min(intensities[k] / max * 255.0f, 255.0f)) : 255;
-				if (colormap_ == 1)
-				{
-					colors[k * 3 + 0] = 255;
-					colors[k * 3 + 2] = 0;
-				}
-				else if (colormap_ == 2)
-				{
-					float r, g, b;
-					util2d::HSVtoRGB(&r, &g, &b, colors[k * 3 + 0] * 299.0f / 255.0f, 1.0f, 1.0f);
-					colors[k * 3 + 0] = r * 255.0f;
-					colors[k * 3 + 1] = g * 255.0f;
-					colors[k * 3 + 2] = b * 255.0f;
-				}
-			}
-			reinterpret_cast<vtkUnsignedCharArray *>(&(*scalars))->SetNumberOfTuples(j);
-			reinterpret_cast<vtkUnsignedCharArray *>(&(*scalars))->SetArray(colors, j * 3, 0, vtkUnsignedCharArray::VTK_DATA_ARRAY_DELETE);
-		}
-		else
-			reinterpret_cast<vtkUnsignedCharArray *>(&(*scalars))->SetNumberOfTuples(0);
-		// delete [] colors;
-		delete[] intensities;
-#if PCL_VERSION_COMPARE(>, 1, 11, 1)
-		return scalars;
-#else
-		return (true);
-#endif
-	}
-
-protected:
-	/** \brief Get the name of the class. */
-	virtual std::string
-	getName() const { return ("PointCloudColorHandlerIntensityField"); }
-
-	/** \brief Get the name of the field used. */
-	virtual std::string
-	getFieldName() const { return ("intensity"); }
-
-private:
-	float maxAbsIntensity_;
-	int colormap_; // 0=grayscale, 1=redYellow, 2=RainbowHSV
-};
-
-} /* namespace rtabmap */
-
-#endif /* GUILIB_SRC_POINTCLOUDCOLORHANDLEINTENSITYFIELD_H_ */

guilib/include/rtabmap/gui/PointCloudColorHandlerMinMaxGenericField.h

@@ -1,187 +0,0 @@
-/*
-Copyright (c) 2010-2025, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without
-modification, are permitted provided that the following conditions are met:
-	* Redistributions of source code must retain the above copyright
-	  notice, this list of conditions and the following disclaimer.
-	* Redistributions in binary form must reproduce the above copyright
-	  notice, this list of conditions and the following disclaimer in the
-	  documentation and/or other materials provided with the distribution.
-	* Neither the name of the Universite de Sherbrooke nor the
-	  names of its contributors may be used to endorse or promote products
-	  derived from this software without specific prior written permission.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
-ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
-WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
-DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
-ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-*/
-
-#ifndef GUILIB_SRC_POINTCLOUDCOLORHANDLEMINMAXGENERICFIELD_H_
-#define GUILIB_SRC_POINTCLOUDCOLORHANDLEMINMAXGENERICFIELD_H_
-
-#include <limits>
-#include <pcl/visualization/point_cloud_color_handlers.h>
-#include <pcl/pcl_config.h>
-
-namespace rtabmap
-{
-
-	/// Same than pcl::visualization::PointCloudColorHandlerGenericField but with min and max parameters
-	class PointCloudColorHandlerMinMaxGenericField : public pcl::visualization::PointCloudColorHandler<pcl::PCLPointCloud2>
-	{
-		using PointCloud = typename PointCloudColorHandler<pcl::PCLPointCloud2>::PointCloud;
-		using PointCloudPtr = typename PointCloud::Ptr;
-		using PointCloudConstPtr = typename PointCloud::ConstPtr;
-
-	public:
-		/** \brief Constructor. */
-		PointCloudColorHandlerMinMaxGenericField(const PointCloudConstPtr &cloud,
-												 const std::string &field_name,
-												 float min = std::numeric_limits<float>::lowest(),
-												 float max = std::numeric_limits<float>::max(),
-												 bool inverted_color_scale = false)
-			: pcl::visualization::PointCloudColorHandler<pcl::PCLPointCloud2>(cloud),
-			  field_name_(field_name),
-			  min_(min),
-			  max_(max),
-			  inverted_color_scale_(inverted_color_scale)
-		{
-			setInputCloud(cloud);
-		}
-
-		/** \brief Destructor. */
-		virtual ~PointCloudColorHandlerMinMaxGenericField() {}
-
-		/** \brief Get the name of the field used. */
-		virtual std::string getFieldName() const { return (field_name_); }
-
-#if PCL_VERSION_COMPARE(>, 1, 11, 1)
-		virtual vtkSmartPointer<vtkDataArray> getColor() const
-		{
-			vtkSmartPointer<vtkDataArray> scalars;
-			if (!capable_ || !cloud_)
-				return scalars;
-#else
-		virtual bool getColor(vtkSmartPointer<vtkDataArray> &scalars) const
-		{
-			if (!capable_ || !cloud_)
-				return (false);
-#endif
-			if (!scalars)
-			    scalars = vtkSmartPointer<vtkFloatArray>::New ();
-			scalars->SetNumberOfComponents(1);
-
-			vtkIdType nr_points = cloud_->width * cloud_->height;
-			scalars->SetNumberOfTuples(nr_points);
-
-			float *colors = new float[nr_points];
-			float field_data;
-			int j = 0;
-			int point_offset = cloud_->fields[field_idx_].offset;
-
-			// If XYZ present, check if the points are invalid
-			int x_idx = pcl::getFieldIndex(*cloud_, "x");
-			if (x_idx != -1)
-			{
-				float x_data, y_data, z_data;
-				int x_point_offset = cloud_->fields[x_idx].offset;
-
-				// Color every point
-				for (vtkIdType cp = 0; cp < nr_points; ++cp,
-							   point_offset += cloud_->point_step,
-							   x_point_offset += cloud_->point_step)
-				{
-					memcpy(&x_data, &cloud_->data[x_point_offset], sizeof(float));
-					memcpy(&y_data, &cloud_->data[x_point_offset + sizeof(float)], sizeof(float));
-					memcpy(&z_data, &cloud_->data[x_point_offset + 2 * sizeof(float)], sizeof(float));
-					if (!std::isfinite(x_data) || !std::isfinite(y_data) || !std::isfinite(z_data))
-						continue;
-
-					// Copy the value at the specified field
-					memcpy(&field_data, &cloud_->data[point_offset], pcl::getFieldSize(cloud_->fields[field_idx_].datatype));
-					if(field_data < min_) {
-						field_data = min_;
-					}
-					if(field_data > max_) {
-						field_data = max_;
-					}
-					colors[j] = field_data * (inverted_color_scale_?-1.0f:1.0f);
-					j++;
-				}
-			}
-			// No XYZ data checks
-			else
-			{
-				// Color every point
-				for (vtkIdType cp = 0; cp < nr_points; ++cp, point_offset += cloud_->point_step)
-				{
-					// Copy the value at the specified field
-					// memcpy (&field_data, &cloud_->data[point_offset], sizeof (float));
-					memcpy(&field_data, &cloud_->data[point_offset], pcl::getFieldSize(cloud_->fields[field_idx_].datatype));
-
-					if (!std::isfinite(field_data))
-						continue;
-
-					if(field_data < min_) {
-						field_data = min_;
-					}
-					if(field_data > max_) {
-						field_data = max_;
-					}
-					
-					colors[j] = field_data * (inverted_color_scale_?-1.0f:1.0f);
-					j++;
-				}
-			}
-			reinterpret_cast<vtkFloatArray *>(&(*scalars))->SetArray(colors, j, 0, vtkFloatArray::VTK_DATA_ARRAY_DELETE);
-
-#if PCL_VERSION_COMPARE(>, 1, 11, 1)
-			return scalars;
-#else
-			return (true);
-#endif
-		}
-
-		using PointCloudColorHandler<pcl::PCLPointCloud2>::getColor;
-
-		/** \brief Set the input cloud to be used.
-		 * \param[in] cloud the input cloud to be used by the handler
-		 */
-		virtual void
-		setInputCloud(const PointCloudConstPtr &cloud)
-		{
-			PointCloudColorHandler<pcl::PCLPointCloud2>::setInputCloud(cloud);
-			field_idx_ = pcl::getFieldIndex(*cloud, field_name_);
-			capable_ = field_idx_ != -1;
-			if (field_idx_ != -1 && cloud_->fields[field_idx_].datatype != pcl::PCLPointField::PointFieldTypes::FLOAT32)
-			{
-				capable_ = false;
-				PCL_ERROR("[pcl::PointCloudColorHandlerGenericField] This currently only works with float32 fields, but field %s has a different type.\n", field_name_.c_str());
-			}
-		}
-
-	protected:
-		/** \brief Class getName method. */
-		virtual std::string
-		getName() const { return ("PointCloudColorHandlerMinMaxGenericField"); }
-
-	private:
-		/** \brief Name of the field used to create the color handler. */
-		std::string field_name_;
-		float min_;
-		float max_;
-		bool inverted_color_scale_;
-	};
-
-} /* namespace rtabmap */
-
-#endif /* GUILIB_SRC_POINTCLOUDCOLORHANDLEMINMAXGENERICFIELD_H_ */

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -208,7 +208,6 @@ 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
@@ -467,7 +466,6 @@ 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 or monochrome images
+ * @param colorMap gradient of color to use to visualize depth
  * @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 = uCvQtDepthBlackToWhite,
+		uCvQtDepthColorMap colorMap = uCvQtDepthWhiteToBlack,
 		float depthMin = 0,
 		float depthMax = 0)
 {
@@ -81,30 +81,9 @@ inline QImage uCvMat2QImage(
 		{
 			// mono grayscale
 			qtemp = QImage(image.data, image.cols, image.rows, image.cols, QImage::Format_Indexed8).copy();
-
 			QVector<QRgb> my_table;
-			my_table.reserve(256);
+			for(int i = 0; i < 256; i++)
-			if(colorMap == uCvQtDepthRedToBlue || colorMap == uCvQtDepthBlueToRed)
+				my_table.push_back(qRgb(i,i,i));
-			{
-				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
@@ -117,7 +96,7 @@ inline QImage uCvMat2QImage(
 		// Assume depth image (float in meters)
 		const float * data = (const float *)image.data;
 		float min,max;
-		if(depthMin != 0 && depthMax != 0 && depthMax > depthMin)
+		if(depthMax>depthMin)
 		{
 			min = depthMin;
 			max = depthMax;
@@ -127,23 +106,23 @@ inline QImage uCvMat2QImage(
 			min = max = data[0];
 			for(unsigned int i=1; i<image.total(); ++i)
 			{
-				if(uIsFinite(data[i]) && data[i] != 0)
+				if(uIsFinite(data[i]) && data[i] > 0)
 				{
-					if(!uIsFinite(min) || (data[i] != 0 && data[i]<min))
+					if(!uIsFinite(min) || (data[i] > 0 && data[i]<min))
 					{
 						min = data[i];
 					}
-					if(!uIsFinite(max) || (data[i] != 0 && data[i]>max))
+					if(!uIsFinite(max) || (data[i] > 0 && data[i]>max))
 					{
 						max = data[i];
 					}
 				}
 			}
-			if(depthMax != 0 && depthMax > depthMin)
+			if(depthMax > 0 && depthMax > depthMin)
 			{
 				max = depthMax;
 			}
-			if(depthMin != 0 && (depthMin < depthMax || depthMin < max))
+			if(depthMin>0 && (depthMin < depthMax || depthMin < max))
 			{
 				min = depthMin;
 			}
@@ -198,7 +177,7 @@ inline QImage uCvMat2QImage(
 		// Assume depth image (unsigned short in mm)
 		const unsigned short * data = (const unsigned short *)image.data;
 		unsigned short min,max;
-		if(depthMin != 0 && depthMax != 0 && depthMax > depthMin)
+		if(depthMax>depthMin)
 		{
 			min = depthMin*1000;
 			max = depthMax*1000;
@@ -208,23 +187,23 @@ inline QImage uCvMat2QImage(
 			min = max = data[0];
 			for(unsigned int i=1; i<image.total(); ++i)
 			{
-				if(uIsFinite(data[i]) && data[i] != 0)
+				if(uIsFinite(data[i]) && data[i] > 0)
 				{
-					if(!uIsFinite(min) || (data[i] != 0 && data[i]<min))
+					if(!uIsFinite(min) || (data[i] > 0 && data[i]<min))
 					{
 						min = data[i];
 					}
-					if(!uIsFinite(max) || (data[i] != 0 && data[i]>max))
+					if(!uIsFinite(max) || (data[i] > 0 && data[i]>max))
 					{
 						max = data[i];
 					}
 				}
 			}
-			if(depthMax != 0 && depthMax > depthMin)
+			if(depthMax > 0 && depthMax > depthMin)
 			{
 				max = depthMax*1000;
 			}
-			if(depthMin != 0 && (depthMin < depthMax || depthMin*1000 < max))
+			if(depthMin>0 && (depthMin < depthMax || depthMin*1000 < max))
 			{
 				min = depthMin*1000;
 			}

guilib/src/CloudViewer.cpp

@@ -27,8 +27,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/gui/CloudViewer.h"
 #include "rtabmap/gui/CloudViewerCellPicker.h"
-#include "rtabmap/gui/PointCloudColorHandlerIntensityField.h"
-#include "rtabmap/gui/PointCloudColorHandlerMinMaxGenericField.h"
 
 #include <rtabmap/core/Version.h>
 #include <rtabmap/core/util3d_transforms.h>
@@ -152,8 +150,6 @@ CloudViewer::CloudViewer(QWidget *parent, CloudViewerInteractorStyle * style) :
 		_renderingRate(5.0),
 		_octomapActor(0),
 		_intensityAbsMax(100.0f),
-		_cloudColorRangeMin(0.0f),
-		_cloudColorRangeMax(0.0f),
 		_coordinateFrameScale(1.0)
 {
 	this->setMinimumSize(200, 200);
@@ -341,12 +337,6 @@ void CloudViewer::createMenu()
 	_aSetIntensityRainbowColormap->setCheckable(true);
 	_aSetIntensityRainbowColormap->setChecked(false);
 	_aSetIntensityMaximum = new QAction("Set maximum absolute intensity...", this);
-	_aSetCloudColorRangeMin = new QAction("Set minimum color range...", this);
-	_aSetCloudColorRangeMax = new QAction("Set maximum color range...", this);
-	_aCloudColorRangeInverted = new QAction("Inverted color scale", this);
-	_aCloudColorRangeInverted->setCheckable(true);
-	_aCloudColorRangeInverted->setChecked(false);
-	_aClearCloudColorRanges = new QAction("Reset ranges", this);
 	_aSetBackgroundColor = new QAction("Set background color...", this);	
 	_aSetRenderingRate = new QAction("Set rendering rate...", this);
 	_aSetEDLShading = new QAction("Eye-Dome Lighting Shading", this);
@@ -412,12 +402,6 @@ void CloudViewer::createMenu()
 	scanMenu->addAction(_aSetIntensityRainbowColormap);
 	scanMenu->addAction(_aSetIntensityMaximum);
 
-	QMenu * cloudMenu = new QMenu("XYZ color", this);
-	cloudMenu->addAction(_aSetCloudColorRangeMin);
-	cloudMenu->addAction(_aSetCloudColorRangeMax);
-	cloudMenu->addAction(_aCloudColorRangeInverted);
-	cloudMenu->addAction(_aClearCloudColorRanges);
-
 	//menus
 	_menu = new QMenu(this);
 	_menu->addMenu(cameraMenu);
@@ -428,7 +412,6 @@ void CloudViewer::createMenu()
 	_menu->addMenu(gridMenu);
 	_menu->addMenu(normalsMenu);
 	_menu->addMenu(scanMenu);
-	_menu->addMenu(cloudMenu);
 	_menu->addAction(_aSetBackgroundColor);
 	_menu->addAction(_aSetRenderingRate);
 	_menu->addAction(_aSetEDLShading);
@@ -482,10 +465,6 @@ void CloudViewer::saveSettings(QSettings & settings, const QString & group) cons
 	settings.setValue("intensity_rainbow_colormap", this->isIntensityRainbowColormap());
 	settings.setValue("intensity_max", (double)this->getIntensityMax());
 
-	settings.setValue("color_range_min", (double)this->getCloudColorRangeMin());
-	settings.setValue("color_range_max", (double)this->getCloudColorRangeMax());
-	settings.setValue("color_range_inverted", (double)this->isCloudColorRangeInverted());
-
 	settings.setValue("trajectory_shown", this->isTrajectoryShown());
 	settings.setValue("trajectory_size", this->getTrajectorySize());
 
@@ -537,10 +516,6 @@ void CloudViewer::loadSettings(QSettings & settings, const QString & group)
 	this->setIntensityRainbowColormap(settings.value("intensity_rainbow_colormap", this->isIntensityRainbowColormap()).toBool());
 	this->setIntensityMax(settings.value("intensity_max", this->getIntensityMax()).toFloat());
 
-	this->setCloudColorRangeMin(settings.value("color_range_min", this->getCloudColorRangeMin()).toFloat());
-	this->setCloudColorRangeMax(settings.value("color_range_max", this->getCloudColorRangeMax()).toFloat());
-	this->setCloudColorRangeInverted(settings.value("color_range_inverted", this->isCloudColorRangeInverted()).toBool());
-
 	this->setTrajectoryShown(settings.value("trajectory_shown", this->isTrajectoryShown()).toBool());
 	this->setTrajectorySize(settings.value("trajectory_size", this->getTrajectorySize()).toUInt());
 
@@ -631,6 +606,153 @@ bool CloudViewer::updateCloudPose(
 	return false;
 }
 
+class PointCloudColorHandlerIntensityField : public pcl::visualization::PointCloudColorHandler<pcl::PCLPointCloud2>
+{
+	typedef pcl::visualization::PointCloudColorHandler<pcl::PCLPointCloud2>::PointCloud PointCloud;
+	typedef PointCloud::Ptr PointCloudPtr;
+	typedef PointCloud::ConstPtr PointCloudConstPtr;
+
+public:
+	typedef boost::shared_ptr<PointCloudColorHandlerIntensityField > Ptr;
+	typedef boost::shared_ptr<const PointCloudColorHandlerIntensityField > ConstPtr;
+
+	/** \brief Constructor. */
+	PointCloudColorHandlerIntensityField (const PointCloudConstPtr &cloud, float maxAbsIntensity = 0.0f, int colorMap = 0) :
+		pcl::visualization::PointCloudColorHandler<pcl::PCLPointCloud2>::PointCloudColorHandler (cloud),
+		maxAbsIntensity_(maxAbsIntensity),
+		colormap_(colorMap)
+		{
+		field_idx_  = pcl::getFieldIndex (*cloud, "intensity");
+		if (field_idx_ != -1)
+			capable_ = true;
+		else
+			capable_ = false;
+		}
+
+	/** \brief Empty destructor */
+	virtual ~PointCloudColorHandlerIntensityField () {}
+
+	/** \brief Obtain the actual color for the input dataset as vtk scalars.
+	 * \param[out] scalars the output scalars containing the color for the dataset
+	 * \return true if the operation was successful (the handler is capable and
+	 * the input cloud was given as a valid pointer), false otherwise
+	 */
+#if PCL_VERSION_COMPARE(>, 1, 11, 1)
+	virtual vtkSmartPointer<vtkDataArray> getColor () const {
+		vtkSmartPointer<vtkDataArray> scalars;
+		if (!capable_ || !cloud_)
+			return scalars;
+#else
+	virtual bool getColor (vtkSmartPointer<vtkDataArray> &scalars) const {
+		if (!capable_ || !cloud_)
+			return (false);
+#endif
+		if (!scalars)
+			scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
+		scalars->SetNumberOfComponents (3);
+
+		vtkIdType nr_points = cloud_->width * cloud_->height;
+		// Allocate enough memory to hold all colors
+		float * intensities = new float[nr_points];
+		float intensity;
+		size_t point_offset = cloud_->fields[field_idx_].offset;
+		size_t j = 0;
+
+		// If XYZ present, check if the points are invalid
+		int x_idx = pcl::getFieldIndex (*cloud_, "x");
+		if (x_idx != -1)
+		{
+			float x_data, y_data, z_data;
+			size_t x_point_offset = cloud_->fields[x_idx].offset;
+
+			// Color every point
+			for (vtkIdType cp = 0; cp < nr_points; ++cp,
+			point_offset += cloud_->point_step,
+			x_point_offset += cloud_->point_step)
+			{
+				// Copy the value at the specified field
+				memcpy (&intensity, &cloud_->data[point_offset], sizeof (float));
+
+				memcpy (&x_data, &cloud_->data[x_point_offset], sizeof (float));
+				memcpy (&y_data, &cloud_->data[x_point_offset + sizeof (float)], sizeof (float));
+				memcpy (&z_data, &cloud_->data[x_point_offset + 2 * sizeof (float)], sizeof (float));
+
+				if (!std::isfinite (x_data) || !std::isfinite (y_data) || !std::isfinite (z_data))
+					continue;
+
+				intensities[j++] = intensity;
+			}
+		}
+		// No XYZ data checks
+		else
+		{
+			// Color every point
+			for (vtkIdType cp = 0; cp < nr_points; ++cp, point_offset += cloud_->point_step)
+			{
+				// Copy the value at the specified field
+				memcpy (&intensity, &cloud_->data[point_offset], sizeof (float));
+
+				intensities[j++] = intensity;
+			}
+		}
+		if (j != 0)
+		{
+			// Allocate enough memory to hold all colors
+			unsigned char* colors = new unsigned char[j * 3];
+			float min, max;
+			if(maxAbsIntensity_>0.0f)
+			{
+				max = maxAbsIntensity_;
+			}
+			else
+			{
+				uMinMax(intensities, j, min, max);
+			}
+			for(size_t k=0; k<j; ++k)
+			{
+				colors[k*3+0] = colors[k*3+1] = colors[k*3+2] = max>0?(unsigned char)(std::min(intensities[k]/max*255.0f, 255.0f)):255;
+				if(colormap_ == 1)
+				{
+					colors[k*3+0] = 255;
+					colors[k*3+2] = 0;
+				}
+				else if(colormap_ == 2)
+				{
+					float r,g,b;
+					util2d::HSVtoRGB(&r, &g, &b, colors[k*3+0]*299.0f/255.0f, 1.0f, 1.0f);
+					colors[k*3+0] = r*255.0f;
+					colors[k*3+1] = g*255.0f;
+					colors[k*3+2] = b*255.0f;
+				}
+			}
+			reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (j);
+			reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors, j*3, 0, vtkUnsignedCharArray::VTK_DATA_ARRAY_DELETE);
+		}
+		else
+			reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (0);
+		//delete [] colors;
+		delete [] intensities;
+#if PCL_VERSION_COMPARE(>, 1, 11, 1)
+		return scalars;
+#else
+		return (true);
+#endif
+	}
+
+protected:
+	/** \brief Get the name of the class. */
+	virtual std::string
+	getName () const { return ("PointCloudColorHandlerIntensityField"); }
+
+	/** \brief Get the name of the field used. */
+	virtual std::string
+	getFieldName () const { return ("intensity"); }
+
+private:
+	float maxAbsIntensity_;
+	int colormap_; // 0=grayscale, 1=redYellow, 2=RainbowHSV
+};
+
 bool CloudViewer::addCloud(
 		const std::string & id,
 		const pcl::PCLPointCloud2Ptr & binaryCloud,
@@ -677,20 +799,11 @@ bool CloudViewer::addCloud(
 		_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, viewport);
 
 		// x,y,z
-		colorHandler.reset (new PointCloudColorHandlerMinMaxGenericField (binaryCloud, "x",
+		colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "x"));
-			_cloudColorRangeMin==0.0f?std::numeric_limits<float>::lowest():_cloudColorRangeMin,
-			_cloudColorRangeMax==0.0f?std::numeric_limits<float>::max():_cloudColorRangeMax,
-			_aCloudColorRangeInverted->isChecked()));
 		_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, viewport);
-		colorHandler.reset (new PointCloudColorHandlerMinMaxGenericField (binaryCloud, "y",
+		colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "y"));
-			_cloudColorRangeMin==0.0f?std::numeric_limits<float>::lowest():_cloudColorRangeMin,
-			_cloudColorRangeMax==0.0f?std::numeric_limits<float>::max():_cloudColorRangeMax,
-			_aCloudColorRangeInverted->isChecked()));
 		_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, viewport);
-		colorHandler.reset (new PointCloudColorHandlerMinMaxGenericField (binaryCloud, "z",
+		colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "z"));
-			_cloudColorRangeMin==0.0f?std::numeric_limits<float>::lowest():_cloudColorRangeMin,
-			_cloudColorRangeMax==0.0f?std::numeric_limits<float>::max():_cloudColorRangeMax,
-			_aCloudColorRangeInverted->isChecked()));
 		_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, viewport);
 
 		if(rgb)
@@ -3172,12 +3285,6 @@ bool CloudViewer::getCloudVisibility(const std::string & id)
 	return false;
 }
 
-
-int CloudViewer::getCloudColorIndex(const std::string & id) const
-{
-	return _visualizer->getColorHandlerIndex(id);
-}
-	
 void CloudViewer::setCloudColorIndex(const std::string & id, int index)
 {
 	if(index>0)
@@ -3186,26 +3293,6 @@ void CloudViewer::setCloudColorIndex(const std::string & id, int index)
 	}
 }
 
-double CloudViewer::getCloudOpacity(const std::string & id) const
-{
-	double opacity = 1.0;
-	if(!_visualizer->getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, id))
-	{
-#if VTK_MAJOR_VERSION >= 7
-		pcl::visualization::ShapeActorMap::iterator am_it = _visualizer->getShapeActorMap()->find (id);
-		if (am_it != _visualizer->getShapeActorMap()->end ())
-		{
-			vtkActor* actor = vtkActor::SafeDownCast (am_it->second);
-			if(actor)
-			{
-				opacity = actor->GetProperty ()->GetOpacity ();
-			}
-		}
-#endif
-	}
-	return opacity;
-}
-	
 void CloudViewer::setCloudOpacity(const std::string & id, double opacity)
 {
 	double lastOpacity;
@@ -3233,12 +3320,6 @@ void CloudViewer::setCloudOpacity(const std::string & id, double opacity)
 #endif
 }
 
-int CloudViewer::getCloudPointSize(const std::string & id) const
-{
-	double size = 1.0;
-	_visualizer->getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, size, id);
-	return (int)size;
-}
 void CloudViewer::setCloudPointSize(const std::string & id, int size)
 {
 	double lastSize;
@@ -3530,33 +3611,9 @@ void CloudViewer::setIntensityMax(float value)
 	}
 	else
 	{
-		UERROR("Cannot set intensity < 0, value=%f", value);
+		UERROR("Cannot set normals scale < 0, value=%f", value);
 	}
 }
-float CloudViewer::getCloudColorRangeMin() const
-{
-	return _cloudColorRangeMin;
-}
-float CloudViewer::getCloudColorRangeMax() const
-{
-	return _cloudColorRangeMax;
-}
-bool CloudViewer::isCloudColorRangeInverted() const
-{
-	return _aCloudColorRangeInverted->isChecked();
-}
-void CloudViewer::setCloudColorRangeMin(float value)
-{
-	_cloudColorRangeMin = value;
-}
-void CloudViewer::setCloudColorRangeMax(float value)
-{
-	_cloudColorRangeMax = value;
-}
-void CloudViewer::setCloudColorRangeInverted(bool enabled)
-{
-	_aCloudColorRangeInverted->setChecked(enabled);
-}
 
 void CloudViewer::buildPickingLocator(bool enable)
 {
@@ -3927,30 +3984,6 @@ void CloudViewer::handleAction(QAction * a)
 	{
 		this->setIntensityRainbowColormap(_aSetIntensityRainbowColormap->isChecked());
 	}
-	else if(a == _aSetCloudColorRangeMin)
-	{
-		bool ok;
-		double value = QInputDialog::getDouble(this, tr("Set minimum axis color range"), tr("Range (0=auto)"), _cloudColorRangeMin, -99999, 99999, 2, &ok);
-		if(ok)
-		{
-			this->setCloudColorRangeMin(value);
-		}
-	}
-	else if(a == _aSetCloudColorRangeMax)
-	{
-		bool ok;
-		double value = QInputDialog::getDouble(this, tr("Set maximum axis color range"), tr("Range (0=auto)"), _cloudColorRangeMax, -99999, 99999, 2, &ok);
-		if(ok)
-		{
-			this->setCloudColorRangeMax(value);
-		}
-	}
-	else if(a == _aClearCloudColorRanges)
-	{
-		_cloudColorRangeMin = 0.0f;
-		_cloudColorRangeMax = 0.0f;
-		_aCloudColorRangeInverted->setChecked(false);
-	}
 	else if(a == _aSetBackgroundColor)
 	{
 		QColor color = this->getDefaultBackgroundColor();

guilib/src/DataRecorder.cpp

@@ -156,12 +156,8 @@ void DataRecorder::addData(const rtabmap::SensorData & data, const Transform & p
 void DataRecorder::showImage(const cv::Mat & image, const cv::Mat & depth)
 {
 	processingImages_ = true;
-	if(!image.empty()) {
+	imageView_->setImage(uCvMat2QImage(image));
-		imageView_->setImage(uCvMat2QImage(image));
+	imageView_->setImageDepth(depth);
-	}
-	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,10 +427,6 @@ 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()));
@@ -445,7 +441,6 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	connect(ui_->graphViewer, SIGNAL(configChanged()), this, SLOT(configModified()));
 	connect(ui_->graphicsView_A, SIGNAL(configChanged()), this, SLOT(configModified()));
 	connect(ui_->graphicsView_B, SIGNAL(configChanged()), this, SLOT(configModified()));
-	connect(cloudViewer_, SIGNAL(configChanged()), this, SLOT(configModified()));
 	connect(ui_->comboBox_logger_level, SIGNAL(currentIndexChanged(int)), this, SLOT(configModified()));
 	connect(ui_->actionVertical_Layout, SIGNAL(toggled(bool)), this, SLOT(configModified()));
 	connect(ui_->actionConcise_Layout, SIGNAL(toggled(bool)), this, SLOT(configModified()));
@@ -462,8 +457,6 @@ 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()));
@@ -623,8 +616,6 @@ 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();
@@ -654,9 +645,6 @@ void DatabaseViewer::readSettings()
 	ui_->graphicsView_A->loadSettings(settings, "ImageViewA");
 	ui_->graphicsView_B->loadSettings(settings, "ImageViewB");
 
-	// CloudViewer
-	cloudViewer_->loadSettings(settings, "CloudViewer");
-
 	// ICP parameters
 	settings.beginGroup("icp");
 	ui_->spinBox_icp_decimation->setValue(settings.value("decimation", ui_->spinBox_icp_decimation->value()).toInt());
@@ -719,8 +707,6 @@ 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();
@@ -751,9 +737,6 @@ void DatabaseViewer::writeSettings()
 	ui_->graphicsView_A->saveSettings(settings, "ImageViewA");
 	ui_->graphicsView_B->saveSettings(settings, "ImageViewB");
 
-	// CloudViewer
-	cloudViewer_->saveSettings(settings, "CloudViewer");
-
 	// save ICP parameters
 	settings.beginGroup("icp");
 	settings.setValue("decimation", ui_->spinBox_icp_decimation->value());
@@ -818,8 +801,6 @@ 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);
 
@@ -1603,11 +1584,6 @@ 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;
 					}
 				}
@@ -1692,14 +1668,7 @@ 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());
-						if(data.depthConfidenceRaw().empty()) {
+						UINFO(QString("Saved rgb/%1.%2 and depth/%1.png").arg(id).arg(ext).toStdString().c_str());
-							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
 					{
@@ -3488,24 +3457,18 @@ void DatabaseViewer::exportOptimizedMesh()
 					path += ".obj";
 				}
 				QString baseName = QFileInfo(path).baseName();
-				UDEBUG("Materials: %d", mesh->tex_materials.size());
 				if(mesh->tex_materials.size() == 1)
 				{
 					mesh->tex_materials.at(0).tex_file = baseName.toStdString() + ".png";
-					std::string filename = (QFileInfo(path).absoluteDir().absolutePath()+QDir::separator()+baseName).toStdString() + ".png";
+					cv::imwrite((QFileInfo(path).absoluteDir().absolutePath()+QDir::separator()+baseName).toStdString() + ".png", textures);
-					cv::imwrite(filename, textures);
-					UDEBUG("Saved %s", filename.c_str());
 				}
 				else
 				{
-					QDir(QFileInfo(path).absoluteDir().absolutePath()).mkdir(baseName);
 					for(unsigned int i=0; i<mesh->tex_materials.size(); ++i)
 					{
 						mesh->tex_materials.at(i).tex_file = (baseName+QDir::separator()+QString::number(i)+".png").toStdString();
 						UASSERT((i+1)*textures.rows <= (unsigned int)textures.cols);
-						std::string filename = (QFileInfo(path).absoluteDir().absolutePath()+QDir::separator()+baseName+QDir::separator()+QString::number(i)+".png").toStdString();
+						cv::imwrite((QFileInfo(path).absoluteDir().absolutePath()+QDir::separator()+baseName+QDir::separator()+QString::number(i)+".png").toStdString(), textures(cv::Range::all(), cv::Range(i*textures.rows, (i+1)*textures.rows)));
-						cv::imwrite(filename, textures(cv::Range::all(), cv::Range(i*textures.rows, (i+1)*textures.rows)));
-						UDEBUG("Saved %s", filename.c_str());
 					}
 				}
 				pcl::io::saveOBJFile(path.toStdString(), *mesh);
@@ -4300,8 +4263,6 @@ 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);
@@ -4349,7 +4310,7 @@ void DatabaseViewer::detectMoreLoopClosures()
 						   delta.getNorm() >= ui_->doubleSpinBox_detectMore_radiusMin->value())
 						{
 							checkedLoopClosures.insert(std::make_pair(from, to));
-							if(addConstraint(from, to, reg.get(), true, useOptimizedGraphAsGuess))
+							if(addConstraint(from, to, true, useOptimizedGraphAsGuess))
 							{
 								UINFO("Added new loop closure between %d and %d.", from, to);
 								++added;
@@ -4607,16 +4568,13 @@ 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(), reg.get(), &regProximity, true);
+				this->refineConstraint(links[i].from(), links[i].to(), true);
 				progressDialog->appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(links.size()));
 			}
 			else
@@ -4844,7 +4802,7 @@ void DatabaseViewer::update(int value,
 
 				if(!imgDepth.empty())
 				{
-					view->setImageDepth(imgDepth, data.depthConfidenceRaw());
+					view->setImageDepth(imgDepth);
 					if(img.isNull())
 					{
 						rect.setWidth(imgDepth.cols);
@@ -5145,15 +5103,6 @@ void DatabaseViewer::update(int value,
 					cloudViewer_->removeAllLines();
 					cloudViewer_->removeAllFrustums();
 					cloudViewer_->removeOccupancyGridMap();
-					std::map<std::string, std::pair<int, int> > colorIndexAndPointSizeMap;
-					for(auto iter=cloudViewer_->getAddedClouds().constBegin(); iter!=cloudViewer_->getAddedClouds().constEnd(); ++iter) {
-						if(uStrContains(iter.key(), "cloud") || uStrContains(iter.key(), "scan")) {
-							colorIndexAndPointSizeMap.insert(std::make_pair(iter.key(), 
-								std::make_pair(
-									cloudViewer_->getCloudColorIndex(iter.key())+1,
-									cloudViewer_->getCloudPointSize(iter.key()))));
-						}
-					}
 					cloudViewer_->removeAllClouds();
 					cloudViewer_->removeOctomap();
 					cloudViewer_->removeElevationMap();
@@ -5211,10 +5160,6 @@ void DatabaseViewer::update(int value,
 							pcl::PointCloud<pcl::PointXYZ>::Ptr scan = util3d::laserScanToPointCloud(laserScanRaw, laserScanRaw.localTransform());
 							cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
 						}
-						if(colorIndexAndPointSizeMap.find("scan") != colorIndexAndPointSizeMap.end()) {
-							cloudViewer_->setCloudColorIndex("scan", colorIndexAndPointSizeMap.at("scan").first);
-							cloudViewer_->setCloudPointSize("scan", colorIndexAndPointSizeMap.at("scan").second);
-						}
 					}
 
 					// add RGB-D cloud
@@ -5301,10 +5246,6 @@ void DatabaseViewer::update(int value,
 								}
 
 								cloudViewer_->addCloud("cloud", cloudValidPoints, pose);
-								if(colorIndexAndPointSizeMap.find("cloud") != colorIndexAndPointSizeMap.end()) {
-									cloudViewer_->setCloudColorIndex("cloud", colorIndexAndPointSizeMap.at("cloud").first);
-									cloudViewer_->setCloudPointSize("cloud", colorIndexAndPointSizeMap.at("cloud").second);
-								}
 							}
 							else
 							{
@@ -5320,12 +5261,6 @@ 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;
@@ -5341,13 +5276,8 @@ 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(),
+											ui_->spinBox_decimation->value(),0,0,indices.get());
-											0,
-											0,
-											(unsigned char)ui_->spinBox_depthConfidence->value(),
-											indices.get());
 									if(indices->size())
 									{
 										clouds.push_back(util3d::transformPointCloud(cloud, data.cameraModels()[0].localTransform()));
@@ -5356,14 +5286,7 @@ void DatabaseViewer::update(int value,
 								}
 								else
 								{
-									clouds = util3d::cloudsRGBFromSensorData(
+									clouds = util3d::cloudsRGBFromSensorData(data, ui_->spinBox_decimation->value(), 0, 0, &allIndices, ui_->parameters_toolbox->getParameters());
-										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)
@@ -5428,12 +5351,7 @@ void DatabaseViewer::update(int value,
 										}
 										if(ui_->checkBox_showCloud->isChecked())
 										{
-											std::string cloudName = uFormat("cloud_%d", i);
+											cloudViewer_->addCloud(uFormat("cloud_%d", i), cloud, pose);
-											cloudViewer_->addCloud(cloudName, cloud, pose);
-											if(colorIndexAndPointSizeMap.find(cloudName) != colorIndexAndPointSizeMap.end()) {
-												cloudViewer_->setCloudColorIndex(cloudName, colorIndexAndPointSizeMap.at(cloudName).first);
-												cloudViewer_->setCloudPointSize(cloudName, colorIndexAndPointSizeMap.at(cloudName).second);
-											}
 										}
 									}
 								}
@@ -5443,14 +5361,7 @@ void DatabaseViewer::update(int value,
 								std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clouds;
 								std::vector<pcl::IndicesPtr> allIndices;
 
-								clouds = util3d::cloudsFromSensorData(
+								clouds = util3d::cloudsFromSensorData(data, ui_->spinBox_decimation->value(), 0, 0, &allIndices, ui_->parameters_toolbox->getParameters());
-									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)
 								{
@@ -5463,12 +5374,7 @@ void DatabaseViewer::update(int value,
 											cloud = util3d::voxelize(cloud, indices, ui_->doubleSpinBox_voxelSize->value());
 										}
 
-										std::string cloudName = uFormat("cloud_%d", i);
+										cloudViewer_->addCloud(uFormat("cloud_%d", i), cloud, pose);
-										cloudViewer_->addCloud(cloudName, cloud, pose);
-										if(colorIndexAndPointSizeMap.find(cloudName) != colorIndexAndPointSizeMap.end()) {
-											cloudViewer_->setCloudColorIndex(cloudName, colorIndexAndPointSizeMap.at(cloudName).first);
-											cloudViewer_->setCloudPointSize(cloudName, colorIndexAndPointSizeMap.at(cloudName).second);
-										}
 									}
 								}
 							}
@@ -6447,12 +6353,11 @@ 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 deg)").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)").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())
 			{
@@ -6583,37 +6488,11 @@ void DatabaseViewer::updateConstraintView(
 			pcl::IndicesPtr indicesTo(new std::vector<int>);
 			if(!dataFrom.imageRaw().empty() && !dataFrom.depthOrRightRaw().empty())
 			{
-				if(!dataFrom.depthRaw().empty() && ui_->doubleSpinBox_depthEdgeBleedingError->value() > 0.0) {
+				cloudFrom=util3d::cloudRGBFromSensorData(dataFrom, ui_->spinBox_decimation->value(), 0, 0, indicesFrom.get(), ui_->parameters_toolbox->getParameters());
-					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())
 			{
-				if(!dataTo.depthRaw().empty() && ui_->doubleSpinBox_depthEdgeBleedingError->value() > 0.0) {
+				cloudTo=util3d::cloudRGBFromSensorData(dataTo, ui_->spinBox_decimation->value(), 0, 0, indicesTo.get(), ui_->parameters_toolbox->getParameters());
-					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())
@@ -7261,7 +7140,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 			}
 			if(!allNodesAreInWM)
 			{
-				ui_->graphViewer->updateNodeColorByValue("In WM", colors, 1, false, 1);
+				ui_->graphViewer->updatePosterior(colors, 1, 1);
 			}
 		}
 		QGraphicsRectItem * rectScaleItem = 0;
@@ -8206,12 +8085,10 @@ void DatabaseViewer::refineConstraint()
 {
 	int from = ids_.at(ui_->horizontalSlider_A->value());
 	int to = ids_.at(ui_->horizontalSlider_B->value());
-	RegistrationIcp regProximity(ui_->parameters_toolbox->getParameters());
+	refineConstraint(from, to, false);
-	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, Registration * reg, RegistrationIcp * regProximity, bool silent)
+void DatabaseViewer::refineConstraint(int from, int to, bool silent)
 {
 	UDEBUG("%d -> %d", from, to);
 	bool switchedIds = false;
@@ -8484,7 +8361,8 @@ void DatabaseViewer::refineConstraint(int from, int to, Registration * reg, Regi
 				fromScan.is2d()?Transform(0,0,fromScan.localTransform().z(),0,0,0):Transform::getIdentity()));
 
 		toS = new Signature(assembledData);
-		transform = regProximity->computeTransformationMod(*fromS, *toS, currentLink.transform(), &info);
+		RegistrationIcp registrationIcp(parameters);
+		transform = registrationIcp.computeTransformationMod(*fromS, *toS, currentLink.transform(), &info);
 		if(!transform.isNull())
 		{
 			// local scan matching proximity detection should have higher variance (see Rtabmap::process())
@@ -8502,6 +8380,7 @@ void DatabaseViewer::refineConstraint(int from, int to, Registration * reg, Regi
 		}
 
 		bool reextractVisualFeatures = uStr2Bool(parameters.at(Parameters::kRGBDLoopClosureReextractFeatures()));
+		Registration * reg = Registration::create(parameters);
 		if( reg->isScanRequired() ||
 			reg->isUserDataRequired() ||
 			reextractVisualFeatures ||
@@ -8598,6 +8477,8 @@ void DatabaseViewer::refineConstraint(int from, int to, Registration * reg, Regi
 			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());
 
@@ -8729,14 +8610,11 @@ void DatabaseViewer::addConstraint()
 {
 	int from = ids_.at(ui_->horizontalSlider_A->value());
 	int to = ids_.at(ui_->horizontalSlider_B->value());
-	std::shared_ptr<Registration> reg(Registration::create(ui_->parameters_toolbox->getParameters()));
+	addConstraint(from, to, false);
-	addConstraint(from, to, reg.get(), false);
 }
 
-bool DatabaseViewer::addConstraint(int from, int to, Registration * reg, bool silent, bool silentlyUseOptimizedGraphAsGuess)
+bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool silentlyUseOptimizedGraphAsGuess)
 {
-	UASSERT(reg);
-
 	bool switchedIds = false;
 	if(from == to)
 	{
@@ -8763,6 +8641,7 @@ bool DatabaseViewer::addConstraint(int from, int to, Registration * reg, bool si
 		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);
@@ -8944,6 +8823,7 @@ bool DatabaseViewer::addConstraint(int from, int to, Registration * reg, bool si
 		{
 			t = reg->computeTransformationMod(*fromS, *toS, guess, &info);
 		}
+		delete reg;
 		UDEBUG("");
 
 		if(!t.isNull())

guilib/src/ExportBundlerDialog.cpp

@@ -194,39 +194,17 @@ 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())));
-			std::shared_ptr<Optimizer> sba(Optimizer::create(sbaType, parametersSBA));
+			Optimizer * sba = Optimizer::create(sbaType, parametersSBA);
 			sba->getConnectedGraph(poses.begin()->first, poses, links, posesOut, linksOut);
-			// set input poses as initial optimization guess
+			poses = sba->optimizeBA(
-			for(std::map<int, Transform>::iterator iter=posesOut.begin(); iter!=posesOut.end(); ++iter)
+					posesOut.begin()->first,
-			{
+					posesOut,
-				iter->second = poses.at(iter->first);
+					linksOut,
-			}
-			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(),
+					_ui->sba_rematchFeatures->isChecked());
-					false,
+			delete sba;
-					parametersSBA);
-			}
 
 			if(poses.empty())
 			{

guilib/src/ExportCloudsDialog.cpp

@@ -151,9 +151,6 @@ 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()));
@@ -212,7 +209,6 @@ 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()));
@@ -404,8 +400,6 @@ 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());
@@ -449,7 +443,6 @@ 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());
@@ -589,8 +582,6 @@ 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());
@@ -637,7 +628,6 @@ 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());
@@ -777,8 +767,6 @@ 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);
@@ -822,7 +810,6 @@ 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);
@@ -2924,7 +2911,6 @@ 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(),
@@ -3694,30 +3680,20 @@ 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,confidence;
+					cv::Mat image,depth;
 					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,confidence;
+					cv::Mat image,depth;
 					_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())
@@ -3738,12 +3714,6 @@ 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())
 					{
@@ -3754,7 +3724,7 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
 
 					if(!depth.empty())
 					{
-						data.setRGBDImage(data.imageRaw(), depth, data.depthConfidenceRaw(), data.cameraModels());
+						data.setRGBDImage(data.imageRaw(), depth, data.cameraModels());
 					}
 
 					UASSERT(iter->first == data.id());
@@ -3779,8 +3749,7 @@ 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/GraphViewer.cpp

@@ -109,10 +109,6 @@ public:
 		_value = value;
 	}
 
-	void setToolTipInfo(const QString & info) {
-		_info = info;
-	}
-
 	void setRadius(float radius)
 	{
 		float r,p,yaw;
@@ -164,10 +160,6 @@ protected:
 		{
 			msg += QString("\n%1=%2").arg(_valueName).arg(_value);
 		}
-		if(!_info.isEmpty())
-		{
-			msg += QString("\n%1").arg(_info);
-		}
 
 		this->setToolTip(msg);
 		
@@ -189,7 +181,6 @@ private:
 	QGraphicsLineItem * _line;
 	QString _valueName;
 	float _value;
-	QString _info;
 };
 
 class NodeGPSItem: public NodeItem
@@ -531,7 +522,6 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 		}
 		iter.value()->hide();
 		iter.value()->setColor(color); // reset color
-		iter.value()->setToolTipInfo(QString());
 		iter.value()->setZValue(iter.key()<0?21:20);
 	}
 	for(QMultiMap<int, LinkItem*>::iterator iter = _linkItems.begin(); iter!=_linkItems.end(); ++iter)
@@ -568,7 +558,7 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 				item->setZValue(iter->first<0?21:20);
 				item->setColor(color);
 				item->setParentItem(_graphRoot);
-				item->show();
+				item->setVisible(_nodeVisible);
 				_nodeItems.insert(iter->first, item);
 			}
 		}
@@ -577,40 +567,33 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 	for(std::multimap<int, Link>::const_iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
 	{
 		// make the first id the smallest one
-		int idFrom = iter->second.from() < iter->second.to() ? iter->second.from() : iter->second.to();
+		int idFrom = iter->first<iter->second.to()?iter->first:iter->second.to();
-		int idTo = iter->second.from() < iter->second.to() ? iter->second.to() : iter->second.from();
+		int idTo = iter->first<iter->second.to()?iter->second.to():iter->first;
-
-		if(idFrom == idTo) {
-			continue;
-		}
 
 		std::map<int, Transform>::const_iterator jterA = poses.find(idFrom);
 		std::map<int, Transform>::const_iterator jterB = poses.find(idTo);
 		LinkItem * linkItem = 0;
 		if(jterA != poses.end() && jterB != poses.end() &&
-		   _nodeItems.contains(idFrom) && _nodeItems.contains(idTo))
+		   _nodeItems.contains(iter->first) && _nodeItems.contains(idTo))
 		{
 			const Transform & poseA = jterA->second;
 			const Transform & poseB = jterB->second;
 
 			QMultiMap<int, LinkItem*>::iterator itemIter = _linkItems.end();
-
 			if(_linkItems.contains(idFrom))
 			{
-				itemIter = _linkItems.find(idFrom);
+				itemIter = _linkItems.find(iter->first);
-				bool alreadyAdded = false;
+				while(itemIter.key() == idFrom && itemIter != _linkItems.end())
-				while(itemIter != _linkItems.end() && itemIter.key() == idFrom)
 				{
-					if(itemIter.value()->to() == idTo && itemIter.value()->isVisible())
+					if(itemIter.value()->to() == idTo && itemIter.value()->type() == iter->second.type())
 					{
-						alreadyAdded = true;
+						itemIter.value()->setPoses(poseA, poseB, _viewPlane);
+						itemIter.value()->show();
+						linkItem = itemIter.value();
 						break;
 					}
 					++itemIter;
 				}
-				if(alreadyAdded){
-					continue;
-				}
 			}
 
 			bool interSessionClosure = false;
@@ -749,7 +732,6 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 		}
 		else
 		{
-			iter.value()->setVisible(_nodeVisible);
 			++iter;
 		}
 	}
@@ -1184,19 +1166,6 @@ void GraphViewer::setCurrentGoalID(int id, const Transform & pose)
 	}
 }
 
-void GraphViewer::setNodeInfo(int id, const QString & info)
-{
-	NodeItem * node = _nodeItems.value(id, 0);
-	if(node)
-	{
-		node->setToolTipInfo(info);
-	}
-	else
-	{
-		UWARN("Node %d not found in the graph", id);
-	}
-}
-
 void GraphViewer::setLocalRadius(float radius)
 {
 	_localRadius->setRect(-radius*100, -radius*100, radius*200, radius*200);

guilib/src/ImageView.cpp

@@ -181,8 +181,7 @@ 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);
@@ -210,12 +209,9 @@ ImageView::ImageView(QWidget * parent) :
 	_showImage = _menu->addAction(tr("Show image"));
 	_showImage->setCheckable(true);
 	_showImage->setChecked(true);
-	_showImageDepth = _menu->addAction(tr("Show depth image"));
+	_showImageDepth = _menu->addAction(tr("Show image depth"));
 	_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);
@@ -266,16 +262,14 @@ ImageView::ImageView(QWidget * parent) :
 	_colorMapBlueToRed = colorMap->addAction(tr("Blue to red"));
 	_colorMapBlueToRed->setCheckable(true);
 	_colorMapBlueToRed->setChecked(false);
-	_colorMapInCameraFrame = colorMap->addAction(tr("Camera Frame"));
+	_colorMapMinRange = colorMap->addAction(tr("Min Range..."));
-	_colorMapInCameraFrame->setCheckable(true);
+	_colorMapMaxRange = colorMap->addAction(tr("Max Range..."));
-	_colorMapInCameraFrame->setChecked(true);
-	_colorMapMinRange = colorMap->addAction(tr("Min Z..."));
-	_colorMapMaxRange = colorMap->addAction(tr("Max Z..."));
 	group = new QActionGroup(this);
 	group->addAction(_colorMapWhiteToBlack);
 	group->addAction(_colorMapBlackToWhite);
 	group->addAction(_colorMapRedToBlue);
 	group->addAction(_colorMapBlueToRed);
+	group->addAction(_colorMapMaxRange);
 	_mouseTracking = _menu->addAction(tr("Show pixel depth"));
 	_mouseTracking->setCheckable(true);
 	_mouseTracking->setChecked(false);
@@ -299,7 +293,6 @@ 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());
@@ -313,7 +306,6 @@ void ImageView::saveSettings(QSettings & settings, const QString & group) const
 	settings.setValue("graphics_view_scale", this->isGraphicsViewScaled());
 	settings.setValue("graphics_view_scale_to_height", this->isGraphicsViewScaledToHeight());
 	settings.setValue("colormap", _colorMapWhiteToBlack->isChecked()?0:_colorMapBlackToWhite->isChecked()?1:_colorMapRedToBlue->isChecked()?2:3);
-	settings.setValue("colormap_camera_frame", this->isDepthColorMapInCameraFrame());
 	settings.setValue("colormap_min_range", this->getDepthColorMapMinRange());
 	settings.setValue("colormap_max_range", this->getDepthColorMapMaxRange());
 	if(!group.isEmpty())
@@ -330,7 +322,6 @@ 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());
@@ -348,7 +339,6 @@ void ImageView::loadSettings(QSettings & settings, const QString & group)
 	_colorMapBlackToWhite->setChecked(colorMap==1);
 	_colorMapRedToBlue->setChecked(colorMap==2);
 	_colorMapBlueToRed->setChecked(colorMap==3);
-	this->setDepthColorMapInCameraFrame(settings.value("colormap_camera_frame", this->isDepthColorMapInCameraFrame()).toBool());
 	this->setDepthColorMapRange(
 			settings.value("colormap_min_range", this->getDepthColorMapMinRange()).toFloat(),
 			settings.value("colormap_max_range", settings.value("colormap_range" /*backward compatibility*/, this->getDepthColorMapMaxRange())).toFloat());
@@ -373,11 +363,6 @@ bool ImageView::isImageDepthShown() const
 	return _showImageDepth->isChecked();
 }
 
-bool ImageView::isImageDepthConfidenceShown() const
-{
-	return _showImageDepthConfidence->isChecked();
-}
-
 bool ImageView::isFeaturesShown() const
 {
 	return _showFeatures->isChecked();
@@ -481,13 +466,6 @@ 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());
@@ -500,28 +478,6 @@ 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();
@@ -561,8 +517,6 @@ 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)
@@ -596,20 +550,11 @@ void ImageView::setGraphicsViewMode(bool on)
 		{
 			_imageDepthItem->setPixmap(_imageDepth);
 		}
-		else if(!_imageDepth.isNull())
+		else
 		{
 			_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())
@@ -767,20 +712,13 @@ void ImageView::setBackgroundColor(const QColor & color)
 	}
 }
 
-void ImageView::setDepthColorMapInCameraFrame(bool enabled) {
-	_colorMapInCameraFrame->setChecked(enabled);
-}
-
-bool ImageView::isDepthColorMapInCameraFrame() const {
-	return _colorMapInCameraFrame->isChecked();
-}
-
 void ImageView::setDepthColorMapRange(float min, float max)
 {
 	_depthColorMapMinRange = min;
 	_depthColorMapMaxRange = max;
 }
 
+
 void ImageView::computeScaleOffsets(const QRect & targetRect, float & scale, float & offsetX, float & offsetY) const
 {
 	scale = 1.0f;
@@ -852,8 +790,7 @@ 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);
 			}
@@ -867,10 +804,6 @@ 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())
@@ -1033,11 +966,6 @@ 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());
@@ -1060,41 +988,31 @@ void ImageView::contextMenuEvent(QContextMenuEvent * e)
 	}
 	else if(action == _colorMapBlackToWhite || action == _colorMapWhiteToBlack || action == _colorMapRedToBlue || action == _colorMapBlueToRed)
 	{
-		if(!_imageDepthCv.empty()) {
+		if(!_imageDepthCv.empty())
-			this->setImageDepth(_imageDepthCv, _imageDepthConfidenceCv);
+			this->setImageDepth(_imageDepthCv);
-		}
-		Q_EMIT configChanged();
-	}
-	else if(action == _colorMapInCameraFrame)
-	{
-		if(!_imageDepthCv.empty()) {
-			this->setImageDepth(_imageDepthCv, _imageDepthConfidenceCv);
-		}
 		Q_EMIT configChanged();
 	}
 	else if(action == _colorMapMinRange)
 	{
 		bool ok = false;
-		double value = QInputDialog::getDouble(this, tr("Set depth colormap min range"), tr("Range (m), 0=no limit"), _depthColorMapMinRange, -9999, 9999, 2, &ok);
+		double value = QInputDialog::getDouble(this, tr("Set depth colormap min range"), tr("Range (m), 0=no limit"), _depthColorMapMinRange, 0, 9999, 1, &ok);
 		if(ok)
 		{
 			this->setDepthColorMapRange(value, _depthColorMapMaxRange);
-			if(!_imageDepthCv.empty()) {
+			if(!_imageDepthCv.empty())
-				this->setImageDepth(_imageDepthCv, _imageDepthConfidenceCv);
+				this->setImageDepth(_imageDepthCv);
-			}
 			Q_EMIT configChanged();
 		}
 	}
 	else if(action == _colorMapMaxRange)
 	{
 		bool ok = false;
-		double value = QInputDialog::getDouble(this, tr("Set depth colormap max range"), tr("Range (m), 0=no limit"), _depthColorMapMaxRange, -9999, 9999, 2, &ok);
+		double value = QInputDialog::getDouble(this, tr("Set depth colormap max range"), tr("Range (m), 0=no limit"), _depthColorMapMaxRange, 0, 9999, 1, &ok);
 		if(ok)
 		{
 			this->setDepthColorMapRange(_depthColorMapMinRange, value);
-			if(!_imageDepthCv.empty()) {
+			if(!_imageDepthCv.empty())
-				this->setImageDepth(_imageDepthCv, _imageDepthConfidenceCv);
+				this->setImageDepth(_imageDepthCv);
-			}
 			Q_EMIT configChanged();
 		}
 	}
@@ -1129,7 +1047,7 @@ void ImageView::contextMenuEvent(QContextMenuEvent * e)
 		}
 	}
 
-	if(action == _showImage || action ==_showImageDepth || action ==_showImageDepthConfidence)
+	if(action == _showImage || action ==_showImageDepth)
 	{
 		this->updateOpacity();
 		Q_EMIT configChanged();
@@ -1141,16 +1059,17 @@ 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() > _imageDepthCv.cols)
+		if(_image.width() > _imageDepth.width())
 		{
-			depthScale = float(_imageDepthCv.cols) / float(_image.width());
+			depthScale = _imageDepth.width() / _image.width();
 		}
 		int ud = int(u*depthScale);
 		int vd = int(v*depthScale);
@@ -1158,47 +1077,33 @@ void ImageView::mouseMoveEvent(QMouseEvent * event)
 			ud < _imageDepthCv.cols &&
 			vd < _imageDepthCv.rows)
 		{
-			if( _showImageDepthConfidence->isChecked() &&
+			float depth = 0;
-				!_imageDepthConfidenceCv.empty() && 
+			if(_imageDepthCv.type() == CV_32FC1)
-				_imageDepthCv.size() == _imageDepthConfidenceCv.size())
 			{
-				unsigned char confidence = _imageDepthConfidenceCv.at<unsigned char>(vd, ud);
+				depth = _imageDepthCv.at<float>(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
 			{
-				float depth = 0;
+				depth = float(_imageDepthCv.at<unsigned short>(vd, ud)) / 1000.0f;
-				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);
+			cv::Point3f pt(0,0,0);
-				if(depth>0 && !_models.empty() && !_pose.isNull())
+			if(depth>0 && !_models.empty() && !_pose.isNull())
-				{
+			{
-					int subImageWidth = _imageDepthCv.cols / _models.size();
+				int subImageWidth = _imageDepthCv.cols / _models.size();
-					int subImageIndex = ud / subImageWidth;
+				int subImageIndex = ud / subImageWidth;
-					UASSERT(subImageIndex < (int)_models.size());
+				UASSERT(subImageIndex < (int)_models.size());
-					float x,y,z;
+				float x,y,z;
-					_models[subImageIndex].project(u, v, depth, x, y, z);
+				_models[subImageIndex].project(u, v, depth, x, y, z);
-					pt = cv::Point3f(x,y,z);
+				pt = cv::Point3f(x,y,z);
-					pt = util3d::transformPoint(pt, _pose*_models[subImageIndex].localTransform());
+				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
 		{
@@ -1210,31 +1115,23 @@ void ImageView::mouseMoveEvent(QMouseEvent * event)
 
 void ImageView::updateOpacity()
 {
-	if(_imageItem && (_imageDepthItem || _imageDepthConfidenceItem))
+	if(_imageItem && _imageDepthItem)
 	{
-		if(_imageItem->isVisible() && 
+		if(_imageItem->isVisible() && _imageDepthItem->isVisible())
-			((_imageDepthItem && _imageDepthItem->isVisible()) || 
-			 (_imageDepthConfidenceItem && _imageDepthConfidenceItem->isVisible())))
 		{
-			if(_imageDepthItem) {
+			QGraphicsOpacityEffect * effect = new QGraphicsOpacityEffect();
-				QGraphicsOpacityEffect * effect = new QGraphicsOpacityEffect();
+			effect->setOpacity(0.5);
-				effect->setOpacity(0.5);
+			_imageDepthItem->setGraphicsEffect(effect);
-				_imageDepthItem->setGraphicsEffect(effect);
+		}
-			}
+		else
-			if(_imageDepthConfidenceItem) {
+		{
-				QGraphicsOpacityEffect * effect = new QGraphicsOpacityEffect();
+			_imageDepthItem->setGraphicsEffect(0);
-				effect->setOpacity(0.5);
-				_imageDepthConfidenceItem->setGraphicsEffect(effect);
-			}
-			return;
 		}
 	}
-	if(_imageDepthItem) {
+	else 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)
@@ -1364,76 +1261,15 @@ void ImageView::setImage(const QImage & image, const std::vector<CameraModel> &
 	}
 }
 
-void ImageView::setImageDepth(const cv::Mat & imageDepth, const cv::Mat & imageDepthConfidence)
+void ImageView::setImageDepth(const cv::Mat & imageDepth)
 {
 	_imageDepthCv = imageDepth;
-	_imageDepthConfidenceCv = imageDepthConfidence;
+	setImageDepth(uCvMat2QImage(_imageDepthCv, true, getDepthColorMap(), _depthColorMapMinRange, _depthColorMapMaxRange));
-
-	QImage depth;
-	if(!_imageDepthCv.empty() && (_imageDepthCv.type() == CV_16UC1 || _imageDepthCv.type() == CV_32FC1)) {
-		if(_colorMapInCameraFrame->isChecked() || _models.empty() || !_models[0].isValidForProjection()) {
-			depth = uCvMat2QImage(_imageDepthCv, true, getDepthColorMap(), _depthColorMapMinRange, _depthColorMapMaxRange);
-			if(!_colorMapInCameraFrame->isChecked()) {
-				UWARN("Trying to set depth color map in base frame but the the camera model "
-					  "is not valid for projection, showing depth in camera frame instead.");
-			}
-		}
-		else {
-			// convert the depth values in height values
-			cv::Mat depthInBaseFrame = _imageDepthCv.clone();
-			int subImageWidth = _imageDepthCv.cols / _models.size();
-			if(depthInBaseFrame.type() == CV_16UC1) {
-				for(int v=0; v<depthInBaseFrame.rows; ++v){
-					unsigned short * rowPtr = depthInBaseFrame.ptr<unsigned short>(v);
-					for(int u=0; u<depthInBaseFrame.cols; ++u){
-						unsigned short & val = rowPtr[u];
-						if(val > 0) {
-							cv::Point3f pt;
-							int cameraIndex = u/subImageWidth;
-							UASSERT(cameraIndex>=0 && cameraIndex < (int)_models.size() && subImageWidth == _models[cameraIndex].imageWidth());
-							_models[cameraIndex].project(u,v,float(val)/1000.0f, pt.x, pt.y, pt.z);
-							pt = util3d::transformPoint(pt, _models[cameraIndex].localTransform());
-							val = (unsigned short)(pt.z*1000.0f);
-						}
-					}
-				}
-			}
-			else { // CV_32FC1
-				for(int v=0; v<depthInBaseFrame.rows; ++v){
-					float * rowPtr = depthInBaseFrame.ptr<float>(v);
-					for(int u=0; u<depthInBaseFrame.cols; ++u){
-						float & val = rowPtr[u];
-						if(val > 0) {
-							cv::Point3f pt;
-							int cameraIndex = u/subImageWidth;
-							UASSERT(cameraIndex>=0 && cameraIndex < (int)_models.size() && subImageWidth == _models[cameraIndex].imageWidth());
-							_models[cameraIndex].project(u,v,val, pt.x, pt.y, pt.z);
-							pt = util3d::transformPoint(pt, _models[cameraIndex].localTransform());
-							val = pt.z;
-						}
-					}
-				}
-			}
-			depth = uCvMat2QImage(depthInBaseFrame, true, getDepthColorMap(), _depthColorMapMinRange, _depthColorMapMaxRange);
-		}
-	}
-	else {
-		// right image grayscale or color
-		depth = uCvMat2QImage(_imageDepthCv, true, uCvQtDepthBlackToWhite);
-	}
-	setImageDepth(
-		depth,
-		uCvMat2QImage(_imageDepthConfidenceCv, true, getDepthColorMap()));
 }
 
-void ImageView::setImageDepth(const QImage & imageDepth, const QImage & imageDepthConfidence)
+void ImageView::setImageDepth(const QImage & imageDepth)
 {
 	_imageDepth = QPixmap::fromImage(imageDepth);
-	if(!imageDepthConfidence.isNull()) {
-		_imageDepthConfidence = QPixmap::fromImage(imageDepthConfidence);
-	} else {
-		_showImageDepthConfidence->setChecked(false);
-	}
 
 	UASSERT(_imageDepth.width() && _imageDepth.height());
 
@@ -1443,9 +1279,6 @@ void ImageView::setImageDepth(const QImage & imageDepth, const QImage & imageDep
 	{
 		// scale depth to rgb
 		_imageDepth = _imageDepth.scaled(_image.size());
-		if(!_imageDepthConfidence.isNull()) {
-			_imageDepthConfidence = _imageDepthConfidence.scaled(_image.size());
-		}
 	}
 
 	if(_graphicsView->isVisible())
@@ -1453,18 +1286,11 @@ void ImageView::setImageDepth(const QImage & imageDepth, const QImage & imageDep
 		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();
 		}
 	}
@@ -1645,12 +1471,6 @@ 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,15 +1225,13 @@ 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(
+				cloud = util3d::cloudRGBFromSensorData(*data,
-						*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);
@@ -1719,7 +1717,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(), data->depthConfidenceRaw());
+				_ui->imageView_odometry->setImageDepth(data->depthOrRightRaw());
 			}
 
 			if( odom.info().type == (int)Odometry::kTypeF2M ||
@@ -1910,8 +1908,9 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 		{
 			if(!odomT.isNull())
 			{
-				_ui->statsToolBox->updateStat("Odometry/TG_error_lin/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), odom.info().transformGroundTruth.getDistance(odomT), _preferencesDialog->isCacheSavedInFigures());
+				rtabmap::Transform diff = odom.info().transformGroundTruth.inverse()*odomT;
-				_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());
+				_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());
 			}
 
 			odom.info().transformGroundTruth.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
@@ -2344,7 +2343,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 				}
 				if(!signature.sensorData().depthOrRightRaw().empty())
 				{
-					_ui->imageView_source->setImageDepth(signature.sensorData().depthOrRightRaw(), signature.sensorData().depthConfidenceRaw());
+					_ui->imageView_source->setImageDepth(signature.sensorData().depthOrRightRaw());
 				}
 				if(img.isNull() && signature.sensorData().depthOrRightRaw().empty())
 				{
@@ -2376,7 +2375,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 				}
 				if(!loopSignature.sensorData().depthOrRightRaw().empty())
 				{
-					_ui->imageView_loopClosure->setImageDepth(loopSignature.sensorData().depthOrRightRaw(), loopSignature.sensorData().depthConfidenceRaw());
+					_ui->imageView_loopClosure->setImageDepth(loopSignature.sensorData().depthOrRightRaw());
 				}
 				if(_ui->imageView_loopClosure->sceneRect().isNull())
 				{
@@ -3719,8 +3718,7 @@ 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);
@@ -5878,11 +5876,8 @@ void MainWindow::startDetection()
 		}
 	}
 
-	if((_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcDatabase ||
+	if(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcDatabase &&
-	    _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcImages ||
+	   camera && camera->odomProvided())
-	    _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoImages ||
-	    _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRGBDImages) &&
-	    camera && camera->odomProvided())
 	{
 		odomSensor = camera;
 	}
@@ -6628,8 +6623,6 @@ 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)
 		{
@@ -6710,6 +6703,7 @@ void MainWindow::postProcessing(
 										{
 											uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "2"));
 										}
+										Registration * registration = Registration::create(parameters);
 
 										if(reextractFeatures)
 										{
@@ -6741,6 +6735,7 @@ 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
@@ -7028,13 +7023,7 @@ 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(
+		std::map<int, Transform>  newPoses = sbaOptimizer->optimizeBA(optimizedPoses.begin()->first, optimizedPoses, linksOut, _cachedSignatures.toStdMap(), sbaRematchFeatures);
-			optimizedPoses.begin()->first,
-			optimizedPoses,
-			linksOut,
-			_cachedSignatures.toStdMap(),
-			sbaRematchFeatures,
-			parametersSBA);
 		delete sbaOptimizer;
 		if(newPoses.size())
 		{
@@ -8192,12 +8181,6 @@ 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)
@@ -8256,14 +8239,7 @@ 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());
-				if(data.depthConfidenceRaw().empty()) {
+				info = tr("Saved rgb/%1.%2 and depth/%1.png.").arg(id).arg(ext);
-					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())
 			{
@@ -8693,13 +8669,11 @@ void MainWindow::changeState(MainWindow::State newState)
 
 		if(_sensorCapture)
 		{
+			_sensorCapture->start();
 			if(_imuThread)
 			{
 				_imuThread->start();
-				// give imu thread a head start
-				uSleep(10);
 			}
-			_sensorCapture->start();
 			ULogger::setTreadIdFilter(_preferencesDialog->getGeneralLoggerThreads());
 		}
 		break;
@@ -8731,13 +8705,11 @@ void MainWindow::changeState(MainWindow::State newState)
 
 			if(_sensorCapture)
 			{
+				_sensorCapture->start();
 				if(_imuThread)
 				{
 					_imuThread->start();
-					// give imu thread a head start
-					uSleep(10);
 				}
-				_sensorCapture->start();
 				ULogger::setTreadIdFilter(_preferencesDialog->getGeneralLoggerThreads());
 			}
 		}

guilib/src/PreferencesDialog.cpp

@@ -551,10 +551,6 @@ 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;
@@ -626,7 +622,6 @@ 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()));
@@ -1261,7 +1256,6 @@ 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());
@@ -2035,7 +2029,6 @@ 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);
@@ -2554,7 +2547,6 @@ 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());
@@ -3163,7 +3155,6 @@ 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());
@@ -6113,11 +6104,6 @@ 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>307</width>
+           <width>419</width>
            <height>389</height>
           </rect>
          </property>
@@ -392,7 +392,7 @@
           <rect>
            <x>0</x>
            <y>0</y>
-           <width>306</width>
+           <width>418</width>
            <height>389</height>
           </rect>
          </property>
@@ -1643,7 +1643,7 @@
      <item>
       <widget class="QToolBox" name="toolBox">
        <property name="currentIndex">
-        <number>1</number>
+        <number>2</number>
        </property>
        <widget class="QWidget" name="page_3">
         <property name="geometry">
@@ -1805,9 +1805,9 @@
         <property name="geometry">
          <rect>
           <x>0</x>
-          <y>-316</y>
+          <y>0</y>
-          <width>518</width>
+          <width>276</width>
-          <height>1071</height>
+          <height>1201</height>
          </rect>
         </property>
         <attribute name="label">
@@ -1816,57 +1816,24 @@
         <layout class="QVBoxLayout" name="verticalLayout_16">
          <item>
           <layout class="QGridLayout" name="gridLayout_9" columnstretch="0,0">
-           <item row="3" column="1">
+           <item row="6" column="1">
-            <widget class="QLabel" name="label_octomap_empty_3">
+            <widget class="QLabel" name="label_53">
              <property name="text">
-              <string>Ground cell color</string>
+              <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>
              </property>
              <property name="wordWrap">
               <bool>true</bool>
              </property>
             </widget>
            </item>
-           <item row="14" column="1">
+           <item row="15" column="0">
-            <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>
@@ -1876,13 +1843,13 @@
              </property>
             </widget>
            </item>
-           <item row="20" column="1">
+           <item row="16" column="0">
-            <widget class="QLabel" name="label_logger_level_2">
+            <widget class="QCheckBox" name="checkBox_grid_showProbMap">
              <property name="text">
-              <string>For stereo data, show disparity instead of right image in main views.</string>
+              <string/>
              </property>
-             <property name="wordWrap">
+             <property name="checked">
-              <bool>true</bool>
+              <bool>false</bool>
              </property>
             </widget>
            </item>
@@ -1902,17 +1869,33 @@
              </property>
             </widget>
            </item>
-           <item row="5" column="0">
+           <item row="10" column="0">
-            <layout class="QHBoxLayout" name="horizontalLayout_17">
+            <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>
-              <widget class="QToolButton" name="toolButton_frontierColor">
+              <widget class="QToolButton" name="toolButton_groundColor">
                <property name="text">
                 <string>...</string>
                </property>
               </widget>
              </item>
              <item>
-              <widget class="QLineEdit" name="lineEdit_frontierColor">
+              <widget class="QLineEdit" name="lineEdit_groundColor">
                <property name="readOnly">
                 <bool>true</bool>
                </property>
@@ -1920,18 +1903,18 @@
              </item>
             </layout>
            </item>
-           <item row="17" column="1">
+           <item row="9" column="1">
-            <widget class="QLabel" name="label_57">
+            <widget class="QLabel" name="label_octomap_cubes">
              <property name="text">
-              <string>Crop radius when filtering empty space from 2d occupancy grid.</string>
+              <string>OctoMap: Rendering type</string>
              </property>
              <property name="wordWrap">
               <bool>true</bool>
              </property>
             </widget>
            </item>
-           <item row="18" column="0">
+           <item row="1" column="0">
-            <widget class="QCheckBox" name="checkBox_grid_showProbMap">
+            <widget class="QCheckBox" name="checkBox_grid_frontiers">
              <property name="text">
               <string/>
              </property>
@@ -1940,20 +1923,10 @@
              </property>
             </widget>
            </item>
-           <item row="19" column="1">
+           <item row="14" column="1">
-            <widget class="QLabel" name="label_60">
+            <widget class="QLabel" name="label_54">
              <property name="text">
-              <string>Create RGB-D cloud from RGB projection on scan.</string>
+              <string>Voxel size (for clouds and scans)</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>
@@ -1970,13 +1943,20 @@
              </property>
             </widget>
            </item>
-           <item row="9" column="1">
+           <item row="11" column="0">
-            <widget class="QLabel" name="label_octomap_cubes">
+            <widget class="QCheckBox" name="checkBox_grid_regenerateFromSavedGrid">
              <property name="text">
-              <string>OctoMap: Rendering type</string>
+              <string/>
              </property>
-             <property name="wordWrap">
+             <property name="checked">
-              <bool>true</bool>
+              <bool>false</bool>
+             </property>
+            </widget>
+           </item>
+           <item row="6" column="0">
+            <widget class="QCheckBox" name="checkBox_octomap">
+             <property name="text">
+              <string/>
              </property>
             </widget>
            </item>
@@ -2000,8 +1980,8 @@
              </property>
             </widget>
            </item>
-           <item row="1" column="0">
+           <item row="18" column="0">
-            <widget class="QCheckBox" name="checkBox_grid_frontiers">
+            <widget class="QCheckBox" name="checkBox_showDisparityInsteadOfRight">
              <property name="text">
               <string/>
              </property>
@@ -2010,85 +1990,33 @@
              </property>
             </widget>
            </item>
-           <item row="19" column="0">
-            <widget class="QCheckBox" name="checkBox_cameraProjection">
-             <property name="text">
-              <string/>
-             </property>
-             <property name="checked">
-              <bool>false</bool>
-             </property>
-            </widget>
-           </item>
-           <item row="0" column="1">
-            <widget class="QLabel" name="label_octomap_empty">
-             <property name="text">
-              <string>Show empty space</string>
-             </property>
-             <property name="wordWrap">
-              <bool>true</bool>
-             </property>
-            </widget>
-           </item>
-           <item row="1" column="1">
-            <widget class="QLabel" name="label_octomap_frontiers">
-             <property name="text">
-              <string>Show frontiers</string>
-             </property>
-             <property name="wordWrap">
-              <bool>true</bool>
-             </property>
-            </widget>
-           </item>
-           <item row="6" column="1">
-            <widget class="QLabel" name="label_53">
-             <property name="text">
-              <string>OctoMap</string>
-             </property>
-            </widget>
-           </item>
            <item row="16" column="1">
-            <widget class="QLabel" name="label_54">
+            <widget class="QLabel" name="label_59">
              <property name="text">
-              <string>Voxel size (for clouds and scans)</string>
+              <string>Show probabilistic occupancy grid in Graph View.</string>
              </property>
              <property name="wordWrap">
               <bool>true</bool>
              </property>
             </widget>
            </item>
-           <item row="9" column="0">
+           <item row="18" column="1">
-            <widget class="QComboBox" name="comboBox_octomap_rendering_type">
+            <widget class="QLabel" name="label_logger_level_2">
-             <item>
+             <property name="text">
-              <property name="text">
+              <string>For stereo data, show disparity instead of right image in main views.</string>
-               <string>Point Cloud</string>
+             </property>
-              </property>
+             <property name="wordWrap">
-             </item>
+              <bool>true</bool>
-             <item>
+             </property>
-              <property name="text">
-               <string>Cube</string>
-              </property>
-             </item>
-             <item>
-              <property name="text">
-               <string>Volume</string>
-              </property>
-             </item>
             </widget>
            </item>
-           <item row="10" column="0">
+           <item row="3" column="1">
-            <widget class="QSpinBox" name="spinBox_grid_depth">
+            <widget class="QLabel" name="label_octomap_empty_3">
-             <property name="prefix">
+             <property name="text">
-              <string/>
+              <string>Ground cell color</string>
              </property>
-             <property name="minimum">
+             <property name="wordWrap">
-              <number>0</number>
+              <bool>true</bool>
-             </property>
-             <property name="maximum">
-              <number>16</number>
-             </property>
-             <property name="value">
-              <number>16</number>
              </property>
             </widget>
            </item>
@@ -2114,16 +2042,6 @@
              </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">
@@ -2134,91 +2052,6 @@
              </property>
             </widget>
            </item>
-           <item row="13" column="1">
-            <widget class="QLabel" name="label_56">
-             <property name="text">
-              <string>Decimation (for images)</string>
-             </property>
-             <property name="wordWrap">
-              <bool>true</bool>
-             </property>
-            </widget>
-           </item>
-           <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>
-             </property>
-             <property name="decimals">
-              <number>3</number>
-             </property>
-             <property name="minimum">
-              <double>0.000000000000000</double>
-             </property>
-             <property name="maximum">
-              <double>99.000000000000000</double>
-             </property>
-             <property name="singleStep">
-              <double>0.010000000000000</double>
-             </property>
-             <property name="value">
-              <double>0.000000000000000</double>
-             </property>
-            </widget>
-           </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="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>
@@ -2237,29 +2070,126 @@
              </item>
             </layout>
            </item>
-           <item row="15" column="1">
+           <item row="2" column="0">
-            <widget class="QLabel" name="label_62">
+            <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">
+            <widget class="QCheckBox" name="checkBox_cameraProjection">
              <property name="text">
-              <string>Depth's edge bleeding filtering error.</string>
+              <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="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>
              </property>
             </widget>
            </item>
-           <item row="15" column="0">
+           <item row="1" column="1">
-            <widget class="QDoubleSpinBox" name="doubleSpinBox_depthEdgeBleedingError">
+            <widget class="QLabel" name="label_octomap_frontiers">
+             <property name="text">
+              <string>Show frontiers</string>
+             </property>
+             <property name="wordWrap">
+              <bool>true</bool>
+             </property>
+            </widget>
+           </item>
+           <item row="17" column="1">
+            <widget class="QLabel" name="label_60">
+             <property name="text">
+              <string>Create RGB-D cloud from RGB projection on scan.</string>
+             </property>
+             <property name="wordWrap">
+              <bool>true</bool>
+             </property>
+            </widget>
+           </item>
+           <item row="0" column="1">
+            <widget class="QLabel" name="label_octomap_empty">
+             <property name="text">
+              <string>Show empty space</string>
+             </property>
+             <property name="wordWrap">
+              <bool>true</bool>
+             </property>
+            </widget>
+           </item>
+           <item row="9" column="0">
+            <widget class="QComboBox" name="comboBox_octomap_rendering_type">
+             <item>
+              <property name="text">
+               <string>Point Cloud</string>
+              </property>
+             </item>
+             <item>
+              <property name="text">
+               <string>Cube</string>
+              </property>
+             </item>
+             <item>
+              <property name="text">
+               <string>Volume</string>
+              </property>
+             </item>
+            </widget>
+           </item>
+           <item row="13" column="1">
+            <widget class="QLabel" name="label_56">
+             <property name="text">
+              <string>Decimation (for images)</string>
+             </property>
+             <property name="wordWrap">
+              <bool>true</bool>
+             </property>
+            </widget>
+           </item>
+           <item row="14" column="0">
+            <widget class="QDoubleSpinBox" name="doubleSpinBox_voxelSize">
              <property name="suffix">
               <string> m</string>
              </property>
              <property name="decimals">
-              <number>2</number>
+              <number>3</number>
              </property>
              <property name="minimum">
               <double>0.000000000000000</double>
              </property>
              <property name="maximum">
-              <double>10.000000000000000</double>
+              <double>99.000000000000000</double>
              </property>
              <property name="singleStep">
               <double>0.010000000000000</double>
@@ -2269,22 +2199,34 @@
              </property>
             </widget>
            </item>
-           <item row="14" column="0">
+           <item row="5" column="1">
-            <widget class="QSpinBox" name="spinBox_depthConfidence">
+            <widget class="QLabel" name="label_octomap_frontiers_3">
-             <property name="suffix">
+             <property name="text">
-              <string> %</string>
+              <string>Frontier cell color</string>
              </property>
-             <property name="minimum">
+             <property name="wordWrap">
-              <number>0</number>
+              <bool>true</bool>
-             </property>
-             <property name="maximum">
-              <number>100</number>
-             </property>
-             <property name="value">
-              <number>0</number>
              </property>
             </widget>
            </item>
+           <item row="5" column="0">
+            <layout class="QHBoxLayout" name="horizontalLayout_17">
+             <item>
+              <widget class="QToolButton" name="toolButton_frontierColor">
+               <property name="text">
+                <string>...</string>
+               </property>
+              </widget>
+             </item>
+             <item>
+              <widget class="QLineEdit" name="lineEdit_frontierColor">
+               <property name="readOnly">
+                <bool>true</bool>
+               </property>
+              </widget>
+             </item>
+            </layout>
+           </item>
           </layout>
          </item>
          <item>
@@ -2526,7 +2468,7 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>296</width>
+          <width>294</width>
           <height>272</height>
          </rect>
         </property>
@@ -2700,8 +2642,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>428</width>
+          <width>181</width>
-          <height>196</height>
+          <height>485</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>0</number>
+           <number>1</number>
           </property>
           <property name="maximum">
            <number>999999</number>