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": {
-      "dockerfile": "Dockerfile"
-    },
+    "image": "introlab3it/rtabmap:android-deps",
     "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/focal/Dockerfile

@@ -1,21 +0,0 @@
-FROM introlab3it/rtabmap:focal-deps
-
-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 mkdir -p /home/${USERNAME}/Documents/RTAB-Map && chown -R ${USERNAME} /home/${USERNAME}
-
-RUN echo "source /usr/share/bash-completion/completions/git" >> /home/${USERNAME}/.bashrc
-
-

.devcontainer/focal/devcontainer.json

@@ -1,18 +1,8 @@
 {
-    "build": {
-      "dockerfile": "Dockerfile"
-    },
+    "image": "introlab3it/rtabmap:20.04",
     "customizations": {
         "vscode": {
           "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools"]
         }
-      },
-    "workspaceMount": "source=${localWorkspaceFolder},target=/home/vscode/rtabmap,type=bind",
-    "workspaceFolder": "/home/vscode/rtabmap",
-    //"mounts": ["source=${localEnv:HOME}/Documents/RTAB-Map,target=/home/vscode/Documents/RTAB-Map,type=bind,consistency=cached"],
-    "settings": {
-      "terminal.integrated.defaultProfile.linux": "bash"
-    },
-    "remoteUser": "vscode",
-    "runArgs": ["--privileged", "--network=host"]
+      }
 }

.devcontainer/jammy/Dockerfile

@@ -1,21 +0,0 @@
-FROM introlab3it/rtabmap:jammy-deps
-
-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 mkdir -p /home/${USERNAME}/Documents/RTAB-Map && chown -R ${USERNAME} /home/${USERNAME}
-
-RUN echo "source /usr/share/bash-completion/completions/git" >> /home/${USERNAME}/.bashrc
-
-

.devcontainer/jammy/devcontainer.json

@@ -1,18 +1,8 @@
 {
-    "build": {
-      "dockerfile": "Dockerfile"
-    },
+    "image": "introlab3it/rtabmap:22.04",
     "customizations": {
         "vscode": {
           "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools"]
         }
-      },
-    "workspaceMount": "source=${localWorkspaceFolder},target=/home/vscode/rtabmap,type=bind",
-    "workspaceFolder": "/home/vscode/rtabmap",
-    //"mounts": ["source=${localEnv:HOME}/Documents/RTAB-Map,target=/home/vscode/Documents/RTAB-Map,type=bind,consistency=cached"],
-    "settings": {
-      "terminal.integrated.defaultProfile.linux": "bash"
-    },
-    "remoteUser": "vscode",
-    "runArgs": ["--privileged", "--network=host"]
+      }
 }

.devcontainer/latest-deps-from-source/Dockerfile

@@ -1,104 +0,0 @@
-ARG ROS_DISTRO=jazzy
-
-FROM osrf/ros:${ROS_DISTRO}-desktop
-
-# Install build dependencies with Qt6 (issue: rtabmap has black window)
-#RUN apt-get update && \
-#    apt-get install -y git software-properties-common ros-${ROS_DISTRO}-rtabmap-ros libqt6* qt6* qml6* && \
-#    apt-get remove -y ros-${ROS_DISTRO}-rtabmap* ros-${ROS_DISTRO}-gtsam ros-${ROS_DISTRO}-libg2o libpcl* libqt5* qt5* libvtk* libopencv* && \
-#    apt-get clean && rm -rf /var/lib/apt/lists/
-
-# Install build dependencies with Qt5
-RUN apt-get update && \
-    apt-get install -y git software-properties-common ros-${ROS_DISTRO}-rtabmap-ros && \
-    apt-get remove -y ros-${ROS_DISTRO}-rtabmap* ros-${ROS_DISTRO}-gtsam ros-${ROS_DISTRO}-libg2o libpcl* libvtk* libopencv* && \
-    apt-get clean && rm -rf /var/lib/apt/lists/
-
-# 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 mkdir -p /home/${USERNAME}/Documents/RTAB-Map
-
-RUN echo "source /usr/share/bash-completion/completions/git" >> /home/${USERNAME}/.bashrc
-
-WORKDIR /home/${USERNAME}/
-
-RUN rm /bin/sh && ln -s /bin/bash /bin/sh
-
-# We build main dependencies from source, cleaning up the
-# build directory after install but keeping the source code
-# to change version/reinstall from inside the dev container
-# if needed.
-
-# Build latest VTK with Qt6
-RUN git clone https://github.com/Kitware/VTK.git
-RUN cd VTK && \
-    mkdir build && \
-    cd build && \
-    cmake -DVTK_GROUP_ENABLE_Qt=YES .. && \
-    make -j$(nproc) && \
-    make install && \
-    make clean
-    
-# Build latest PCL with latest VTK
-# Make sure all libraries depending on Eigen are built with same CXX standard (17)
-RUN git clone https://github.com/PointCloudLibrary/pcl.git
-RUN cd pcl && \
-    mkdir build && \
-    cd build && \
-    cmake -DCMAKE_CXX_STANDARD=17 -DBUILD_tools=ON -DPCL_ENABLE_AVX=OFF -DPCL_ENABLE_MARCHNATIVE=OFF -DPCL_ENABLE_SSE=OFF .. && \
-    make -j$(nproc) && \
-    make install && \
-    make clean
-    
-# Build latest OpenCV
-RUN git clone https://github.com/opencv/opencv.git
-RUN git clone https://github.com/opencv/opencv_contrib.git
-RUN cd opencv && \
-    mkdir build && \
-    cd build && \
-    cmake -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=../../opencv_contrib/modules .. && \
-    make -j$(nproc) && \
-    make install && \
-    make clean
-
-# Build latest gtsam
-RUN git clone https://github.com/borglab/gtsam.git
-RUN cd gtsam && \
-    mkdir build && \
-    cd build && \
-    cmake -DCMAKE_CXX_STANDARD=17 -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_STATIC_LIBRARY=OFF -DGTSAM_BUILD_UNSTABLE=OFF -DGTSAM_INSTALL_CPPUNILITE=OFF -DGTSAM_USE_SYSTEM_EIGEN=ON -DCMAKE_BUILD_TYPE=Release .. && \
-    make -j$(nproc) && \
-    make install && \
-    make clean
-
-# Build latest g2o
-RUN git clone https://github.com/RainerKuemmerle/g2o.git
-RUN cd g2o && \
-    mkdir build && \
-    cd build && \
-    cmake -DCMAKE_CXX_STANDARD=17 -DBUILD_WITH_MARCH_NATIVE=OFF -DG2O_BUILD_APPS=OFF -DG2O_BUILD_EXAMPLES=OFF -DG2O_USE_OPENGL=OFF -DCMAKE_BUILD_TYPE=Release .. && \
-    make -j$(nproc) && \
-    make install && \
-    make clean
-
-# ros2 seems not sourcing by default its multi-arch folders
-ENV LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/opt/ros/${ROS_DISTRO}/lib/x86_64-linux-gnu
-
-RUN ldconfig
-
-RUN chown -R ${USERNAME} /home/${USERNAME}

.devcontainer/latest-deps-from-source/devcontainer.json

@@ -1,21 +0,0 @@
-{
-    "build": {
-      "dockerfile": "Dockerfile",
-      "args": {
-        "ROS_DISTRO": "jazzy"
-      }
-    },
-    "customizations": {
-        "vscode": {
-          "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools"]
-        }
-      },
-    "workspaceMount": "source=${localWorkspaceFolder},target=/home/vscode/rtabmap,type=bind",
-    "workspaceFolder": "/home/vscode/rtabmap",
-    //"mounts": ["source=${localEnv:HOME}/Documents/RTAB-Map,target=/home/vscode/Documents/RTAB-Map,type=bind,consistency=cached"],
-    "settings": {
-      "terminal.integrated.defaultProfile.linux": "bash"
-    },
-    "remoteUser": "vscode",
-    "runArgs": ["--privileged", "--network=host"]
-}

.devcontainer/noble/Dockerfile

@@ -1,25 +0,0 @@
-FROM introlab3it/rtabmap:noble-deps
-
-# 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 mkdir -p /home/${USERNAME}/Documents/RTAB-Map && chown -R ${USERNAME} /home/${USERNAME}
-
-RUN echo "source /usr/share/bash-completion/completions/git" >> /home/${USERNAME}/.bashrc
-
-

.devcontainer/noble/devcontainer.json

@@ -1,18 +1,8 @@
 {
-    "build": {
-      "dockerfile": "Dockerfile"
-    },
+    "image": "introlab3it/rtabmap:24.04",
     "customizations": {
         "vscode": {
           "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools"]
         }
-      },
-    "workspaceMount": "source=${localWorkspaceFolder},target=/home/vscode/rtabmap,type=bind",
-    "workspaceFolder": "/home/vscode/rtabmap",
-    //"mounts": ["source=${localEnv:HOME}/Documents/RTAB-Map,target=/home/vscode/Documents/RTAB-Map,type=bind,consistency=cached"],
-    "settings": {
-      "terminal.integrated.defaultProfile.linux": "bash"
-    },
-    "remoteUser": "vscode",
-    "runArgs": ["--privileged", "--network=host"]
+      }
 }

.devcontainer/rolling/Dockerfile

@@ -1,79 +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 mkdir -p /home/${USERNAME}/Documents/RTAB-Map && chown -R ${USERNAME} /home/${USERNAME}
-
-RUN echo "source /usr/share/bash-completion/completions/git" >> /home/${USERNAME}/.bashrc
-

.devcontainer/rolling/devcontainer.json

@@ -1,18 +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",
-    //"mounts": ["source=${localEnv:HOME}/Documents/RTAB-Map,target=/home/vscode/Documents/RTAB-Map,type=bind,consistency=cached"],
-    "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: 
-      - master
+      - noetic-devel
   pull_request:
     branches: 
       - '**'
@@ -23,29 +23,12 @@ jobs:
     strategy:
       fail-fast: false
       matrix:
-        ros_distribution: [ humble, jazzy, kilted, rolling ]
+        ros_distribution: [ noetic ]
         include:
-        - ros_distribution: 'humble'
-          os: ubuntu-22.04
-        - ros_distribution: 'jazzy'
-          os: ubuntu-24.04
-        - ros_distribution: 'kilted'
-          os: ubuntu-24.04
-        - ros_distribution: 'rolling'
-          os: ubuntu-24.04
+        - ros_distribution: 'noetic'
+          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 23)
-SET(RTABMAP_PATCH_VERSION 2)
+SET(RTABMAP_MINOR_VERSION 21)
+SET(RTABMAP_PATCH_VERSION 13)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   
@@ -203,11 +203,10 @@ option(WITH_ZED           "Include ZED sdk support"              ON)
 option(WITH_ZEDOC         "Include ZED Open Capture support"     ON)
 option(WITH_REALSENSE     "Include RealSense support"            ON)
 option(WITH_REALSENSE_SLAM "Include RealSenseSlam support"       ON)
-option(WITH_REALSENSE2    "Include RealSense2 support"           ON)
+option(WITH_REALSENSE2    "Include RealSense support"            ON)
 option(WITH_MYNTEYE       "Include mynteye-s support"            ON)
 option(WITH_DEPTHAI       "Include depthai-core support"         OFF)
 option(WITH_XVSDK         "Include XVisio SDK support"           OFF)
-option(WITH_ORBBEC_SDK    "Include Orbbec SDK v2 support"        OFF)
 option(WITH_OCTOMAP       "Include OctoMap support"              ON)
 option(WITH_GRIDMAP       "Include GridMap support"              OFF)
 option(WITH_CPUTSDF       "Include CPUTSDF support"              OFF)
@@ -219,9 +218,8 @@ option(WITH_DVO           "Include DVO support"                  OFF)
 option(WITH_ORB_SLAM      "Include ORB_SLAM2 or ORB_SLAM3  support" OFF)
 option(WITH_OKVIS         "Include OKVIS support"                OFF)
 option(WITH_MSCKF_VIO     "Include MSCKF_VIO support"            OFF)
-option(WITH_VINS_FUSION   "Include VINS-Fusion support"          OFF)
+option(WITH_VINS          "Include VINS-Fusion support"          OFF)
 option(WITH_OPENVINS      "Include OpenVINS support"             OFF)
-option(WITH_CUVSLAM       "Include cuVSLAM support"              OFF)
 option(WITH_MADGWICK      "Include Madgwick IMU filtering support" ON)
 option(WITH_FASTCV        "Include FastCV support"               ON)
 option(WITH_OPENMP        "Include OpenMP support"               ON)
@@ -398,7 +396,7 @@ IF(NOT VTK_FOUND)
 ENDIF(NOT VTK_FOUND)
 
 IF(WITH_TORCH)
-    FIND_PACKAGE(Torch)
+    FIND_PACKAGE(Torch QUIET)
     IF(TORCH_FOUND)
         MESSAGE(STATUS "Found Torch: ${TORCH_INCLUDE_DIRS}")
     ENDIF(TORCH_FOUND)
@@ -420,14 +418,14 @@ IF(WITH_PDAL)
 ENDIF(WITH_PDAL)
 
 IF(WITH_LIBLAS)
-    FIND_PACKAGE(libLAS)
+    FIND_PACKAGE(libLAS QUIET)
     IF(libLAS_FOUND)
        MESSAGE(STATUS "Found libLAS ${libLAS_VERSION}: ${libLAS_INCLUDE_DIRS}")
     ENDIF(libLAS_FOUND)
 ENDIF(WITH_LIBLAS)
 
 IF(WITH_CUDASIFT)
-    FIND_PACKAGE(CudaSift 3)
+    FIND_PACKAGE(CudaSift 3 QUIET)
     IF(CudaSift_FOUND)
        MESSAGE(STATUS "Found CudaSift")
     ENDIF(CudaSift_FOUND)
@@ -498,30 +496,22 @@ ENDIF(WITH_DC1394)
 IF(WITH_G2O)
     FIND_PACKAGE(g2o NO_MODULE)
     IF(g2o_FOUND)
-        MESSAGE(STATUS "Found g2o (targets)")
-        SET(G2O_FOUND ${g2o_FOUND})
-        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}
-            NO_DEFAULT_PATH)
-        FILE(READ ${G2O_FACTORY_FILE} TMPTXT)
-        STRING(FIND "${TMPTXT}" "shared_ptr" matchres)
-        IF(${matchres} EQUAL -1)
-            MESSAGE(STATUS "Old g2o factory version detected without shared ptr (factory file: ${G2O_FACTORY_FILE}).")
-            SET(G2O_CPP11 2)
-        ELSE()
-            MESSAGE(STATUS "Latest g2o factory version detected with shared ptr (factory file: ${G2O_FACTORY_FILE}).")
-            SET(G2O_CPP11 1)
-        ENDIF()
-        FIND_FILE(G2O_SBA_UTILS_FILE g2o/types/sba/sba_utils.h
-            PATHS ${G2O_INCLUDES}
-            NO_DEFAULT_PATH)
-        IF(G2O_SBA_UTILS_FILE)
-            SET(G2O_WITH_SBA_UTILS 1)
-        ELSE()
-            SET(G2O_WITH_SBA_UTILS 0)
-        ENDIF()
+       MESSAGE(STATUS "Found g2o (targets)")
+         SET(G2O_FOUND ${g2o_FOUND})
+         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}
+         NO_DEFAULT_PATH)
+       FILE(READ ${G2O_FACTORY_FILE} TMPTXT)
+       STRING(FIND "${TMPTXT}" "shared_ptr" matchres)
+       IF(${matchres} EQUAL -1)
+           MESSAGE(STATUS "Old g2o factory version detected without shared ptr (factory file: ${G2O_FACTORY_FILE}).")
+           SET(G2O_CPP11 2)
+       ELSE()
+           MESSAGE(STATUS "Latest g2o factory version detected with shared ptr (factory file: ${G2O_FACTORY_FILE}).")
+           SET(G2O_CPP11 1)
+       ENDIF()
     ELSE()
         FIND_PACKAGE(G2O QUIET)
         IF(G2O_FOUND)
@@ -551,7 +541,7 @@ IF(WITH_FLYCAPTURE2)
 ENDIF(WITH_FLYCAPTURE2)
 
 IF(WITH_CVSBA)
-    FIND_PACKAGE(cvsba)
+    FIND_PACKAGE(cvsba QUIET)
     IF(cvsba_FOUND)
        MESSAGE(STATUS "Found cvsba: ${cvsba_INCLUDE_DIRS}")
     ENDIF(cvsba_FOUND)
@@ -577,14 +567,10 @@ IF(WITH_POINTMATCHER)
 ENDIF(WITH_POINTMATCHER)
 
 IF(libpointmatcher_FOUND OR GTSAM_FOUND)
-    find_package(Boost COMPONENTS thread filesystem program_options date_time REQUIRED)
-    IF(Boost_MINOR_VERSION GREATER 80)
-        find_package(Boost COMPONENTS thread filesystem program_options date_time chrono timer serialization REQUIRED)
-    ELSEIF(Boost_MINOR_VERSION GREATER 47)
+    find_package(Boost COMPONENTS thread filesystem system program_options date_time REQUIRED)
+    IF(Boost_MINOR_VERSION GREATER 47)
         find_package(Boost COMPONENTS thread filesystem system program_options date_time chrono timer serialization REQUIRED)
-    ELSE()
-        find_package(Boost COMPONENTS thread filesystem system program_options date_time REQUIRED)
-    ENDIF()
+    ENDIF(Boost_MINOR_VERSION GREATER 47)
     IF(WIN32)
         MESSAGE(STATUS "Boost_LIBRARY_DIRS=${Boost_LIBRARY_DIRS}")
         link_directories(${Boost_LIBRARY_DIRS})
@@ -592,7 +578,7 @@ IF(libpointmatcher_FOUND OR GTSAM_FOUND)
 ENDIF(libpointmatcher_FOUND OR GTSAM_FOUND)
 
 IF(WITH_CCCORELIB)
-    find_package(CCCoreLib)
+    find_package(CCCoreLib QUIET)
     IF(CCCoreLib_FOUND)
        MESSAGE(STATUS "Found CCCoreLib: ${CCCoreLib_INCLUDE_DIRS}")
     ENDIF(CCCoreLib_FOUND)
@@ -604,7 +590,7 @@ IF(WITH_OPEN3D)
     ELSE()
        # Build Open3D like this to avoid linker errors in rtabmap:
        # cmake -DBUILD_SHARED_LIBS=ON -DGLIBCXX_USE_CXX11_ABI=ON -DCMAKE_BUILD_TYPE=Release ..
-       find_package(Open3D)
+       find_package(Open3D QUIET)
        IF(Open3D_FOUND)
           MESSAGE(STATUS "Found Open3D: ${Open3DINCLUDE_DIRS}")
        ENDIF(Open3D_FOUND)
@@ -612,17 +598,17 @@ IF(WITH_OPEN3D)
 ENDIF(WITH_OPEN3D)
 
 IF(WITH_LOAM)
-    find_package(loam_velodyne)
+    find_package(loam_velodyne QUIET)
     IF(loam_velodyne_FOUND)
        MESSAGE(STATUS "Found loam_velodyne: ${loam_velodyne_INCLUDE_DIRS}")
     ENDIF(loam_velodyne_FOUND)
 ENDIF(WITH_LOAM)
 
 IF(WITH_FLOAM)
-    find_package(floam)
+    find_package(floam QUIET)
     IF(floam_FOUND)
        MESSAGE(STATUS "Found floam: ${floam_INCLUDE_DIRS}")
-       FIND_PACKAGE(Ceres REQUIRED)
+       FIND_PACKAGE(Ceres QUIET REQUIRED)
     ENDIF(floam_FOUND)
 ENDIF(WITH_FLOAM)
 
@@ -689,26 +675,19 @@ IF(WITH_MYNTEYE)
 ENDIF(WITH_MYNTEYE)
 
 IF(WITH_DEPTHAI)
-    FIND_PACKAGE(depthai 2.24)
+    FIND_PACKAGE(depthai 2.24 QUIET)
     IF(depthai_FOUND)
        MESSAGE(STATUS "Found depthai-core (targets)")
     ENDIF(depthai_FOUND)
 ENDIF(WITH_DEPTHAI)
 
 IF(WITH_XVSDK)
-    FIND_PACKAGE(xvsdk)
+    FIND_PACKAGE(xvsdk QUIET)
     IF(xvsdk_FOUND)
        MESSAGE(STATUS "Found xvsdk (targets)")
     ENDIF(xvsdk_FOUND)
 ENDIF(WITH_XVSDK)
 
-IF(WITH_ORBBEC_SDK)
-    FIND_PACKAGE(OrbbecSDK 2)
-    IF(OrbbecSDK_FOUND)
-       MESSAGE(STATUS "Found OrbbecSDK v2 (targets)")
-    ENDIF(OrbbecSDK_FOUND)
-ENDIF(WITH_ORBBEC_SDK)
-
 IF(WITH_OCTOMAP)
     FIND_PACKAGE(octomap QUIET)
     IF(octomap_FOUND)
@@ -720,35 +699,35 @@ IF(WITH_OCTOMAP)
 ENDIF(WITH_OCTOMAP)
 
 IF(WITH_GRIDMAP)
-    FIND_PACKAGE(grid_map_core)
+    FIND_PACKAGE(grid_map_core QUIET)
     IF(grid_map_core_FOUND)
        MESSAGE(STATUS "Found grid_map_core ${grid_map_core_VERSION}: ${grid_map_core_INCLUDE_DIRS}")
     ENDIF(grid_map_core_FOUND)
 ENDIF(WITH_GRIDMAP)
 
 IF(WITH_CPUTSDF)
-    FIND_PACKAGE(CPUTSDF)
+    FIND_PACKAGE(CPUTSDF QUIET)
     IF(CPUTSDF_FOUND)
        MESSAGE(STATUS "Found CPUTSDF: ${CPUTSDF_INCLUDE_DIRS}")
     ENDIF(CPUTSDF_FOUND)
 ENDIF(WITH_CPUTSDF)
 
 IF(WITH_OPENCHISEL)
-    find_package(open_chisel)
+    find_package(open_chisel QUIET)
     if(open_chisel_FOUND)
         MESSAGE(STATUS "Found open_chisel: ${open_chisel_INCLUDE_DIRS}")
     endif(open_chisel_FOUND)
 ENDIF(WITH_OPENCHISEL)
 
 IF(WITH_ALICE_VISION)
-    find_package(AliceVision CONFIG)
+    find_package(AliceVision CONFIG QUIET)
     IF(AliceVision_FOUND)
         IF(${AliceVision_VERSION} VERSION_LESS_EQUAL "2.2")
            find_package(Boost COMPONENTS log log_setup container REQUIRED)
         ENDIF(${AliceVision_VERSION} VERSION_LESS_EQUAL "2.2")
         SET(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH};/usr/local/lib/cmake/modules")
-        find_package(Geogram REQUIRED)
-        find_package(assimp)
+        find_package(Geogram REQUIRED QUIET)
+        find_package(assimp QUIET)
         add_definitions("-DRTABMAP_ALICE_VISION_MAJOR=${AliceVision_VERSION_MAJOR}")
         add_definitions("-DRTABMAP_ALICE_VISION_MINOR=${AliceVision_VERSION_MINOR}")
         add_definitions("-DRTABMAP_ALICE_VISION_PATCH=${AliceVision_VERSION_PATCH}")
@@ -756,28 +735,28 @@ IF(WITH_ALICE_VISION)
 ENDIF(WITH_ALICE_VISION)
 
 IF(WITH_FOVIS)
-    FIND_PACKAGE(libfovis)
+    FIND_PACKAGE(libfovis QUIET)
     IF(libfovis_FOUND)
        MESSAGE(STATUS "Found libfovis: ${libfovis_INCLUDE_DIRS}")
     ENDIF(libfovis_FOUND)
 ENDIF(WITH_FOVIS)
 
 IF(WITH_VISO2)
-    FIND_PACKAGE(libviso2)
+    FIND_PACKAGE(libviso2 QUIET)
     IF(libviso2_FOUND)
        MESSAGE(STATUS "Found libviso2: ${libviso2_INCLUDE_DIRS}")
     ENDIF(libviso2_FOUND)
 ENDIF(WITH_VISO2)
 
 IF(WITH_DVO)
-    FIND_PACKAGE(dvo_core)
+    FIND_PACKAGE(dvo_core QUIET)
     IF(dvo_core_FOUND)
        MESSAGE(STATUS "Found dvo_core: ${dvo_core_INCLUDE_DIRS}")
     ENDIF(dvo_core_FOUND)
 ENDIF(WITH_DVO)
 
 IF(WITH_OKVIS)
-    FIND_PACKAGE(okvis 1.1)
+    FIND_PACKAGE(okvis 1.1 QUIET)
     IF(okvis_FOUND)
        MESSAGE(STATUS "Found okvis: ${OKVIS_INCLUDE_DIRS}")
        find_package(brisk 2 REQUIRED)
@@ -792,7 +771,7 @@ ENDIF(WITH_OKVIS)
 # If built with okvis, we found already ceres above
 IF(WITH_CERES)
   IF(NOT okvis_FOUND AND NOT floam_FOUND)
-    FIND_PACKAGE(Ceres)
+    FIND_PACKAGE(Ceres QUIET)
     MESSAGE(STATUS "Found ceres ${Ceres_VERSION}: ${CERES_INCLUDE_DIRS}")
   ENDIF(NOT okvis_FOUND AND NOT floam_FOUND)
 ELSEIF(Ceres_FOUND)
@@ -800,33 +779,28 @@ ELSEIF(Ceres_FOUND)
 ENDIF()
 
 IF(WITH_MSCKF_VIO)
-    FIND_PACKAGE(msckf_vio)
+    FIND_PACKAGE(msckf_vio QUIET)
     IF(msckf_vio_FOUND)
        MESSAGE(STATUS "Found msckf_vio: ${msckf_vio_INCLUDE_DIRS}")
     ENDIF(msckf_vio_FOUND)
 ENDIF(WITH_MSCKF_VIO)
 
-IF(WITH_VINS AND NOT WITH_VINS_FUSION)
-message(DEPRECATION "The option WITH_VINS is deprecated and will be removed in a future version. Please use WITH_VINS_FUSION instead.")
-set(WITH_VINS_FUSION ON)
-ENDIF(WITH_VINS AND NOT WITH_VINS_FUSION)
-
-IF(WITH_VINS_FUSION)
-    FIND_PACKAGE(vins)
+IF(WITH_VINS)
+    FIND_PACKAGE(vins QUIET)
     IF(vins_FOUND)
-       MESSAGE(STATUS "Found vins-fusion: ${vins_INCLUDE_DIRS}")
+       MESSAGE(STATUS "Found vins: ${vins_INCLUDE_DIRS}")
        IF(okvis_FOUND)
-           MESSAGE(WARNING "VINS-Fusion and OKVIS will be both linked to project, make sure VINS-Fusion has been built with against same Ceres version than OKVIS to avoid some crashes.")
+           MESSAGE(WARNING "VINS and OKVIS will be both linked to project, make sure VINS has been built with against same Ceres version than OKVIS to avoid some crashes.")
        ENDIF(okvis_FOUND)
     ENDIF(vins_FOUND)
-ENDIF(WITH_VINS_FUSION)
+ENDIF(WITH_VINS)
 
 IF(WITH_OPENVINS)
-    FIND_PACKAGE(ov_msckf)
+    FIND_PACKAGE(ov_msckf QUIET)
     # On ROS2, the indirect includes and libraries
     # are not forwarded by ov_msckf target, append them manually
-    FIND_PACKAGE(ov_core)
-    FIND_PACKAGE(ov_init)
+    FIND_PACKAGE(ov_core QUIET)
+    FIND_PACKAGE(ov_init QUIET)
     IF(ov_msckf_FOUND AND ov_core_FOUND AND ov_init_FOUND)
       SET(ov_msckf_INCLUDE_DIRS
          ${ov_msckf_INCLUDE_DIRS}
@@ -855,19 +829,12 @@ IF(WITH_OPENGV)
 ENDIF(WITH_OPENGV)
 
 IF(WITH_ORB_SLAM AND NOT G2O_FOUND)
-    FIND_PACKAGE(ORB_SLAM)
+    FIND_PACKAGE(ORB_SLAM QUIET)
     IF(ORB_SLAM_FOUND)
        MESSAGE(STATUS "Found ORB_SLAM${ORB_SLAM_VERSION}: ${ORB_SLAM_INCLUDE_DIRS}")
     ENDIF(ORB_SLAM_FOUND)
 ENDIF(WITH_ORB_SLAM AND NOT G2O_FOUND)
 
-IF(WITH_CUVSLAM)
-    FIND_PACKAGE(CuVSLAM)
-    IF(CUVSLAM_FOUND)
-        MESSAGE(STATUS "Found cuVSLAM: ${CUVSLAM_INCLUDE_DIRS}")
-    ENDIF()
-ENDIF(WITH_CUVSLAM)
-
 SET(DISABLE_NEW_DTAGS_FLAG "--disable-new-dtags")
 IF(NOT (APPLE OR WIN32) AND BUILD_WITH_RPATH_NOT_RUNPATH)
     ADD_LINK_OPTIONS(LINKER:${DISABLE_NEW_DTAGS_FLAG})
@@ -962,14 +929,10 @@ ENDIF()
 IF(NOT G2O_FOUND)
    SET(G2O "//")
    SET(G2O_CPP_CONF "//")
-   SET(G2O_WITH_SBA_UTILS "//")
 ELSE()
    IF(NOT G2O_CPP11)
      SET(G2O_CPP_CONF "//")
    ENDIF(NOT G2O_CPP11)
-   IF(NOT G2O_WITH_SBA_UTILS)
-     SET(G2O_WITH_SBA_UTILS_CONF "//")
-   ENDIF(NOT G2O_WITH_SBA_UTILS)
 ENDIF()
 IF(NOT GTSAM_FOUND)
    SET(GTSAM "//")
@@ -1095,9 +1058,6 @@ IF(NOT xvsdk_FOUND)
 ELSE()
    SET(CONF_WITH_XVSDK 1)
 ENDIF()
-IF(NOT OrbbecSDK_FOUND)
-   SET(ORBBEC_SDK "//")
-ENDIF(NOT OrbbecSDK_FOUND)
 IF(NOT octomap_FOUND)
    SET(OCTOMAP "//")
    SET(CONF_WITH_OCTOMAP 0)
@@ -1132,14 +1092,11 @@ IF(NOT msckf_vio_FOUND)
    SET(MSCKF_VIO "//")
 ENDIF()
 IF(NOT vins_FOUND)
-   SET(VINSFUSION "//")
+   SET(VINS "//")
 ENDIF()
 IF(NOT ov_msckf_FOUND)
    SET(OPENVINS "//")
 ENDIF()
-IF(NOT CUVSLAM_FOUND)
-   SET(CUVSLAM "//")
-ENDIF()
 IF(NOT ORB_SLAM_FOUND)
    SET(ORB_SLAM "//")
 ENDIF()
@@ -1453,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)
@@ -1610,7 +1567,7 @@ ENDIF()
 
 IF(grid_map_core_FOUND)
 MESSAGE(STATUS "  With GridMap ${grid_map_core_VERSION}        = YES (License: BSD)")
-ELSEIF(NOT WITH_GRIDMAP)
+ELSEIF(NOT WITH_OCTOMAP)
 MESSAGE(STATUS "  With GridMap              = NO (WITH_GRIDMAP=OFF)")
 ELSE()
 MESSAGE(STATUS "  With GridMap              = NO (grid_map_core not found)")
@@ -1773,14 +1730,6 @@ ELSE()
 MESSAGE(STATUS "  With XVisio SDK           = NO (xvsdk not found)")
 ENDIF()
 
-IF(OrbbecSDK_FOUND)
-MESSAGE(STATUS "  With Orbbec SDK ${OrbbecSDK_VERSION}     = YES (License: MIT)")
-ELSEIF(NOT WITH_ORBBEC_SDK)
-MESSAGE(STATUS "  With Orbbec SDK           = NO (WITH_ORBBEC_SDK=OFF)")
-ELSE()
-MESSAGE(STATUS "  With Orbbec SDK           = NO (OrbbecSDK v2 not found)")
-ENDIF()
-
 MESSAGE(STATUS "")
 MESSAGE(STATUS " Odometry Approaches:")
 IF(loam_velodyne_FOUND)
@@ -1842,7 +1791,7 @@ ENDIF()
 IF(vins_FOUND)
 MESSAGE(STATUS "  With VINS-Fusion          = YES (License: GPLv3)")
 ELSEIF(NOT WITH_VINS)
-MESSAGE(STATUS "  With VINS-Fusion          = NO (WITH_VINS_FUSION=OFF)")
+MESSAGE(STATUS "  With VINS-Fusion          = NO (WITH_VINS=OFF)")
 ELSE()
 MESSAGE(STATUS "  With VINS-Fusion          = NO (VINS-Fusion not found)")
 ENDIF()
@@ -1865,14 +1814,6 @@ ELSE()
 MESSAGE(STATUS "  With ORB_SLAM             = NO (ORB_SLAM2 and ORB_SLAM3 not found, make sure environment variable ORB_SLAM_ROOT_DIR is set)")
 ENDIF()
 
-IF(CUVSLAM_FOUND)
-MESSAGE(STATUS "  With cuVSLAM              = YES (License: NVIDIA ISAAC ROS SOFTWARE LICENSE)")
-ELSEIF(NOT WITH_CUVSLAM)
-MESSAGE(STATUS "  With cuVSLAM              = NO (WITH_CUVSLAM=OFF)")
-ELSE()
-MESSAGE(STATUS "  With cuVSLAM              = NO (cuVSLAM not found, make sure cuVSLAM is installed)")
-ENDIF()
-
 MESSAGE(STATUS "Show all options with: cmake -LA | grep WITH_")
 MESSAGE(STATUS "--------------------------------------------")
 

Version.h.in

@@ -41,7 +41,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 @TORO@#define RTABMAP_TORO
 @G2O@#define RTABMAP_G2O
 @G2O_CPP_CONF@#define RTABMAP_G2O_CPP11 @G2O_CPP11@
-@G2O_WITH_SBA_UTILS_CONF@#define RTABMAP_G2O_WITH_SBA_UTILS
 @GTSAM@#define RTABMAP_GTSAM
 @CERES@#define RTABMAP_CERES
 @MRPT@#define RTABMAP_MRPT
@@ -73,7 +72,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 @MYNTEYE@#define RTABMAP_MYNTEYE
 @DEPTHAI@#define RTABMAP_DEPTHAI
 @XVSDK@#define RTABMAP_XVSDK
-@ORBBEC_SDK@#define RTABMAP_ORBBEC_SDK
 @OCTOMAP@#define RTABMAP_OCTOMAP
 @GRIDMAP@#define RTABMAP_GRIDMAP
 @CPUTSDF@#define RTABMAP_CPUTSDF
@@ -84,9 +82,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 @DVO@#define RTABMAP_DVO
 @OKVIS@#define RTABMAP_OKVIS
 @MSCKF_VIO@#define RTABMAP_MSCKF_VIO
-@VINSFUSION@#define RTABMAP_VINS_FUSION
+@VINS@#define RTABMAP_VINS
 @OPENVINS@#define RTABMAP_OPENVINS
-@CUVSLAM@#define RTABMAP_CUVSLAM
 @ORB_SLAM@#define RTABMAP_ORB_SLAM @ORB_SLAM_VERSION@
 @ORB_OCTREE@#define RTABMAP_ORB_OCTREE
 @TORCH@#define RTABMAP_TORCH

app/android/jni/CameraARCore.cpp

@@ -37,8 +37,8 @@ namespace rtabmap {
 //////////////////////////////
 // CameraARCore
 //////////////////////////////
-CameraARCore::CameraARCore(void* env, void* context, void* activity, bool depthFromMotion, float upstreamRelocalizationAccThr):
-	CameraMobile(upstreamRelocalizationAccThr),
+CameraARCore::CameraARCore(void* env, void* context, void* activity, bool depthFromMotion, bool smoothing, float 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):
-	CameraMobile(upstreamRelocalizationAccThr),
+CameraAREngine::CameraAREngine(void* env, void* context, void* activity, bool smoothing, float 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
-        if(!originOffset_.isNull())
-        {
-            dataPose_ = originOffset_ * dataPose_;
-            viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * originOffset_ *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_));
-            occlusionModel_.setLocalTransform(originOffset_ * occlusionModel_.localTransform());
-        }
+	{
+		// adjust origin
+		if(!originOffset_.isNull())
+		{
+			dataPose_ = originOffset_ * dataPose_;
+			viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * originOffset_ *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_));
+			occlusionModel_.setLocalTransform(originOffset_ * occlusionModel_.localTransform());
+		}
 
-        if(lastKnownGPS_.stamp() > 0.0 && data_.stamp()-lastKnownGPS_.stamp()<1.0)
-        {
-            data_.setGPS(lastKnownGPS_);
-        }
-        else if(lastKnownGPS_.stamp()>0.0)
-        {
-            LOGD("GPS too old (current time=%f, gps time = %f)", data_.stamp(), lastKnownGPS_.stamp());
-        }
+		if(lastKnownGPS_.stamp() > 0.0 && data_.stamp()-lastKnownGPS_.stamp()<1.0)
+		{
+			data_.setGPS(lastKnownGPS_);
+		}
+		else if(lastKnownGPS_.stamp()>0.0)
+		{
+			LOGD("GPS too old (current time=%f, gps time = %f)", data_.stamp(), lastKnownGPS_.stamp());
+		}
 
-        if(lastEnvSensors_.size())
-        {
-            data_.setEnvSensors(lastEnvSensors_);
-            lastEnvSensors_.clear();
-        }
+		if(lastEnvSensors_.size())
+		{
+			data_.setEnvSensors(lastEnvSensors_);
+			lastEnvSensors_.clear();
+		}
 
+		if(smoothing_ && !data_.depthRaw().empty())
+		{
+			//UTimer t;
+			data_.setDepthOrRightRaw(rtabmap::util2d::fastBilateralFiltering(data_.depthRaw(), bilateralFilteringSigmaS, bilateralFilteringSigmaR));
+			//LOGD("Bilateral filtering, time=%fs", t.ticks());
+		}
 
-        // Rotate image depending on the camera orientation
-        if(colorCameraToDisplayRotation_ == ROTATION_90)
-        {
-            UDEBUG("ROTATION_90");
-            cv::Mat rgb, depth, confidence;
-            cv::Mat rgbt;
-            cv::flip(data_.imageRaw(),rgb,1);
-            cv::transpose(rgb,rgbt);
-            rgb = rgbt;
-            cv::Mat deptht;
-            cv::flip(data_.depthRaw(),depth,1);
-            cv::transpose(depth,deptht);
-            depth = deptht;
-            if(!data_.depthConfidenceRaw().empty()) {
-                cv::Mat conft;
-                cv::flip(data_.depthConfidenceRaw(),confidence,1);
-                cv::transpose(confidence,conft);
-                confidence = conft;
-            }
-            CameraModel model = data_.cameraModels()[0];
-            cv::Size sizet(model.imageHeight(), model.imageWidth());
-            model = CameraModel(
-                                model.fy(),
-                                model.fx(),
-                                model.cy(),
-                                model.cx()>0?model.imageWidth()-model.cx():0,
-                                model.localTransform()*rtabmap::Transform(0,-1,0,0, 1,0,0,0, 0,0,1,0));
-            model.setImageSize(sizet);
-            data_.setRGBDImage(rgb, depth, confidence, model);
+		// Rotate image depending on the camera orientation
+		if(colorCameraToDisplayRotation_ == ROTATION_90)
+		{
+			UDEBUG("ROTATION_90");
+			cv::Mat rgb, depth;
+			cv::Mat rgbt(data_.imageRaw().cols, data_.imageRaw().rows, data_.imageRaw().type());
+			cv::flip(data_.imageRaw(),rgb,1);
+			cv::transpose(rgb,rgbt);
+			rgb = rgbt;
+			cv::Mat deptht(data_.depthRaw().cols, data_.depthRaw().rows, data_.depthRaw().type());
+			cv::flip(data_.depthRaw(),depth,1);
+			cv::transpose(depth,deptht);
+			depth = deptht;
+			CameraModel model = data_.cameraModels()[0];
+			cv::Size sizet(model.imageHeight(), model.imageWidth());
+			model = CameraModel(
+					model.fy(),
+					model.fx(),
+					model.cy(),
+					model.cx()>0?model.imageWidth()-model.cx():0,
+					model.localTransform()*rtabmap::Transform(0,-1,0,0, 1,0,0,0, 0,0,1,0));
+			model.setImageSize(sizet);
+			data_.setRGBDImage(rgb, depth, model);
 
-            std::vector<cv::KeyPoint> keypoints = data_.keypoints();
-            for(size_t i=0; i<keypoints.size(); ++i)
-            {
-                keypoints[i].pt.x = data_.keypoints()[i].pt.y;
-                keypoints[i].pt.y = rgb.rows - data_.keypoints()[i].pt.x;
-            }
-            data_.setFeatures(keypoints, data_.keypoints3D(), cv::Mat());
-        }
-        else if(colorCameraToDisplayRotation_ == ROTATION_180)
-        {
-            UDEBUG("ROTATION_180");
-            cv::Mat rgb, depth, confidence;
-            cv::flip(data_.imageRaw(),rgb,1);
-            cv::flip(rgb,rgb,0);
-            cv::flip(data_.depthOrRightRaw(),depth,1);
-            cv::flip(depth,depth,0);
-            if(!data_.depthConfidenceRaw().empty()) {
-                cv::flip(data_.depthConfidenceRaw(),confidence,1);
-                cv::flip(confidence,confidence,0);
-            }
-            CameraModel model = data_.cameraModels()[0];
-            cv::Size sizet(model.imageWidth(), model.imageHeight());
-            model = CameraModel(
-                                model.fx(),
-                                model.fy(),
-                                model.cx()>0?model.imageWidth()-model.cx():0,
-                                model.cy()>0?model.imageHeight()-model.cy():0,
-                                model.localTransform()*rtabmap::Transform(0,0,0,0,0,1,0));
-            model.setImageSize(sizet);
-            data_.setRGBDImage(rgb, depth, confidence, model);
+			std::vector<cv::KeyPoint> keypoints = data_.keypoints();
+			for(size_t i=0; i<keypoints.size(); ++i)
+			{
+				keypoints[i].pt.x = data_.keypoints()[i].pt.y;
+				keypoints[i].pt.y = rgb.rows - data_.keypoints()[i].pt.x;
+			}
+			data_.setFeatures(keypoints, data_.keypoints3D(), cv::Mat());
+		}
+		else if(colorCameraToDisplayRotation_ == ROTATION_180)
+		{
+			UDEBUG("ROTATION_180");
+			cv::Mat rgb, depth;
+			cv::flip(data_.imageRaw(),rgb,1);
+			cv::flip(rgb,rgb,0);
+			cv::flip(data_.depthOrRightRaw(),depth,1);
+			cv::flip(depth,depth,0);
+			CameraModel model = data_.cameraModels()[0];
+			cv::Size sizet(model.imageWidth(), model.imageHeight());
+			model = CameraModel(
+					model.fx(),
+					model.fy(),
+					model.cx()>0?model.imageWidth()-model.cx():0,
+					model.cy()>0?model.imageHeight()-model.cy():0,
+					model.localTransform()*rtabmap::Transform(0,0,0,0,0,1,0));
+			model.setImageSize(sizet);
+			data_.setRGBDImage(rgb, depth, model);
 
-            std::vector<cv::KeyPoint> keypoints = data_.keypoints();
-            for(size_t i=0; i<keypoints.size(); ++i)
-            {
-                keypoints[i].pt.x = rgb.cols - data_.keypoints()[i].pt.x;
-                keypoints[i].pt.y = rgb.rows - data_.keypoints()[i].pt.y;
-            }
-            data_.setFeatures(keypoints, data_.keypoints3D(), cv::Mat());
-        }
-        else if(colorCameraToDisplayRotation_ == ROTATION_270)
-        {
-            UDEBUG("ROTATION_270");
-            cv::Mat rgb, depth, confidence;
-            cv::transpose(data_.imageRaw(),rgb);
-            cv::flip(rgb,rgb,1);
-            cv::transpose(data_.depthOrRightRaw(),depth);
-            cv::flip(depth,depth,1);
-            if(!data_.depthConfidenceRaw().empty()) {
-                cv::transpose(data_.depthConfidenceRaw(),confidence);
-                cv::flip(confidence,confidence,1);
-            }
-            CameraModel model = data_.cameraModels()[0];
-            cv::Size sizet(model.imageHeight(), model.imageWidth());
-            model = CameraModel(
-                                model.fy(),
-                                model.fx(),
-                                model.cy()>0?model.imageHeight()-model.cy():0,
-                                model.cx(),
-                                model.localTransform()*rtabmap::Transform(0,1,0,0, -1,0,0,0, 0,0,1,0));
-            model.setImageSize(sizet);
-            data_.setRGBDImage(rgb, depth, confidence, model);
+			std::vector<cv::KeyPoint> keypoints = data_.keypoints();
+			for(size_t i=0; i<keypoints.size(); ++i)
+			{
+				keypoints[i].pt.x = rgb.cols - data_.keypoints()[i].pt.x;
+				keypoints[i].pt.y = rgb.rows - data_.keypoints()[i].pt.y;
+			}
+			data_.setFeatures(keypoints, data_.keypoints3D(), cv::Mat());
+		}
+		else if(colorCameraToDisplayRotation_ == ROTATION_270)
+		{
+			UDEBUG("ROTATION_270");
+			cv::Mat rgb(data_.imageRaw().cols, data_.imageRaw().rows, data_.imageRaw().type());
+			cv::transpose(data_.imageRaw(),rgb);
+			cv::flip(rgb,rgb,1);
+			cv::Mat depth(data_.depthOrRightRaw().cols, data_.depthOrRightRaw().rows, data_.depthOrRightRaw().type());
+			cv::transpose(data_.depthOrRightRaw(),depth);
+			cv::flip(depth,depth,1);
+			CameraModel model = data_.cameraModels()[0];
+			cv::Size sizet(model.imageHeight(), model.imageWidth());
+			model = CameraModel(
+					model.fy(),
+					model.fx(),
+					model.cy()>0?model.imageHeight()-model.cy():0,
+					model.cx(),
+					model.localTransform()*rtabmap::Transform(0,1,0,0, -1,0,0,0, 0,0,1,0));
+			model.setImageSize(sizet);
+			data_.setRGBDImage(rgb, depth, model);
 
-            std::vector<cv::KeyPoint> keypoints = data_.keypoints();
-            for(size_t i=0; i<keypoints.size(); ++i)
-            {
-                keypoints[i].pt.x = rgb.cols - data_.keypoints()[i].pt.y;
-                keypoints[i].pt.y = data_.keypoints()[i].pt.x;
-            }
-            data_.setFeatures(keypoints, data_.keypoints3D(), cv::Mat());
-        }
-        else
-        {
-            UDEBUG("ROTATION_0");
-        }
+			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());
+		}
 	}
 }
 

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_(100), // iOS
-#else
-		depthConfidence_(0),
-#endif
+        depthConfidence_(2),
         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>),
-        quadSample_(new pcl::PointCloud<pcl::PointXYZ>),
-        quadSamplePolygons_(2),
+		optTextureMesh_(new pcl::TextureMesh),
+		optRefId_(0),
+		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
-                {
-                    cv::resize(textures, optTexture_, cv::Size(4096, 4096), 0.0f, 0.0f, cv::INTER_AREA);
-                }
-                else {
-                    optTexture_ = textures;
-                }
+				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;
-						data.uncompressData(&tmpA, &tmpB, 0, 0, 0, 0, 0, depthConfidence_>0?&tmpC:0);
+						cv::Mat tmpA, depth;
+						data.uncompressData(&tmpA, &depth);
 
 						if(!(!data.imageRaw().empty() && !data.depthRaw().empty()) && !data.laserScanCompressed().isEmpty())
 						{
@@ -601,29 +587,7 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 							{
                                 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(), 
-									rtabmap::ParametersMap(), 
-									std::vector<float>(), 
-									depthConfidence_);
+								cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get());
 							}
 							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)
-				if(main_scene_.background_renderer_ == 0 && camera_->getTextureId() != 0 && (!visualizingMesh_ || !(rtabmapThread_ == 0 || !rtabmapThread_->isRunning())))
+                // We detect if we are in measuring mode if rtabmap is not running
+				if(main_scene_.background_renderer_ == 0 && camera_->getTextureId() != 0 && !(rtabmapThread_ == 0 || !rtabmapThread_->isRunning()))
 				{
 					main_scene_.background_renderer_ = new BackgroundRenderer();
 					main_scene_.background_renderer_->InitializeGlContent(((rtabmap::CameraMobile*)camera_)->getTextureId(), cameraDriver_ <= 2);
@@ -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;
-									data.uncompressData(&tmpA, &tmpB, 0, 0, 0, 0, 0, depthConfidence_>0?&tmpC:0);
+									cv::Mat tmpA, depth;
+									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)
-											{
-												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_);
+											cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get());
 										}
 										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]
-    if(depthConfidence_>100)
+    depthConfidence_ = value;
+    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)
-								{
-									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_);
+								cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation, maxCloudDepth_, minCloudDepth_);
 								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)
-						{
-							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_);
+						cloud = rtabmap::util3d::cloudRGBFromSensorData(data, 1, maxCloudDepth_, minCloudDepth_, indices.get());
 					}
                     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)
-							{
-								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_);
+							cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get());
 						}
                         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
-                {
-                    cv::resize(textures, optTexture_, cv::Size(4096, 4096), 0.0f, 0.0f, cv::INTER_AREA);
-                }
-                else {
-                    optTexture_ = textures;
-                }
+                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
-							// 0 = low
-							// 1 = medium
-							// 2 = high
-							// Re-scale confidence from [0,2] to [0,100]
-							cv::Mat(depthHeight, depthWidth, CV_8UC1, (void*)conf).convertTo(outputDepthConfidence, CV_8UC1, 50, 0);
+                            const unsigned char * confPtr = (const unsigned char *)conf;
+                            float * depthPtr = outputDepth.ptr<float>();
+                            int i=0;
+                            for (int y = 0; y < outputDepth.rows; ++y)
+                            {
+                                for (int x = 0; x < outputDepth.cols; ++x)
+                                {
+                                    // https://developer.apple.com/documentation/arkit/arconfidencelevel
+                                    // 0 = low
+                                    // 1 = medium
+                                    // 2 = high
+                                    if(confPtr[y*outputDepth.cols + x] < depthConfidence_)
+                                    {
+                                        depthPtr[y*outputDepth.cols + x] = 0.0f;
+                                        ++i;
+                                    }
+                                }
+                            }
                         }
                     }
                     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, outputDepthConfidence, depthK, outputDepth.size(), colorK, rgbToDepth, regConfidence);
-						outputDepthConfidence = regConfidence;
+						outputDepth = rtabmap::util2d::registerDepth(outputDepth, depthK, outputDepth.size(), colorK, rgbToDepth);
 #ifndef DISABLE_LOG
 						UDEBUG("Depth registration time: %fs", time.elapsed());
 #endif
@@ -4781,7 +4667,7 @@ void RTABMapApp::postOdometryEvent(
                         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 */,
-				4E10D89D2DE409870015926D /* librtabmap_utilite.a in Frameworks */,
-				4E10D89E2DE409A60015926D /* libopencv_xfeatures2d.a in Frameworks */,
-				4E10D89F2DE409B20015926D /* libopencv_video.a in Frameworks */,
-				4E10D8A02DE409BB0015926D /* libopencv_tracking.a in Frameworks */,
-				4E10D8A12DE409C00015926D /* libopencv_optflow.a in Frameworks */,
-				4E10D8A22DE409C70015926D /* libopencv_imgproc.a in Frameworks */,
-				4E10D8A32DE409CA0015926D /* libopencv_features2d.a in Frameworks */,
-				4E10D8A42DE409CE0015926D /* libopencv_core.a in Frameworks */,
-				4E10D8A52DE409D10015926D /* libopencv_calib3d.a in Frameworks */,
-				4E10D8A62DE409D50015926D /* libopencv_aruco.a in Frameworks */,
-				4E10D8A72DE409F40015926D /* libopencv_flann.a in Frameworks */,
-				4E10D8A82DE40A1B0015926D /* libopencv_ximgproc.a in Frameworks */,
-				4E10D8A92DE40A3A0015926D /* libopencv_photo.a in Frameworks */,
-				4E10D8AA2DE40A6C0015926D /* libopencv_objdetect.a in Frameworks */,
-				4E10D8AB2DE40A8C0015926D /* libopencv_stitching.a in Frameworks */,
-				4E10D8AC2DE40AA20015926D /* libopencv_imgcodecs.a in Frameworks */,
-				4E3525672DE4C72D00F39D8F /* libamd.a in Frameworks */,
-				4E3525642DE4C71C00F39D8F /* libsuitesparseconfig.a in Frameworks */,
-				4E3525652DE4C71C00F39D8F /* libspqr.a in Frameworks */,
-				4E35255E2DE4C71000F39D8F /* libcxsparse.a in Frameworks */,
-				4E35255F2DE4C71000F39D8F /* libcolamd.a in Frameworks */,
-				4E3525602DE4C71000F39D8F /* libcamd.a in Frameworks */,
-				4E3525612DE4C71000F39D8F /* libccolamd.a in Frameworks */,
-				4E3525592DE4C6F600F39D8F /* libcholmod.a in Frameworks */,
-				4E10D8AD2DE40E990015926D /* libg2o_core.a in Frameworks */,
-				4E3525572DE4C6E500F39D8F /* libg2o_solver_cholmod.a in Frameworks */,
-				4E3525542DE4C6E500F39D8F /* libg2o_csparse_extension.a in Frameworks */,
-				4E3525552DE4C6E500F39D8F /* libg2o_solver_csparse.a in Frameworks */,
-				4E10D8AE2DE40E990015926D /* libg2o_solver_dense.a in Frameworks */,
-				4E10D8AF2DE40E990015926D /* libg2o_solver_eigen.a in Frameworks */,
-				4E10D8B02DE40E990015926D /* libg2o_solver_pcg.a in Frameworks */,
-				4E10D8B12DE40E990015926D /* libg2o_solver_slam2d_linear.a in Frameworks */,
-				4E10D8B22DE40E990015926D /* libg2o_solver_structure_only.a in Frameworks */,
-				4E10D8B32DE40E990015926D /* libg2o_stuff.a in Frameworks */,
-				4E10D8B42DE40E990015926D /* libg2o_types_data.a in Frameworks */,
-				4E10D8B52DE40E990015926D /* libg2o_types_icp.a in Frameworks */,
-				4E10D8B62DE40E990015926D /* libg2o_types_sba.a in Frameworks */,
-				4E10D8B72DE40E990015926D /* libg2o_types_sclam2d.a in Frameworks */,
-				4E10D8B82DE40E990015926D /* libg2o_types_sim3.a in Frameworks */,
-				4E10D8B92DE40E990015926D /* libg2o_types_slam2d_addons.a in Frameworks */,
-				4E10D8BA2DE40E990015926D /* libg2o_types_slam2d.a in Frameworks */,
-				4E10D8BB2DE40E990015926D /* libg2o_types_slam3d_addons.a in Frameworks */,
-				4E10D8BC2DE40E990015926D /* libg2o_types_slam3d.a in Frameworks */,
-				4E10D8BD2DE40EC70015926D /* libpcl_common.a in Frameworks */,
-				4E10D8BE2DE40EC70015926D /* libpcl_features.a in Frameworks */,
-				4E10D8BF2DE40EC70015926D /* libpcl_filters.a in Frameworks */,
-				4E10D8C02DE40EC70015926D /* libpcl_io_ply.a in Frameworks */,
-				4E10D8C12DE40EC70015926D /* libpcl_io.a in Frameworks */,
-				4E10D8C22DE40EC70015926D /* libpcl_kdtree.a in Frameworks */,
-				4E10D8C32DE40EC70015926D /* libpcl_keypoints.a in Frameworks */,
-				4E10D8C42DE40EC70015926D /* libpcl_ml.a in Frameworks */,
-				4E10D8C52DE40EC70015926D /* libpcl_octree.a in Frameworks */,
-				4E10D8C62DE40EC70015926D /* libpcl_registration.a in Frameworks */,
-				4E10D8C72DE40EC70015926D /* libpcl_sample_consensus.a in Frameworks */,
-				4E10D8C82DE40EC70015926D /* libpcl_search.a in Frameworks */,
-				4E10D8C92DE40EC70015926D /* libpcl_segmentation.a in Frameworks */,
-				4E10D8CA2DE40EC70015926D /* libpcl_stereo.a in Frameworks */,
-				4E10D8CB2DE40EC70015926D /* libpcl_surface.a in Frameworks */,
-				4E10D8CC2DE40EE30015926D /* libgtsam_unstable.a in Frameworks */,
-				4E10D8CD2DE40EE30015926D /* libgtsam.a in Frameworks */,
-				4E10D8CE2DE40EE30015926D /* libmetis-gtsam.a in Frameworks */,
-				4E10D8CF2DE40EF90015926D /* libboost_serialization.a in Frameworks */,
-				4E10D8D02DE40F060015926D /* libboost_timer.a in Frameworks */,
-				4E10D8D42DE40F2A0015926D /* libboost_thread.a in Frameworks */,
-				4E10D8D12DE40F190015926D /* liblas_c.a in Frameworks */,
-				4E10D8D22DE40F190015926D /* liblas.a in Frameworks */,
-				4E10D8D32DE40F190015926D /* liblaszip.a in Frameworks */,
-				4E10D8E12DE4101B0015926D /* liblibjpeg-turbo.a in Frameworks */,
-				4E10D8E22DE4101B0015926D /* liblibpng.a in Frameworks */,
+				4EFD0B36259D4DE900575D88 /* libboost_filesystem.a in Frameworks */,
+				4EFD0B37259D4DE900575D88 /* libboost_program_options.a in Frameworks */,
+				4EFD0B38259D4DE900575D88 /* libboost_regex.a in Frameworks */,
+				4EFD0B39259D4DE900575D88 /* libboost_iostreams.a in Frameworks */,
+				4EFD0B3A259D4DE900575D88 /* libboost_date_time.a in Frameworks */,
+				4EFD0B3B259D4DE900575D88 /* libboost_serialization.a in Frameworks */,
+				4EFD0B3C259D4DE900575D88 /* libboost_system.a in Frameworks */,
+				4EFD0B3D259D4DE900575D88 /* libboost_signals.a in Frameworks */,
+				4EFD0B3E259D4DE900575D88 /* libboost_thread.a in Frameworks */,
+				4EFD0B3F259D4DE900575D88 /* libboost_timer.a in Frameworks */,
+				4EFD0B40259D4DE900575D88 /* libboost_chrono.a in Frameworks */,
+				4EFD0B51259D4DF100575D88 /* libpcl_stereo.a in Frameworks */,
+				4EFD0B52259D4DF100575D88 /* libpcl_registration.a in Frameworks */,
+				4EFD0B53259D4DF100575D88 /* libpcl_features.a in Frameworks */,
+				4EFD0B54259D4DF100575D88 /* libpcl_filters.a in Frameworks */,
+				4EFD0B55259D4DF100575D88 /* libpcl_sample_consensus.a in Frameworks */,
+				4EFD0B56259D4DF100575D88 /* libpcl_kdtree.a in Frameworks */,
+				4EFD0B57259D4DF100575D88 /* libpcl_search.a in Frameworks */,
+				4EFD0B58259D4DF100575D88 /* libpcl_surface.a in Frameworks */,
+				4EFD0B59259D4DF100575D88 /* libpcl_keypoints.a in Frameworks */,
+				4EFD0B5A259D4DF100575D88 /* libpcl_recognition.a in Frameworks */,
+				4EFD0B5B259D4DF100575D88 /* libpcl_octree.a in Frameworks */,
+				4EFD0B5C259D4DF100575D88 /* libpcl_io.a in Frameworks */,
+				4EFD0B5D259D4DF100575D88 /* libpcl_common.a in Frameworks */,
+				4EFD0B5E259D4DF100575D88 /* libpcl_io_ply.a in Frameworks */,
+				4EFD0B5F259D4DF100575D88 /* libpcl_ml.a in Frameworks */,
+				4EFD0B60259D4DF100575D88 /* libpcl_segmentation.a in Frameworks */,
 			);
 			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/rtabmap-0.22\"",
+					"\"$(SRCROOT)/RTABMapApp/Libraries/include/pcl-1.11\"",
+					"\"$(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/rtabmap-0.22\"",
+					"\"$(SRCROOT)/RTABMapApp/Libraries/include/pcl-1.11\"",
+					"\"$(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")!);
@@ -1915,8 +1919,6 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
             
         }))
 
-        let previousState = mState
-        
         updateState(state: .STATE_PROCESSING);
         
         present(alertView, animated: true, completion: {
@@ -1988,10 +1990,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
                             
                             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
-  tar -xzf 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_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
-  tar -xzf 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.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
-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
+#rm -r 3.3.9.tar.gz eigen-3.3.9
 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
-  git apply 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.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
-  git apply 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_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.githubusercontent.com/matlabbe/b9ccfeae8f0744b275cab23510872680/raw/6fe2ffe5ba8fba59171adbd2f38f9c3999c61f75/g2o_20241228_ios.patch -o g2o_20241228_ios.patch
-  git apply g2o_20241228_ios.patch
+  curl -L https://gist.github.com/matlabbe/b9ccfeae8f0744b275cab23510872680/raw/a58e06accba3976420d4b61341685c123193810e/g2o_ios_fix.patch -o g2o_ios_fix.patch
+  git apply g2o_ios_fix.patch
 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
-  git apply 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_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>SIFT</string>
-				<string>GFTT-FREAK</string>
-				<string>GFTT-BRIEF</string>
-				<string>GFTT-ORB</string>
-				<string>ORB-OCTREE</string>
+				<string>BRIEF</string>
+				<string>FREAK</string>
 			</array>
 			<key>Values</key>
 			<array>
-				<string>0</string>
-				<string>1</string>
-				<string>5</string>
 				<string>6</string>
-				<string>8</string>
-				<string>10</string>
+				<string>5</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>

app/src/CMakeLists.txt

@@ -34,7 +34,7 @@ ENDIF()
 IF(APPLE AND BUILD_AS_BUNDLE)
   ADD_EXECUTABLE(rtabmap_app MACOSX_BUNDLE ${SRC_FILES})
 ELSEIF(WIN32 AND BUILD_AS_BUNDLE)
-  ADD_EXECUTABLE(rtabmap_app ${SRC_FILES})
+  ADD_EXECUTABLE(rtabmap_app WIN32 ${SRC_FILES})
 ELSE()
   ADD_EXECUTABLE(rtabmap_app ${SRC_FILES})
 ENDIF()
@@ -110,33 +110,6 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
 	ENDIF(WIN32)
   ENDIF(k4a_FOUND)
   
-  IF(ZED_FOUND)
-	# Install needed zlibwapi.dll
-    IF(WIN32)
-		file(TO_CMAKE_PATH "$ENV{ZED_SDK_ROOT_DIR}" ENV_ZED_SDK_ROOT_DIR)
-		INSTALL(FILES "${ENV_ZED_SDK_ROOT_DIR}/bin/zlibwapi.dll" 
-          DESTINATION ${thirdparty_dest_dir} 
-          COMPONENT runtime)
-	ENDIF(WIN32)
-  ENDIF(ZED_FOUND)
-
-  IF(OrbbecSDK_FOUND)
-	# Install needed "extensions" folder
-    IF(WIN32)
-        find_path(OrbbecSDK_BIN_DIR NAMES OrbbecSDK.dll)
-        IF(NOT OrbbecSDK_BIN_DIR)
-            MESSAGE(FATAL_ERROR "OrbbecSDK.dll not found! Verify your PATH.")
-        ENDIF(NOT OrbbecSDK_BIN_DIR)
-		MESSAGE(FATAL "OrbbecSDK_BIN_DIR=${OrbbecSDK_BIN_DIR}")
-        INSTALL(DIRECTORY "${OrbbecSDK_BIN_DIR}/extensions" 
-          DESTINATION ${thirdparty_dest_dir} 
-          COMPONENT runtime 
-          FILES_MATCHING
-          PATTERN "*.lib" EXCLUDE
-          PATTERN "*")
-	ENDIF(WIN32)
-  ENDIF(OrbbecSDK_FOUND)
-  
   IF(Torch_FOUND)
 	# Install needed cudnn_ops_infer64_8.dll and cudnn_cnn_infer64_8.dll
 	# TODO: should be a more general way to include them if version is different

archive/2022-IlluminationInvariant/results_common.m

@@ -15,7 +15,7 @@ RAMaddOverhead = 0;
 % Inliers_ratio = 'Loop/Visual_inliers/' ./ 'Keypoint/Current_frame/words'
 % Odometry_average = 'Memory/Distance_travelled/m'(2:end) - 'Memory/Distance_travelled/m'(1:end-1)
 
-statNames = {'Loop/Odom_correction_norm/m', 'Loop/Visual_inliers/', 'Inliers_ratio_%', 'Timing/Total/ms', 'Memory/RAM_usage/MB', 'Memory/RAM_estimated/MB', 'Keypoint/Current_frame/words', 'Loop/Map_id/', 'Memory/Local_graph_size/', 'Keypoint/Dictionary_size/words', 'Loop/Distance_since_last_loc/m'}; % 'Odometry_average'
+statNames = {'Loop/Odom_correction_norm/m', 'Loop/Visual_inliers/', 'Inliers_ratio_%', 'Timing/Total/ms', 'Memory/RAM_usage/MB', 'Memory/RAM_estimated/MB', 'Keypoint/Current_frame/words', 'Loop/Map_id/', 'Memory/Local_graph_size/', 'Keypoint/Dictionary_size/words', 'Loop/Distance_since_last_loc/'}; % 'Odometry_average'
 
  
 datasets = [ 0 1 6 7 9 14 11 111 ]; % 0 1 6 7 9 12 14 11
@@ -26,7 +26,7 @@ if resultsToShow == 2
   sep = [0, 1000, 3000, 5000, 7000, 9000];
   sepName = {'17:27', '17:54', '18:27', '18:56', '19:35'};
   prefix = 'Consecutive';
-  statNames = {'Loop/Distance_since_last_loc/m', 'Distance_since_last_loc_under_50cm'};
+  statNames = {'Loop/Distance_since_last_loc/', 'Distance_since_last_loc_under_50cm'};
 endif
 
 MapsN = length(sepName);
@@ -52,7 +52,7 @@ if strcmp(statName,'Inliers_ratio_%')
 elseif strcmp(statName, 'Odometry_average')
   data = dlmread([dataDir '/' prefix num2str(datasets(d)) '-' 'Memory-Distance_travelled-m' '.txt'], '\t', 1, 0, "emptyvalue", 0);
 elseif strcmp(statName, 'Distance_since_last_loc_under_50cm')
-  data = dlmread([dataDir '/' prefix num2str(datasets(d)) '-' 'Loop-Distance_since_last_loc-m' '.txt'], '\t', 1, 0, "emptyvalue", 0);
+  data = dlmread([dataDir '/' prefix num2str(datasets(d)) '-' 'Loop-Distance_since_last_loc-' '.txt'], '\t', 1, 0, "emptyvalue", 0);
 else
   data = dlmread([dataDir '/' prefix num2str(datasets(d)) '-' statName '.txt'], '\t', 1, 0, "emptyvalue", 0);
 endif

archive/2022-IlluminationInvariant/scripts/export_stats.sh

@@ -13,7 +13,7 @@ source rtabmap_latest.bash
 
 for d in "${DETECTOR[@]}"
 do
-  rtabmap-report --export --export_prefix "Stat$d" --loc 32  Loop/Odom_correction_norm/m Loop/Visual_inliers/ Timing/Total/ms Timing/Proximity_by_space_visual/ms Timing/Likelihood_computation/ms Timing/Posterior_computation/ms TimingMem/Keypoints_detection/ms TimingMem/Descriptors_extraction/ms TimingMem/Add_new_words/ms Loop/Map_id/ Keypoint/Current_frame/words Memory/RAM_usage/MB Memory/RAM_estimated/MB Memory/Distance_travelled/m Loop/Distance_since_last_loc/m Memory/Local_graph_size/ Keypoint/Dictionary_size/words "$DATA/$d/loc"
-  rtabmap-report --export --export_prefix "Consecutive$d" --loc 32 Loop/Map_id/ Loop/Distance_since_last_loc/m "$DATA/$d/consecutive_loc"
+  rtabmap-report --export --export_prefix "Stat$d" --loc 32  Loop/Odom_correction_norm/m Loop/Visual_inliers/ Timing/Total/ms Timing/Proximity_by_space_visual/ms Timing/Likelihood_computation/ms Timing/Posterior_computation/ms TimingMem/Keypoints_detection/ms TimingMem/Descriptors_extraction/ms TimingMem/Add_new_words/ms Loop/Map_id/ Keypoint/Current_frame/words Memory/RAM_usage/MB Memory/RAM_estimated/MB Memory/Distance_travelled/m Loop/Distance_since_last_loc/ Memory/Local_graph_size/ Keypoint/Dictionary_size/words "$DATA/$d/loc"
+  rtabmap-report --export --export_prefix "Consecutive$d" --loc 32 Loop/Map_id/ Loop/Distance_since_last_loc/ "$DATA/$d/consecutive_loc"
 done
 

cmake_modules/FindCuVSLAM.cmake

@@ -1,79 +0,0 @@
-# - Find cuVSLAM library (https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_visual_slam)
-#
-#  CUVSLAM_ROOT_DIR environment variable can be set to find the library.
-#
-# It sets the following variables:
-#  CUVSLAM_FOUND         - Set to false, or undefined, if cuVSLAM isn't found.
-#  CUVSLAM_INCLUDE_DIRS  - The cuVSLAM include directory.
-#  CUVSLAM_LIBRARIES     - The cuVSLAM library to link against.
-
-find_package(CUDA REQUIRED)
-find_package(Eigen3 REQUIRED)
-
-find_path(CUVSLAM_INCLUDE_DIRS 
-    NAMES cuvslam.h
-    PATHS
-        /usr/include
-        /usr/local/include
-        /opt/cuvslam/include
-        /opt/ros/humble/share/isaac_ros_nitros/cuvslam/include
-        $ENV{CUVSLAM_ROOT}/include
-        $ENV{CUVSLAM_ROOT_DIR}/include
-)
-
-find_library(CUVSLAM_LIBRARY 
-    NAMES cuvslam
-    PATHS
-        /usr/lib
-        /usr/local/lib
-        /opt/cuvslam/lib
-        /opt/ros/humble/share/isaac_ros_nitros/cuvslam/lib
-        $ENV{CUVSLAM_ROOT}/lib
-        $ENV{CUVSLAM_ROOT_DIR}/lib
-)
-
-if(CUVSLAM_INCLUDE_DIRS AND CUVSLAM_LIBRARY)
-    set(CUVSLAM_FOUND TRUE)
-    set(CUVSLAM_LIBRARIES 
-        ${CUVSLAM_LIBRARY} 
-        ${CUDA_LIBRARIES}
-        # Eigen3 is header-only, so we don't need to link to it
-    )
-    set(CUVSLAM_INCLUDE_DIRS 
-        ${CUVSLAM_INCLUDE_DIRS} 
-        ${CUDA_INCLUDE_DIRS} 
-        ${EIGEN3_INCLUDE_DIR}
-    )
-endif()
-
-# Handle the QUIET and REQUIRED arguments
-include(FindPackageHandleStandardArgs)
-find_package_handle_standard_args(CuVSLAM
-    FOUND_VAR CUVSLAM_FOUND
-    REQUIRED_VARS CUVSLAM_LIBRARIES CUVSLAM_INCLUDE_DIRS
-    HANDLE_COMPONENTS
-)
-
-if(CUVSLAM_FOUND)
-    # Create imported target for modern CMake usage
-    if(NOT TARGET cuvslam::cuvslam)
-        add_library(cuvslam::cuvslam UNKNOWN IMPORTED)
-        set_target_properties(cuvslam::cuvslam PROPERTIES
-            IMPORTED_LOCATION "${CUVSLAM_LIBRARY}"
-            INTERFACE_INCLUDE_DIRECTORIES "${CUVSLAM_INCLUDE_DIRS}"
-            INTERFACE_LINK_LIBRARIES "${CUVSLAM_LIBRARIES};Eigen3::Eigen"
-        )
-    endif()
-    
-    # Show which cuVSLAM was found only if not quiet
-    if(NOT CUVSLAM_FIND_QUIETLY)
-        message(STATUS "Found cuVSLAM: ${CUVSLAM_LIBRARIES}")
-    endif()
-else()
-    # Fatal error if cuVSLAM is required but not found
-    if(CUVSLAM_FIND_REQUIRED)
-        message(FATAL_ERROR "Could not find cuVSLAM library")
-    endif()
-endif()
-
-mark_as_advanced(CUVSLAM_INCLUDE_DIRS CUVSLAM_LIBRARY)

cmake_modules/FindEigen3.cmake

@@ -27,10 +27,10 @@
 
 if(NOT Eigen3_FIND_VERSION)
   if(NOT Eigen3_FIND_VERSION_MAJOR)
-    set(Eigen3_FIND_VERSION_MAJOR 3)
+    set(Eigen3_FIND_VERSION_MAJOR 2)
   endif()
   if(NOT Eigen3_FIND_VERSION_MINOR)
-    set(Eigen3_FIND_VERSION_MINOR 0)
+    set(Eigen3_FIND_VERSION_MINOR 91)
   endif()
   if(NOT Eigen3_FIND_VERSION_PATCH)
     set(Eigen3_FIND_VERSION_PATCH 0)

cmake_modules/FindG2O.cmake

@@ -26,10 +26,6 @@ FIND_FILE(G2O_FACTORY_FILE g2o/core/factory.h
   PATHS ${G2O_INCLUDE_DIR}
   NO_DEFAULT_PATH)
 
-FIND_FILE(G2O_SBA_UTILS_FILE g2o/types/sba/sba_utils.h
-  PATHS ${G2O_INCLUDE_DIR}
-  NO_DEFAULT_PATH)
-
 #ifdef G2O_NUMBER_FORMAT_STR
 #define G2O_CPP11 // we assume that if G2O_NUMBER_FORMAT_STR is defined, this is the new g2o code with c++11 interface
 #endif
@@ -122,29 +118,22 @@ IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_CONFIG_FIL
 	  ${CHOLMOD_LIB})
   ENDIF(G2O_SOLVER_CHOLMOD)
 
-  FILE(READ ${G2O_FACTORY_FILE} TMPTXT)
-  STRING(FIND "${TMPTXT}" "shared_ptr" matchres)
+  FILE(READ ${G2O_CONFIG_FILE} TMPTXT)
+  STRING(FIND "${TMPTXT}" "G2O_NUMBER_FORMAT_STR" matchres)
   IF(${matchres} EQUAL -1)
-    FILE(READ ${G2O_CONFIG_FILE} TMPTXT)
-    STRING(FIND "${TMPTXT}" "G2O_NUMBER_FORMAT_STR" matchres)
-    IF(${matchres} EQUAL -1)
-      MESSAGE(STATUS "Old g2o version detected with c++03 interface (config file: ${G2O_CONFIG_FILE}).")
-      SET(G2O_CPP11 0)
-    ELSE()
-      MESSAGE(WARNING "Latest g2o version detected with c++11 interface (config file: ${G2O_CONFIG_FILE}). Make sure g2o is built with \"-DBUILD_WITH_MARCH_NATIVE=OFF\" to avoid segmentation faults caused by Eigen.")
-      MESSAGE(STATUS "Old g2o factory version detected without shared ptr (factory file: ${G2O_FACTORY_FILE}).")
-      SET(G2O_CPP11 2)
-    ENDIF()
+    MESSAGE(STATUS "Old g2o version detected with c++03 interface (config file: ${G2O_CONFIG_FILE}).")
+    SET(G2O_CPP11 0)
   ELSE()
     MESSAGE(WARNING "Latest g2o version detected with c++11 interface (config file: ${G2O_CONFIG_FILE}). Make sure g2o is built with \"-DBUILD_WITH_MARCH_NATIVE=OFF\" to avoid segmentation faults caused by Eigen.")
-    MESSAGE(STATUS "Latest g2o factory version detected with shared ptr (factory file: ${G2O_FACTORY_FILE}).")
-    SET(G2O_CPP11 1)
-  ENDIF()
-
-  IF(G2O_SBA_UTILS_FILE)
-    SET(G2O_WITH_SBA_UTILS 1)
-  ELSE()
-    SET(G2O_WITH_SBA_UTILS 0)
+    FILE(READ ${G2O_FACTORY_FILE} TMPTXT)
+    STRING(FIND "${TMPTXT}" "shared_ptr" matchres)
+    IF(${matchres} EQUAL -1)
+      MESSAGE(STATUS "Old g2o factory version detected without shared ptr (factory file: ${G2O_FACTORY_FILE}).")
+      SET(G2O_CPP11 2)
+    ELSE()
+      MESSAGE(STATUS "Latest g2o factory version detected with shared ptr (factory file: ${G2O_FACTORY_FILE}).")
+      SET(G2O_CPP11 1)
+    ENDIF()
   ENDIF()
 
   SET(G2O_FOUND "YES")

cmake_modules/MacOSXBundleInfo.plist.in

@@ -10,6 +10,8 @@
 	<string>${MACOSX_BUNDLE_INFO_STRING}</string>
 	<key>CFBundleIconFile</key>
 	<string>${MACOSX_BUNDLE_ICON_FILE}</string>
+	<key>CFBundleIdentifier</key>
+	<string>${MACOSX_BUNDLE_GUI_IDENTIFIER}</string>
 	<key>CFBundleInfoDictionaryVersion</key>
 	<string>6.0</string>
 	<key>CFBundleLongVersionString</key>
@@ -30,14 +32,6 @@
 	<true/>
 	<key>NSHumanReadableCopyright</key>
 	<string>${MACOSX_BUNDLE_COPYRIGHT}</string>
-    <key>com.apple.security.app-sandbox</key>
-    <true/>
-    <key>com.apple.security.files.downloads.read-write</key>
-    <true/>
-    <key>com.apple.security.files.downloads.read-only</key>
-    <false/>
-    <key>com.apple.security.device.camera</key>
-    <true/>
 
 <!-- File type associations -->
 <key>CFBundleDocumentTypes</key>

corelib/include/rtabmap/core/CameraRGBD.h

@@ -38,4 +38,3 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/camera/CameraRGBDImages.h>
 #include <rtabmap/core/camera/CameraK4A.h>
 #include <rtabmap/core/camera/CameraSeerSense.h>
-#include <rtabmap/core/camera/CameraOrbbecSDK.h>

corelib/include/rtabmap/core/DBDriver.h

@@ -129,7 +129,6 @@ public:
 			std::vector<std::vector<Eigen::Vector2f> > * texCoords = 0,
 #endif
 			cv::Mat * textures = 0) const;
-	void saveFlannIndex(const std::vector<unsigned char> & indexData) const;
 
 public:
 	// Mutex-protected methods of abstract versions below
@@ -162,20 +161,19 @@ public:
 	void executeNoResult(const std::string & sql) const;
 
 	// Load objects
-	void load(VWDictionary & dictionary, bool lastStateOnly = true) const;
-	void loadLastNodes(std::list<Signature *> & signatures, bool loadWordIdsOnly = false) const; // returned signatures must be freed after usage
+	void load(VWDictionary * dictionary, bool lastStateOnly = true) const;
+	void loadLastNodes(std::list<Signature *> & signatures) const; // returned signatures must be freed after usage
 	Signature * loadSignature(int id, bool * loadedFromTrash = 0); // returned signature must be freed after usage, call loadSignatures() instead if more than one signature should be loaded
-	void loadSignatures(const std::list<int> & ids, std::list<Signature *> & signatures, std::set<int> * loadedFromTrash = 0, bool loadWordIdsOnly = false); // returned signatures must be freed after usage
+	void loadSignatures(const std::list<int> & ids, std::list<Signature *> & signatures, std::set<int> * loadedFromTrash = 0); // returned signatures must be freed after usage
 	void loadWords(const std::set<int> & wordIds, std::list<VisualWord *> & vws); // returned words must be freed after usage
 
 	// Specific queries...
-	void loadNodeData(Signature & signature, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
+	void loadNodeData(Signature * signature, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
 	void loadNodeData(std::list<Signature *> & signatures, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
 	void getNodeData(int signatureId, SensorData & data, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
 	bool getCalibration(int signatureId, std::vector<CameraModel> & models, std::vector<StereoCameraModel> & stereoModels) const;
 	bool getLaserScanInfo(int signatureId, LaserScan & info) const;
 	bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const;
-	void getLocalFeatures(int signatureId, std::multimap<int, int> & words, std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> & points, cv::Mat & descriptors) const;
 	void loadLinks(int signatureId, std::multimap<int, Link> & links, Link::Type type = Link::kUndef) const;
 	void getWeight(int signatureId, int & weight) const;
 	void getLastNodeIds(std::set<int> & ids) const;
@@ -276,12 +274,11 @@ protected:
 				std::vector<std::vector<Eigen::Vector2f> > * texCoords,
 #endif
 				cv::Mat * textures) const = 0;
-	virtual void saveFlannIndexQuery(const std::vector<unsigned char> & indexData) const = 0;
 
 	// Load objects
-	virtual void loadQuery(VWDictionary & dictionary, bool lastStateOnly = true) const = 0;
-	virtual void loadLastNodesQuery(std::list<Signature *> & signatures, bool loadWordIdsOnly) const = 0;
-	virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures, bool loadWordIdsOnly) const = 0;
+	virtual void loadQuery(VWDictionary * dictionary, bool lastStateOnly = true) const = 0;
+	virtual void loadLastNodesQuery(std::list<Signature *> & signatures) const = 0;
+	virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const = 0;
 	virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const = 0;
 	virtual void loadLinksQuery(int signatureId, std::multimap<int, Link> & links, Link::Type type = Link::kUndef) const = 0;
 
@@ -289,7 +286,6 @@ protected:
 	virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, std::vector<StereoCameraModel> & stereoModels) const = 0;
 	virtual bool getLaserScanInfoQuery(int signatureId, LaserScan & info) const = 0;
 	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const = 0;
-	virtual void getLocalFeaturesQuery(int signatureId, std::multimap<int, int> & words, std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> & points, cv::Mat & descriptors) const = 0;
 	virtual void getLastNodeIdsQuery(std::set<int> & ids) const = 0;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures, bool ignoreIntermediateNodes) const = 0;
 	virtual void getAllOdomPosesQuery(std::map<int, Transform> & poses, bool ignoreChildren, bool ignoreIntermediateNodes) const = 0;

corelib/include/rtabmap/core/DBDriverSqlite3.h

@@ -135,12 +135,10 @@ protected:
 #endif
 			cv::Mat * textures) const;
 
-	virtual void saveFlannIndexQuery(const std::vector<unsigned char> & indexData) const;
-
 	// Load objects
-	virtual void loadQuery(VWDictionary & dictionary, bool lastStateOnly = true) const;
-	virtual void loadLastNodesQuery(std::list<Signature *> & signatures, bool loadWordIdsOnly) const;
-	virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures, bool loadWordIdsOnly) const;
+	virtual void loadQuery(VWDictionary * dictionary, bool lastStateOnly = true) const;
+	virtual void loadLastNodesQuery(std::list<Signature *> & signatures) const;
+	virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const;
 	virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const;
 	virtual void loadLinksQuery(int signatureId, std::multimap<int, Link> & links, Link::Type type = Link::kUndef) const;
 
@@ -148,7 +146,6 @@ protected:
 	virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, std::vector<StereoCameraModel> & stereoModels) const;
 	virtual bool getLaserScanInfoQuery(int signatureId, LaserScan & info) const;
 	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const;
-	virtual void getLocalFeaturesQuery(int signatureId, std::multimap<int, int> & words, std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> & points, cv::Mat & descriptors) const;
 	virtual void getLastNodeIdsQuery(std::set<int> & ids) const;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures, bool ignoreIntermediateNodes) const;
 	virtual void getAllOdomPosesQuery(std::map<int, Transform> & poses, bool ignoreChildren, bool ignoreIntermediateNodes) const;
@@ -193,8 +190,6 @@ private:
 			const cv::Point3f & viewpoint) const;
 
 private:
-	void loadWordsQuery(std::list<Signature *> & signatures) const;
-	void loadWordIdsQuery(std::list<Signature *> & signatures) const;
 	void loadLinksQuery(std::list<Signature *> & signatures) const;
 	int loadOrSaveDb(sqlite3 *pInMemory, const std::string & fileName, int isSave) const;
 

corelib/include/rtabmap/core/FlannIndex.h

@@ -37,48 +37,37 @@ namespace rtabmap {
 class RTABMAP_CORE_EXPORT FlannIndex
 {
 public:
-	// A forward of the internal enum, indexes should match. See src/rtflann/defines.h
-	enum flann_algorithm_t
-	{
-		FLANN_INDEX_LINEAR 			= 0,
-		FLANN_INDEX_KDTREE 			= 1,
-		FLANN_INDEX_KDTREE_SINGLE   = 4,
-		FLANN_INDEX_LSH 			= 6,
-	};
-
 	FlannIndex();
 	virtual ~FlannIndex();
 
 	void release();
-	std::vector<unsigned char> serializeIndex(bool computeChecksum = true) const;
-
 	size_t indexedFeatures() const;
 
 	// return Bytes
 	size_t memoryUsed() const;
 
 	// Note that useDistanceL1 doesn't have any effect if LSH is used
-	void buildIndex(
-			flann_algorithm_t algorithm,
+	void buildLinearIndex(
 			const cv::Mat & features,
 			bool useDistanceL1 = false,
 			float rebalancingFactor = 2.0f);
-	// Return false if the indexData doesn't correspond to expected features used and parameters.
-	bool loadIndex(
-		const std::vector<unsigned char> & indexData,
-		flann_algorithm_t algorithm,
-		const cv::Mat & features,
-		bool useDistanceL1 = false,
-		float rebalancingFactor = 2.0f,
-		std::string * errorMsg = NULL);
-	bool loadIndex(
-		const unsigned char * indexData,
-		size_t indexDataSize,
-		flann_algorithm_t algorithm,
-		const cv::Mat & features,
-		bool useDistanceL1 = false,
-		float rebalancingFactor = 2.0f,
-		std::string * errorMsg = NULL);
+	void buildKDTreeIndex(
+			const cv::Mat & features,
+			int trees = 4,
+			bool useDistanceL1 = false,
+			float rebalancingFactor = 2.0f);
+	void buildKDTreeSingleIndex(
+			const cv::Mat & features,
+			int leafMaxSize = 10,
+			bool reorder = true,
+			bool useDistanceL1 = false,
+			float rebalancingFactor = 2.0f);
+	void buildLSHIndex(
+			const cv::Mat & features,
+			unsigned int table_number = 12,
+			unsigned int key_size = 20,
+			unsigned int multi_probe_level = 2,
+			float rebalancingFactor = 2.0f);
 
 	bool isBuilt();
 
@@ -115,9 +104,9 @@ private:
 	unsigned int nextIndex_;
 	int featuresType_;
 	int featuresDim_;
+	bool isLSH_;
 	bool useDistanceL1_; // true=EUCLEDIAN_L2 false=MANHATTAN_L1
 	float rebalancingFactor_;
-	flann_algorithm_t algorithm_;
 
 	// keep feature in memory until the tree is rebuilt
 	// (in case the word is deleted when removed from the VWDictionary)

corelib/include/rtabmap/core/GlobalMap.h

@@ -53,7 +53,6 @@ public:
 public:
 	virtual ~GlobalMap();
 
-	bool fullUpdateNeeded(const std::map<int, Transform> & poses) const;
 	bool update(const std::map<int, Transform> & poses); // return true if map has changed
 
 	virtual void clear();

corelib/include/rtabmap/core/Graph.h

@@ -56,7 +56,7 @@ bool RTABMAP_CORE_EXPORT exportPoses(
 
 bool RTABMAP_CORE_EXPORT importPoses(
 		const std::string & filePath,
-		int format, // 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame, 11=10+ID), 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe, 9=EuRoC MAV, 12=rgbd_bonn
+		int format, // 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame, 11=10+ID), 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe, 9=EuRoC MAV
 		std::map<int, Transform> & poses,
 		std::multimap<int, Link> * constraints = 0, // optional for formats 3 and 4
 		std::map<int, double> * stamps = 0); // optional for format 1 and 9

corelib/include/rtabmap/core/Memory.h

@@ -264,7 +264,6 @@ private:
 	void addSignatureToStm(Signature * signature, const cv::Mat & covariance);
 	void clear();
 	void loadDataFromDb(bool postInitClosingEvents);
-	void saveFlannIndex(bool postInitClosingEvents);
 	void moveToTrash(Signature * s, bool keepLinkedToGraph = true, std::list<int> * deletedWords = 0);
 
 	void moveSignatureToWMFromSTM(int id, int * reducedTo = 0);
@@ -300,7 +299,6 @@ private:
 	float _similarityThreshold;
 	bool _binDataKept;
 	bool _rawDescriptorsKept;
-	bool _loadVisualLocalFeaturesOnInit;
 	bool _saveDepth16Format;
 	bool _notLinkedNodesKeptInDb;
 	bool _saveIntermediateNodeData;
@@ -308,7 +306,6 @@ private:
 	std::string _depthCompressionFormat;
 	bool _incrementalMemory;
 	bool _localizationDataSaved;
-	bool _flannIndexSaved;
 	bool _reduceGraph;
 	int _maxStMemSize;
 	float _recentWmRatio;
@@ -356,7 +353,6 @@ private:
 	bool _linksChanged; // False by default, become true when links are modified.
 	int _signaturesAdded;
 	bool _allNodesInWM;
-	bool _receivingOdometryFeatures;
 	GPS _gpsOrigin;
 	std::vector<CameraModel> _rectCameraModels;
 	std::vector<StereoCameraModel> _rectStereoCameraModels;

corelib/include/rtabmap/core/Odometry.h

@@ -53,11 +53,10 @@ public:
 		kTypeOkvis = 6,
 		kTypeLOAM = 7,
 		kTypeMSCKF = 8,
-		kTypeVINSFusion = 9,
+		kTypeVINS = 9,
 		kTypeOpenVINS = 10,
 		kTypeFLOAM = 11,
-		kTypeOpen3D = 12,
-		kTypeCuVSLAM = 13
+		kTypeOpen3D = 12
 	};
 
 public:

corelib/include/rtabmap/core/OdometryThread.h

@@ -29,7 +29,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define ODOMETRYTHREAD_H_
 
 #include <rtabmap/core/rtabmap_core_export.h>
-#include <rtabmap/core/SensorEvent.h>
 #include <rtabmap/core/SensorData.h>
 #include <rtabmap/utilite/UThread.h>
 #include <rtabmap/utilite/UEventsHandler.h>
@@ -56,21 +55,20 @@ private:
 	// MAIN LOOP
 	//============================================================
 	virtual void mainLoop();
-	void addData(const SensorEvent & data);
-	bool getData(SensorEvent & data);
+	void addData(const SensorData & data);
+	bool getData(SensorData & data);
 
 private:
 	USemaphore _dataAdded;
 	UMutex _dataMutex;
-	std::list<SensorEvent> _dataBuffer;
+	std::list<SensorData> _dataBuffer;
 	std::list<SensorData> _imuBuffer;
 	Odometry * _odometry;
 	unsigned int _dataBufferMaxSize;
 	bool _resetOdometry;
 	Transform _resetPose;
-	Transform _previousGuessPose;
-	double _oldestAsyncImuStamp;
-	double _newestAsyncImuStamp;
+	double _lastImuStamp;
+	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,
-			const ParametersMap & registrationParameters = ParametersMap());
+			bool rematchFeatures = false);
 
 	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,
-			const ParametersMap & registrationParameters = ParametersMap());
+			bool rematchFeatures = false);
 
 	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,
-			ParametersMap registrationParameters = ParametersMap());
+			bool useLinkTransformAsGuess = false);
 
 protected:
 	Optimizer(

corelib/include/rtabmap/core/Parameters.h

@@ -204,7 +204,6 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(Mem, ImageKept,                   bool, false,    "Keep raw images in RAM.");
     RTABMAP_PARAM(Mem, BinDataKept,                 bool, true,     "Keep binary data in db.");
     RTABMAP_PARAM(Mem, RawDescriptorsKept,          bool, true,     "Raw descriptors kept in memory.");
-    RTABMAP_PARAM(Mem, LoadVisualLocalFeaturesOnInit, bool, true,   "Load all local visual features (keypoints, descriptors and 3D points) in RAM when loading an existing database. This can add significant time to initialize the memory but the features will be already loaded before computing loop closure transforms. If false, the features are loaded on-demand from the database when a loop closure transformation should be estimated.");
     RTABMAP_PARAM(Mem, MapLabelsAdded,              bool, true,     "Create map labels. The first node of a map will be labeled as \"map#\" where # is the map ID.");
     RTABMAP_PARAM(Mem, SaveDepth16Format,           bool, false,    "Save depth image into 16 bits format to reduce memory used. Warning: values over ~65 meters are ignored (maximum 65535 millimeters).");
     RTABMAP_PARAM(Mem, NotLinkedNodesKept,          bool, true,     "Keep not linked nodes in db (rehearsed nodes and deleted nodes).");
@@ -213,8 +212,8 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM_STR(Mem, DepthCompressionFormat,   ".rvl",        "Depth image compression format for 16UC1 depth type. It should be \".png\" or \".rvl\". If depth type is 32FC1, \".png\" is used.");
     RTABMAP_PARAM(Mem, STMSize,                   unsigned int, 10, "Short-term memory size.");
     RTABMAP_PARAM(Mem, IncrementalMemory,           bool, true,     "SLAM mode, otherwise it is Localization mode.");
-    RTABMAP_PARAM(Mem, LocalizationDataSaved,       bool, false,    uFormat("Save localization data during localization session (when %s=false). When enabled, the database will then also grow in localization mode. This mode would be used only for debugging purpose.", kMemIncrementalMemory().c_str()).c_str());
-    RTABMAP_PARAM(Mem, ReduceGraph,                 bool, false,    uFormat("Reduce graph. Merge nodes when loop closures are added (ignoring those with user data). Note that this approach assumes that 100%% of the loop closures accepted are good, so it is highly recommended to enable \"%s\" at the same time.", kRGBDOptimizeMaxError().c_str()));
+    RTABMAP_PARAM(Mem, LocalizationDataSaved,       bool, false,     uFormat("Save localization data during localization session (when %s=false). When enabled, the database will then also grow in localization mode. This mode would be used only for debugging purpose.", kMemIncrementalMemory().c_str()).c_str());
+    RTABMAP_PARAM(Mem, ReduceGraph,                 bool, false,    "Reduce graph. Merge nodes when loop closures are added (ignoring those with user data set).");
     RTABMAP_PARAM(Mem, RecentWmRatio,               float, 0.2,     "Ratio of locations after the last loop closure in WM that cannot be transferred.");
     RTABMAP_PARAM(Mem, TransferSortingByWeightId,   bool, false,    "On transfer, signatures are sorted by weight->ID only (i.e. the oldest of the lowest weighted signatures are transferred first). If false, the signatures are sorted by weight->Age->ID (i.e. the oldest inserted in WM of the lowest weighted signatures are transferred first). Note that retrieval updates the age, not the ID.");
     RTABMAP_PARAM(Mem, RehearsalIdUpdatedToNewOne,  bool, false,    "On merge, update to new id. When false, no copy.");
@@ -261,8 +260,6 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM_STR(Kp, RoiRatios,       "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
     RTABMAP_PARAM_STR(Kp, DictionaryPath,       "",           "Path of the pre-computed dictionary");
     RTABMAP_PARAM(Kp, NewWordsComparedTogether, bool, true,   "When adding new words to dictionary, they are compared also with each other (to detect same words in the same signature).");
-    RTABMAP_PARAM(Kp, FlannIndexSaved,          bool, false,  uFormat("Save FLANN index during localization session (when %s=false). The FLANN index will be saved to database after the first time localization mode is used, then on next sessions, the index is reloaded from the database instead of being rebuilt again. This can save significant loading time when the visual word dictionary is big (>1M words). Note that if the dictionary is modified (parameters or data), the index will be rebuilt and saved again on the next session.", kMemIncrementalMemory().c_str()).c_str());
-    RTABMAP_PARAM(Kp, SerializeWithChecksum,    bool, true,   "On serialization of the FLANN index, compute checksum of the data used by the FLANN index. This adds a slight overhead on serialization/deserialization to make sure that the dictionary data correspond to same data used when the index was built.");
     RTABMAP_PARAM(Kp, SubPixWinSize,            int, 3,       "See cv::cornerSubPix().");
     RTABMAP_PARAM(Kp, SubPixIterations,         int, 0,       "See cv::cornerSubPix(). 0 disables sub pixel refining.");
     RTABMAP_PARAM(Kp, SubPixEps,                double, 0.02, "See cv::cornerSubPix().");
@@ -369,7 +366,7 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(RGBD, AggressiveLoopThr,        float, 0.05, uFormat("Loop closure threshold used (overriding %s) when a new mapping session is not yet linked to a map of the highest loop closure hypothesis. In localization mode, this threshold is used when there are no loop closure constraints with any map in the cache (%s). In all cases, the goal is to aggressively loop on a previous map in the database. Only used when %s is enabled. Set 1 to disable.", kRtabmapLoopThr().c_str(), kRGBDMaxOdomCacheSize().c_str(), kRGBDEnabled().c_str()));
     RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0,    "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled).");
     RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd,     bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest node of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
-    RTABMAP_PARAM(RGBD, OptimizeMaxError,         float, 3.0,   uFormat("Reject loop closures if optimization error ratio is greater than this value (0=disabled). Ratio is computed as absolute error over standard deviation of each link. This will help to detect when a wrong loop closure is added to the graph. If used with \"%s\", the disabled loop closure links will be removed.", kOptimizerRobust().c_str()));
+    RTABMAP_PARAM(RGBD, OptimizeMaxError,         float, 3.0,   uFormat("Reject loop closures if optimization error ratio is greater than this value (0=disabled). Ratio is computed as absolute error over standard deviation of each link. This will help to detect when a wrong loop closure is added to the graph. Not compatible with \"%s\" if enabled.", kOptimizerRobust().c_str()));
     RTABMAP_PARAM(RGBD, MaxLoopClosureDistance,   float, 0.0,   "Reject loop closures/localizations if the distance from the map is over this distance (0=disabled).");
     RTABMAP_PARAM(RGBD, ForceOdom3DoF,            bool, true,  uFormat("Force odometry pose to be 3DoF if %s=true.", kRegForce3DoF().c_str()));
     RTABMAP_PARAM(RGBD, StartAtOrigin,            bool, false, uFormat("If true, rtabmap will assume the robot is starting from origin of the map. If false, rtabmap will assume the robot is restarting from the last saved localization pose from previous session (the place where it shut down previously). Used only in localization mode (%s=false).", kMemIncrementalMemory().c_str()));
@@ -393,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.");
@@ -431,7 +427,7 @@ class RTABMAP_CORE_EXPORT Parameters
 #endif
 #endif
     RTABMAP_PARAM(Optimizer, VarianceIgnored, bool, false,     "Ignore constraints' variance. If checked, identity information matrix is used for each constraint. Otherwise, an information matrix is generated from the variance saved in the links.");
-    RTABMAP_PARAM(Optimizer, Robust,          bool, false,     "Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies).");
+    RTABMAP_PARAM(Optimizer, Robust,          bool, false,     uFormat("Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies). Not compatible with \"%s\" if enabled.", kRGBDOptimizeMaxError().c_str()));
     RTABMAP_PARAM(Optimizer, PriorsIgnored,   bool, true,      "Ignore prior constraints (global pose or GPS) while optimizing. Currently only g2o and gtsam optimization supports this.");
     RTABMAP_PARAM(Optimizer, LandmarksIgnored,   bool, false,  "Ignore landmark constraints while optimizing. Currently only g2o and gtsam optimization supports this.");
 #if defined(RTABMAP_G2O) || defined(RTABMAP_GTSAM)
@@ -456,8 +452,8 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(GTSAM, IncRelinearizeSkip,       int, 1, "Only relinearize any variables every X calls to ISAM2::update(). See GTSAM::ISAM2 doc for more info.");
 
     // Odometry
-    RTABMAP_PARAM(Odom, Strategy,               int, 0,       "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM2 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion 10=OpenVINS 11=FLOAM 12=Open3D 13=cuVSLAM");
-    RTABMAP_PARAM(Odom, ResetCountdown,         int, 0,       "Automatically reset odometry after X consecutive images where odometry cannot be computed (a value of 0 disables auto-reset). When a reset occurs, odometry resumes from the last successfully computed pose with large covariance to trigger a new map. If external odometry is used, it will also be reset based on the motion estimated relative to the last computed pose but no large covariance will be received, so that a new map won't be triggered.");
+    RTABMAP_PARAM(Odom, Strategy,               int, 0,       "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM2 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion 10=OpenVINS 11=FLOAM 12=Open3D");
+    RTABMAP_PARAM(Odom, ResetCountdown,         int, 0,       "Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset).");
     RTABMAP_PARAM(Odom, Holonomic,              bool, true,   "If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw)).");
     RTABMAP_PARAM(Odom, FillInfoData,           bool, true,   "Fill info with data (inliers/outliers features).");
     RTABMAP_PARAM(Odom, ImageBufferSize,     unsigned int, 1, "Data buffer size (0 min inf).");
@@ -606,8 +602,8 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(OdomMSCKF, InitCovExTrans,  double,  0.000025,  "");
     RTABMAP_PARAM(OdomMSCKF, MaxCamStateSize,  int,  20,  "");
 
-    // Odometry VINS-Fusion
-    RTABMAP_PARAM_STR(OdomVINSFusion, ConfigPath,     "",  "Path of VINS-Fusion config file.");
+    // Odometry VINS
+    RTABMAP_PARAM_STR(OdomVINS, ConfigPath,     "",  "Path of VINS config file.");
 
     // Odometry OpenVINS
     RTABMAP_PARAM(OdomOpenVINS, UseStereo,                 bool,   true,   "If we have more than 1 camera, if we should try to track stereo constraints between pairs");

corelib/include/rtabmap/core/PythonInterface.h

@@ -8,7 +8,6 @@
 #ifndef CORELIB_SRC_PYTHON_PYTHONINTERFACE_H_
 #define CORELIB_SRC_PYTHON_PYTHONINTERFACE_H_
 
-#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 
 #include <string>
 #include <rtabmap/utilite/UMutex.h>
@@ -24,7 +23,7 @@ namespace rtabmap {
  * Create a single PythonInterface on main thread at
  * global scope before any Python classes.
  */
-class RTABMAP_CORE_EXPORT PythonInterface
+class PythonInterface
 {
 public:
 	PythonInterface();
@@ -35,7 +34,7 @@ private:
 	pybind11::gil_scoped_release* release_;
 };
 
-std::string RTABMAP_CORE_EXPORT getPythonTraceback();
+std::string getPythonTraceback();
 
 }
 

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,
@@ -98,17 +88,6 @@ public:
 			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(
 			const cv::Mat & rgb,
@@ -118,16 +97,6 @@ public:
 			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(
 			const LaserScan & laserScan,
@@ -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 * depthConfidenceRaw = 0);
+			cv::Mat * emptyCellsRaw = 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 * depthConfidenceRaw = 0) const;
+			cv::Mat * emptyCellsRaw = 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/VWDictionary.h

@@ -107,11 +107,7 @@ public:
 	bool isIncrementalFlann() const {return _incrementalFlann;}
 	void setIncrementalDictionary();
 	void setFixedDictionary(const std::string & dictionaryPath);
-	bool isModified() const;
 
-	std::vector<unsigned char> serializeIndex() const;
-	void deserializeIndex(const std::vector<unsigned char> & data);
-	void deserializeIndex(const unsigned char * data, size_t size);
 	void exportDictionary(const char * fileNameReferences, const char * fileNameDescriptors) const;
 
 	void clear(bool printWarningsIfNotEmpty = true);
@@ -141,12 +137,10 @@ private:
 	std::string _dictionaryPath; // a pre-computed dictionary (.txt or .db)
 	std::string _newDictionaryPath; // a pre-computed dictionary (.txt or .db)
 	bool _newWordsComparedTogether;
-	bool _serializeWithChecksum;
 	int _lastWordId;
 	bool useDistanceL1_;
 	FlannIndex * _flannIndex;
 	cv::Mat _dataTree;
-	bool _modified;
 	NNStrategy _strategy;
 	std::map<int ,int> _mapIndexId;
 	std::map<int ,int> _mapIdIndex;

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

@@ -94,14 +94,14 @@ public:
 		_depthFromScanFillHolesFromBorder = fillHolesFromBorder;
 	}
 
-	// 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame, 11=10+ID), 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe, 9=EuRoC MAV, 12=rgbd_bonn
+	// Format: 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe
 	void setOdometryPath(const std::string & filePath, int format = 0)
 	{
 		_odometryPath = filePath;
 		_odometryFormat = format;
 	}
 
-	// 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame, 11=10+ID), 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe, 9=EuRoC MAV, 12=rgbd_bonn
+	// Format: 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe
 	void setGroundTruthPath(const std::string & filePath, int format = 0)
 	{
 		_groundTruthPath = filePath;

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

@@ -1,105 +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.
-*/
-
-#pragma once
-
-#include "rtabmap/core/CameraModel.h"
-#include "rtabmap/core/Camera.h"
-#include "rtabmap/core/Version.h"
-
-#ifdef RTABMAP_ORBBEC_SDK
-namespace ob
-{
-    class Pipeline;
-    class Align;
-}
-#endif
-
-namespace rtabmap
-{
-
-class RTABMAP_CORE_EXPORT CameraOrbbecSDK :
-    public Camera
-{
-public:
-    static bool available();
-
-public:
-    // deviceId can be either an index (e.g., "0"), an UID (e.g, "2-1-2" or "gmsl-1") or a serial ("AAA6454S")
-    CameraOrbbecSDK(
-        std::string deviceId = "",
-        int colorWidth = 800,
-        int colorHeight = 600,
-        int depthWidth = 800,
-        int depthHeight = 600,
-        float imageRate = 0.0f,
-        const Transform & localTransform = Transform::getIdentity());
-    virtual ~CameraOrbbecSDK();
-
-    virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-    virtual bool isCalibrated() const;
-    virtual std::string getSerial() const;
-
-    void close();
-
-    // Should be set before initializing
-    void enableColorRectification(bool enabled);
-    void enableImu(bool enabled);
-
-    void enableDepthMM(bool enabled);
-
-protected:
-    virtual SensorData captureImage(SensorCaptureInfo * info = 0);
-
-private:
-
-#ifdef RTABMAP_ORBBEC_SDK
-    std::string deviceId_;
-    int colorWidth_;
-    int colorHeight_;
-    int depthWidth_;
-    int depthHeight_;
-    ob::Pipeline * pipeline_;
-    ob::Pipeline * imuPipeline_;
-    ob::Align * alignFilter_;
-    CameraModel model_;
-    Transform imuLocalTransform_;
-    bool imuLocalTransformInitialized_;
-    uint64_t lastAccStamp_;
-    uint64_t lastImageStamp_;
-    bool globalTimestampAvailable_;
-    bool rectifyColor_;
-    bool convertDepthToMM_;
-    bool imuPublished_;
-    std::map<double, cv::Vec6f> imuBuffer_;
-    UMutex imuMutex_;
-#endif
-
-};
-
-
-} // namespace rtabmap

corelib/include/rtabmap/core/odometry/OdometryCuVSLAM.h

@@ -1,86 +0,0 @@
-/*
-Copyright (c) 2025 Felix Toft
-Copyright (c) 2010-2016, 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 ODOMETRYCUVSLAM_H_
-#define ODOMETRYCUVSLAM_H_
-
-#include <rtabmap/core/Odometry.h>
-#include <memory>
-
-#ifdef RTABMAP_CUVSLAM
-#include <cuvslam.h>
-#include <ground_constraint.h>
-#include <cuda_runtime.h>
-#endif
-
-namespace rtabmap {
-
-class RTABMAP_CORE_EXPORT OdometryCuVSLAM : public Odometry
-{
-public:
-	OdometryCuVSLAM(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
-	virtual ~OdometryCuVSLAM();
-
-	virtual void reset(const Transform & initialPose = Transform::getIdentity());
-	virtual Odometry::Type getType() {return Odometry::kTypeCuVSLAM;}
-
-private:
-	virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);
-
-private:
-#ifdef RTABMAP_CUVSLAM
-	CUVSLAM_TrackerHandle cuvslam_handle_;
-	CUVSLAM_GroundConstraintHandle ground_constraint_handle_;
-
-	std::vector<CUVSLAM_Camera> cuvslam_cameras_;
-	std::vector<std::array<float, 12>> intrinsics_;
-	
-	// State tracking
-	bool initialized_;
-	bool lost_;
-	bool tracking_;
-	bool planar_constraints_;
-	Transform previous_pose_;
-	double last_timestamp_;
-
-	//visualization
-	std::vector<CUVSLAM_Observation> observations_;
-	std::vector<CUVSLAM_Landmark> landmarks_;
-	
-	// GPU memory management
-	std::vector<uint8_t *> gpu_left_image_data_; // pointers to all gpu images
-	std::vector<uint8_t *> gpu_right_image_data_;
-	std::vector<size_t> gpu_left_image_sizes_; // size of one image
-	std::vector<size_t> gpu_right_image_sizes_;
-	cudaStream_t cuda_stream_;
-#endif
-};
-
-}
-
-#endif /* ODOMETRYCUVSLAM_H_ */

corelib/include/rtabmap/core/odometry/OdometryVINS.h

@@ -25,7 +25,39 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#pragma once
-#pragma message("Warning: OdometryVINS.h is deprecated. Please use OdometryVINSFusion.h instead.")
+#ifndef ODOMETRYVINS_H_
+#define ODOMETRYVINS_H_
 
-#include "rtabmap/core/odometry/OdometryVINSFusion.h"
+#include <rtabmap/core/Odometry.h>
+
+namespace rtabmap {
+
+class VinsEstimator;
+
+class RTABMAP_CORE_EXPORT OdometryVINS : public Odometry
+{
+public:
+	OdometryVINS(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
+	virtual ~OdometryVINS();
+
+	virtual void reset(const Transform & initialPose = Transform::getIdentity());
+	virtual Odometry::Type getType() {return Odometry::kTypeVINS;}
+	virtual bool canProcessRawImages() const {return true;}
+	virtual bool canProcessAsyncIMU() const {return true;}
+
+private:
+	virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);
+
+private:
+#ifdef RTABMAP_VINS
+	VinsEstimator * vinsEstimator_;
+	bool initGravity_;
+	Transform previousPose_;
+	Transform previousLocalTransform_;
+	IMU lastImu_;
+#endif
+};
+
+}
+
+#endif /* ODOMETRYVINS_H_ */

corelib/include/rtabmap/core/odometry/OdometryVINSFusion.h

@@ -1,64 +0,0 @@
-/*
-Copyright (c) 2010-2016, 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 ODOMETRYVINSFUSION_H_
-#define ODOMETRYVINSFUSION_H_
-
-#include <rtabmap/core/Odometry.h>
-
-namespace rtabmap {
-
-class VinsFusionEstimator;
-
-class RTABMAP_CORE_EXPORT OdometryVINSFusion : public Odometry
-{
-public:
-	OdometryVINSFusion(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
-	virtual ~OdometryVINSFusion();
-
-	virtual void reset(const Transform & initialPose = Transform::getIdentity());
-	virtual Odometry::Type getType() {return Odometry::kTypeVINSFusion;}
-	virtual bool canProcessRawImages() const {return true;}
-	virtual bool canProcessAsyncIMU() const {return true;}
-
-private:
-	virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);
-
-private:
-#ifdef RTABMAP_VINS_FUSION
-	VinsFusionEstimator * vinsEstimator_;
-	bool initGravity_;
-	Transform previousPose_;
-	Transform previousLocalTransform_;
-	IMU lastImu_;
-	double lastImuStamp_;
-#endif
-};
-
-}
-
-#endif /* ODOMETRYVINSFUSION_H_ */

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
-		unsigned char confidenceThr = 0);                            // ignored for stereo
+		const std::vector<float> & roiRatios = std::vector<float>()); // 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
-		unsigned char confidenceThr = 0);                                // ignored for stereo
+		const std::vector<float> & roiRatios = std::vector<float>()); // 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
-		unsigned char confidenceThr = 0);                            // ignored for stereo
+		const std::vector<float> & roiRatios = std::vector<float>()); // 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
-		unsigned char confidenceThr = 0);                            // ignored for stereo
+		const std::vector<float> & roiRatios = std::vector<float>()); // 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

@@ -41,7 +41,6 @@ SET(SRC_FILES
     camera/CameraMyntEye.cpp
     camera/CameraDepthAI.cpp
 	camera/CameraSeerSense.cpp
-	camera/CameraOrbbecSDK.cpp
     
     EpipolarGeometry.cpp
     VisualWord.cpp
@@ -99,10 +98,9 @@ SET(SRC_FILES
     odometry/OdometryLOAM.cpp
     odometry/OdometryFLOAM.cpp
     odometry/OdometryMSCKF.cpp
-    odometry/OdometryVINSFusion.cpp
+    odometry/OdometryVINS.cpp
     odometry/OdometryOpenVINS.cpp
     odometry/OdometryOpen3D.cpp
-	odometry/OdometryCuVSLAM.cpp
     
     IMU.cpp
     IMUThread.cpp
@@ -165,6 +163,10 @@ IF(MSVC)
 ENDIF(MSVC)
 
 SET(INCLUDE_DIRS
+    ${CMAKE_CURRENT_SOURCE_DIR}
+    ${CMAKE_CURRENT_SOURCE_DIR}/../include
+    ${CMAKE_CURRENT_BINARY_DIR}
+    ${CMAKE_CURRENT_BINARY_DIR}/include
 	${ZLIB_INCLUDE_DIRS}
 )
 
@@ -392,13 +394,6 @@ IF(xvsdk_FOUND)
 	)
 ENDIF(xvsdk_FOUND)
 
-IF(OrbbecSDK_FOUND)
-	SET(LIBRARIES
-		${LIBRARIES}
-		ob::OrbbecSDK
-	)
-ENDIF(OrbbecSDK_FOUND)
-
 IF(TARGET OpenMP::OpenMP_CXX)
 	SET(LIBRARIES
 		${LIBRARIES}
@@ -773,13 +768,6 @@ IF(ORB_SLAM_FOUND)
 	)
 ENDIF(ORB_SLAM_FOUND)
 
-IF(CUVSLAM_FOUND)
-    SET(LIBRARIES
-		${LIBRARIES}
-		cuvslam::cuvslam
-	)
-ENDIF(CUVSLAM_FOUND)
-
 IF(GTSAM_FOUND)
 	# Make sure GTSAM is built with system Eigen, not the included one in its package
 	IF(GTSAM_INCLUDE_DIR)
@@ -828,8 +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_22_0.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
@@ -870,12 +856,8 @@ generate_export_header(rtabmap_core
  DEPRECATED_MACRO_NAME RTABMAP_DEPRECATED)
 
 target_include_directories(rtabmap_core PUBLIC 
-  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR};${CMAKE_CURRENT_SOURCE_DIR}/../include;${CMAKE_CURRENT_BINARY_DIR};${CMAKE_CURRENT_BINARY_DIR}/include>"
-  "$<INSTALL_INTERFACE:${INSTALL_INCLUDE_DIR}>")
-
-target_include_directories(rtabmap_core SYSTEM PUBLIC 
-  "$<BUILD_INTERFACE:${PUBLIC_INCLUDE_DIRS};${INCLUDE_DIRS}>"
-  "$<INSTALL_INTERFACE:${PUBLIC_INCLUDE_DIRS}>")
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/../include;${CMAKE_CURRENT_BINARY_DIR}/include;${PUBLIC_INCLUDE_DIRS};${INCLUDE_DIRS}>"
+  "$<INSTALL_INTERFACE:${INSTALL_INCLUDE_DIR};${PUBLIC_INCLUDE_DIRS}>")
   
 TARGET_LINK_LIBRARIES(rtabmap_core 
   PUBLIC 

corelib/src/CameraModel.cpp

@@ -554,7 +554,7 @@ unsigned int CameraModel::deserialize(const unsigned char * data, unsigned int d
 			int iR = 8;
 			int iP = 9;
 			int iL = 10;
-			//UDEBUG("Header: %d %d %d %d %d %d %d %d %d %d %d", header[0],header[1],header[2],header[3],header[4],header[5],header[6],header[7],header[8],header[9],header[10]);
+			UDEBUG("Header: %d %d %d %d %d %d %d %d %d %d %d", header[0],header[1],header[2],header[3],header[4],header[5],header[6],header[7],header[8],header[9],header[10]);
 			unsigned int requiredDataSize = sizeof(int)*headerSize +
 					sizeof(double)*(header[iK]+header[iD]+header[iR]+header[iP]) +
 					sizeof(float)*header[iL];

corelib/src/DBDriver.cpp

@@ -383,7 +383,7 @@ void DBDriver::asyncSave(Signature * s)
 {
 	if(s)
 	{
-		//UDEBUG("s=%d", s->id());
+		UDEBUG("s=%d", s->id());
 		_trashesMutex.lock();
 		{
 			_trashSignatures.insert(std::pair<int, Signature*>(s->id(), s));
@@ -531,17 +531,17 @@ void DBDriver::updateLaserScan(int nodeId, const LaserScan & scan)
 	_dbSafeAccessMutex.unlock();
 }
 
-void DBDriver::load(VWDictionary & dictionary, bool lastStateOnly) const
+void DBDriver::load(VWDictionary * dictionary, bool lastStateOnly) const
 {
 	_dbSafeAccessMutex.lock();
 	this->loadQuery(dictionary, lastStateOnly);
 	_dbSafeAccessMutex.unlock();
 }
 
-void DBDriver::loadLastNodes(std::list<Signature *> & signatures, bool loadWordIdsOnly) const
+void DBDriver::loadLastNodes(std::list<Signature *> & signatures) const
 {
 	_dbSafeAccessMutex.lock();
-	this->loadLastNodesQuery(signatures, loadWordIdsOnly);
+	this->loadLastNodesQuery(signatures);
 	_dbSafeAccessMutex.unlock();
 }
 
@@ -564,8 +564,7 @@ Signature * DBDriver::loadSignature(int id, bool * loadedFromTrash)
 }
 void DBDriver::loadSignatures(const std::list<int> & signIds,
 		std::list<Signature *> & signatures,
-		std::set<int> * loadedFromTrash,
-		bool loadWordIdsOnly)
+		std::set<int> * loadedFromTrash)
 {
 	UDEBUG("");
 	// look up in the trash before the database
@@ -610,7 +609,7 @@ void DBDriver::loadSignatures(const std::list<int> & signIds,
 	if(ids.size())
 	{
 		_dbSafeAccessMutex.lock();
-		this->loadSignaturesQuery(ids, signatures, loadWordIdsOnly);
+		this->loadSignaturesQuery(ids, signatures);
 		_dbSafeAccessMutex.unlock();
 	}
 }
@@ -657,10 +656,10 @@ void DBDriver::loadWords(const std::set<int> & wordIds, std::list<VisualWord *>
 	}
 }
 
-void DBDriver::loadNodeData(Signature & signature, bool images, bool scan, bool userData, bool occupancyGrid) const
+void DBDriver::loadNodeData(Signature * signature, bool images, bool scan, bool userData, bool occupancyGrid) const
 {
 	std::list<Signature *> signatures;
-	signatures.push_back(&signature);
+	signatures.push_back(signature);
 	this->loadNodeData(signatures, images, scan, userData, occupancyGrid);
 }
 
@@ -824,45 +823,6 @@ bool DBDriver::getNodeInfo(
 	return found;
 }
 
-void DBDriver::getLocalFeatures(
-	int signatureId,
-	std::multimap<int, int> & words,
-	std::vector<cv::KeyPoint> & keypoints,
-	std::vector<cv::Point3f> & points,
-	cv::Mat & descriptors) const
-{
-	bool found = false;
-	// look in the trash
-	_trashesMutex.lock();
-	if(uContains(_trashSignatures, signatureId))
-	{
-		const Signature * s = _trashSignatures.at(signatureId);
-		UASSERT(s != 0);
-		found = true;
-		if(!s->getWords().empty())
-		{
-			words = s->getWords();
-			if(s->getWordsKpts().empty()){
-				found = false; // Force checking the database in case the local features were not loaded in RAM
-			}
-			else
-			{
-				words = s->getWords();
-				keypoints = s->getWordsKpts();
-				points = s->getWords3();
-				descriptors = s->getWordsDescriptors().clone();
-			}
-		}
-	}
-	_trashesMutex.unlock();
-
-	if(!found)
-	{
-		UScopeMutex lock(_dbSafeAccessMutex);
-		getLocalFeaturesQuery(signatureId, words, keypoints, points, descriptors);
-	}
-}
-
 void DBDriver::loadLinks(int signatureId, std::multimap<int, Link> & links, Link::Type type) const
 {
 	bool found = false;
@@ -1327,13 +1287,6 @@ cv::Mat DBDriver::loadOptimizedMesh(
 	return cloud;
 }
 
-void DBDriver::saveFlannIndex(const std::vector<unsigned char> & indexData) const
-{
-	_dbSafeAccessMutex.lock();
-	saveFlannIndexQuery(indexData);
-	_dbSafeAccessMutex.unlock();
-}
-
 void DBDriver::generateGraph(
 		const std::string & fileName,
 		const std::set<int> & idsInput,

corelib/src/DBDriverSqlite3.cpp

@@ -34,8 +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_22_0_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"
@@ -404,8 +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("0.22.0", DATABASESCHEMA_0_22_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)
 			{
@@ -1298,8 +1294,8 @@ std::map<int, std::vector<int> > DBDriverSqlite3::getAllStatisticsWmStatesQuery(
 
 void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, bool images, bool scan, bool userData, bool occupancyGrid) const
 {
-	//UDEBUG("load data for %d signatures images=%d scan=%d userData=%d, grid=%d",
-	//		(int)signatures.size(), images?1:0, scan?1:0, userData?1:0, occupancyGrid?1:0);
+	UDEBUG("load data for %d signatures images=%d scan=%d userData=%d, grid=%d",
+			(int)signatures.size(), images?1:0, scan?1:0, userData?1:0, occupancyGrid?1:0);
 
 	if(!images && !scan && !userData && !occupancyGrid)
 	{
@@ -1321,15 +1317,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 
 			if(images)
 			{
-				if(uStrNumCmp(_version, "0.22.0") >= 0)
-				{
-					fields << "image, depth, depth_confidence, calibration";
-				}
-				else
-				{
-					fields << "image, depth, calibration";
-				}
-				
+				fields << "image, depth, calibration";
 				if(scan || userData || occupancyGrid)
 				{
 					fields << ", ";
@@ -1447,7 +1435,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 		{
 			UASSERT(*iter != 0);
 
-			//ULOGGER_DEBUG("Loading data for %d...", (*iter)->id());
+			ULOGGER_DEBUG("Loading data for %d...", (*iter)->id());
 			// bind id
 			rc = sqlite3_bind_int(ppStmt, 1, (*iter)->id());
 			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
@@ -1460,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();
@@ -1485,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
@@ -1847,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
 					{
@@ -1876,7 +1852,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 		// Finalize (delete) the statement
 		rc = sqlite3_finalize(ppStmt);
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
-		//ULOGGER_DEBUG("Time=%fs", timer.ticks());
+		ULOGGER_DEBUG("Time=%fs", timer.ticks());
 	}
 }
 
@@ -2393,23 +2369,6 @@ bool DBDriverSqlite3::getNodeInfoQuery(int signatureId,
 	return found;
 }
 
-void DBDriverSqlite3::getLocalFeaturesQuery(
-	int signatureId,
-	std::multimap<int, int> & words,
-	std::vector<cv::KeyPoint> & keypoints,
-	std::vector<cv::Point3f> & points,
-	cv::Mat & descriptors) const
-{
-	Signature s(signatureId);
-	std::list<Signature *> ids;
-	ids.push_back(&s);
-	this->loadWordsQuery(ids);
-	words = ids.front()->getWords();
-	keypoints = ids.front()->getWordsKpts();
-	points = ids.front()->getWords3();
-	descriptors = ids.front()->getWordsDescriptors().clone();
-}
-
 void DBDriverSqlite3::getLastNodeIdsQuery(std::set<int> & ids) const
 {
 	if(_ppDb)
@@ -3006,7 +2965,7 @@ void DBDriverSqlite3::getWeightQuery(int nodeId, int & weight) const
 }
 
 //may be slower than the previous version but don't have a limit of words that can be loaded at the same time
-void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & nodes, bool loadWordIdsOnly) const
+void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & nodes) const
 {
 	ULOGGER_DEBUG("count=%d", (int)ids.size());
 	if(_ppDb && ids.size())
@@ -3170,7 +3129,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 			// create the node
 			if(id)
 			{
-				//ULOGGER_DEBUG("Creating %d (map=%d, pose=%s)", *iter, mapId, pose.prettyPrint().c_str());
+				ULOGGER_DEBUG("Creating %d (map=%d, pose=%s)", *iter, mapId, pose.prettyPrint().c_str());
 				Signature * s = new Signature(
 						id,
 						mapId,
@@ -3209,17 +3168,175 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 		ULOGGER_DEBUG("Time=%fs", timer.ticks());
 
 		// Prepare the query... Get the map from signature and visual words
-		UDEBUG("Loading local features (ids only=%s)....", loadWordIdsOnly?"true":"false");
-		if(loadWordIdsOnly) {
-			this->loadWordIdsQuery(nodes);
+		std::stringstream query2;
+		if(uStrNumCmp(_version, "0.13.0") >= 0)
+		{
+			query2 << "SELECT word_id, pos_x, pos_y, size, dir, response, octave, depth_x, depth_y, depth_z, descriptor_size, descriptor "
+					 "FROM Feature "
+					 "WHERE node_id = ? ";
 		}
-		else {
-			this->loadWordsQuery(nodes);
+		else if(uStrNumCmp(_version, "0.12.0") >= 0)
+		{
+			query2 << "SELECT word_id, pos_x, pos_y, size, dir, response, octave, depth_x, depth_y, depth_z, descriptor_size, descriptor "
+					 "FROM Map_Node_Word "
+					 "WHERE node_id = ? ";
 		}
-		UDEBUG("Loading local features.... done! (in %f s)", timer.ticks());
+		else if(uStrNumCmp(_version, "0.11.2") >= 0)
+		{
+			query2 << "SELECT word_id, pos_x, pos_y, size, dir, response, depth_x, depth_y, depth_z, descriptor_size, descriptor "
+					 "FROM Map_Node_Word "
+					 "WHERE node_id = ? ";
+		}
+		else
+		{
+			query2 << "SELECT word_id, pos_x, pos_y, size, dir, response, depth_x, depth_y, depth_z "
+					 "FROM Map_Node_Word "
+					 "WHERE node_id = ? ";
+		}
+
+		query2 << " ORDER BY word_id"; // Needed for fast insertion below
+		query2 << ";";
+
+		rc = sqlite3_prepare_v2(_ppDb, query2.str().c_str(), -1, &ppStmt, 0);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		float nanFloat = std::numeric_limits<float>::quiet_NaN ();
+
+		for(std::list<Signature*>::const_iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
+		{
+			//ULOGGER_DEBUG("Loading words of %d...", (*iter)->id());
+			// bind id
+			rc = sqlite3_bind_int(ppStmt, 1, (*iter)->id());
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+			int visualWordId = 0;
+			int descriptorSize = 0;
+			const void * descriptor = 0;
+			int dRealSize = 0;
+			cv::KeyPoint kpt;
+			std::multimap<int, int> visualWords;
+			std::vector<cv::KeyPoint> visualWordsKpts;
+			std::vector<cv::Point3f> visualWords3;
+			cv::Mat descriptors;
+			bool allWords3NaN = true;
+			cv::Point3f depth(0,0,0);
+
+			// Process the result if one
+			rc = sqlite3_step(ppStmt);
+			while(rc == SQLITE_ROW)
+			{
+				int index = 0;
+				visualWordId = sqlite3_column_int(ppStmt, index++);
+				kpt.pt.x = sqlite3_column_double(ppStmt, index++);
+				kpt.pt.y = sqlite3_column_double(ppStmt, index++);
+				kpt.size = sqlite3_column_int(ppStmt, index++);
+				kpt.angle = sqlite3_column_double(ppStmt, index++);
+				kpt.response = sqlite3_column_double(ppStmt, index++);
+				if(uStrNumCmp(_version, "0.12.0") >= 0)
+				{
+					kpt.octave = sqlite3_column_int(ppStmt, index++);
+				}
+
+				if(sqlite3_column_type(ppStmt, index) == SQLITE_NULL)
+				{
+					depth.x = nanFloat;
+					++index;
+				}
+				else
+				{
+					depth.x = sqlite3_column_double(ppStmt, index++);
+				}
+
+				if(sqlite3_column_type(ppStmt, index) == SQLITE_NULL)
+				{
+					depth.y = nanFloat;
+					++index;
+				}
+				else
+				{
+					depth.y = sqlite3_column_double(ppStmt, index++);
+				}
+
+				if(sqlite3_column_type(ppStmt, index) == SQLITE_NULL)
+				{
+					depth.z = nanFloat;
+					++index;
+				}
+				else
+				{
+					depth.z = sqlite3_column_double(ppStmt, index++);
+				}
+
+				visualWordsKpts.push_back(kpt);
+				visualWords.insert(visualWords.end(), std::make_pair(visualWordId, visualWordsKpts.size()-1));
+				visualWords3.push_back(depth);
+
+				if(allWords3NaN && util3d::isFinite(depth))
+				{
+					allWords3NaN = false;
+				}
+
+				if(uStrNumCmp(_version, "0.11.2") >= 0)
+				{
+					descriptorSize = sqlite3_column_int(ppStmt, index++); // VisualWord descriptor size
+					descriptor = sqlite3_column_blob(ppStmt, index); 	// VisualWord descriptor array
+					dRealSize = sqlite3_column_bytes(ppStmt, index++);
+
+					if(descriptor && descriptorSize>0 && dRealSize>0)
+					{
+						cv::Mat d;
+						if(dRealSize == descriptorSize)
+						{
+							// CV_8U binary descriptors
+							d = cv::Mat(1, descriptorSize, CV_8U);
+						}
+						else if(dRealSize/int(sizeof(float)) == descriptorSize)
+						{
+							// CV_32F
+							d = cv::Mat(1, descriptorSize, CV_32F);
+						}
+						else
+						{
+							UFATAL("Saved buffer size (%d bytes) is not the same as descriptor size (%d)", dRealSize, descriptorSize);
+						}
+
+						memcpy(d.data, descriptor, dRealSize);
+
+						descriptors.push_back(d);
+					}
+				}
+
+				rc = sqlite3_step(ppStmt);
+			}
+			UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+			if(visualWords.size()==0)
+			{
+				UDEBUG("Empty signature detected! (id=%d)", (*iter)->id());
+			}
+			else
+			{
+				if(allWords3NaN)
+				{
+					visualWords3.clear();
+				}
+				(*iter)->setWords(visualWords, visualWordsKpts, visualWords3, descriptors);
+				ULOGGER_DEBUG("Add %d keypoints, %d 3d points and %d descriptors to node %d", (int)visualWords.size(), allWords3NaN?0:(int)visualWords3.size(), (int)descriptors.rows, (*iter)->id());
+			}
+
+			//reset
+			rc = sqlite3_reset(ppStmt);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+		}
+
+		// Finalize (delete) the statement
+		rc = sqlite3_finalize(ppStmt);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		ULOGGER_DEBUG("Time=%fs", timer.ticks());
 
 		this->loadLinksQuery(nodes);
-		ULOGGER_DEBUG("Time loading links=%fs", timer.ticks());
+		ULOGGER_DEBUG("Time load links=%fs", timer.ticks());
 
 		for(std::list<Signature*>::iterator iter = nodes.begin(); iter!=nodes.end(); ++iter)
 		{
@@ -3487,7 +3604,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 	}
 }
 
-void DBDriverSqlite3::loadLastNodesQuery(std::list<Signature *> & nodes, bool loadWordIdsOnly) const
+void DBDriverSqlite3::loadLastNodesQuery(std::list<Signature *> & nodes) const
 {
 	ULOGGER_DEBUG("");
 	if(_ppDb)
@@ -3533,15 +3650,15 @@ void DBDriverSqlite3::loadLastNodesQuery(std::list<Signature *> & nodes, bool lo
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 
 		ULOGGER_DEBUG("Loading %d signatures...", ids.size());
-		this->loadSignaturesQuery(ids, nodes, loadWordIdsOnly);
+		this->loadSignaturesQuery(ids, nodes);
 		ULOGGER_DEBUG("loaded=%d, Time=%fs", nodes.size(), timer.ticks());
 	}
 }
 
-void DBDriverSqlite3::loadQuery(VWDictionary & dictionary, bool lastStateOnly) const
+void DBDriverSqlite3::loadQuery(VWDictionary * dictionary, bool lastStateOnly) const
 {
 	ULOGGER_DEBUG("");
-	if(_ppDb)
+	if(_ppDb && dictionary)
 	{
 		std::string type;
 		UTimer timer;
@@ -3604,11 +3721,11 @@ void DBDriverSqlite3::loadQuery(VWDictionary & dictionary, bool lastStateOnly) c
 			memcpy(d.data, descriptor, dRealSize);
 			VisualWord * vw = new VisualWord(id, d);
 			vw->setSaved(true);
-			dictionary.addWord(vw);
+			dictionary->addWord(vw);
 
 			if(++count % 5000 == 0)
 			{
-				//ULOGGER_DEBUG("Loaded %d words...", count);
+				ULOGGER_DEBUG("Loaded %d words...", count);
 			}
 			rc = sqlite3_step(ppStmt); // next result...
 		}
@@ -3619,50 +3736,9 @@ void DBDriverSqlite3::loadQuery(VWDictionary & dictionary, bool lastStateOnly) c
 
 		// Get Last word id
 		getLastWordId(id);
-		dictionary.setLastWordId(id);
+		dictionary->setLastWordId(id);
 
-		if(uStrNumCmp(_version, "0.23.0") >= 0) {
-			// load dictionary index
-			std::stringstream query3;
-			query3 << "SELECT dictionary_index "
-				<< "FROM Admin "
-				<< "WHERE version='" << _version.c_str()
-				<<"';";
-
-			rc = sqlite3_prepare_v2(_ppDb, query3.str().c_str(), -1, &ppStmt, 0);
-			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
-
-			// Process the result if one
-			rc = sqlite3_step(ppStmt);
-			UASSERT_MSG(rc == SQLITE_ROW, uFormat("DB error (%s): Not found first Admin row: query=\"%s\"", _version.c_str(), query3.str().c_str()).c_str());
-			if(rc == SQLITE_ROW)
-			{
-				const void * data = 0;
-				int dataSize = 0;
-				int index = 0;
-
-				//opt_poses
-				data = sqlite3_column_blob(ppStmt, index);
-				dataSize = sqlite3_column_bytes(ppStmt, index++);
-				if(dataSize>4 && data)
-				{
-					UDEBUG("A flann index was saved in the database (size=%ld).", dataSize);
-					dictionary.deserializeIndex((const unsigned char*)data, dataSize);
-				}
-				else {
-					UDEBUG("No flann index was saved in the database.");
-				}
-
-				rc = sqlite3_step(ppStmt); // next result...
-			}
-			UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
-
-			// Finalize (delete) the statement
-			rc = sqlite3_finalize(ppStmt);
-			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
-		}
-
-		ULOGGER_DEBUG("Loaded %d words... time=%fs", count, timer.ticks());
+		ULOGGER_DEBUG("Time=%fs", timer.ticks());
 	}
 }
 
@@ -3762,262 +3838,6 @@ void DBDriverSqlite3::loadWordsQuery(const std::set<int> & wordIds, std::list<Vi
 	}
 }
 
-void DBDriverSqlite3::loadWordIdsQuery(std::list<Signature *> & signatures) const
-{
-	if(_ppDb)
-	{
-		int rc = SQLITE_OK;
-		sqlite3_stmt * ppStmt = 0;
-		std::stringstream query;
-
-		if(uStrNumCmp(_version, "0.13.0") >= 0)
-		{
-			query << "SELECT word_id "
-						"FROM Feature "
-						"WHERE node_id = ? ";
-		}
-		else if(uStrNumCmp(_version, "0.12.0") >= 0)
-		{
-			query << "SELECT word_id "
-						"FROM Map_Node_Word "
-						"WHERE node_id = ? ";
-		}
-		else if(uStrNumCmp(_version, "0.11.2") >= 0)
-		{
-			query << "SELECT word_id "
-						"FROM Map_Node_Word "
-						"WHERE node_id = ? ";
-		}
-		else
-		{
-			query << "SELECT word_id "
-						"FROM Map_Node_Word "
-						"WHERE node_id = ? ";
-		}
-
-		query << " ORDER BY word_id"; // Needed for fast insertion below
-		query << ";";
-
-		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
-		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
-
-		for(std::list<Signature*>::const_iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
-		{
-			//ULOGGER_DEBUG("Loading words of %d...", (*iter)->id());
-			// bind id
-			rc = sqlite3_bind_int(ppStmt, 1, (*iter)->id());
-			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
-
-			int visualWordId = 0;
-			std::multimap<int, int> visualWords;
-
-			// Process the result if one
-			rc = sqlite3_step(ppStmt);
-			while(rc == SQLITE_ROW)
-			{
-				int index = 0;
-				visualWordId = sqlite3_column_int(ppStmt, index++);
-				visualWords.insert(visualWords.end(), std::make_pair(visualWordId, -1));
-
-				rc = sqlite3_step(ppStmt);
-			}
-			UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
-
-			if(visualWords.size()==0)
-			{
-				UDEBUG("Empty signature detected! (id=%d)", (*iter)->id());
-			}
-			else
-			{
-				(*iter)->setWords(visualWords, std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
-				//ULOGGER_DEBUG("Add %d keypoints, %d 3d points and %d descriptors to node %d", (int)visualWords.size(), allWords3NaN?0:(int)visualWords3.size(), (int)descriptors.rows, (*iter)->id());
-			}
-
-			//reset
-			rc = sqlite3_reset(ppStmt);
-			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
-		}
-
-		// Finalize (delete) the statement
-		rc = sqlite3_finalize(ppStmt);
-		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
-	}
-}
-
-void DBDriverSqlite3::loadWordsQuery(std::list<Signature *> & signatures) const
-{
-	if(_ppDb)
-	{
-		int rc = SQLITE_OK;
-		sqlite3_stmt * ppStmt = 0;
-		std::stringstream query;
-
-		if(uStrNumCmp(_version, "0.13.0") >= 0)
-		{
-			query << "SELECT word_id, pos_x, pos_y, size, dir, response, octave, depth_x, depth_y, depth_z, descriptor_size, descriptor "
-					 "FROM Feature "
-					 "WHERE node_id = ? ";
-		}
-		else if(uStrNumCmp(_version, "0.12.0") >= 0)
-		{
-			query << "SELECT word_id, pos_x, pos_y, size, dir, response, octave, depth_x, depth_y, depth_z, descriptor_size, descriptor "
-					 "FROM Map_Node_Word "
-					 "WHERE node_id = ? ";
-		}
-		else if(uStrNumCmp(_version, "0.11.2") >= 0)
-		{
-			query << "SELECT word_id, pos_x, pos_y, size, dir, response, depth_x, depth_y, depth_z, descriptor_size, descriptor "
-					 "FROM Map_Node_Word "
-					 "WHERE node_id = ? ";
-		}
-		else
-		{
-			query << "SELECT word_id, pos_x, pos_y, size, dir, response, depth_x, depth_y, depth_z "
-					 "FROM Map_Node_Word "
-					 "WHERE node_id = ? ";
-		}
-
-		query << " ORDER BY word_id"; // Needed for fast insertion below
-		query << ";";
-
-		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
-		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
-
-		float nanFloat = std::numeric_limits<float>::quiet_NaN ();
-
-		for(std::list<Signature*>::const_iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
-		{
-			//ULOGGER_DEBUG("Loading words of %d...", (*iter)->id());
-			// bind id
-			rc = sqlite3_bind_int(ppStmt, 1, (*iter)->id());
-			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
-
-			int visualWordId = 0;
-			int descriptorSize = 0;
-			const void * descriptor = 0;
-			int dRealSize = 0;
-			cv::KeyPoint kpt;
-			std::multimap<int, int> visualWords;
-			std::vector<cv::KeyPoint> visualWordsKpts;
-			std::vector<cv::Point3f> visualWords3;
-			cv::Mat descriptors;
-			bool allWords3NaN = true;
-			cv::Point3f depth(0,0,0);
-
-			// Process the result if one
-			rc = sqlite3_step(ppStmt);
-			while(rc == SQLITE_ROW)
-			{
-				int index = 0;
-				visualWordId = sqlite3_column_int(ppStmt, index++);
-				kpt.pt.x = sqlite3_column_double(ppStmt, index++);
-				kpt.pt.y = sqlite3_column_double(ppStmt, index++);
-				kpt.size = sqlite3_column_int(ppStmt, index++);
-				kpt.angle = sqlite3_column_double(ppStmt, index++);
-				kpt.response = sqlite3_column_double(ppStmt, index++);
-				if(uStrNumCmp(_version, "0.12.0") >= 0)
-				{
-					kpt.octave = sqlite3_column_int(ppStmt, index++);
-				}
-
-				if(sqlite3_column_type(ppStmt, index) == SQLITE_NULL)
-				{
-					depth.x = nanFloat;
-					++index;
-				}
-				else
-				{
-					depth.x = sqlite3_column_double(ppStmt, index++);
-				}
-
-				if(sqlite3_column_type(ppStmt, index) == SQLITE_NULL)
-				{
-					depth.y = nanFloat;
-					++index;
-				}
-				else
-				{
-					depth.y = sqlite3_column_double(ppStmt, index++);
-				}
-
-				if(sqlite3_column_type(ppStmt, index) == SQLITE_NULL)
-				{
-					depth.z = nanFloat;
-					++index;
-				}
-				else
-				{
-					depth.z = sqlite3_column_double(ppStmt, index++);
-				}
-
-				visualWordsKpts.push_back(kpt);
-				visualWords.insert(visualWords.end(), std::make_pair(visualWordId, visualWordsKpts.size()-1));
-				visualWords3.push_back(depth);
-
-				if(allWords3NaN && util3d::isFinite(depth))
-				{
-					allWords3NaN = false;
-				}
-
-				if(uStrNumCmp(_version, "0.11.2") >= 0)
-				{
-					descriptorSize = sqlite3_column_int(ppStmt, index++); // VisualWord descriptor size
-					descriptor = sqlite3_column_blob(ppStmt, index); 	// VisualWord descriptor array
-					dRealSize = sqlite3_column_bytes(ppStmt, index++);
-
-					if(descriptor && descriptorSize>0 && dRealSize>0)
-					{
-						cv::Mat d;
-						if(dRealSize == descriptorSize)
-						{
-							// CV_8U binary descriptors
-							d = cv::Mat(1, descriptorSize, CV_8U);
-						}
-						else if(dRealSize/int(sizeof(float)) == descriptorSize)
-						{
-							// CV_32F
-							d = cv::Mat(1, descriptorSize, CV_32F);
-						}
-						else
-						{
-							UFATAL("Saved buffer size (%d bytes) is not the same as descriptor size (%d)", dRealSize, descriptorSize);
-						}
-
-						memcpy(d.data, descriptor, dRealSize);
-
-						descriptors.push_back(d);
-					}
-				}
-
-				rc = sqlite3_step(ppStmt);
-			}
-			UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
-
-			if(visualWords.size()==0)
-			{
-				UDEBUG("Empty signature detected! (id=%d)", (*iter)->id());
-			}
-			else
-			{
-				if(allWords3NaN)
-				{
-					visualWords3.clear();
-				}
-				(*iter)->setWords(visualWords, visualWordsKpts, visualWords3, descriptors);
-				//ULOGGER_DEBUG("Add %d keypoints, %d 3d points and %d descriptors to node %d", (int)visualWords.size(), allWords3NaN?0:(int)visualWords3.size(), (int)descriptors.rows, (*iter)->id());
-			}
-
-			//reset
-			rc = sqlite3_reset(ppStmt);
-			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
-		}
-
-		// Finalize (delete) the statement
-		rc = sqlite3_finalize(ppStmt);
-		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
-	}
-}
-
 void DBDriverSqlite3::loadLinksQuery(
 		int signatureId,
 		std::multimap<int, Link> & links,
@@ -4332,7 +4152,7 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
 			//reset
 			rc = sqlite3_reset(ppStmt);
 			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
-			//UDEBUG("time=%fs, node=%d, links.size=%d", timer.ticks(), (*iter)->id(), links.size());
+			UDEBUG("time=%fs, node=%d, links.size=%d", timer.ticks(), (*iter)->id(), links.size());
 		}
 
 		// Finalize (delete) the statement
@@ -4658,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() ||
@@ -5275,8 +5094,8 @@ std::map<int, Transform> DBDriverSqlite3::loadOptimizedPosesQuery(Transform * la
 					Transform t(serializedPoses.at<float>(i*12), serializedPoses.at<float>(i*12+1), serializedPoses.at<float>(i*12+2), serializedPoses.at<float>(i*12+3),
 							serializedPoses.at<float>(i*12+4), serializedPoses.at<float>(i*12+5), serializedPoses.at<float>(i*12+6), serializedPoses.at<float>(i*12+7),
 							serializedPoses.at<float>(i*12+8), serializedPoses.at<float>(i*12+9), serializedPoses.at<float>(i*12+10), serializedPoses.at<float>(i*12+11));
-					poses.insert(poses.end(), std::make_pair(serializedIds.at<int>(i), t));
-					//UDEBUG("Optimized pose %d: %s", serializedIds.at<int>(i), t.prettyPrint().c_str());
+					poses.insert(std::make_pair(serializedIds.at<int>(i), t));
+					UDEBUG("Optimized pose %d: %s", serializedIds.at<int>(i), t.prettyPrint().c_str());
 				}
 			}
 
@@ -5684,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);
 					}
 				}
@@ -5701,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
@@ -5714,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);
 						}
 					}
@@ -5749,47 +5566,6 @@ cv::Mat DBDriverSqlite3::loadOptimizedMeshQuery(
 	return cloud;
 }
 
-void DBDriverSqlite3::saveFlannIndexQuery(const std::vector<unsigned char> & data) const
-{
-	UDEBUG("");
-	if(_ppDb && uStrNumCmp(_version, "0.23.0") >= 0)
-	{
-		UTimer timer;
-		timer.start();
-		int rc = SQLITE_OK;
-		sqlite3_stmt * ppStmt = 0;
-		std::string query;
-		
-		// Update table Admin
-		query = uFormat("UPDATE Admin SET dictionary_index=? WHERE version='%s';", _version.c_str());
-		rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
-		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
-
-		int index = 1;
-
-		if(data.empty())
-		{
-			rc = sqlite3_bind_null(ppStmt, index++);
-			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
-		}
-		else
-		{
-			rc = sqlite3_bind_blob(ppStmt, index++, data.data(), data.size(), SQLITE_STATIC);
-			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
-		}
-
-		//execute query
-		rc=sqlite3_step(ppStmt);
-		UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
-
-		// Finalize (delete) the statement
-		rc = sqlite3_finalize(ppStmt);
-		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
-
-		UDEBUG("Time=%fs", timer.ticks());
-	}
-}
-
 std::string DBDriverSqlite3::queryStepNode() const
 {
 	if(uStrNumCmp(_version, "0.18.0") >= 0)
@@ -6410,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)
-	{
-		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)
+	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(?,?,?,?,?,?,?,?,?,?,?,?,?,?);";
 	}
@@ -6478,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;
@@ -6797,7 +6555,7 @@ void DBDriverSqlite3::stepLink(
 	{
 		UFATAL("");
 	}
-	//UDEBUG("Save link from %d to %d, type=%d", link.from(), link.to(), link.type());
+	UDEBUG("Save link from %d to %d, type=%d", link.from(), link.to(), link.type());
 
 	// Don't save virtual links
 	if(link.type()==Link::kVirtualClosure)

corelib/src/DBReader.cpp

@@ -260,7 +260,7 @@ bool DBReader::init(
 			else
 			{
 				Signature * s = _dbDriver->loadSignature(*_ids.begin());
-				_dbDriver->loadNodeData(*s);
+				_dbDriver->loadNodeData(s);
 				if( s->sensorData().imageCompressed().empty() &&
 					s->getWords().empty() &&
 					!s->sensorData().laserScanCompressed().empty())
@@ -510,41 +510,22 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
 					}
 					else
 					{
-						// In case the graph was reduced, look for forward neighbor link from previous id
-						bool covAdded = false;
-						if(_currentId != _ids.begin()) {
-							std::set<int>::iterator previousId = _currentId;
-							--previousId;
-							std::multimap<int, Link> previousLinks;
-							_dbDriver->loadLinks(*previousId, previousLinks, Link::kNeighbor);
-							if(previousLinks.size() && previousLinks.rbegin()->first == *_currentId)
-							{
-								// assume the last is the forward neighbor pointing to current ID, take its covariance
-								infMatrix = previousLinks.rbegin()->second.infMatrix();
-								_previousInfMatrix = infMatrix;
-								covAdded = true;
-							}
+						// if localization data saved in database, covariance will be set in a prior link
+						_dbDriver->loadLinks(*_currentId, links, Link::kPosePrior);
+						if(links.size())
+						{
+							// assume the first is the backward neighbor, take its variance
+							infMatrix = links.begin()->second.infMatrix();
+							_previousInfMatrix = infMatrix;
 						}
-
-						if(!covAdded) {
-							// if localization data saved in database, covariance will be set in a prior link
-							_dbDriver->loadLinks(*_currentId, links, Link::kPosePrior);
-							if(links.size())
+						else
+						{
+							if(_previousInfMatrix.empty())
 							{
-								// assume the first is the backward neighbor, take its variance
-								infMatrix = links.begin()->second.infMatrix();
-								_previousInfMatrix = infMatrix;
-							}
-							else
-							{
-								if(_previousInfMatrix.empty())
-								{
-									_previousInfMatrix = cv::Mat::eye(6,6,CV_64FC1);
-								}
-								// we have a node not linked to map, use last variance
-								UWARN("The node loaded (%d) doesn't have neighbor, re-using the covariance of the previous link for odometry.", s->id());
-								infMatrix = _previousInfMatrix;
+								_previousInfMatrix = cv::Mat::eye(6,6,CV_64FC1);
 							}
+							// we have a node not linked to map, use last variance
+							infMatrix = _previousInfMatrix;
 						}
 					}
 				}
@@ -626,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)
@@ -665,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())
@@ -703,7 +671,7 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
 				}
 				if(!combinedModels.empty())
 				{
-					data.setRGBDImage(combinedImages, combinedDepthImages, combinedDepthConfidenceImages, combinedModels);
+					data.setRGBDImage(combinedImages, combinedDepthImages, combinedModels);
 				}
 				else
 				{
@@ -760,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,
@@ -813,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);
-						UASSERT_MSG(cameraIndex >= 0 && cameraIndex < (int)dbModels.size(),
-								uFormat("cameraIndex=%d, db models=%d, kpt.x=%f, image width=%d",
-										cameraIndex, (int)dbModels.size(), keypoints[i].pt.x, subImageWidth).c_str());
-						cv::Point3f pt = util3d::transformPoint(keypoints3D.at(i), dbModels[cameraIndex].localTransform().inverse());
-						pt = util3d::transformPoint(pt, combinedLocalTransforms[cameraIndex]);
+						cv::Point3f pt = util3d::transformPoint(keypoints3D.at(i), dbModels[i].localTransform().inverse());
+						pt = util3d::transformPoint(pt, combinedLocalTransforms[i]);
 						newKeypoints3D.push_back(pt);
 					}
 					data.setFeatures(keypoints, newKeypoints3D, descriptors);
@@ -856,7 +817,6 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
 				if(info)
 				{
 					info->odomPose = pose;
-					UASSERT(!infMatrix.empty());
 					info->odomCovariance = infMatrix.inv();
 					info->odomVelocity = s->getVelocity();
 					UDEBUG("odom variance = %f/%f", info->odomCovariance.at<double>(0,0), info->odomCovariance.at<double>(5,5));

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

@@ -27,16 +27,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap/core/FlannIndex.h>
 #include <rtabmap/utilite/ULogger.h>
-#include <rtabmap/utilite/UTimer.h>
-#include <rtabmap/utilite/UConversion.h>
-#include <rtabmap/core/Compression.h>
-#include <rtabmap/core/Version.h>
-#ifdef WIN32
-#include <rtabmap/core/Parameters.h>
-#endif
 
 #include "rtflann/flann.hpp"
-#include <boost/crc.hpp>
 
 namespace rtabmap {
 
@@ -45,6 +37,7 @@ FlannIndex::FlannIndex():
 		nextIndex_(0),
 		featuresType_(0),
 		featuresDim_(0),
+		isLSH_(false),
 		useDistanceL1_(false),
 		rebalancingFactor_(2.0f)
 {
@@ -56,9 +49,9 @@ FlannIndex::~FlannIndex()
 
 void FlannIndex::release()
 {
+	UDEBUG("");
 	if(index_)
 	{
-		UDEBUG("Clearing flann index...");
 		if(featuresType_ == CV_8UC1)
 		{
 			delete (rtflann::Index<rtflann::Hamming<unsigned char> >*)index_;
@@ -79,139 +72,12 @@ void FlannIndex::release()
 			}
 		}
 		index_ = 0;
-		UDEBUG("Clearing flann index... done!");
 	}
 	nextIndex_ = 0;
+	isLSH_ = false;
 	addedDescriptors_.clear();
 	removedIndexes_.clear();
-}
-
-#define FLANN_INDEX_HEADER_SIZE 12
-
-std::vector<unsigned char> FlannIndex::serializeIndex(bool computeChecksum) const {
-	if(index_ && !addedDescriptors_.empty())
-	{
-#ifdef WIN32
-		UERROR("FLANN index serialization is not yet implemented on Windows. Parameter \"%s\" cannot be used.", Parameters::kKpFlannIndexSaved().c_str());
-#else
-		UTimer timer;
-		const int headerSizeBytes = sizeof(int)*FLANN_INDEX_HEADER_SIZE;
-		std::vector<unsigned char> indexData(1024*1024*100 + headerSizeBytes); // Max 100 MB
-        FILE* indexDataPtr = fmemopen(indexData.data()+headerSizeBytes, indexData.size() - headerSizeBytes, "wb");
-		long bytes_written = 0;
-        if (indexDataPtr) {
-			if(featuresType_ == CV_8UC1)
-			{
-				((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->save(indexDataPtr);
-			}
-			else
-			{
-				if(useDistanceL1_)
-				{
-					((rtflann::Index<rtflann::L1<float> >*)index_)->save(indexDataPtr);;
-				}
-				else if(featuresDim_ <= 3)
-				{
-					((rtflann::Index<rtflann::L2_Simple<float> >*)index_)->save(indexDataPtr);;
-				}
-				else
-				{
-					((rtflann::Index<rtflann::L2<float> >*)index_)->save(indexDataPtr);;
-				}
-			}
-			bytes_written = ftell(indexDataPtr);
-            fclose(indexDataPtr);
-        }
-		if(bytes_written < long(indexData.size()-headerSizeBytes))
-		{
-			//Expected data size and type
-			int dataRows = 0;
-			int dataCols = 0;
-			int dataType = -1;
-			cv::Mat dataset;
-			std::set<int> removedDescriptors;
-			if(computeChecksum){
-				removedDescriptors.insert(removedIndexes_.begin(), removedIndexes_.end());
-			}
-			for(const auto & iter: addedDescriptors_)
-			{
-				UASSERT(!iter.second.empty());
-				dataRows += iter.second.rows;
-				if(dataCols <= 0) {
-					dataCols = iter.second.cols;
-				}
-				else {
-					UASSERT(dataCols == iter.second.cols);
-				}
-				if(dataType < 0) {
-					dataType = iter.second.type();
-				}
-				else {
-					UASSERT(dataType == iter.second.type());
-				}
-				if(computeChecksum){
-					if(removedDescriptors.find(iter.first) == removedDescriptors.end()) {
-						if(dataset.empty()) {
-							dataset = iter.second.clone();
-						}
-						else {
-							dataset.push_back(iter.second);
-						}
-					}
-					else {
-						dataRows -= iter.second.rows;
-					}
-				}
-			}
-			if(!computeChecksum) {
-				for(const auto & index: removedIndexes_)
-				{
-					dataRows -= addedDescriptors_.at(index).rows;
-				}
-			}
-
-			unsigned int crcValue = 0;
-			if(computeChecksum) {
-				boost::crc_32_type result;
-				result.process_bytes(dataset.data, dataset.total()*dataset.elemSize());
-				crcValue = result.checksum(); 
-			}
-			
-			indexData.resize(bytes_written+headerSizeBytes);
-			indexData.shrink_to_fit();
-			int rebalancingFactorAsInt;
-			memcpy(&rebalancingFactorAsInt, &rebalancingFactor_, sizeof(rebalancingFactor_));
-			int crcValueAsInt;
-			memcpy(&crcValueAsInt, &crcValue, sizeof(crcValue));
-			int header[FLANN_INDEX_HEADER_SIZE] = {
-					RTABMAP_VERSION_MAJOR, RTABMAP_VERSION_MINOR, RTABMAP_VERSION_PATCH, // 0,1,2
-					algorithm_,                                                          // 3,
-					featuresDim_,                                                        // 4,
-					useDistanceL1_?1:0,                                                  // 5,
-					rebalancingFactorAsInt,                                              // 6,
-					dataRows,                                                            // 7,
-					dataCols,                                                            // 8,			
-					dataType,                                                            // 9,
-					crcValueAsInt,                                                       // 10         
-					(int)bytes_written};                                                 // 11
-			UDEBUG("Header: \"%d.%d.%d\" alg=%d dim=%d L1=%d factor=%f data(%dx%d type=%d, crc=%X) %d", 
-				header[0],header[1],header[2],
-				header[3],
-				header[4],
-				header[5],
-				rebalancingFactor_,
-				header[7], header[8], header[9], crcValueAsInt, 
-				header[11]);
-			memcpy(indexData.data(), header, headerSizeBytes);
-			return indexData;
-		}
-		else {
-			UERROR("Target buffer too small to serialize index, aborting.");
-		}
-		UDEBUG("Flann serialization: %fs", timer.ticks());
-#endif
-	}
-	return std::vector<unsigned char>();
+	UDEBUG("");
 }
 
 size_t FlannIndex::indexedFeatures() const
@@ -273,13 +139,12 @@ size_t FlannIndex::memoryUsed() const
 	return memoryUsage;
 }
 
-void FlannIndex::buildIndex(
-		flann_algorithm_t algorithm,
+void FlannIndex::buildLinearIndex(
 		const cv::Mat & features,
 		bool useDistanceL1,
 		float rebalancingFactor)
 {
-	UDEBUG("algorithm=%d", (int)algorithm);
+	UDEBUG("");
 	this->release();
 	UASSERT(index_ == 0);
 	UASSERT(features.type() == CV_32FC1 || features.type() == CV_8UC1);
@@ -287,29 +152,8 @@ void FlannIndex::buildIndex(
 	featuresDim_ = features.cols;
 	useDistanceL1_ = useDistanceL1;
 	rebalancingFactor_ = rebalancingFactor;
-	algorithm_ = algorithm;
 
-	rtflann::IndexParams params;
-
-	switch (algorithm)
-	{
-	case FLANN_INDEX_LINEAR:
-		params = rtflann::LinearIndexParams();
-		break;
-	case FLANN_INDEX_KDTREE:
-		params = rtflann::KDTreeIndexParams(4);
-		break;
-	case FLANN_INDEX_KDTREE_SINGLE:
-		params = rtflann::KDTreeSingleIndexParams(10, true);
-		break;
-	case FLANN_INDEX_LSH:
-		UASSERT(features.type() == CV_8UC1);
-		params = rtflann::LshIndexParams(12, 20, 2);
-		break;
-	default:
-		UFATAL("The flann algorithm type %d is not supported!", (int)algorithm);
-		break;
-	}
+	rtflann::LinearIndexParams params;
 
 	if(featuresType_ == CV_8UC1)
 	{
@@ -355,140 +199,13 @@ void FlannIndex::buildIndex(
 	UDEBUG("");
 }
 
-bool FlannIndex::loadIndex(
-	const std::vector<unsigned char> & indexData,
-	flann_algorithm_t algorithm,
-	const cv::Mat & features,
-	bool useDistanceL1,
-	float rebalancingFactor,
-	std::string * error)
+void FlannIndex::buildKDTreeIndex(
+		const cv::Mat & features,
+		int trees,
+		bool useDistanceL1,
+		float rebalancingFactor)
 {
-	return loadIndex(
-		indexData.data(),
-		indexData.size(),
-		algorithm,
-		features,
-		useDistanceL1,
-		rebalancingFactor),
-		error;
-}
-bool FlannIndex::loadIndex(
-	const unsigned char * indexData,
-	size_t indexDataSize,
-	flann_algorithm_t algorithm,
-	const cv::Mat & features,
-	bool useDistanceL1,
-	float rebalancingFactor,
-	std::string * error)
-{
-	UASSERT(indexData!=NULL);
-	if(indexDataSize == 0) {
-		UWARN("Trying to load empty index....");
-		return false;
-	}
-
-#ifdef WIN32
-	UERROR("FLANN index deserialization is not yet implemented on Windows. Index cannot be loaded from memory buffer.");
-	return false;
-#else
-
-	// Check if the features match the expected data from the index
-	size_t headerSizeBytes = sizeof(int)*FLANN_INDEX_HEADER_SIZE;
-	if(indexDataSize < headerSizeBytes) {
-		if(error) {
-			*error = uFormat("Wrong header size detected (%ld vs expected %ld).", indexDataSize, headerSizeBytes);
-		}
-		return false;
-	}
-	const int * header = (const int *)indexData;
-	
-	int savedAlgorithm = header[3];
-	int savedDim = header[4];
-	bool savedDistanceL1 = header[5]==1;
-	float savedRebalancingFactor;
-	memcpy(&savedRebalancingFactor, &header[6], sizeof(header[6]));
-	int savedRows = header[7];
-	int savedCols = header[8];
-	int savedType = header[9];
-	unsigned int savedCrc;
-	 memcpy(&savedCrc, &header[10], sizeof(header[10]));
-	int savedIndexSize = header[11];
-
-	UDEBUG("Header: \"%d.%d.%d\" alg=%d dim=%d L1=%d factor=%f data(%dx%d type=%d, crc=%X) %d", 
-				header[0],header[1],header[2],
-				header[3],
-				header[4],
-				header[5],
-				savedRebalancingFactor,
-				header[7], header[8], header[9], savedCrc, 
-				header[11]);
-
-	if(savedAlgorithm != algorithm) {
-		if(error) {
-			*error = uFormat("Serialized flann algorithm (%d) doesn't match the expected one (%d).", savedAlgorithm, algorithm);
-		}
-		return false;
-	}
-	if(savedDim != features.cols) {
-		if(error) {
-			*error = uFormat("Serialized feature dimension (%d) doesn't match the expected one (%d).", savedDim, features.cols);
-		}
-		return false;
-	}
-	if(savedDistanceL1 != useDistanceL1) {
-		if(error) {
-			*error = uFormat("Serialized \"use distance L1\" (%s) doesn't match the expected one (%s).", savedDistanceL1?"true":"false", useDistanceL1?"true":"false");
-		}
-		return false;
-	}
-	if(savedRebalancingFactor != rebalancingFactor) {
-		if(error) {
-			*error = uFormat("Serialized \"rebalancing factor\" (%f) doesn't match the expected one (%f).", savedRebalancingFactor, rebalancingFactor);
-		}
-		return false;
-	}
-	if(savedRows != features.rows) {
-		if(error) {
-			*error = uFormat("Serialized feature count (%d) doesn't match the expected one (%d).", savedRows, features.rows);
-		}
-		return false;
-	}
-	if(savedCols != features.cols) {
-		if(error) {
-			*error = uFormat("Serialized feature dimension (%d) doesn't match the expected one (%d).", savedCols, features.cols);
-		}
-		return false;
-	}
-	if(savedType != features.type()) {
-		if(error) {
-			*error = uFormat("Serialized feature type (%d) doesn't match the expected one (%d).", savedType, features.type());
-		}
-		return false;
-	}
-	if(savedCrc != 0) {
-		// Compute checksum and compare
-		boost::crc_32_type result;
-		result.process_bytes(features.data, features.total()*features.elemSize());
-		if(savedCrc != result.checksum()) {
-			if(error) {
-				*error = uFormat("Serialized feature crc (%X) doesn't match the expected one (%X).", savedCrc, result.checksum());
-			}
-			return false;
-		}
-	}
-	if(savedIndexSize != int(indexDataSize - headerSizeBytes)) {
-		if(error) {
-			*error = uFormat("Serialized flann index size (%ld) doesn't match the expected one (%ld).", savedIndexSize, indexDataSize - headerSizeBytes);
-		}
-		return false;
-	}
-	if(savedIndexSize == 0) {
-		if(error) {
-			*error = "Serialized flann index is empty.";
-		}
-		return false;
-	}
-
+	UDEBUG("");
 	this->release();
 	UASSERT(index_ == 0);
 	UASSERT(features.type() == CV_32FC1 || features.type() == CV_8UC1);
@@ -496,39 +213,14 @@ bool FlannIndex::loadIndex(
 	featuresDim_ = features.cols;
 	useDistanceL1_ = useDistanceL1;
 	rebalancingFactor_ = rebalancingFactor;
-	algorithm_ = algorithm;
 
-	UDEBUG("algorithm=%d", (int)algorithm);
-
-	rtflann::IndexParams params;
-
-	switch (algorithm)
-	{
-	case FLANN_INDEX_LINEAR:
-		params = rtflann::LinearIndexParams();
-		break;
-	case FLANN_INDEX_KDTREE:
-		params = rtflann::KDTreeIndexParams(4);
-		break;
-	case FLANN_INDEX_KDTREE_SINGLE:
-		params = rtflann::KDTreeSingleIndexParams(10, true);
-		break;
-	case FLANN_INDEX_LSH:
-		UASSERT(features.type() == CV_8UC1);
-		params = rtflann::LshIndexParams(12, 20, 2);
-		break;
-	default:
-		UFATAL("The flann algorithm type %d is not supported!", (int)algorithm);
-		break;
-	}
-
-	FILE* indexDataPtr = fmemopen((void*)(indexData+headerSizeBytes), indexDataSize - headerSizeBytes, "r");
+	rtflann::KDTreeIndexParams params(trees);
 
 	if(featuresType_ == CV_8UC1)
 	{
 		rtflann::Matrix<unsigned char> dataset(features.data, features.rows, features.cols);
 		index_ = new rtflann::Index<rtflann::Hamming<unsigned char> >(dataset, params);
-		((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->load_saved_index(indexDataPtr);
+		((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->buildIndex();
 	}
 	else
 	{
@@ -536,24 +228,22 @@ bool FlannIndex::loadIndex(
 		if(useDistanceL1_)
 		{
 			index_ = new rtflann::Index<rtflann::L1<float> >(dataset, params);
-			((rtflann::Index<rtflann::L1<float> >*)index_)->load_saved_index(indexDataPtr);
+			((rtflann::Index<rtflann::L1<float> >*)index_)->buildIndex();
 		}
 		else if(featuresDim_ <=3)
 		{
 			index_ = new rtflann::Index<rtflann::L2_Simple<float> >(dataset, params);
-			((rtflann::Index<rtflann::L2_Simple<float> >*)index_)->load_saved_index(indexDataPtr);
+			((rtflann::Index<rtflann::L2_Simple<float> >*)index_)->buildIndex();
 		}
 		else
 		{
 			index_ = new rtflann::Index<rtflann::L2<float> >(dataset, params);
-			((rtflann::Index<rtflann::L2<float> >*)index_)->load_saved_index(indexDataPtr);
+			((rtflann::Index<rtflann::L2<float> >*)index_)->buildIndex();
 		}
 	}
-	fclose(indexDataPtr);
 
 	// incremental FLANN: we should add all headers separately in case we remove
 	// some indexes (to keep underlying matrix data allocated)
-	
 	if(rebalancingFactor_ > 1.0f)
 	{
 		for(int i=0; i<features.rows; ++i)
@@ -567,8 +257,107 @@ bool FlannIndex::loadIndex(
 		addedDescriptors_.insert(std::make_pair(nextIndex_, features));
 		nextIndex_ += features.rows;
 	}
-	return true;
-#endif
+	UDEBUG("");
+}
+
+void FlannIndex::buildKDTreeSingleIndex(
+		const cv::Mat & features,
+		int leafMaxSize,
+		bool reorder,
+		bool useDistanceL1,
+		float rebalancingFactor)
+{
+	UDEBUG("");
+	this->release();
+	UASSERT(index_ == 0);
+	UASSERT(features.type() == CV_32FC1 || features.type() == CV_8UC1);
+	featuresType_ = features.type();
+	featuresDim_ = features.cols;
+	useDistanceL1_ = useDistanceL1;
+	rebalancingFactor_ = rebalancingFactor;
+
+	rtflann::KDTreeSingleIndexParams params(leafMaxSize, reorder);
+
+	if(featuresType_ == CV_8UC1)
+	{
+		rtflann::Matrix<unsigned char> dataset(features.data, features.rows, features.cols);
+		index_ = new rtflann::Index<rtflann::Hamming<unsigned char> >(dataset, params);
+		((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->buildIndex();
+	}
+	else
+	{
+		rtflann::Matrix<float> dataset((float*)features.data, features.rows, features.cols);
+		if(useDistanceL1_)
+		{
+			index_ = new rtflann::Index<rtflann::L1<float> >(dataset, params);
+			((rtflann::Index<rtflann::L1<float> >*)index_)->buildIndex();
+		}
+		else if(featuresDim_ <=3)
+		{
+			index_ = new rtflann::Index<rtflann::L2_Simple<float> >(dataset, params);
+			((rtflann::Index<rtflann::L2_Simple<float> >*)index_)->buildIndex();
+		}
+		else
+		{
+			index_ = new rtflann::Index<rtflann::L2<float> >(dataset, params);
+			((rtflann::Index<rtflann::L2<float> >*)index_)->buildIndex();
+		}
+	}
+
+	// incremental FLANN: we should add all headers separately in case we remove
+	// some indexes (to keep underlying matrix data allocated)
+	if(rebalancingFactor_ > 1.0f)
+	{
+		for(int i=0; i<features.rows; ++i)
+		{
+			addedDescriptors_.insert(std::make_pair(nextIndex_++, features.row(i)));
+		}
+	}
+	else
+	{
+		// tree won't ever be rebalanced, so just keep only one header for the data
+		addedDescriptors_.insert(std::make_pair(nextIndex_, features));
+		nextIndex_ += features.rows;
+	}
+	UDEBUG("");
+}
+
+void FlannIndex::buildLSHIndex(
+		const cv::Mat & features,
+		unsigned int table_number,
+		unsigned int key_size,
+		unsigned int multi_probe_level,
+		float rebalancingFactor)
+{
+	UDEBUG("");
+	this->release();
+	UASSERT(index_ == 0);
+	UASSERT(features.type() == CV_8UC1);
+	featuresType_ = features.type();
+	featuresDim_ = features.cols;
+	useDistanceL1_ = true;
+	rebalancingFactor_ = rebalancingFactor;
+
+	rtflann::Matrix<unsigned char> dataset(features.data, features.rows, features.cols);
+	index_ = new rtflann::Index<rtflann::Hamming<unsigned char> >(dataset, rtflann::LshIndexParams(12, 20, 2));
+	((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->buildIndex();
+
+	// incremental FLANN: we should add all headers separately in case we remove
+	// some indexes (to keep underlying matrix data allocated)
+	if(rebalancingFactor_ > 1.0f)
+	{
+		for(int i=0; i<features.rows; ++i)
+		{
+			addedDescriptors_.insert(std::make_pair(nextIndex_++, features.row(i)));
+		}
+	}
+	else
+	{
+		// tree won't ever be rebalanced, so just keep only one header for the data
+		addedDescriptors_.insert(std::make_pair(nextIndex_, features));
+		nextIndex_ += features.rows;
+	}
+	UDEBUG("");
 }
 
 bool FlannIndex::isBuilt()

corelib/src/GlobalMap.cpp

@@ -99,12 +99,15 @@ unsigned long GlobalMap::getMemoryUsed() const
 	return memoryUsage;
 }
 
-bool GlobalMap::fullUpdateNeeded(const std::map<int, Transform> & poses) const
+bool GlobalMap::update(const std::map<int, Transform> & poses)
 {
+	UDEBUG("Update (poses=%d addedNodes_=%d)", (int)poses.size(), (int)addedNodes_.size());
+
+	// First, check of the graph has changed. If so, re-create the octree by moving all occupied nodes.
 	bool graphOptimized = false; // If a loop closure happened (e.g., poses are modified)
 	bool graphChanged = addedNodes_.size()>0; // If the new map doesn't have any node from the previous map
 	float updateErrorSqrd = updateError_*updateError_;
-	for(std::map<int, Transform>::const_iterator iter=addedNodes_.begin(); iter!=addedNodes_.end(); ++iter)
+	for(std::map<int, Transform>::iterator iter=addedNodes_.begin(); iter!=addedNodes_.end(); ++iter)
 	{
 		std::map<int, Transform>::const_iterator jter = poses.find(iter->first);
 		if(jter != poses.end())
@@ -122,15 +125,7 @@ bool GlobalMap::fullUpdateNeeded(const std::map<int, Transform> & poses) const
 		}
 	}
 
-	return graphOptimized || graphChanged;
-}
-
-bool GlobalMap::update(const std::map<int, Transform> & poses)
-{
-	UDEBUG("Update (poses=%d addedNodes_=%d)", (int)poses.size(), (int)addedNodes_.size());
-
-	// First, check of the graph has changed. If so, re-create the octree by moving all occupied nodes.
-	if(fullUpdateNeeded(poses))
+	if(graphOptimized || graphChanged)
 	{
 		// clear all but keep cache
 		clear();

corelib/src/Graph.cpp

@@ -196,7 +196,7 @@ bool exportPoses(
 
 bool importPoses(
 		const std::string & filePath,
-		int format, // 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame, 11=10+ID), 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe, 9=EuRoC MAV, 12=rgbd_bonn
+		int format, // 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame, 11=10+ID), 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe, 9=EuRoC MAV
 		std::map<int, Transform> & poses,
 		std::multimap<int, Link> * constraints, // optional for formats 3 and 4
 		std::map<int, double> * stamps) // optional for format 1 and 9
@@ -440,15 +440,11 @@ bool importPoses(
 					UERROR("Error parsing \"%s\" with NewCollege format (should have 3 values: stamp x y, found %d)", str.c_str(), (int)strList.size());
 				}
 			}
-			else if(format == 1 || format==10 || format==11 || format==12) // rgbd-slam format
+			else if(format == 1 || format==10 || format==11) // rgbd-slam format
 			{
 				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)
@@ -481,20 +477,6 @@ bool importPoses(
 										   1, 0, 0, 0);
 							pose = t*pose;
 						}
-						else if(format == 12)
-						{
-							// See https://www.ipb.uni-bonn.de/data/rgbd-dynamic-dataset/index.html
-							Transform T_ros(-1, 0, 0, 0,
-											 0, 0, 1, 0,
-											 0, 1, 0, 0);
-							Transform T_m(
-										1.0157,    0.1828,   -0.2389,    0.0113,
-										0.0009,   -0.8431,   -0.6413,   -0.00980,
-										-0.3009,    0.6147,   -0.8085,    0.0111);
-
-							// we remove the optical rotation
-							pose = T_ros*pose*T_ros*T_m*CameraModel::opticalRotation().inverse();
-						}
 						poses.insert(std::make_pair(id, pose));
 					}
 				}
@@ -762,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);
-		float t_err = pose_delta_result.getDistance(pose_delta_gt);
+		Transform pose_error        = pose_delta_result.inverse()*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));
@@ -1010,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;
-				if(force3DoF)
-				{
-					float opt_roll,opt_pitch,opt_yaw;
-					float link_roll,link_pitch,link_yaw;
-					t.getEulerAngles(opt_roll, opt_pitch, opt_yaw);
-					linkT.getEulerAngles(link_roll, link_pitch, link_yaw);
-					angularError = fabs(opt_yaw - link_yaw);
-					angularError = angularError>M_PI?2*M_PI-angularError:angularError;
-				}
-				else
-				{
-					angularError = t.getAngle(linkT);
-				}
-
+				float opt_roll,opt_pitch,opt_yaw;
+				float link_roll,link_pitch,link_yaw;
+				t.getEulerAngles(opt_roll, opt_pitch, opt_yaw);
+				linkT.getEulerAngles(link_roll, link_pitch, link_yaw);
+				float angularError = uMax3(
+						force3DoF?0:fabs(opt_roll - link_roll),
+						force3DoF?0:fabs(opt_pitch - link_pitch),
+						fabs(opt_yaw - link_yaw));
+				angularError = angularError>M_PI?2*M_PI-angularError:angularError;
 				UASSERT(iter->second.rotVariance(false)>0.0);
 				float stddevAngular = sqrt(iter->second.rotVariance(false));
 				float angularErrorRatio = angularError/stddevAngular;

corelib/src/LocalGrid.cpp

@@ -64,8 +64,8 @@ void LocalGridCache::add(int nodeId,
 
 void LocalGridCache::add(int nodeId, const LocalGrid & localGrid)
 {
-	//UDEBUG("nodeId=%d (ground=%d/%d obstacles=%d/%d empty=%d/%d)",
-	//		nodeId, localGrid.groundCells.cols,  localGrid.groundCells.channels(),  localGrid.obstacleCells.cols,  localGrid.obstacleCells.channels(), localGrid.emptyCells.cols,  localGrid.emptyCells.channels());
+	UDEBUG("nodeId=%d (ground=%d/%d obstacles=%d/%d empty=%d/%d)",
+			nodeId, localGrid.groundCells.cols,  localGrid.groundCells.channels(),  localGrid.obstacleCells.cols,  localGrid.obstacleCells.channels(), localGrid.emptyCells.cols,  localGrid.emptyCells.channels());
 	if(nodeId < 0)
 	{
 		UWARN("Cannot add nodes with negative id (nodeId=%d)", nodeId);

corelib/src/Memory.cpp

@@ -76,7 +76,6 @@ Memory::Memory(const ParametersMap & parameters) :
 	_similarityThreshold(Parameters::defaultMemRehearsalSimilarity()),
 	_binDataKept(Parameters::defaultMemBinDataKept()),
 	_rawDescriptorsKept(Parameters::defaultMemRawDescriptorsKept()),
-	_loadVisualLocalFeaturesOnInit(Parameters::defaultMemLoadVisualLocalFeaturesOnInit()),
 	_saveDepth16Format(Parameters::defaultMemSaveDepth16Format()),
 	_notLinkedNodesKeptInDb(Parameters::defaultMemNotLinkedNodesKept()),
 	_saveIntermediateNodeData(Parameters::defaultMemIntermediateNodeDataKept()),
@@ -84,7 +83,6 @@ Memory::Memory(const ParametersMap & parameters) :
 	_depthCompressionFormat(Parameters::defaultMemDepthCompressionFormat()),
 	_incrementalMemory(Parameters::defaultMemIncrementalMemory()),
 	_localizationDataSaved(Parameters::defaultMemLocalizationDataSaved()),
-	_flannIndexSaved(Parameters::defaultKpFlannIndexSaved()),
 	_reduceGraph(Parameters::defaultMemReduceGraph()),
 	_maxStMemSize(Parameters::defaultMemSTMSize()),
 	_recentWmRatio(Parameters::defaultMemRecentWmRatio()),
@@ -131,7 +129,6 @@ Memory::Memory(const ParametersMap & parameters) :
 	_linksChanged(false),
 	_signaturesAdded(0),
 	_allNodesInWM(true),
-	_receivingOdometryFeatures(false),
 	_badSignRatio(Parameters::defaultKpBadSignRatio()),
 	_tfIdfLikelihoodUsed(Parameters::defaultKpTfIdfLikelihoodUsed()),
 	_parallelized(Parameters::defaultKpParallelized()),
@@ -248,13 +245,13 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
 			if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading all nodes to WM...")));
 			std::set<int> ids;
 			_dbDriver->getAllNodeIds(ids, true);
-			_dbDriver->loadSignatures(std::list<int>(ids.begin(), ids.end()), dbSignatures, 0, !_loadVisualLocalFeaturesOnInit);
+			_dbDriver->loadSignatures(std::list<int>(ids.begin(), ids.end()), dbSignatures);
 		}
 		else
 		{
 			// load previous session working memory
 			if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading last nodes to WM...")));
-			_dbDriver->loadLastNodes(dbSignatures, !_loadVisualLocalFeaturesOnInit);
+			_dbDriver->loadLastNodes(dbSignatures);
 		}
 		for(std::list<Signature*>::reverse_iterator iter=dbSignatures.rbegin(); iter!=dbSignatures.rend(); ++iter)
 		{
@@ -420,22 +417,20 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
 			}
 			else
 			{
-				_dbDriver->load(*_vwd, false);
+				_dbDriver->load(_vwd, false);
 			}
 		}
 		else
 		{
 			UDEBUG("load words");
 			// load the last dictionary
-			_dbDriver->load(*_vwd, _vwd->isIncremental());
+			_dbDriver->load(_vwd, _vwd->isIncremental());
 		}
 		UDEBUG("%d words loaded!", _vwd->getUnusedWordsSize());
 		_vwd->update();
 		if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Loading dictionary, done! (%d words)", (int)_vwd->getUnusedWordsSize())));
 
 		if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Adding word references...")));
-		UDEBUG("Adding word references...");
-		UTimer timer;
 		// Enable loaded signatures
 		const std::map<int, Signature *> & signatures = this->getSignatures();
 		for(std::map<int, Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
@@ -446,7 +441,7 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
 			const std::multimap<int, int> & words = s->getWords();
 			if(words.size())
 			{
-				//UDEBUG("node=%d, word references=%d", s->id(), words.size());
+				UDEBUG("node=%d, word references=%d", s->id(), words.size());
 				for(std::multimap<int, int>::const_iterator iter = words.begin(); iter!=words.end(); ++iter)
 				{
 					if(iter->first > 0)
@@ -463,7 +458,7 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
 		{
 			UWARN("_vwd->getUnusedWordsSize() must be empty... size=%d", _vwd->getUnusedWordsSize());
 		}
-		UDEBUG("Total word references added = %d (in %f s)", _vwd->getTotalActiveReferences(), timer.ticks());
+		UDEBUG("Total word references added = %d", _vwd->getTotalActiveReferences());
 
 		if(_lastSignature == 0)
 		{
@@ -487,37 +482,6 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
 	UDEBUG("map ids start with %d", _idMapCount);
 }
 
-void Memory::saveFlannIndex(bool postInitClosingEvents)
-{
-	if(!_dbDriver) {
-		return;
-	}
-	if(uStrNumCmp(_dbDriver->getDatabaseVersion(), "0.23.0") >= 0) {
-		if(_flannIndexSaved && !_incrementalMemory) {
-			if(_vwd->isModified()) {
-				UINFO("Saving flann index to database... (%s=true)", Parameters::kKpFlannIndexSaved().c_str());
-				if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Saving flann index to database..."));
-				_dbDriver->saveFlannIndex(_vwd->serializeIndex());
-				if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Saving flann index to database, done!"));
-			}
-			else
-			{
-				UDEBUG("The dictionary didn't change since loaded, do not need to save again to database.");
-			}
-		}
-		else {
-			// clear if exists
-			_dbDriver->saveFlannIndex(std::vector<unsigned char>());
-		}
-	}
-	else if(_flannIndexSaved)
-	{
-		UWARN("Parameter %s is enabled, but database version is too old (%s < 0.23). Flann index cannot be saved.",
-			Parameters::kKpFlannIndexSaved().c_str(),
-			_dbDriver->getDatabaseVersion().c_str());
-	}
-}
-
 void Memory::close(bool databaseSaved, bool postInitClosingEvents, const std::string & ouputDatabasePath)
 {
 	UINFO("databaseSaved=%d, postInitClosingEvents=%d", databaseSaved?1:0, postInitClosingEvents?1:0);
@@ -529,8 +493,6 @@ void Memory::close(bool databaseSaved, bool postInitClosingEvents, const std::st
 		databaseNameChanged = ouputDatabasePath.size() && _dbDriver->getUrl().size() && _dbDriver->getUrl().compare(ouputDatabasePath) != 0?true:false;
 	}
 
-	UDEBUG("_memoryChanged=%d _linksChanged=%d databaseNameChanged=%d", _memoryChanged?1:0, _linksChanged?1:0, databaseNameChanged?1:0);
-
 	if(!databaseSaved || (!_memoryChanged && !_linksChanged && !databaseNameChanged))
 	{
 		if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("No changes added to database.")));
@@ -538,7 +500,6 @@ void Memory::close(bool databaseSaved, bool postInitClosingEvents, const std::st
 		UINFO("No changes added to database.");
 		if(_dbDriver)
 		{
-			saveFlannIndex(postInitClosingEvents);
 			if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Closing database \"%s\"...", _dbDriver->getUrl().c_str())));
 			_dbDriver->closeConnection(false, ouputDatabasePath);
 			delete _dbDriver;
@@ -553,15 +514,11 @@ void Memory::close(bool databaseSaved, bool postInitClosingEvents, const std::st
 	{
 		UINFO("Saving memory...");
 		if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Saving memory..."));
-		if(!_memoryChanged && _dbDriver)
+		if(!_memoryChanged && _linksChanged && _dbDriver)
 		{
-			saveFlannIndex(postInitClosingEvents);
-
-			if(_linksChanged) {
-				// don't update the time stamps!
-				UDEBUG("");
-				_dbDriver->setTimestampUpdateEnabled(false);
-			}
+			// don't update the time stamps!
+			UDEBUG("");
+			_dbDriver->setTimestampUpdateEnabled(false);
 		}
 		this->clear();
 		if(_dbDriver)
@@ -608,7 +565,6 @@ void Memory::parseParameters(const ParametersMap & parameters)
 
 	Parameters::parse(params, Parameters::kMemBinDataKept(), _binDataKept);
 	Parameters::parse(params, Parameters::kMemRawDescriptorsKept(), _rawDescriptorsKept);
-	Parameters::parse(params, Parameters::kMemLoadVisualLocalFeaturesOnInit(), _loadVisualLocalFeaturesOnInit);
 	Parameters::parse(params, Parameters::kMemSaveDepth16Format(), _saveDepth16Format);
 	Parameters::parse(params, Parameters::kMemReduceGraph(), _reduceGraph);
 	Parameters::parse(params, Parameters::kMemNotLinkedNodesKept(), _notLinkedNodesKeptInDb);
@@ -662,7 +618,6 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(params, Parameters::kMarkerVarianceAngular(), _markerAngVariance);
 	Parameters::parse(params, Parameters::kMarkerVarianceOrientationIgnored(), _markerOrientationIgnored);
 	Parameters::parse(params, Parameters::kMemLocalizationDataSaved(), _localizationDataSaved);
-	Parameters::parse(params, Parameters::kKpFlannIndexSaved(), _flannIndexSaved);
 
 	if(_markerAngVariance>=9999)
 	{
@@ -699,7 +654,10 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	}
 
 	// Keypoint stuff
-	_vwd->parseParameters(params);
+	if(_vwd)
+	{
+		_vwd->parseParameters(params);
+	}
 
 	Parameters::parse(params, Parameters::kKpTfIdfLikelihoodUsed(), _tfIdfLikelihoodUsed);
 	Parameters::parse(params, Parameters::kKpParallelized(), _parallelized);
@@ -898,7 +856,7 @@ void Memory::preUpdate()
 	{
 		this->cleanUnusedWords();
 	}
-	if(!_parallelized)
+	if(_vwd && !_parallelized)
 	{
 		//When parallelized, it is done in CreateSignature
 		_vwd->update();
@@ -1156,7 +1114,10 @@ void Memory::addSignatureToStm(Signature * signature, const cv::Mat & covariance
 		}
 		++_signaturesAdded;
 
-		UDEBUG("%d words ref for the signature %d (weight=%d)", signature->getWords().size(), signature->id(), signature->getWeight());
+		if(_vwd)
+		{
+			UDEBUG("%d words ref for the signature %d (weight=%d)", signature->getWords().size(), signature->id(), signature->getWeight());
+		}
 		if(signature->getWords().size())
 		{
 			signature->setEnabled(true);
@@ -1263,17 +1224,16 @@ void Memory::moveSignatureToWMFromSTM(int id, int * reducedTo)
 				std::multimap<int, Link> linksCopy = links;
 				for(std::multimap<int, Link>::iterator iter=linksCopy.begin(); iter!=linksCopy.end(); ++iter)
 				{
-					if(iter->second.type() == Link::kNeighborMerged)
+					if(iter->second.type() == Link::kNeighbor ||
+					   iter->second.type() == Link::kNeighborMerged)
 					{
-						// Removing only merged neighbor links, we keep original neighbor 
-						// links to be able to reprocess databases with correct odometry covariance.
 						s->removeLink(iter->first);
-					}
-					if(iter->second.type() == Link::kNeighbor)
-					{
-						if(_lastGlobalLoopClosureId == s->id())
+						if(iter->second.type() == Link::kNeighbor)
 						{
-							_lastGlobalLoopClosureId = iter->first;
+							if(_lastGlobalLoopClosureId == s->id())
+							{
+								_lastGlobalLoopClosureId = iter->first;
+							}
 						}
 					}
 				}
@@ -1876,24 +1836,13 @@ void Memory::clear()
 	UDEBUG("");
 
 	//Get the tree root (parents)
-	if(!_dbDriver) {
-		// We are not saving to database anyway, just delete now.
-		for(std::map<int, Signature *>::iterator iter=_signatures.begin(); iter!=_signatures.end(); ++iter)
+	std::map<int, Signature*> mem = _signatures;
+	for(std::map<int, Signature *>::iterator i=mem.begin(); i!=mem.end(); ++i)
+	{
+		if(i->second)
 		{
-			delete iter->second;
-		}
-		_workingMem.clear();
-		_signatures.clear();
-	}
-	else {
-		std::map<int, Signature*> mem = _signatures;
-		for(std::map<int, Signature *>::iterator i=mem.begin(); i!=mem.end(); ++i)
-		{
-			if(i->second)
-			{
-				//UDEBUG("deleting from the working and the short-term memory: %d", i->first);
-				this->moveToTrash(i->second);
-			}
+			UDEBUG("deleting from the working and the short-term memory: %d", i->first);
+			this->moveToTrash(i->second);
 		}
 	}
 
@@ -1917,7 +1866,6 @@ void Memory::clear()
 	UDEBUG("");
 	_lastSignature = 0;
 	_lastGlobalLoopClosureId = 0;
-	_signaturesAdded = 0;
 	_idCount = kIdStart;
 	_idMapCount = kIdStart;
 	_memoryChanged = false;
@@ -1931,7 +1879,6 @@ void Memory::clear()
 	_landmarksIndex.clear();
     _landmarksSize.clear();
 	_allNodesInWM = true;
-	_receivingOdometryFeatures = false;
 
 	if(_dbDriver)
 	{
@@ -1939,7 +1886,14 @@ void Memory::clear()
 		cleanUnusedWords();
 		_dbDriver->emptyTrashes();
 	}
-	_vwd->clear(_dbDriver!=NULL);
+	else
+	{
+		cleanUnusedWords();
+	}
+	if(_vwd)
+	{
+		_vwd->clear();
+	}
 	UDEBUG("");
 }
 
@@ -2474,7 +2428,7 @@ std::list<Signature *> Memory::getRemovableSignatures(int count, const std::set<
  */
 void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> * deletedWords)
 {
-	//UDEBUG("id=%d", s?s->id():0);
+	UDEBUG("id=%d", s?s->id():0);
 	if(s)
 	{
 		// Cleanup landmark indexes
@@ -2967,37 +2921,6 @@ Transform Memory::computeTransform(
 			_registrationPipeline->isScanRequired()?&laserBuf:0,
 			_registrationPipeline->isUserDataRequired()?&userBuf:0);
 
-	// Load word descriptors and keypoints on-demand if necessary
-	if( !_reextractLoopClosureFeatures &&
-		(_registrationPipeline->isImageRequired() || guess.isNull()) &&
-		!fromS.getWords().empty() && fromS.getWordsKpts().empty() &&
-		_dbDriver)
-	{
-		// We assume "toS" has already features in RAM, so just lookup "fromS"
-		UDEBUG("Loading local visual features for signature %d", fromS.id());
-		std::multimap<int, int> words;
-		std::vector<cv::KeyPoint> keypoints;
-		std::vector<cv::Point3f> points;
-		cv::Mat descriptors;
-		UTimer timer;
-		_dbDriver->getLocalFeatures(fromS.id(), words, keypoints, points, descriptors);
-		if(!words.empty() && !keypoints.empty()) {
-			UASSERT(words.size() == fromS.getWords().size());
-			std::map<int, int> wordsChanged = fromS.getWordsChanged();
-			bool wasEnabled = fromS.isEnabled();
-			fromS.setWords(words, keypoints, points, descriptors);
-			for(const auto & iter: wordsChanged) {
-				fromS.changeWordsRef(iter.first, iter.second);
-			}
-			fromS.setEnabled(wasEnabled);
-			UDEBUG("Loaded %ld local visual features for signature %d! (in %f s)", words.size(), fromS.id(), timer.ticks());
-		}
-		else
-		{
-			UDEBUG("Failed to load local visual features for signature %d.", fromS.id());
-		}
-		
-	}
 
 	// compute transform fromId -> toId
 	std::vector<int> inliersV;
@@ -3057,10 +2980,8 @@ Transform Memory::computeTransform(
 			   !_invertedReg &&
 			   !tmpTo.getWordsDescriptors().empty() &&
 			   !tmpTo.getWords().empty() &&
-			   !tmpTo.getWordsKpts().empty() &&
 			   !tmpFrom.getWordsDescriptors().empty() &&
 			   !tmpFrom.getWords().empty() &&
-			   !tmpFrom.getWordsKpts().empty() &&
 			   !tmpFrom.getWords3().empty() &&
 			   fromS.hasLink(0, Link::kNeighbor)) // If doesn't have neighbors, skip bundle
 		{
@@ -3094,12 +3015,8 @@ Transform Memory::computeTransform(
 				if(id != fromS.id() && iter->second.type() == Link::kNeighbor) // assemble only neighbors for the local feature map
 				{
 					const Signature * s = this->getSignature(id);
-					if(s)
+					if(s && !s->getWords3().empty())
 					{
-						if(s->getWordsKpts().empty() && s->getWords3().empty() && s->getWordsDescriptors().empty()) {
-							UDEBUG("Signature %d doesn't have features set. Cannot be added in the local feature map.", s->id());
-							continue;
-						}
 						const std::map<int, int> & wordsTo = uMultimapToMapUnique(s->getWords());
 						for(std::map<int, int>::const_iterator jter=wordsTo.begin(); jter!=wordsTo.end(); ++jter)
 						{
@@ -3198,11 +3115,6 @@ Transform Memory::computeTransform(
 								bundlePoses.insert(std::make_pair(id, iter->second.transform()));
 							}
 
-							if(s->getWordsKpts().empty())
-							{
-								UDEBUG("Signature %d doesn't have features set. Keypoints won't be added in local bundle adjustment.", s->id());
-								continue;
-							}
 							const std::map<int,int> & words = uMultimapToMapUnique(s->getWords());
 							for(std::map<int, int>::const_iterator jter=words.begin(); jter!=words.end(); ++jter)
 							{
@@ -3678,7 +3590,7 @@ void Memory::removeAllVirtualLinks()
 
 void Memory::removeVirtualLinks(int signatureId)
 {
-	//UDEBUG("");
+	UDEBUG("");
 	Signature * s = this->_getSignature(signatureId);
 	if(s)
 	{
@@ -3717,7 +3629,10 @@ void Memory::dumpMemory(std::string directory) const
 
 void Memory::dumpDictionary(const char * fileNameRef, const char * fileNameDesc) const
 {
-	_vwd->exportDictionary(fileNameRef, fileNameDesc);
+	if(_vwd)
+	{
+		_vwd->exportDictionary(fileNameRef, fileNameDesc);
+	}
 }
 
 void Memory::dumpSignatures(const char * fileNameSign, bool words3D) const
@@ -3838,7 +3753,10 @@ unsigned long Memory::getMemoryUsed() const
 	{
 		memoryUsage += iter->second->getMemoryUsed(true);
 	}
-	memoryUsage += _vwd->getMemoryUsed();
+	if(_vwd)
+	{
+		memoryUsage += _vwd->getMemoryUsed();
+	}
 	memoryUsage += _stMem.size() * (sizeof(int)+sizeof(std::set<int>::iterator)) + sizeof(std::set<int>);
 	memoryUsage += _workingMem.size() * (sizeof(int)+sizeof(double)+sizeof(std::map<int, double>::iterator)) + sizeof(std::map<int, double>);
 	memoryUsage += _groundTruths.size() * (sizeof(int)+sizeof(Transform)+12*sizeof(float) + sizeof(std::map<int, Transform>::iterator)) + sizeof(std::map<int, Transform>);
@@ -4303,11 +4221,6 @@ void Memory::getNodeWordsAndGlobalDescriptors(int nodeId,
 			}
 		}
 	}
-	if(!words.empty() && wordsKpts.empty() && _dbDriver)
-	{
-		std::multimap<int, int> tmpWords;
-		_dbDriver->getLocalFeatures(nodeId, tmpWords, wordsKpts, words3, wordsDescriptors);
-	}
 }
 
 void Memory::getNodeCalibration(int nodeId,
@@ -4852,11 +4765,6 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 	{
 		meanWordsPerLocation = _vwd->getTotalActiveReferences() / (treeSize-1); // ignore virtual signature
 	}
-	else if(_useOdometryFeatures) {
-		// To not detect first image as bad signature if odometry 
-		// is using less features than feature2D->getMaxFeatures()
-		meanWordsPerLocation = 0;
-	}
 
 	if(_parallelized && !isIntermediateNode)
 	{
@@ -4960,12 +4868,10 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 	SensorData decimatedData;
 	UDEBUG("Received kpts=%d kpts3D=%d, descriptors=%d _useOdometryFeatures=%s",
 			(int)data.keypoints().size(), (int)data.keypoints3D().size(), data.descriptors().rows, _useOdometryFeatures?"true":"false");
-	// TODO: do we still need the third and fouth comparisons?
-	// TODO: there is significant repetitive code between the if and the else, could we combine them?!
 	if(!_useOdometryFeatures ||
-		(!_receivingOdometryFeatures && data.keypoints().empty()) ||
+		data.keypoints().empty() ||
 		(int)data.keypoints().size() != data.descriptors().rows ||
-		(!_receivingOdometryFeatures && _feature2D->getType() == Feature2D::kFeatureOrbOctree && data.descriptors().empty()))
+		(_feature2D->getType() == Feature2D::kFeatureOrbOctree && data.descriptors().empty()))
 	{
 		if(_feature2D->getMaxFeatures() >= 0 && !data.imageRaw().empty() && !isIntermediateNode)
 		{
@@ -4987,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)
@@ -5002,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);
 				}
 
@@ -5302,7 +5206,6 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 	}
 	else if(_feature2D->getMaxFeatures() >= 0 && !isIntermediateNode)
 	{
-		_receivingOdometryFeatures = true;
 		UINFO("Use odometry features: kpts=%d 3d=%d desc=%d (dim=%d, type=%d)",
 				(int)data.keypoints().size(),
 				(int)data.keypoints3D().size(),
@@ -5753,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();
 
@@ -5765,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();
 		}
@@ -5979,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);
 
@@ -6029,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())
@@ -6047,10 +5944,6 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			{
 				ctDepth.start();
 			}
-			if(!depthConfidence.empty())
-			{
-				ctDepthConfidence.start();
-			}
 			if(!laserScan.isEmpty())
 			{
 				ctLaserScan.start();
@@ -6061,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();
 		}
@@ -6075,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());
 		}
@@ -6126,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,
@@ -6224,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
 	{
@@ -6425,7 +6314,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 
 void Memory::disableWordsRef(int signatureId)
 {
-	//UDEBUG("id=%d", signatureId);
+	UDEBUG("id=%d", signatureId);
 
 	Signature * ss = this->_getSignature(signatureId);
 	if(ss && ss->isEnabled())
@@ -6441,7 +6330,7 @@ void Memory::disableWordsRef(int signatureId)
 
 		count -= _vwd->getTotalActiveReferences();
 		ss->setEnabled(false);
-		//UDEBUG("%d words total ref removed from signature %d... (total active ref = %d)", count, ss->id(), _vwd->getTotalActiveReferences());
+		UDEBUG("%d words total ref removed from signature %d... (total active ref = %d)", count, ss->id(), _vwd->getTotalActiveReferences());
 	}
 }
 
@@ -6504,7 +6393,7 @@ void Memory::enableWordsRef(const std::list<int> & signatureIds)
 
 	UDEBUG("oldWordIds.size()=%d, getOldIds time=%fs", oldWordIds.size(), timer.ticks());
 
-	// the words were deleted, so try to match it with an active word
+	// the words were deleted, so try to math it with an active word
 	std::list<VisualWord *> vws;
 	if(oldWordIds.size() && _dbDriver)
 	{

corelib/src/Odometry.cpp

@@ -36,10 +36,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/odometry/OdometryLOAM.h"
 #include "rtabmap/core/odometry/OdometryFLOAM.h"
 #include "rtabmap/core/odometry/OdometryMSCKF.h"
-#include "rtabmap/core/odometry/OdometryVINSFusion.h"
+#include "rtabmap/core/odometry/OdometryVINS.h"
 #include "rtabmap/core/odometry/OdometryOpenVINS.h"
 #include "rtabmap/core/odometry/OdometryOpen3D.h"
-#include "rtabmap/core/odometry/OdometryCuVSLAM.h"
 #include "rtabmap/core/OdometryInfo.h"
 #include "rtabmap/core/util3d.h"
 #include "rtabmap/core/util3d_mapping.h"
@@ -104,8 +103,8 @@ Odometry * Odometry::create(Odometry::Type & type, const ParametersMap & paramet
 	case Odometry::kTypeMSCKF:
 		odometry = new OdometryMSCKF(parameters);
 		break;
-	case Odometry::kTypeVINSFusion:
-		odometry = new OdometryVINSFusion(parameters);
+	case Odometry::kTypeVINS:
+		odometry = new OdometryVINS(parameters);
 		break;
 	case Odometry::kTypeOpenVINS:
 		odometry = new OdometryOpenVINS(parameters);
@@ -113,9 +112,6 @@ Odometry * Odometry::create(Odometry::Type & type, const ParametersMap & paramet
 	case Odometry::kTypeOpen3D:
 		odometry = new OdometryOpen3D(parameters);
 		break;
-	case Odometry::kTypeCuVSLAM:
-		odometry = new OdometryCuVSLAM(parameters);
-		break;
 	default:
 		UERROR("Unknown odometry type %d, using F2M instead...", (int)type);
 		odometry = new OdometryF2M(parameters);
@@ -326,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));
@@ -629,7 +623,6 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 	if(!guessIn.isNull())
 	{
 		guess = guessIn;
-		UDEBUG("Using provided guess %s", guessIn.prettyPrint().c_str());
 	}
 	else if(!imus_.empty())
 	{
@@ -646,16 +639,12 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 			{
 				guess = guess.to3DoF();
 			}
-			UDEBUG("Adjusting guess from motion with IMU %s", guess.prettyPrint().c_str());
 		}
 		else if(!imuLastTransform_.isNull())
 		{
 			UWARN("Could not find imu transform at %f", data.stamp());
 		}
 	}
-	else if(!guess.isNull()) {
-		UDEBUG("Using guess from motion %s", guess.prettyPrint().c_str());
-	}
 
 	UTimer time;
 
@@ -1020,28 +1009,21 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 		--_resetCurrentCount;
 		if(_resetCurrentCount == 0)
 		{
-			if(!guess.isNull() && !guessIn.isNull()) {
-				UWARN("Odometry automatically reset to latest pose (%s) + guess (%s)!", _pose.prettyPrint().c_str(), guess.prettyPrint().c_str());
-				this->reset(_pose * guess);
-			}
-			else {
-				UWARN("Odometry automatically reset to latest pose (%s)!", _pose.prettyPrint().c_str());
-				this->reset(_pose);
-			}
+			UWARN("Odometry automatically reset to latest pose!");
+			this->reset(_pose);
 			_resetCurrentCount = _resetCountdown;
 			if(info)
 			{
 				*info = OdometryInfo();
 			}
-			this->computeTransform(data, Transform(), info);
-			return _pose;
+			return this->computeTransform(data, Transform(), info);
 		}
+	}
 
 	previousVelocities_.clear();
 	velocityGuess_.setNull();
 	previousStamp_ = 0;
 
-}
 	return Transform();
 }
 
@@ -1071,7 +1053,6 @@ void Odometry::initKalmanFilter(const Transform & initialPose, float vx, float v
 	    0, 0, 0, 0, 0, 0.17 } };
 	static const boost::array<double, 36> STANDARD_TWIST_COVARIANCE =
 	{ { 0.05, 0, 0, 0, 0, 0,
-	}
 	    0, 0.05, 0, 0, 0, 0,
 	    0, 0, 0.05, 0, 0, 0,
 	    0, 0, 0, 0.09, 0, 0,

corelib/src/OdometryInfo.cpp

@@ -126,10 +126,10 @@ std::map<std::string, float> OdometryInfo::statistics(const Transform & pose)
 	stats.insert(std::make_pair("Odometry/ICPStructuralComplexity/", reg.icpStructuralComplexity));
 	stats.insert(std::make_pair("Odometry/ICPStructuralDistribution/", reg.icpStructuralDistribution));
 	stats.insert(std::make_pair("Odometry/ICPCorrespondences/", reg.icpCorrespondences));
-	stats.insert(std::make_pair("Odometry/StdDevLin/", reg.covariance.empty()?0:sqrt((float)reg.covariance.at<double>(0,0))));
-	stats.insert(std::make_pair("Odometry/StdDevAng/", reg.covariance.empty()?0:sqrt((float)reg.covariance.at<double>(5,5))));
-	stats.insert(std::make_pair("Odometry/VarianceLin/", reg.covariance.empty()?0:(float)reg.covariance.at<double>(0,0)));
-	stats.insert(std::make_pair("Odometry/VarianceAng/", reg.covariance.empty()?0:(float)reg.covariance.at<double>(5,5)));
+	stats.insert(std::make_pair("Odometry/StdDevLin/", sqrt((float)reg.covariance.at<double>(0,0))));
+	stats.insert(std::make_pair("Odometry/StdDevAng/", sqrt((float)reg.covariance.at<double>(5,5))));
+	stats.insert(std::make_pair("Odometry/VarianceLin/", (float)reg.covariance.at<double>(0,0)));
+	stats.insert(std::make_pair("Odometry/VarianceAng/", (float)reg.covariance.at<double>(5,5)));
 	stats.insert(std::make_pair("Odometry/TimeEstimation/ms", timeEstimation*1000.0f));
 	stats.insert(std::make_pair("Odometry/TimeFiltering/ms", timeParticleFiltering*1000.0f));
 	stats.insert(std::make_pair("Odometry/LocalMapSize/", localMapSize));
@@ -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)));
-			stats.insert(std::make_pair("Odometry/TG_error_ang/deg", transformGroundTruth.getAngle(transform)*180.0/CV_PI));
+			rtabmap::Transform diff = transformGroundTruth.inverse()*transform;
+			stats.insert(std::make_pair("Odometry/TG_error_lin/m", diff.getNorm()));
+			stats.insert(std::make_pair("Odometry/TG_error_ang/deg", diff.getAngle()*180.0/CV_PI));
 		}
 
 		transformGroundTruth.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);

corelib/src/OdometryThread.cpp

@@ -32,7 +32,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/OdometryInfo.h"
 #include "rtabmap/core/OdometryEvent.h"
 #include "rtabmap/utilite/ULogger.h"
-#include "rtabmap/utilite/UTimer.h"
 
 namespace rtabmap {
 
@@ -41,8 +40,8 @@ OdometryThread::OdometryThread(Odometry * odometry, unsigned int dataBufferMaxSi
 	_dataBufferMaxSize(dataBufferMaxSize),
 	_resetOdometry(false),
 	_resetPose(Transform::getIdentity()),
-	_oldestAsyncImuStamp(0.0),
-	_newestAsyncImuStamp(0.0)
+	_lastImuStamp(0.0),
+	_imuEstimatedDelay(0.0)
 {
 	UASSERT(_odometry != 0);
 }
@@ -64,7 +63,7 @@ bool OdometryThread::handleEvent(UEvent * event)
 			SensorEvent * sensorEvent = (SensorEvent*)event;
 			if(sensorEvent->getCode() == SensorEvent::kCodeData)
 			{
-				this->addData(*sensorEvent);
+				this->addData(sensorEvent->data());
 			}
 		}
 		else if(event->getClassName().compare("IMUEvent") == 0)
@@ -111,53 +110,32 @@ void OdometryThread::mainLoop()
 		UScopeMutex lock(_dataMutex);
 		_dataBuffer.clear();
 		_imuBuffer.clear();
-		_oldestAsyncImuStamp = 0.0;
-		_newestAsyncImuStamp = 0.0;
-		_previousGuessPose.setNull();
+		_lastImuStamp = 0.0f;
 	}
 
-	SensorEvent event;
-	if(getData(event))
+	SensorData data;
+	if(getData(data))
 	{
 		OdometryInfo info;
 		UDEBUG("Processing data...");
-		Transform guess;
-		UDEBUG("event.info().odomPose=%s", event.info().odomPose.prettyPrint().c_str());
-		if(!_previousGuessPose.isNull() && !event.info().odomPose.isNull()) {
-			guess = _previousGuessPose.inverse() * event.info().odomPose;
-		}
-
-		SensorData data = event.data();
-		Transform pose = _odometry->process(data, guess , &info);
+		Transform pose = _odometry->process(data, &info);
 		if(!data.imageRaw().empty() || !data.laserScanRaw().empty() || (pose.isNull() && data.imu().empty()))
 		{
 			UDEBUG("Odom pose = %s", pose.prettyPrint().c_str());
-			if(!pose.isNull()) {
-				_previousGuessPose = event.info().odomPose;
-				UASSERT(event.info().odomPose.isNull() || !info.reg.covariance.empty());
-				if(!event.info().odomPose.isNull() && info.reg.covariance.at<double>(0,0) >= 9999 &&
-					(pose.x() != 0.0f || pose.y() != 0.0f || pose.z() != 0.0f)) // not the first frame
-				{
-					// In case of external guess and auto reset, keep reporting lost till we
-					// process the second frame with valid covariance. This way it
-					// won't trigger a new map.
-					pose = Transform();
-				}
-			}
 			// a null pose notify that odometry could not be computed
 			this->post(new OdometryEvent(data, pose, info));
 		}
 	}
 }
 
-void OdometryThread::addData(const SensorEvent & event)
+void OdometryThread::addData(const SensorData & data)
 {
-	if(event.data().imu().empty())
+	if(data.imu().empty())
 	{
 		if(dynamic_cast<OdometryMono*>(_odometry) == 0)
 		{
-			if((event.data().imageRaw().empty() || event.data().depthOrRightRaw().empty() || (event.data().cameraModels().empty() && event.data().stereoCameraModels().empty())) &&
-					event.data().laserScanRaw().empty())
+			if((data.imageRaw().empty() || data.depthOrRightRaw().empty() || (data.cameraModels().empty() && data.stereoCameraModels().empty())) &&
+					data.laserScanRaw().empty())
 			{
 				ULOGGER_ERROR("Missing some information (images/scans empty or missing calibration)!?");
 				return;
@@ -166,7 +144,7 @@ void OdometryThread::addData(const SensorEvent & event)
 		else
 		{
 			// Mono can accept RGB only
-			if(event.data().imageRaw().empty() || (event.data().cameraModels().empty() && event.data().stereoCameraModels().empty()))
+			if(data.imageRaw().empty() || (data.cameraModels().empty() && data.stereoCameraModels().empty()))
 			{
 				ULOGGER_ERROR("Missing some information (image empty or missing calibration)!?");
 				return;
@@ -177,47 +155,28 @@ void OdometryThread::addData(const SensorEvent & event)
 	bool notify = true;
 	_dataMutex.lock();
 	{
-		if( !event.data().imageRaw().empty() ||
-			!event.data().imageCompressed().empty() ||
-			!event.data().laserScanRaw().isEmpty() ||
-			!event.data().laserScanCompressed().empty() || 
-			event.data().imu().empty())
+		if( !data.imageRaw().empty() ||
+			!data.imageCompressed().empty() ||
+			!data.laserScanRaw().isEmpty() ||
+			!data.laserScanCompressed().empty() || 
+			data.imu().empty())
 		{
-			if(_oldestAsyncImuStamp > 0.0 && event.data().stamp() < _oldestAsyncImuStamp) {
-				UWARN("Received image/lidar with stamp (%f) older than oldest 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)."
-					"Current camera/lidar delay is %fs.",
-					event.data().stamp(), _oldestAsyncImuStamp, _imuBuffer.size(), UTimer::now() - event.data().stamp());
+			_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 if(_newestAsyncImuStamp > 0.0 && event.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). "
-					"Current camera/lidar delay is %fs.",
-					event.data().stamp(), _newestAsyncImuStamp, _imuBuffer.size(), UTimer::now() - event.data().stamp());
-				notify = false;
-			}
-			else {
-				_dataBuffer.push_back(event);
-				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(event.data());
-			if(_oldestAsyncImuStamp == 0) {
-				_oldestAsyncImuStamp = event.data().stamp();
+			_imuBuffer.push_back(data);
+			if(_lastImuStamp != 0.0 && data.stamp() > _lastImuStamp)
+			{
+				_imuEstimatedDelay = data.stamp() - _lastImuStamp;
 			}
-			_newestAsyncImuStamp = event.data().stamp();
+			_lastImuStamp = data.stamp();
 		}
 	}
 	_dataMutex.unlock();
@@ -228,7 +187,7 @@ void OdometryThread::addData(const SensorEvent & event)
 	}
 }
 
-bool OdometryThread::getData(SensorEvent & event)
+bool OdometryThread::getData(SensorData & data)
 {
 	bool dataFilled = false;
 	_dataAdded.acquire();
@@ -236,18 +195,22 @@ bool OdometryThread::getData(SensorEvent & event)
 	{
 		if(!_dataBuffer.empty())
 		{
-			// Send IMU up to stamp greater than image (OpenVINS needs this).
-			while(!_imuBuffer.empty())
+			if(!_imuBuffer.empty())
 			{
-				_odometry->process(_imuBuffer.front());
-				double stamp =_imuBuffer.front().stamp();
-				_imuBuffer.pop_front();
-				if(stamp > _dataBuffer.front().data().stamp()) {
-					break;
+				// Send IMU up to stamp greater than image (OpenVINS needs this).
+				while(!_imuBuffer.empty())
+				{
+					_odometry->process(_imuBuffer.front());
+					double stamp = _imuBuffer.front().stamp();
+					_imuBuffer.pop_front();
+					if(stamp > _dataBuffer.front().stamp())
+					{
+						break;
+					}
 				}
 			}
 
-			event = _dataBuffer.front();
+			data = _dataBuffer.front();
 			_dataBuffer.pop_front();
 			dataFilled = true;
 		}

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,
-		const ParametersMap & registrationParameters)
+		bool rematchFeatures)
 {
 	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,
-		const ParametersMap & registrationParameters)
+		bool rematchFeatures)
 {
 	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,
-		ParametersMap registrationParameters)
+		bool useLinkTransformAsGuess)
 {
 	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)
 	{
@@ -608,10 +597,17 @@ void Optimizer::computeBACorrespondences(
 					}
 				}
 
-				if(sFrom.getWordsKpts().size() &&
-					sTo.getWordsKpts().size() &&
+				if(sFrom.getWords().size() &&
+					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/Parameters.cpp

@@ -236,9 +236,6 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 	{
 		// removed parameters
 
-		// 0.23.1
-		removedParameters_.insert(std::make_pair("OdomVINS/ConfigPath",    std::make_pair(true, Parameters::kOdomVINSFusionConfigPath())));
-
 		// 0.21.13
 		removedParameters_.insert(std::make_pair("Vis/ForwardEstOnly",    std::make_pair(false, "")));
 
@@ -939,7 +936,7 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
 				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
 #endif
 				str = "With VINS-Fusion:";
-#ifdef RTABMAP_VINS_FUSION
+#ifdef RTABMAP_VINS
 				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
 #else
 				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;

corelib/src/RegistrationVis.cpp

@@ -375,9 +375,6 @@ Transform RegistrationVis::computeTransformationImpl(
 	{
 		UDEBUG("");
 		// just some checks to make sure that input data are ok
-		UASSERT(fromSignature.getWords().empty() ||
-				fromSignature.getWordsKpts().empty() ||
-				(fromSignature.getWords().size() == fromSignature.getWordsKpts().size()));
 		UASSERT(fromSignature.getWords().empty() ||
 				fromSignature.getWords3().empty() ||
 				(fromSignature.getWords().size() == fromSignature.getWords3().size()));
@@ -385,11 +382,8 @@ Transform RegistrationVis::computeTransformationImpl(
 				(int)fromSignature.getWords().size() == fromSignature.getWordsDescriptors().rows ||
 				fromSignature.sensorData().descriptors().empty() ||
 				fromSignature.getWordsDescriptors().empty() == 0);
-		UASSERT(toSignature.getWords().empty() ||
-				toSignature.getWordsKpts().empty() ||
-				(toSignature.getWords().size() == toSignature.getWordsKpts().size()));
-		UASSERT(toSignature.getWords().empty() ||
-				toSignature.getWords3().empty() ||
+		UASSERT((toSignature.getWords().empty() && toSignature.getWords3().empty())||
+				(toSignature.getWords().size() && toSignature.getWords3().empty())||
 				(toSignature.getWords().size() == toSignature.getWords3().size()));
 		UASSERT((int)toSignature.sensorData().keypoints().size() == toSignature.sensorData().descriptors().rows ||
 				(int)toSignature.getWords().size() == toSignature.getWordsDescriptors().rows ||

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);
@@ -1502,8 +1500,8 @@ bool Rtabmap::process(
 	float angleToClosestNodeInTheGraph = 0;
 	if(_rgbdSlamMode)
 	{
-		double linVar = odomCovariance.empty()?0.0f:uMax3(odomCovariance.at<double>(0,0), odomCovariance.at<double>(1,1)>=9999?0:odomCovariance.at<double>(1,1), odomCovariance.at<double>(2,2)>=9999?0:odomCovariance.at<double>(2,2));
-		double angVar = odomCovariance.empty()?0.0f:uMax3(odomCovariance.at<double>(3,3)>=9999?0:odomCovariance.at<double>(3,3), odomCovariance.at<double>(4,4)>=9999?0:odomCovariance.at<double>(4,4), odomCovariance.at<double>(5,5));
+		double linVar = odomCovariance.empty()?1.0f:uMax3(odomCovariance.at<double>(0,0), odomCovariance.at<double>(1,1)>=9999?0:odomCovariance.at<double>(1,1), odomCovariance.at<double>(2,2)>=9999?0:odomCovariance.at<double>(2,2));
+		double angVar = odomCovariance.empty()?1.0f:uMax3(odomCovariance.at<double>(3,3)>=9999?0:odomCovariance.at<double>(3,3), odomCovariance.at<double>(4,4)>=9999?0:odomCovariance.at<double>(4,4), odomCovariance.at<double>(5,5));
 		statistics_.addStatistic(Statistics::kMemoryOdometry_variance_lin(), (float)linVar);
 		statistics_.addStatistic(Statistics::kMemoryOdometry_variance_ang(), (float)angVar);
 
@@ -1607,7 +1605,6 @@ bool Rtabmap::process(
 					Transform t = _memory->computeTransform(oldId, signature->id(), guess, &info);
 					if(!t.isNull())
 					{
-						UASSERT(!info.covariance.empty() && info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
 						UINFO("Odometry refining: update neighbor link (%d->%d, variance:lin=%f, ang=%f) from %s to %s",
 								oldId,
 								signature->id(),
@@ -1615,6 +1612,7 @@ bool Rtabmap::process(
 								info.covariance.at<double>(5,5),
 								guess.prettyPrint().c_str(),
 								t.prettyPrint().c_str());
+						UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
 						_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), t, info.covariance.inv()));
 
 						if(_optimizeFromGraphEnd)
@@ -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();
 			}
 		}
 
@@ -1894,7 +1892,7 @@ bool Rtabmap::process(
 								*iter,
 								transform.prettyPrint().c_str());
 						// Add a loop constraint
-						UASSERT(!info.covariance.empty() && info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
+						UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
 						if(_memory->addLink(Link(signature->id(), *iter, Link::kLocalTimeClosure, transform, getInformation(info.covariance))))
 						{
 							++proximityDetectionsInTimeFound;
@@ -2395,22 +2393,24 @@ bool Rtabmap::process(
 					distanceSoFar += _path[i-1].second.getDistance(_path[i].second);
 				}
 
-				if(_memory->getSignature(_path[i].first) != 0)
+				if(distanceSoFar <= _localRadius)
 				{
-					if(immunizedLocations.insert(_path[i].first).second)
+					if(_memory->getSignature(_path[i].first) != 0)
 					{
-						++immunizedLocally;
+						if(immunizedLocations.insert(_path[i].first).second)
+						{
+							++immunizedLocally;
+						}
+						UDEBUG("Path immunization: node %d (dist=%fm)", _path[i].first, distanceSoFar);
+					}
+					else if(retrievalLocalIds.size() < _maxLocalRetrieved)
+					{
+						UINFO("retrieval of node %d on path (dist=%fm)", _path[i].first, distanceSoFar);
+						retrievalLocalIds.push_back(_path[i].first);
+						// retrieved locations are automatically immunized
 					}
-					UDEBUG("Path immunization: node %d (dist=%fm)", _path[i].first, distanceSoFar);
 				}
-				else if(retrievalLocalIds.size() < _maxLocalRetrieved)
-				{
-					UINFO("retrieval of node %d on path (dist=%fm)", _path[i].first, distanceSoFar);
-					retrievalLocalIds.push_back(_path[i].first);
-					// retrieved locations are automatically immunized
-				}
-				
-				if(distanceSoFar > _localRadius)
+				else
 				{
 					UDEBUG("Stop on node %d (dist=%fm > %fm)",
 							_path[i].first, distanceSoFar, _localRadius);
@@ -2782,7 +2782,7 @@ bool Rtabmap::process(
 											signature->id(),
 											nearestId,
 											transform.prettyPrint().c_str());
-									UASSERT(!info.covariance.empty() && info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
+									UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
 
 									//for statistics
 									loopClosureVisualInliersMeanDist = info.inliersMeanDistance;
@@ -2993,7 +2993,7 @@ bool Rtabmap::process(
 										}
 
 										// set Identify covariance for laser scan matching only
-										UASSERT(!info.covariance.empty() && info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
+										UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
 										_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, getInformation(info.covariance)/_proximityMergedScanCovFactor, scanMatchingIds));
 										loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
 
@@ -3081,7 +3081,7 @@ bool Rtabmap::process(
 			if(!rejectedLoopClosure)
 			{
 				// Make the new one the parent of the old one
-				UASSERT(!info.covariance.empty() && info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
+				UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
 				
 				loopClosureLinearVariance = uMax3(info.covariance.at<double>(0,0), info.covariance.at<double>(1,1)>=9999?0:info.covariance.at<double>(1,1), info.covariance.at<double>(2,2)>=9999?0:info.covariance.at<double>(2,2));
 				loopClosureAngularVariance = uMax3(info.covariance.at<double>(3,3)>=9999?0:info.covariance.at<double>(3,3), info.covariance.at<double>(4,4)>=9999?0:info.covariance.at<double>(4,4), info.covariance.at<double>(5,5));
@@ -3156,7 +3156,17 @@ bool Rtabmap::process(
 			UASSERT(uContains(_optimizedPoses, signature->id()));
 			UASSERT_MSG(uContains(_optimizedPoses, _path[_pathCurrentIndex].first), uFormat("id=%d", _path[_pathCurrentIndex].first).c_str());
 			Transform virtualLoop = _optimizedPoses.at(signature->id()).inverse() * _optimizedPoses.at(_path[_pathCurrentIndex].first);
-			_memory->addLink(Link(signature->id(), _path[_pathCurrentIndex].first, Link::kVirtualClosure, virtualLoop, cv::Mat::eye(6,6,CV_64FC1)*0.01)); // set high variance
+
+			if(_localRadius == 0.0f || virtualLoop.getNorm() < _localRadius)
+			{
+				_memory->addLink(Link(signature->id(), _path[_pathCurrentIndex].first, Link::kVirtualClosure, virtualLoop, cv::Mat::eye(6,6,CV_64FC1)*0.01)); // set high variance
+			}
+			else
+			{
+				UERROR("Virtual link larger than local radius (%fm > %fm). Aborting the plan!",
+						virtualLoop.getNorm(), _localRadius);
+				this->clearPath(-1);
+			}
 		}
 	}
 
@@ -3171,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);
@@ -3290,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
 				{
@@ -3415,8 +3424,7 @@ bool Rtabmap::process(
 				}
 
 				bool hasGlobalLoopClosuresOrLandmarks = false;
-				if(rejectLocalization && 
-					(_localizationSecondTryWithoutProximityLinks && !graph::filterLinks(constraints, Link::kLocalSpaceClosure, true).empty()))
+				if(rejectLocalization && !graph::filterLinks(constraints, Link::kLocalSpaceClosure, true).empty())
 				{
 					// Let's try again without local loop closures
 					localizationLinks = graph::filterLinks(localizationLinks, Link::kLocalSpaceClosure);
@@ -3440,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
 						{
@@ -3576,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
@@ -3638,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
@@ -3942,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
@@ -4091,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));
-						statistics_.addStatistic(Statistics::kGtLocalization_angular_error(), loopIter->second.transform().getAngle(transformGT)*180/M_PI);
+						Transform error = loopIter->second.transform().inverse() * transformGT;
+						statistics_.addStatistic(Statistics::kGtLocalization_linear_error(), error.getNorm());
+						statistics_.addStatistic(Statistics::kGtLocalization_angular_error(), error.getAngle(1,0,0)*180/M_PI);
 					}
 				}
 
 				_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);
@@ -4113,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);
@@ -4134,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()?
@@ -6915,24 +6911,24 @@ void Rtabmap::updateGoalIndex()
 				{
 					distanceSoFar += _path[i-1].second.getDistance(_path[i].second);
 				}
-				
-				if(_path[i].first != _path[i-1].first)
+				if(distanceSoFar <= _localRadius)
 				{
-					const Signature * s = _memory->getSignature(_path[i].first);
-					if(s)
+					if(_path[i].first != _path[i-1].first)
 					{
-						if(!s->hasLink(_path[i-1].first) && _memory->getSignature(_path[i-1].first) != 0)
+						const Signature * s = _memory->getSignature(_path[i].first);
+						if(s)
 						{
-							Transform virtualLoop = _path[i].second.inverse() * _path[i-1].second;
-							_memory->addLink(Link(_path[i].first, _path[i-1].first, Link::kVirtualClosure, virtualLoop, cv::Mat::eye(6,6,CV_64FC1)*0.01)); // on the optimized path
-							UINFO("Added Virtual link between %d and %d", _path[i-1].first, _path[i].first);
+							if(!s->hasLink(_path[i-1].first) && _memory->getSignature(_path[i-1].first) != 0)
+							{
+								Transform virtualLoop = _path[i].second.inverse() * _path[i-1].second;
+								_memory->addLink(Link(_path[i].first, _path[i-1].first, Link::kVirtualClosure, virtualLoop, cv::Mat::eye(6,6,CV_64FC1)*0.01)); // on the optimized path
+								UINFO("Added Virtual link between %d and %d", _path[i-1].first, _path[i].first);
+							}
 						}
 					}
 				}
-
-				if(distanceSoFar > _localRadius)
+				else
 				{
-					UDEBUG("Farthest goal=%d : %f m", _path[i].first, distanceSoFar);
 					break;
 				}
 			}
@@ -6992,8 +6988,11 @@ void Rtabmap::updateGoalIndex()
 					if((goalIndex == _pathCurrentIndex && i == _path.size()-1) ||
 					   _pathUnreachableNodes.find(i) == _pathUnreachableNodes.end())
 					{
-						goalIndex = i;
-						if(distanceFromCurrentNode > _localRadius)
+						if(distanceFromCurrentNode <= _localRadius)
+						{
+							goalIndex = i;
+						}
+						else
 						{
 							break;
 						}

corelib/src/RtabmapThread.cpp

@@ -506,10 +506,9 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 				ignoreFrame = true;
 			}
 		}
-		UASSERT(!odomEvent.info().reg.covariance.empty());
 		if(!lastPose_.isIdentity() &&
-			(odomEvent.pose().isIdentity() ||
-			odomEvent.info().reg.covariance.at<double>(0,0)>=9999))
+						(odomEvent.pose().isIdentity() ||
+						odomEvent.info().reg.covariance.at<double>(0,0)>=9999))
 		{
 			if(odomEvent.pose().isIdentity())
 			{

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,
@@ -548,7 +449,7 @@ void SensorData::setOccupancyGrid(
 			float cellSize,
 			const cv::Point3f & viewPoint)
 {
-	//UDEBUG("ground=%d obstacles=%d empty=%d", ground.cols, obstacles.cols, empty.cols);
+	UDEBUG("ground=%d obstacles=%d empty=%d", ground.cols, obstacles.cols, empty.cols);
 	if((!ground.empty() && (!_groundCellsCompressed.empty() || !_groundCellsRaw.empty())) ||
 	   (!obstacles.empty() && (!_obstacleCellsCompressed.empty() || !_obstacleCellsRaw.empty())) ||
 	   (!empty.empty() && (!_emptyCellsCompressed.empty() || !_emptyCellsRaw.empty())))
@@ -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,
-				_depthConfidenceCompressed.empty()?0:&tmpH);
+				_emptyCellsCompressed.empty()?0:&tmpG);
 }
 
 void SensorData::uncompressData(
@@ -646,27 +546,16 @@ void SensorData::uncompressData(
 		cv::Mat * userDataRaw,
 		cv::Mat * groundCellsRaw,
 		cv::Mat * obstacleCellsRaw,
-		cv::Mat * emptyCellsRaw,
-		cv::Mat * depthConfidenceRaw)
+		cv::Mat * emptyCellsRaw)
 {
-	/*UDEBUG("%d data(%d,%d,%d,%d,%d,%d,%d,%d)", 
-		this->id(),
-		imageRaw?1:0,
-		depthRaw?1:0,
-		laserScanRaw?1:0,
-		userDataRaw?1:0,
-		groundCellsRaw?1:0,
-		obstacleCellsRaw?1:0, 
-		emptyCellsRaw?1:0,
-		depthConfidenceRaw?1:0);*/
+	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);
 	if(imageRaw == 0 &&
 		depthRaw == 0 &&
 		laserScanRaw == 0 &&
 		userDataRaw == 0 &&
 		groundCellsRaw == 0 &&
 		obstacleCellsRaw == 0 &&
-		emptyCellsRaw == 0 &&
-		depthConfidenceRaw == 0)
+		emptyCellsRaw == 0)
 	{
 		return;
 	}
@@ -677,8 +566,7 @@ void SensorData::uncompressData(
 			userDataRaw,
 			groundCellsRaw,
 			obstacleCellsRaw,
-			emptyCellsRaw,
-			depthConfidenceRaw);
+			emptyCellsRaw);
 
 	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 * depthConfidenceRaw) const
+		cv::Mat * emptyCellsRaw) 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/Signature.cpp

@@ -118,7 +118,7 @@ void Signature::addLinks(const std::map<int, Link> & links)
 }
 void Signature::addLink(const Link & link)
 {
-	//UDEBUG("Add link %d to %d (type=%d/%s var=%f,%f)", link.to(), this->id(), (int)link.type(), link.typeName().c_str(), link.transVariance(), link.rotVariance());
+	UDEBUG("Add link %d to %d (type=%d/%s var=%f,%f)", link.to(), this->id(), (int)link.type(), link.typeName().c_str(), link.transVariance(), link.rotVariance());
 	UASSERT_MSG(link.from() == this->id(), uFormat("%d->%d for signature %d (type=%d)", link.from(), link.to(), this->id(), link.type()).c_str());
 	UASSERT_MSG((link.to() != this->id()) || link.type()==Link::kPosePrior || link.type()==Link::kGravity, uFormat("%d->%d for signature %d (type=%d)", link.from(), link.to(), this->id(), link.type()).c_str());
 	UASSERT_MSG(link.to() == this->id() || _links.find(link.to()) == _links.end(), uFormat("Link %d (type=%d) already added to signature %d!", link.to(), link.type(), this->id()).c_str());
@@ -318,7 +318,7 @@ void Signature::setWords(const std::multimap<int, int> & words,
 	UASSERT_MSG(descriptors.empty() || descriptors.rows == (int)words.size(), uFormat("words=%d, descriptors=%d", (int)words.size(), descriptors.rows).c_str());
 	UASSERT_MSG(points.empty() || points.size() == words.size(),  uFormat("words=%d, points=%d", (int)words.size(), (int)points.size()).c_str());
 	UASSERT_MSG(keypoints.empty() || keypoints.size() == words.size(),  uFormat("words=%d, descriptors=%d", (int)words.size(), (int)keypoints.size()).c_str());
-	//UASSERT(words.empty() || !keypoints.empty() || !points.empty() || !descriptors.empty());
+	UASSERT(words.empty() || !keypoints.empty() || !points.empty() || !descriptors.empty());
 
 	_invalidWordsCount = 0;
 	for(std::multimap<int, int>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
@@ -328,7 +328,7 @@ void Signature::setWords(const std::multimap<int, int> & words,
 			++_invalidWordsCount;
 		}
 		// make sure indexes are all valid!
-		UASSERT_MSG(iter->second<0 || iter->second < (int)words.size(), uFormat("iter->second=%d words.size()=%d", iter->second, (int)words.size()).c_str());
+		UASSERT_MSG(iter->second >=0 && iter->second < (int)words.size(), uFormat("iter->second=%d words.size()=%d", iter->second, (int)words.size()).c_str());
 	}
 
 	_enabled = false;

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

@@ -51,6 +51,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <fstream>
 #include <string>
 
+#define KDTREE_SIZE 4
 #define KNN_CHECKS 32
 
 namespace rtabmap
@@ -68,11 +69,9 @@ VWDictionary::VWDictionary(const ParametersMap & parameters) :
 	_nndrRatio(Parameters::defaultKpNndrRatio()),
 	_newDictionaryPath(Parameters::defaultKpDictionaryPath()),
 	_newWordsComparedTogether(Parameters::defaultKpNewWordsComparedTogether()),
-	_serializeWithChecksum(Parameters::defaultKpSerializeWithChecksum()),
 	_lastWordId(0),
 	useDistanceL1_(false),
 	_flannIndex(new FlannIndex()),
-	_modified(true),
 	_strategy(kNNBruteForce)
 {
 	this->setNNStrategy((NNStrategy)Parameters::defaultKpNNStrategy());
@@ -90,7 +89,6 @@ void VWDictionary::parseParameters(const ParametersMap & parameters)
 	ParametersMap::const_iterator iter;
 	Parameters::parse(parameters, Parameters::kKpNndrRatio(), _nndrRatio);
 	Parameters::parse(parameters, Parameters::kKpNewWordsComparedTogether(), _newWordsComparedTogether);
-	Parameters::parse(parameters, Parameters::kKpSerializeWithChecksum(), _serializeWithChecksum);
 	Parameters::parse(parameters, Parameters::kKpIncrementalFlann(), _incrementalFlann);
 	Parameters::parse(parameters, Parameters::kKpFlannRebalancingFactor(), _rebalancingFactor);
 	bool byteToFloat = _byteToFloat;
@@ -162,7 +160,7 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath)
 				DBDriver * driver = DBDriver::create();
 				if(driver->openConnection(dictionaryPath, false))
 				{
-					driver->load(*this, false);
+					driver->load(this, false);
 					for(std::map<int, VisualWord*>::iterator iter=_visualWords.begin(); iter!=_visualWords.end(); ++iter)
 					{
 						iter->second->setSaved(true);
@@ -291,11 +289,6 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath)
 	_newDictionaryPath = dictionaryPath;
 }
 
-bool VWDictionary::isModified() const
-{
-	return _modified;
-}
-
 bool VWDictionary::setNNStrategy(NNStrategy strategy)
 {
 #if CV_MAJOR_VERSION < 3
@@ -491,13 +484,7 @@ void VWDictionary::update()
 
 	if(_notIndexedWords.size() || _visualWords.size() == 0 || _removedIndexedWords.size())
 	{
-		_modified = true;
-		bool firstUpdate = _removedIndexedWords.empty() && _visualWords.size() == _notIndexedWords.size();
-		UDEBUG("firstUpdate=%s (_removedIndexedWords=%ld, _visualWords=%ld, _notIndexedWords=%ld)", 
-			firstUpdate?"true":"false", _removedIndexedWords.size(), _visualWords.size(), _notIndexedWords.size());
-
-		if(!firstUpdate &&
-			_incrementalFlann &&
+		if(_incrementalFlann &&
 		   _strategy < kNNBruteForce &&
 		   _visualWords.size())
 		{
@@ -514,9 +501,7 @@ void VWDictionary::update()
 
 			if(_notIndexedWords.size())
 			{
-				UTimer timer;
-				timer.start();
-				ULOGGER_DEBUG("Incremental FLANN: Inserting %d words...", (int)_notIndexedWords.size(), _byteToFloat?"true":"false");
+				ULOGGER_DEBUG("Incremental FLANN: Inserting %d words...", (int)_notIndexedWords.size());
 				for(std::set<int>::iterator iter=_notIndexedWords.begin(); iter!=_notIndexedWords.end(); ++iter)
 				{
 					VisualWord* w = uValue(_visualWords, *iter, (VisualWord*)0);
@@ -543,13 +528,24 @@ void VWDictionary::update()
 					int index = 0;
 					if(!_flannIndex->isBuilt())
 					{
-						UDEBUG("Building FLANN index... (strategy=%s, byteToFloat=%s, useDistanceL1=%s, rebalancingFactor=%f)",
-							nnStrategyName(_strategy).c_str(), _byteToFloat?"true":"false", useDistanceL1_?"true":"false", _rebalancingFactor);
-						_flannIndex->buildIndex(
-							_strategy == kNNFlannNaive ? FlannIndex::FLANN_INDEX_LINEAR:
-							_strategy == kNNFlannLSH ? FlannIndex::FLANN_INDEX_LSH:
-							FlannIndex::FLANN_INDEX_KDTREE, // kNNFlannKdTree
-							descriptor, useDistanceL1_, _rebalancingFactor);
+						UDEBUG("Building FLANN index...");
+						switch(_strategy)
+						{
+						case kNNFlannNaive:
+							_flannIndex->buildLinearIndex(descriptor, useDistanceL1_, _rebalancingFactor);
+							break;
+						case kNNFlannKdTree:
+							UASSERT_MSG(descriptor.type() == CV_32F, "To use KdTree dictionary, float descriptors are required!");
+							_flannIndex->buildKDTreeIndex(descriptor, KDTREE_SIZE, useDistanceL1_, _rebalancingFactor);
+							break;
+						case kNNFlannLSH:
+							UASSERT_MSG(descriptor.type() == CV_8U, "To use LSH dictionary, binary descriptors are required!");
+							_flannIndex->buildLSHIndex(descriptor, 12, 20, 2, _rebalancingFactor);
+							break;
+						default:
+							UFATAL("Not supposed to be here!");
+							break;
+						}
 						UDEBUG("Building FLANN index... done!");
 					}
 					else
@@ -565,7 +561,7 @@ void VWDictionary::update()
 					inserted = _mapIdIndex.insert(std::pair<int, int>(w->id(), index));
 					UASSERT(inserted.second);
 				}
-				ULOGGER_DEBUG("Incremental FLANN: Inserting %d words... done! (in %f s)", (int)_notIndexedWords.size(), timer.ticks());
+				ULOGGER_DEBUG("Incremental FLANN: Inserting %d words... done!", (int)_notIndexedWords.size());
 			}
 		}
 		else if(_strategy >= kNNBruteForce &&
@@ -661,13 +657,23 @@ void VWDictionary::update()
 				ULOGGER_DEBUG("_mapIndexId.size() = %d, words.size()=%d, _dim=%d",_mapIndexId.size(), _visualWords.size(), dim);
 				ULOGGER_DEBUG("copying data = %f s", timer.ticks());
 
-				_flannIndex->buildIndex(
-					_strategy == kNNFlannNaive ? FlannIndex::FLANN_INDEX_LINEAR:
-						_strategy == kNNFlannLSH ? FlannIndex::FLANN_INDEX_LSH:
-							FlannIndex::FLANN_INDEX_KDTREE, // kNNFlannKdTree
-					_dataTree,
-					useDistanceL1_,
-					_incrementalDictionary&&_incrementalFlann?_rebalancingFactor:1);
+				switch(_strategy)
+				{
+				case kNNFlannNaive:
+					_flannIndex->buildLinearIndex(_dataTree, useDistanceL1_, _incrementalDictionary&&_incrementalFlann?_rebalancingFactor:1);
+					break;
+				case kNNFlannKdTree:
+					UASSERT_MSG(type == CV_32F, "To use KdTree dictionary, float descriptors are required!");
+					_flannIndex->buildKDTreeIndex(_dataTree, KDTREE_SIZE, useDistanceL1_, _incrementalDictionary&&_incrementalFlann?_rebalancingFactor:1);
+					break;
+				case kNNFlannLSH:
+					UASSERT_MSG(type == CV_8U, "To use LSH dictionary, binary descriptors are required!");
+					_flannIndex->buildLSHIndex(_dataTree, 12, 20, 2, _incrementalDictionary&&_incrementalFlann?_rebalancingFactor:1);
+					break;
+				default:
+					break;
+				}
+
 				ULOGGER_DEBUG("Time to create kd tree = %f s", timer.ticks());
 			}
 		}
@@ -683,146 +689,6 @@ void VWDictionary::update()
 	UDEBUG("");
 }
 
-std::vector<unsigned char> VWDictionary::serializeIndex() const
-{
-	if(_strategy >= kNNBruteForce) {
-		UINFO("Not flann strategy, ignoring serialization...");
-		return std::vector<unsigned char>();
-	}
-	if(!_flannIndex->isBuilt() || !_removedIndexedWords.empty() || !_notIndexedWords.empty() || _visualWords.empty()) {
-		UWARN("Flann index is not buit, or there are words not indexed, cannot do serialization.");
-		return std::vector<unsigned char>();
-	}
-
-	return _flannIndex->serializeIndex(_serializeWithChecksum);
-}
-
-void VWDictionary::deserializeIndex(const std::vector<unsigned char> & data)
-{
-	deserializeIndex(data.data(), data.size());
-}
-
-void VWDictionary::deserializeIndex(const unsigned char * data, size_t size)
-{
-	if(data== NULL || size == 0)
-	{
-		UWARN("Trying to deserialize empty data, aborting.");
-		return;
-	}
-	UDEBUG("Loading flann index... (data size=%ld bytes)", size);
-	if(_strategy >= kNNBruteForce) {
-		//ignore
-		return;
-	}
-
-	if(_flannIndex->isBuilt()) {
-		UERROR("Flann index is already built, cannot deserialize data!");
-		return;
-	}
-
-	if(_visualWords.empty()) {
-		UERROR("Descriptors should be added before deserializing flann index! See VWDictionary::addWord()");
-		return;
-	}
-
-	if(!(_removedIndexedWords.empty() && _visualWords.size() == _notIndexedWords.size())) {
-		UERROR("State of dictionary not as expected before deserializing. (removed words=%ld, words=%ld, not indexed=%ld)", 
-			_removedIndexedWords.size(), _visualWords.size(), _notIndexedWords.size());
-		return;
-	}
-		
-	std::map<int, int> mapIndexId;
-	std::map<int, int> mapIdIndex;
-	cv::Mat dataTree;
-
-	UTimer timer;
-	timer.start();
-
-	int dim = _visualWords.begin()->second->getDescriptor().cols;
-	int type;
-	if(_visualWords.begin()->second->getDescriptor().type() == CV_8U)
-	{
-		useDistanceL1_ = true;
-		if(_strategy == kNNFlannKdTree)
-		{
-			type = CV_32F;
-			if(!_byteToFloat)
-			{
-				dim *= 8;
-			}
-		}
-		else
-		{
-			type = _visualWords.begin()->second->getDescriptor().type();
-		}
-	}
-	else
-	{
-		type = _visualWords.begin()->second->getDescriptor().type();
-	}
-
-	UASSERT(type == CV_32F || type == CV_8U);
-	UASSERT(dim > 0);
-
-	// Create the data matrix
-	dataTree = cv::Mat(_visualWords.size(), dim, type); // SURF descriptors are CV_32F
-	std::map<int, VisualWord*>::const_iterator iter = _visualWords.begin();
-	for(unsigned int i=0; i < _visualWords.size(); ++i, ++iter)
-	{
-		cv::Mat descriptor;
-		if(iter->second->getDescriptor().type() == CV_8U)
-		{
-			if(_strategy == kNNFlannKdTree)
-			{
-				descriptor = convertBinTo32F(iter->second->getDescriptor(), _byteToFloat);
-			}
-			else
-			{
-				descriptor = iter->second->getDescriptor();
-			}
-		}
-		else
-		{
-			descriptor = iter->second->getDescriptor();
-		}
-
-		UASSERT_MSG(descriptor.type() == type, uFormat("%d vs %d", descriptor.type(), type).c_str());
-		UASSERT_MSG(descriptor.cols == dim, uFormat("%d vs %d", descriptor.cols, dim).c_str());
-
-		descriptor.copyTo(dataTree.row(i));
-		mapIndexId.insert(mapIndexId.end(), std::pair<int, int>(i, iter->second->id()));
-		mapIdIndex.insert(mapIdIndex.end(), std::pair<int, int>(iter->second->id(), i));
-	}
-
-	ULOGGER_DEBUG("mapIndexId.size() = %d, words.size()=%d, dim=%d", mapIndexId.size(), _visualWords.size(), dim);
-	ULOGGER_DEBUG("copying data = %f s", timer.ticks());
-
-	std::string errorMsg;
-	if(_flannIndex->loadIndex(
-		data,
-		size,
-		_strategy == kNNFlannNaive ? FlannIndex::FLANN_INDEX_LINEAR:
-			_strategy == kNNFlannLSH ? FlannIndex::FLANN_INDEX_LSH:
-				FlannIndex::FLANN_INDEX_KDTREE, 
-		dataTree, 
-		useDistanceL1_,
-		_incrementalDictionary && _incrementalFlann ? _rebalancingFactor:1,
-		&errorMsg))
-	{
-		_mapIndexId = mapIndexId;
-		_mapIdIndex = mapIdIndex;
-		_dataTree = dataTree;
-		_notIndexedWords.clear();
-		_modified = false;
-	}
-	else {
-		UWARN("Failed deserializing flann index data (error: %s), the index will be rebuilt on next update.", errorMsg.c_str());
-		_flannIndex->release(); // reset to initial state
-	}
-
-	ULOGGER_DEBUG("Time to load flann index = %f s", timer.ticks());
-}
-
 void VWDictionary::clear(bool printWarningsIfNotEmpty)
 {
 	ULOGGER_DEBUG("");
@@ -852,7 +718,6 @@ void VWDictionary::clear(bool printWarningsIfNotEmpty)
 	_unusedWords.clear();
 	_flannIndex->release();
 	useDistanceL1_ = false;
-	_modified = true;
 }
 
 int VWDictionary::getNextId()
@@ -919,21 +784,14 @@ std::list<int> VWDictionary::addNewWords(
 		type = _visualWords.begin()->second->getDescriptor().type();
 		UASSERT(type == CV_32F || type == CV_8U);
 	}
-	static std::string moreInfo = uFormat( 
-		"This could happen if the computer doesn't have access to same "
-		"feature detectors than when the database was created. This could "
-		"also happen if we enabled \"%s\" but the first frame received "
-		"was empty, thus features were re-extracted with a different detector "
-		"than the one used by the odometry.",
-		Parameters::kMemUseOdomFeatures().c_str());
 	if(dim && dim != descriptorsIn.cols)
 	{
-		UERROR("Descriptors (size=%d) are not the same size as already added words in dictionary (size=%d). %s", descriptorsIn.cols, dim, moreInfo.c_str());
+		UERROR("Descriptors (size=%d) are not the same size as already added words in dictionary(size=%d)", descriptorsIn.cols, dim);
 		return wordIds;
 	}
 	if(type>=0 && type != descriptorsIn.type())
 	{
-		UERROR("Descriptors (type=%d) are not the same type as already added words in dictionary (type=%d). %s", descriptorsIn.type(), type, moreInfo.c_str());
+		UERROR("Descriptors (type=%d) are not the same type as already added words in dictionary(type=%d)", descriptorsIn.type(), type);
 		return wordIds;
 	}
 
@@ -1536,15 +1394,15 @@ void VWDictionary::addWord(VisualWord * vw)
 {
 	if(vw)
 	{
-		_visualWords.insert(_visualWords.end(), std::pair<int, VisualWord *>(vw->id(), vw));
-		_notIndexedWords.insert(_notIndexedWords.end(), vw->id());
+		_visualWords.insert(std::pair<int, VisualWord *>(vw->id(), vw));
+		_notIndexedWords.insert(vw->id());
 		if(vw->getReferences().size())
 		{
 			_totalActiveReferences += uSum(uValues(vw->getReferences()));
 		}
 		else
 		{
-			_unusedWords.insert(_unusedWords.end(), std::pair<int, VisualWord *>(vw->id(), vw));
+			_unusedWords.insert(std::pair<int, VisualWord *>(vw->id(), vw));
 		}
 		if(_lastWordId < vw->id())
 		{

corelib/src/VisualWord.cpp

@@ -57,7 +57,7 @@ void VisualWord::addRef(int signatureId)
 	}
 	else
 	{
-		_references.insert(_references.end(), std::pair<int, int>(signatureId, 1));
+		_references.insert(std::pair<int, int>(signatureId, 1));
 	}
 	++_totalReferences;
 }

corelib/src/camera/CameraDepthAI.cpp

@@ -370,10 +370,9 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 		matrix[2][0], matrix[2][1], matrix[2][2]);
 
 	std::vector<float> coeffs = calibHandler.getDistortionCoefficients(cameraId);
-	if(calibHandler.getDistortionModel(cameraId) == dai::CameraModel::Perspective) {
-		UASSERT(coeffs.size()>=14);
-		distCoeffs = (cv::Mat_<double>(1,14) << coeffs[0], coeffs[1], coeffs[2], coeffs[3], coeffs[4], coeffs[5], coeffs[6], coeffs[7], coeffs[8], coeffs[9], coeffs[10], coeffs[11], coeffs[12], coeffs[13]);
-	}
+	if(calibHandler.getDistortionModel(cameraId) == dai::CameraModel::Perspective)
+		distCoeffs = (cv::Mat_<double>(1,8) << coeffs[0], coeffs[1], coeffs[2], coeffs[3], coeffs[4], coeffs[5], coeffs[6], coeffs[7]);
+
 	if(alphaScaling_>-1.0f)
 		newCameraMatrix = cv::getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, targetSize_, alphaScaling_);
 	else

corelib/src/camera/CameraImages.cpp

@@ -523,19 +523,19 @@ bool CameraImages::readPoses(
 		UERROR("Cannot read pose file \"%s\".", filePath.c_str());
 		return false;
 	}
-	else if((format != 1 && format != 10 && format != 12 && format != 5 && format != 6 && format != 7 && format != 9) && poses.size() != this->imagesCount())
+	else if((format != 1 && format != 10 && format != 5 && format != 6 && format != 7 && format != 9) && poses.size() != this->imagesCount())
 	{
 		UERROR("The pose count is not the same as the images (%d vs %d)! Please remove "
 				"the pose file path if you don't want to use it (current file path=%s).",
 				(int)poses.size(), this->imagesCount(), filePath.c_str());
 		return false;
 	}
-	else if((format == 1 || format == 10 || format == 12 || format == 5 || format == 6 || format == 7 || format == 9) && (inOutStamps.empty() && stamps.size()!=poses.size()))
+	else if((format == 1 || format == 10 || format == 5 || format == 6 || format == 7 || format == 9) && (inOutStamps.empty() && stamps.size()!=poses.size()))
 	{
 		UERROR("When using RGBD-SLAM, GPS, MALAGA, ST LUCIA and EuRoC MAV formats, images must have timestamps!");
 		return false;
 	}
-	else if(format == 1 || format == 10 || format == 12 || format == 5 || format == 6 || format == 7 || format == 9)
+	else if(format == 1 || format == 10 || format == 5 || format == 6 || format == 7 || format == 9)
 	{
 		UDEBUG("");
 		//Match ground truth values with images

corelib/src/camera/CameraOrbbecSDK.cpp

@@ -1,728 +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
-WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
-DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
-ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- */
-
-#include <rtabmap/core/camera/CameraOrbbecSDK.h>
-#include <rtabmap/core/util2d.h>
-#include <rtabmap/utilite/UTimer.h>
-#include <rtabmap/utilite/UStl.h>
-#include <rtabmap/utilite/UThread.h>
-
-#ifdef RTABMAP_ORBBEC_SDK
-#include <libobsensor/ObSensor.hpp>
-#endif
-
-namespace rtabmap
-{
-#ifdef RTABMAP_ORBBEC_SDK
-Transform obToRtabmap(const OBExtrinsic & t)
-{
-    return Transform(t.rot[0], t.rot[1], t.rot[2], t.trans[0]/1000.0f,
-                     t.rot[3], t.rot[4], t.rot[5], t.trans[1]/1000.0f,
-                     t.rot[6], t.rot[7], t.rot[8], t.trans[2]/1000.0f);
-}
-
-
-cv::Mat obColorFrameToCv(const ob::VideoFrame & videoFrame)
-{
-    cv::Mat rgb;
-    switch(videoFrame.getFormat()) {
-    case OB_FORMAT_MJPG: {
-        cv::Mat rawMat(1, videoFrame.getDataSize(), CV_8UC1, videoFrame.getData());
-        rgb = cv::imdecode(rawMat, 1);
-    } break;
-    case OB_FORMAT_NV21: {
-        cv::Mat rawMat(videoFrame.getHeight() * 3 / 2, videoFrame.getWidth(), CV_8UC1, videoFrame.getData());
-        cv::cvtColor(rawMat, rgb, cv::COLOR_YUV2BGR_NV21);
-    } break;
-    case OB_FORMAT_YUYV:
-    case OB_FORMAT_YUY2: {
-        cv::Mat rawMat(videoFrame.getHeight(), videoFrame.getWidth(), CV_8UC2, videoFrame.getData());
-        cv::cvtColor(rawMat, rgb, cv::COLOR_YUV2BGR_YUY2);
-    } break;
-    case OB_FORMAT_BGR: {
-        cv::Mat rawMat(videoFrame.getHeight(), videoFrame.getWidth(), CV_8UC3, videoFrame.getData());
-        cv::cvtColor(rawMat, rgb, cv::COLOR_BGR2RGB);
-    } break;
-    case OB_FORMAT_RGB: {
-        cv::Mat rawMat(videoFrame.getHeight(), videoFrame.getWidth(), CV_8UC3, videoFrame.getData());
-        cv::cvtColor(rawMat, rgb, cv::COLOR_RGB2BGR);
-    } break;
-    case OB_FORMAT_RGBA: {
-        cv::Mat rawMat(videoFrame.getHeight(), videoFrame.getWidth(), CV_8UC4, videoFrame.getData());
-        cv::cvtColor(rawMat, rgb, cv::COLOR_RGBA2BGR);
-    } break;
-    case OB_FORMAT_BGRA: {
-        cv::Mat rawMat(videoFrame.getHeight(), videoFrame.getWidth(), CV_8UC4, videoFrame.getData());
-        cv::cvtColor(rawMat, rgb, cv::COLOR_BGRA2RGB);
-    } break;
-    case OB_FORMAT_UYVY: {
-        cv::Mat rawMat(videoFrame.getHeight(), videoFrame.getWidth(), CV_8UC2, videoFrame.getData());
-        cv::cvtColor(rawMat, rgb, cv::COLOR_YUV2BGR_UYVY);
-    } break;
-    case OB_FORMAT_I420: {
-        cv::Mat rawMat(videoFrame.getHeight() * 3 / 2, videoFrame.getWidth(), CV_8UC1, videoFrame.getData());
-        cv::cvtColor(rawMat, rgb, cv::COLOR_YUV2BGR_I420);
-    } break;
-    case OB_FORMAT_Y8: {
-        rgb = cv::Mat(videoFrame.getHeight(), videoFrame.getWidth(), CV_8UC1, videoFrame.getData()).clone();
-    } break;
-    case OB_FORMAT_Y16: {
-        cv::Mat rawMat(videoFrame.getHeight(), videoFrame.getWidth(), CV_16UC1, videoFrame.getData());
-        rawMat.convertTo(rgb, CV_8UC1, 255.0 / 65535.0);
-    } break;
-    default:
-        break;
-    }
-    return rgb;
-}
-
-cv::Mat obDepthFrameToCv(const ob::DepthFrame & depthFrame)
-{
-    cv::Mat depth;
-    if(depthFrame.getFormat() == OB_FORMAT_Y16 || depthFrame.getFormat() == OB_FORMAT_Z16 || depthFrame.getFormat() == OB_FORMAT_Y12C4) {
-        cv::Mat rawMat = cv::Mat(depthFrame.getHeight(), depthFrame.getWidth(), CV_16UC1, depthFrame.getData());
-        float scale = depthFrame.getValueScale() / 1000.0f;
-        rawMat.convertTo(depth, CV_32F, scale);
-    }
-    return depth;
-}
-
-cv::Mat obIntrinsicToK(const OBCameraIntrinsic & intrinsics)
-{
-    cv::Mat K = cv::Mat::eye(3,3,CV_64FC1);
-    K.at<double>(0,0) = intrinsics.fx;
-    K.at<double>(1,1) = intrinsics.fy;
-    K.at<double>(0,2) = intrinsics.cx;
-    K.at<double>(1,2) = intrinsics.cy;
-    return K;
-}
-cv::Mat obIntrinsicToP(const OBCameraIntrinsic & intrinsics)
-{
-    cv::Mat P = cv::Mat::eye(3,4,CV_64FC1);
-    obIntrinsicToK(intrinsics).copyTo(P.colRange(0,3));
-    return P;
-}
-cv::Mat obDistortionToD(const OBCameraDistortion & distortion)
-{
-    cv::Mat D = cv::Mat(1,8,CV_64FC1);
-    D.at<double>(0,0) = distortion.k1;
-    D.at<double>(0,1) = distortion.k2;
-    D.at<double>(0,2) = distortion.p1;
-    D.at<double>(0,3) = distortion.p2;
-    D.at<double>(0,4) = distortion.k3;
-    D.at<double>(0,5) = distortion.k4;
-    D.at<double>(0,6) = distortion.k5;
-    D.at<double>(0,7) = distortion.k6;
-    if(distortion.k4 == 0 && distortion.k5 == 0 && distortion.k6 == 0)
-    {
-        D = D.colRange(0,5);
-    }
-    return D;
-}
-#endif
-
-bool CameraOrbbecSDK::available()
-{
-#ifdef RTABMAP_ORBBEC_SDK
-    return true;
-#else
-    return false;
-#endif
-}
-
-CameraOrbbecSDK::CameraOrbbecSDK(
-        std::string deviceId,
-        int colorWidth,
-        int colorHeight,
-        int depthWidth,
-        int depthHeight,
-        float imageRate,
-        const Transform & localTransform) :
-            Camera(imageRate, localTransform)
-#ifdef RTABMAP_ORBBEC_SDK
-            , deviceId_(deviceId),
-            colorWidth_(colorWidth),
-            colorHeight_(colorHeight),
-            depthWidth_(depthWidth),
-            depthHeight_(depthHeight),
-            pipeline_(nullptr),
-            imuPipeline_(nullptr),
-            alignFilter_(nullptr),
-            imuLocalTransformInitialized_(false),
-            lastAccStamp_(0),
-            lastImageStamp_(0),
-            globalTimestampAvailable_(false),
-            rectifyColor_(false),
-            convertDepthToMM_(true),
-            imuPublished_(true)
-#endif
-{
-}
-
-CameraOrbbecSDK::~CameraOrbbecSDK()
-{
-#ifdef RTABMAP_ORBBEC_SDK
-    this->close();
-#endif
-}
-
-void CameraOrbbecSDK::close()
-{
-#ifdef RTABMAP_ORBBEC_SDK
-    if(imuPipeline_) {
-        imuPipeline_->stop();
-        delete imuPipeline_;
-        imuPipeline_=nullptr;
-    }
-    
-    if(pipeline_) {
-        pipeline_->stop();
-        delete pipeline_;
-        pipeline_=nullptr;
-    }
-    delete alignFilter_;
-    alignFilter_ = nullptr;
-    imuLocalTransform_ = Transform();
-    imuLocalTransformInitialized_ = false;
-    lastAccStamp_ = 0;
-    lastImageStamp_ = 0;
-    globalTimestampAvailable_ = false;
-    model_ = CameraModel();
-    imuBuffer_.clear();
-#endif
-}
-
-bool CameraOrbbecSDK::init(const std::string & calibrationFolder, const std::string & cameraName)
-{
-#ifdef RTABMAP_ORBBEC_SDK
-    this->close();
-
-    std::shared_ptr<ob::Device> device;
-
-    ob::Context context;
-    auto devices = context.queryDeviceList();
-    UINFO("%d device(s) found", devices->getCount());
-    for(uint32_t i=0; i<devices->getCount(); ++i)
-    {
-        auto currentDevice = devices->getDevice(i);
-        auto info = currentDevice->getDeviceInfo();
-
-        if(deviceId_.find('-') != std::string::npos)
-        {
-            // UID
-            if(deviceId_.compare(info->getUid()) == 0) {
-                device = currentDevice;
-            }
-        }
-        else if(uSplitNumChar(deviceId_).size() > 1)
-        {
-            // Serial
-            if(deviceId_.compare(info->getSerialNumber()) == 0) {
-                device = currentDevice;
-            }
-        }
-        else if((deviceId_.empty() && i==0) ||
-                (!deviceId_.empty() && uStr2Int(deviceId_) == (int)i)) {
-            // Index
-            device = currentDevice;
-        }
-
-        std::string type = "Unknown";
-        switch(info->getDeviceType()) 
-        {
-            case OB_STRUCTURED_LIGHT_MONOCULAR_CAMERA:
-                type = "Structured Light Monocular Camera";
-                break;
-            case OB_STRUCTURED_LIGHT_BINOCULAR_CAMERA:
-                type = "Structured Light Binocular Camera";
-                break;
-            case OB_TOF_CAMERA:
-                type = "TOF Camera";
-                break;
-            default:
-                break;
-        }
-        UINFO("Device %ld:", i);
-        UINFO("   Name: %s", info->getName());
-        UINFO("   Type: %s", type.c_str());
-        UINFO("   Serial: %s", info->getSerialNumber());
-        UINFO("   UID: %s", info->getUid());
-        UINFO("   Chip: %s", info->getAsicName());
-        UINFO("   Hardware version: %s", info->getHardwareVersion());
-        UINFO("   Firmware version: %s", info->getFirmwareVersion());
-    }
-
-    if(device.get() == nullptr) {
-        if(deviceId_.empty()) {
-            UERROR( "Could not find any orbbec compatible devices! Verify that the "
-                    "camera is correctly connected and the udev rules are installed.");
-        }
-        else {
-            UERROR("Could not find an orbbec device with ID \"%s\"! Verify that the "
-                   "camera is correctly connected and the udev rules are installed. "
-                   "Unset the ID to choose the first camera found.");
-        }
-        
-        return false;
-    }
-
-    bool hasGyro = false;
-    bool hasAccel = false;
-    auto sensors = device->getSensorList();
-
-    if(device->isGlobalTimestampSupported())
-    {
-        UINFO("Global (host time sync) timestamp is supported.");
-        device->enableGlobalTimestamp(true);
-        globalTimestampAvailable_ = true;
-    }
-    else
-    {
-        UWARN("Global (host time sync) timestamp is not supported! We will use device timestamp, so the camera frames won't be synchronizable with other sensors.");
-    }
-
-    uint32_t maxColorFps = 0;
-    uint32_t maxDepthFps = 0;
-    for(uint32_t i=0; i<sensors->getCount(); ++i)
-    {
-        if(sensors->getSensorType(i) == OB_SENSOR_GYRO)
-        {
-            hasGyro = true;
-        }
-        if(sensors->getSensorType(i) == OB_SENSOR_ACCEL)
-        {
-            hasAccel = true;
-        }
-        if( sensors->getSensorType(i) == OB_SENSOR_DEPTH ||
-            sensors->getSensorType(i) == OB_SENSOR_COLOR)
-        {
-            auto profiles = sensors->getSensor(i)->getStreamProfileList();
-            UINFO("Supported %s profiles:", sensors->getSensorType(i) == OB_SENSOR_DEPTH?"depth":"color");
-            for(uint32_t j=0; j<profiles->getCount(); ++j)
-            {
-                auto profile = profiles->getProfile(j)->as<ob::VideoStreamProfile>();
-                UINFO("Resolution: %ldx%ld, FPS: %ld, Format: %d", 
-                    profile->getWidth(), profile->getHeight(), profile->getFps(), profile->getFormat(), j==0?" (default)":"");
-                
-                if(sensors->getSensorType(i) == OB_SENSOR_DEPTH) {
-                    if(profile->getFps() > maxDepthFps) {
-                        maxDepthFps = profile->getFps();
-                    }
-                }
-                else
-                {
-                    if(profile->getFps() > maxColorFps) {
-                        maxColorFps = profile->getFps();
-                    }
-                }
-                
-            }
-        }
-    }
-
-    std::shared_ptr<ob::Config> imuConfig;
-    if(imuPublished_)
-    {
-        if(hasGyro && hasAccel)
-        {
-            imuPipeline_ = new ob::Pipeline(device);
-            imuConfig = std::make_shared<ob::Config>();
-            imuConfig->enableGyroStream();
-            imuConfig->enableAccelStream();
-            try {
-                UINFO("Starting imu pipeline");
-                imuPipeline_->start(imuConfig, [&](std::shared_ptr<ob::FrameSet> frameSet) {
-                    if(frameSet->getCount() != 2)
-                    {
-                        return;
-                    }
-
-                    if(!imuLocalTransformInitialized_)
-                    {
-                        return;
-                    }
-
-                    UASSERT(frameSet->getFrame(OB_FRAME_ACCEL) != nullptr && 
-                            frameSet->getFrame(OB_FRAME_GYRO) != nullptr);
-
-                    auto accel = frameSet->getFrame(OB_FRAME_ACCEL)->as<const ob::AccelFrame>();
-                    auto gyro = frameSet->getFrame(OB_FRAME_GYRO)->as<const ob::GyroFrame>();
-
-                    uint64_t accelStampUs = globalTimestampAvailable_?accel->getGlobalTimeStampUs():accel->getTimeStampUs();
-                    uint64_t gyroStampUs = globalTimestampAvailable_?gyro->getGlobalTimeStampUs():gyro->getTimeStampUs();
-    
-                    if(accelStampUs != gyroStampUs)
-                    {
-                        UWARN("Received accel and gyro frames with different timestamps (%llu vs %llu), skipping.",
-                            accelStampUs, gyroStampUs);
-                        return;
-                    }
-                    
-                    double accStamp = double(accelStampUs)/1e6;
-
-                    if(accelStampUs <= lastAccStamp_) {
-                        return;
-                    }
-
-                    lastAccStamp_ = accelStampUs;
-
-                    auto accelValue = accel->getValue();
-                    auto gyroValue = gyro->getValue();
-                    if(isInterIMUPublishing())
-                    {
-                        IMU imu(cv::Vec3f(gyroValue.x, gyroValue.y, gyroValue.z), cv::Mat::eye(3,3,CV_64FC1),
-                                cv::Vec3f(accelValue.x, accelValue.y, accelValue.z), cv::Mat::eye(3,3,CV_64FC1),
-                                imuLocalTransform_);
-                        this->postInterIMU(imu, accStamp);
-                    }
-                    else
-                    {
-                        UScopeMutex lock(imuMutex_);
-                        imuBuffer_.emplace_hint(imuBuffer_.end(), accStamp, cv::Vec6f(gyroValue.x, gyroValue.y, gyroValue.z, accelValue.x, accelValue.y, accelValue.z));
-                        if(imuBuffer_.size()>1000) {
-                            imuBuffer_.erase(imuBuffer_.begin());
-                        }
-                    }
-                });
-            }
-            catch(const ob::Error & e) {
-                UERROR("Unexpected error when configuring IMU stream: %s", e.what());
-            }
-        }
-        else
-        {
-            UWARN("IMU option is enabled but the camera doesn't have an IMU, ignoring.");
-        }
-    }
-
-    pipeline_ = new ob::Pipeline(device);
-    auto config = std::make_shared<ob::Config>();
-
-    // Set highest frame rate possible to reduce color/depth sync diff
-    config->enableVideoStream(OB_STREAM_COLOR, colorWidth_, colorHeight_, maxColorFps, OB_FORMAT_RGB);
-    config->enableVideoStream(OB_STREAM_DEPTH, depthWidth_, depthHeight_, maxDepthFps, OB_FORMAT_Y16);
-
-    UINFO("Using color profile: %dx%d", colorWidth_, colorHeight_);
-    UINFO("Using depth profile: %dx%d", depthWidth_, depthHeight_);
-
-    config->setFrameAggregateOutputMode(OB_FRAME_AGGREGATE_OUTPUT_ALL_TYPE_FRAME_REQUIRE);
-
-    config->setAlignMode(ALIGN_DISABLE);
-    config->setDepthScaleRequire(true);
-
-    pipeline_->enableFrameSync();
-
-    try {
-        UINFO("Starting camera pipeline");
-           pipeline_->start(config);
-
-        auto enabledStreams = pipeline_->getConfig()->getEnabledStreamProfileList();
-        if(imuPipeline_ != nullptr) {
-            for(uint32_t i=0; i<enabledStreams->getCount() && !imuLocalTransformInitialized_; ++i)
-            {
-                if(enabledStreams->getProfile(i)->getType() == OB_STREAM_COLOR)
-                {
-                    auto enabledImuStreams = imuPipeline_->getConfig()->getEnabledStreamProfileList();
-                    for(uint32_t j=0; j<enabledImuStreams->getCount(); ++j)
-                    {
-                        if(enabledImuStreams->getProfile(j)->getType() == OB_STREAM_ACCEL)
-                        {
-                            auto extrinsics = enabledStreams->getProfile(i)->as<ob::VideoStreamProfile>()->getExtrinsicTo(enabledImuStreams->getProfile(j)->as<ob::AccelStreamProfile>());
-                            // base -> color -> imu
-                            imuLocalTransform_ = this->getLocalTransform() * obToRtabmap(extrinsics);
-                            UINFO("IMU local transform: %s", imuLocalTransform_.prettyPrint().c_str());
-                            imuLocalTransformInitialized_ = true;
-                            break;
-                        }
-                    }
-                }
-            }
-        }
-
-        std::shared_ptr<ob::StreamProfile> colorProfile;
-        std::shared_ptr<ob::StreamProfile> depthProfile;
-        for(uint32_t i=0; i<enabledStreams->getCount(); ++i)
-        {
-            if(enabledStreams->getProfile(i)->getType() == OB_STREAM_COLOR) {
-                colorProfile = enabledStreams->getProfile(i);
-            }
-            else if(enabledStreams->getProfile(i)->getType() == OB_STREAM_DEPTH) {
-                depthProfile = enabledStreams->getProfile(i);
-            }
-        }
-        bool currentSelectionSupportsHwD2C = false;
-        auto hwD2CSupportedDepthStreamProfiles = pipeline_->getD2CDepthProfileList(colorProfile, ALIGN_D2C_HW_MODE);
-        if(hwD2CSupportedDepthStreamProfiles->count() == 0) {
-            UWARN("Current color profile selected doesn't support any hardware depth to color registration. Software registration is done instead.");
-        }
-        else
-        {
-            auto depthVsp = depthProfile->as<ob::VideoStreamProfile>();
-            auto count    = hwD2CSupportedDepthStreamProfiles->getCount();
-            for(uint32_t i = 0; i < count; i++) {
-                auto vsp = hwD2CSupportedDepthStreamProfiles->getProfile(i)->as<ob::VideoStreamProfile>();
-                UINFO("Supported depth to color format: Resolution: %ldx%ld, FPS: %ld, Format: %d", vsp->getWidth(), vsp->getHeight(), vsp->getFps(), vsp->getFormat(), i==0?" (default)":"");
-                if(vsp->getWidth() == depthVsp->getWidth() && vsp->getHeight() == depthVsp->getHeight() && vsp->getFormat() == depthVsp->getFormat()
-                && vsp->getFps() == depthVsp->getFps()) {
-                    currentSelectionSupportsHwD2C = true;
-                }
-            }
-        }
-        if(!currentSelectionSupportsHwD2C) {
-            UWARN("Hardware depth to color registration cannot be done with the selected color and depth profiles. "
-                  "Software registration is done instead, so more CPU will be needed on the host computer. "
-                  "Set logger level to info to see comptible depth formats for the selected color profile.");
-            alignFilter_ = new ob::Align(OB_STREAM_COLOR);
-            alignFilter_->setMatchTargetResolution(false);
-        }
-        else {
-            UINFO("Enabling hardware depth to color registration!");
-            config->setAlignMode(ALIGN_D2C_HW_MODE);
-            config->setDepthScaleRequire(false);
-            pipeline_->stop();
-            pipeline_->start(config);
-        }
-    }
-    catch(const ob::Error & e)
-    {
-        UERROR("Configuration not supported! Exception: %s", e.what());
-        UERROR("Supported formats:");
-        for(uint32_t i=0; i<sensors->getCount(); ++i)
-        {
-            if( sensors->getSensorType(i) == OB_SENSOR_DEPTH ||
-                sensors->getSensorType(i) == OB_SENSOR_COLOR)
-            {
-                auto profiles = sensors->getSensor(i)->getStreamProfileList();
-                for(uint32_t j=0; j<profiles->getCount(); ++j)
-                {
-                    auto profile = profiles->getProfile(j)->as<ob::VideoStreamProfile>();
-                    UERROR("%sResolution: %ldx%ld, FPS: %ld, Format: %d", 
-                        sensors->getSensorType(i) == OB_SENSOR_DEPTH?"Depth":"Color", 
-                        profile->getWidth(), 
-                        profile->getHeight(), 
-                        profile->getFps(), 
-                        profile->getFormat(), 
-                        j==0?" (default)":"");
-                }
-            }
-        }
-        return false;
-    }
-    
-    return true;
-#else
-    UERROR("CameraOrbbecSDK: RTAB-Map is not built with Orbbec SDK support!");
-    return false;
-#endif
-}
-
-bool CameraOrbbecSDK::isCalibrated() const
-{
-    return true;
-}
-
-std::string CameraOrbbecSDK::getSerial() const
-{
-#ifdef RTABMAP_ORBBEC_SDK
-    if(pipeline_) {
-        return pipeline_->getDevice()->getDeviceInfo()->getSerialNumber();
-    }
-#endif
-    return "";
-}
-
-void CameraOrbbecSDK::enableColorRectification(bool enabled)
-{
-#ifdef RTABMAP_ORBBEC_SDK
-    rectifyColor_ = enabled;
-#endif
-}
-
-void CameraOrbbecSDK::enableImu(bool enabled)
-{
-#ifdef RTABMAP_ORBBEC_SDK
-    imuPublished_ = enabled;
-#endif
-}
-
-void CameraOrbbecSDK::enableDepthMM(bool enabled)
-{
-#ifdef RTABMAP_ORBBEC_SDK
-    convertDepthToMM_ = enabled;
-#endif
-}
-
-SensorData CameraOrbbecSDK::captureImage(SensorCaptureInfo * info)
-{
-    SensorData data;
-
-#ifdef RTABMAP_ORBBEC_SDK
-    if(!pipeline_) {
-        UERROR("Camera is not initialized!");
-        return data;
-    }
-    auto frameset = pipeline_->waitForFrameset();
-    if(frameset == nullptr || frameset->getCount() == 0) {
-        UWARN("No frame received!");
-        return data;
-    }
-    if(frameset->getCount() != 2) {
-        UWARN("Received %s frames, expecting 2!", frameset->getCount());
-        return data;
-    }
-
-    if(alignFilter_ != nullptr) {
-        // Software depth to color registration
-        frameset = alignFilter_->process(frameset)->as<ob::FrameSet>();
-        UASSERT(frameset != nullptr);
-    }
-
-    auto colorFrame = frameset->getFrame(OB_FRAME_COLOR);
-    UASSERT(colorFrame != nullptr);
-    
-    auto depthFrame = frameset->getFrame(OB_FRAME_DEPTH);
-    UASSERT(depthFrame != nullptr);
-
-    auto colorVideoFrame = colorFrame->as<const ob::VideoFrame>();
-    auto depthVideoFrame = depthFrame->as<const ob::DepthFrame>();
-
-    cv::Mat rgb = obColorFrameToCv(*colorVideoFrame);
-    cv::Mat depth = obDepthFrameToCv(*depthVideoFrame);
-
-    if(rgb.empty()) {
-        UERROR("Could not convert the color frame! Type=%d Format=%d", colorFrame->getType(), colorVideoFrame->getFormat());
-    }
-    else if(depth.empty()) {
-        UERROR("Could not convert the depth frame! Type=%d Format=%d", depthFrame->getType(), depthVideoFrame->getFormat());
-    }
-    else if(!rgb.empty() && !depth.empty())
-    {
-        if(!model_.isValidForProjection())
-        {
-            auto streamProfile = colorFrame->getStreamProfile();
-            auto videoStreamProfile = streamProfile->as<ob::VideoStreamProfile>();
-
-            auto intrinsics = videoStreamProfile->getIntrinsic();
-            model_ = CameraModel(
-                getSerial(), 
-                cv::Size(intrinsics.width, intrinsics.height),
-                obIntrinsicToK(intrinsics),
-                obDistortionToD(videoStreamProfile->getDistortion()),
-                cv::Mat::eye(3,3,CV_64FC1),
-                obIntrinsicToP(intrinsics),
-                this->getLocalTransform());
-            if(rectifyColor_ && !model_.initRectificationMap()) {
-                UWARN("Could not initialize rectification map, color images won't be rectified.");
-            }
-        }
-    
-        if(rectifyColor_ && model_.isValidForRectification())
-        {
-            rgb = model_.rectifyImage(rgb);
-        }
-
-        if(convertDepthToMM_)
-        {
-            depth = util2d::cvtDepthFromFloat(depth);
-        }
-
-        uint64_t colorStampUs = globalTimestampAvailable_?colorFrame->getGlobalTimeStampUs():colorFrame->getTimeStampUs();
-        uint64_t depthStampUs = globalTimestampAvailable_?depthFrame->getGlobalTimeStampUs():depthFrame->getTimeStampUs();
-        double colorStamp = double(colorStampUs) / 1e6;
-        double depthStamp = double(depthStampUs) / 1e6;
-        if(fabs(colorStamp - depthStamp) > 0.018) {
-            // The difference seems varying between 0 and 17 ms normally
-            UWARN("Large timestamp difference (%fs) between color (%f) and depth (%f) frames. "
-                "Depth registration would be wrong on fast motion.", 
-                colorStamp - depthStamp, colorStamp, depthStamp);
-        }
-
-        uint64_t stampUs = colorStampUs < depthStampUs ? colorStampUs : depthStampUs;
-
-#ifdef WIN32
-        // On Windows, there is an issue that timestamps are not populated by default without following instructions from:
-        // https://github.com/orbbec/OrbbecSDK_v2/blob/main/scripts/env_setup/obsensor_metadata_win10.md
-        // Detect if the consecutive timestamps are identical, then send error!
-        if (stampUs <= lastImageStamp_)
-        {
-            UERROR("We detected non-consecutive timestamps, make sure you applied the fix from https://github.com/orbbec/OrbbecSDK_v2/blob/main/scripts/env_setup/obsensor_metadata_win10.md .");
-        }
-        lastImageStamp_ = stampUs;
-#endif
-        double stamp = double(stampUs)/1e6;
-
-        data = SensorData(rgb, depth, model_, this->getNextSeqID(), stamp);
-
-        if(imuPublished_ && !imuBuffer_.empty() && !this->isInterIMUPublishing())
-        {
-            cv::Vec6f imuVec;
-            std::map<double, cv::Vec6f>::const_iterator iterA, iterB;
-        
-            imuMutex_.lock();
-            int maximumTries = 10;
-            while(imuBuffer_.rbegin()->first < stamp && maximumTries-- > 0)
-            {
-                imuMutex_.unlock();
-                uSleep(1);
-                imuMutex_.lock();
-            }
-
-            if(imuBuffer_.rbegin()->first < stamp)
-            {
-                UWARN("Could not get IMU data at request image stamp %f after waiting 10 ms, latest imu stamp is %f", stamp, imuBuffer_.rbegin()->first);
-                imuMutex_.unlock();
-            }
-            else
-            {
-                // Interpolate imu data on image stamp
-                iterB = imuBuffer_.lower_bound(stamp);
-                iterA = iterB;
-                if(iterA != imuBuffer_.begin())
-                    iterA = --iterA;
-                if(iterA == iterB || stamp == iterB->first)
-                {
-                    imuVec = iterB->second;
-                }
-                else if(stamp > iterA->first && stamp < iterB->first)
-                {
-                    float t = (stamp-iterA->first) / (iterB->first-iterA->first);
-                    imuVec = iterA->second + t*(iterB->second - iterA->second);
-                }
-                imuBuffer_.erase(imuBuffer_.begin(), iterB);
-
-                imuMutex_.unlock();
-                data.setIMU(IMU(cv::Vec3d(imuVec[0], imuVec[1], imuVec[2]), cv::Mat::eye(3, 3, CV_64FC1), cv::Vec3d(imuVec[3], imuVec[4], imuVec[5]), cv::Mat::eye(3, 3, CV_64FC1), imuLocalTransform_));
-            }
-        }
-    }
-
-#else
-    UERROR("CameraOrbbecSDK: RTAB-Map is not built with Orbbec SDK support!");
-#endif
-    return data;
-}
-
-} // namespace rtabmap