Updated Multisession3ItMemoryThr flaky test checks

.dockerignore

@@ -1,10 +1,2 @@
 build/*
 build_*
-
-data/tests/*.db
-data/tests/*.7z
-data/tests/*.zip
-data/tests/*.pt
-data/tests/*.pth
-data/tests/*.py
-data/tests/__pycache__/

.github/workflows/android.yml

@@ -1,93 +0,0 @@
-name: android
-
-# Android build environments (docker/noble/android/rtabmap_apiXX).
-# amd64 only, so a single native runner and no manifest juggling.
-# Note: these build FROM introlab3it/rtabmap:android-noble-deps, which is not
-# produced by any workflow; it is still built and pushed by hand.
-
-on:
-  push:
-    branches:
-      - 'master'
-    paths: &android_paths
-      # What an Android build actually compiles: CMakeLists.txt adds only
-      # utilite, corelib and app under IF(ANDROID), and rtabmap.bash builds with
-      # WITH_OPENGV=OFF, BUILD_EXAMPLES=OFF, BUILD_TOOLS=OFF.
-      - 'CMakeLists.txt'
-      - 'Version.h.in'
-      - 'RTABMapConfig.cmake.in'
-      - 'cmake_uninstall.cmake.in'
-      - 'cmake_modules/**'
-      - 'utilite/**'
-      - 'corelib/**'
-      - 'app/**'
-      - 'docker/noble/android/**'
-      - '.github/workflows/android.yml'
-  pull_request:
-    branches:
-      - '**'
-    paths: *android_paths
-  workflow_dispatch:
-
-concurrency:
-  group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }}
-  cancel-in-progress: true
-
-jobs:
-  docker:
-    # A manual dispatch is honored only on master, the only ref we push from.
-    if: ${{ github.event_name != 'workflow_dispatch' || github.ref == 'refs/heads/master' }}
-    runs-on: ubuntu-latest
-
-    strategy:
-      fail-fast: false
-      matrix:
-        docker_tag: [android23, android24, android26, android30]
-        include:
-        - docker_tag: android23
-          docker_tags: |
-            introlab3it/rtabmap:android23
-            introlab3it/rtabmap:tango
-          api_version: 23
-        - docker_tag: android24
-          docker_tags: |
-            introlab3it/rtabmap:android24
-          api_version: 24
-        - docker_tag: android26
-          docker_tags: |
-            introlab3it/rtabmap:android26
-          api_version: 26
-        - docker_tag: android30
-          docker_tags: |
-            introlab3it/rtabmap:android30
-          api_version: 30
-
-    steps:
-      -
-        name: Checkout
-        uses: actions/checkout@v4
-      -
-        name: Set up Docker Buildx
-        uses: docker/setup-buildx-action@v3
-      -
-        name: Login to DockerHub
-        # Only needed when pushing; skipped on pull requests (secrets are
-        # unavailable for fork PRs and we don't push there anyway).
-        if: github.event_name != 'pull_request'
-        uses: docker/login-action@v3
-        with:
-          username: ${{ secrets.DOCKERHUB_USERNAME }}
-          password: ${{ secrets.DOCKERHUB_TOKEN }}
-      -
-        name: Build and push
-        uses: docker/build-push-action@v6
-        with:
-          context: .
-          push: ${{ github.event_name != 'pull_request' }}
-          platforms: linux/amd64
-          file: ./docker/noble/android/rtabmap_apiXX/Dockerfile
-          build-args: |
-            API_VERSION=${{ matrix.api_version }}
-          tags: ${{ matrix.docker_tags }}
-          cache-from: type=registry,ref=introlab3it/rtabmap:${{ matrix.docker_tag }}
-          cache-to: type=inline

.github/workflows/cmake-linux.yml

@@ -4,16 +4,9 @@ on:
   push:
     branches: 
       - master
-    paths-ignore: &platform_only
-      - '.github/workflows/android.yml'
-      - '.github/workflows/ios.yml'
-      - 'app/android/**'
-      - 'app/ios/**'
-      - 'docker/noble/android/**'
   pull_request:
     branches:
       - '**'
-    paths-ignore: *platform_only
   workflow_dispatch:
 
 env:

.github/workflows/cmake-macos.yml

@@ -4,16 +4,9 @@ on:
   push:
     branches: 
       - master
-    paths-ignore: &platform_only
-      - '.github/workflows/android.yml'
-      - '.github/workflows/ios.yml'
-      - 'app/android/**'
-      - 'app/ios/**'
-      - 'docker/noble/android/**'
   pull_request:
     branches:
       - '**'
-    paths-ignore: *platform_only
   workflow_dispatch:
 
 env:

.github/workflows/cmake-ros.yml

@@ -4,16 +4,9 @@ on:
   push:
     branches: 
       - master
-    paths-ignore: &platform_only
-      - '.github/workflows/android.yml'
-      - '.github/workflows/ios.yml'
-      - 'app/android/**'
-      - 'app/ios/**'
-      - 'docker/noble/android/**'
   pull_request:
     branches:
       - '**'
-    paths-ignore: *platform_only
   workflow_dispatch:
 
 env:

.github/workflows/cmake-windows.yml

@@ -4,16 +4,9 @@ on:
   push:
     branches: 
       - master
-    paths-ignore: &platform_only
-      - '.github/workflows/android.yml'
-      - '.github/workflows/ios.yml'
-      - 'app/android/**'
-      - 'app/ios/**'
-      - 'docker/noble/android/**'
   pull_request:
     branches:
       - '**'
-    paths-ignore: *platform_only
   workflow_dispatch:
 
 env:
@@ -60,48 +53,6 @@ jobs:
       shell: bash
       run: bash scripts/fetch_test_data.sh
 
-    - name: Install VC++ 2012 runtime
-      # The Kinect for Windows SDK 2.0 (WITH_K4W2=ON) is a VS2012 build, so
-      # Kinect20.dll needs MSVCR110.dll and MSVCP110.dll, and it reaches
-      # rtabmap_core as a load-time import. bundle_windows_deps.bat stages only
-      # Kinect20.dll itself into the vcpkg export, not the runtime it was built
-      # against, and the windows-2022 image lists no VC++ 2012 runtime (only
-      # 2013 and 2022). When nothing else on the machine happens to supply them,
-      # every executable linking rtabmap_core dies in the loader with 0xc0000135
-      # (STATUS_DLL_NOT_FOUND) before reaching main(), while the utilite tests,
-      # which link nothing but psapi, keep passing.
-      #
-      # The durable fix is to stage the two DLLs beside Kinect20.dll in the
-      # bundle, which would cover the shipped package too; that needs the bundle
-      # rebuilt and the cache key bumped, so install them here for now.
-      #
-      # Not pinned to one matrix leg: both build the package, and a package with
-      # Kinect support carries the same requirement.
-      shell: pwsh
-      run: |
-        $need = @('msvcr110.dll', 'msvcp110.dll')
-        function Get-Missing {
-          $need | Where-Object { -not (Test-Path (Join-Path "$env:SystemRoot\System32" $_)) }
-        }
-
-        if (-not (Get-Missing)) {
-          Write-Host "VC++ 2012 runtime already present in System32, nothing to do"
-          exit 0
-        }
-
-        Write-Host "Missing before install: $((Get-Missing) -join ', ')"
-        choco install -y vcredist2012 --no-progress
-        Write-Host "choco exit code: $LASTEXITCODE"
-
-        $still = Get-Missing
-        if ($still) {
-          # Warn rather than fail: the dependency dump in the next step reports
-          # the whole picture, which is more useful than stopping here.
-          Write-Host "::warning::Still missing from System32 after vcredist2012: $($still -join ', ')"
-        } else {
-          Write-Host "VC++ 2012 runtime installed: $($need -join ', ')"
-        }
-
     - name: Install Windows Dependencies
       if: matrix.build_name == 'windows-2022'
       uses: ./.github/actions/install-windows-deps
@@ -141,105 +92,6 @@ jobs:
     - name: Build
       run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} --target ALL_BUILD
 
-    - name: Diagnose loader dependencies
-      # ctest reports a loader failure as nothing but "Exit code 0xc0000135"
-      # (STATUS_DLL_NOT_FOUND): the process dies before main(), so gtest prints
-      # no output and the log never names the DLL that was not found. This walks
-      # the import tree of the executables ctest is about to run and reports the
-      # ones that do not resolve against the search path those processes see.
-      #
-      # Runs before Test, and keeps going on failure, so the report is in the log
-      # whether or not ctest then fails. Diagnostic only: it asserts nothing.
-      if: matrix.build_name != 'windows-2022-cuda'
-      continue-on-error: true
-      shell: pwsh
-      working-directory: ${{github.workspace}}/build/bin
-      run: |
-        $vswhere = "${env:ProgramFiles(x86)}\Microsoft Visual Studio\Installer\vswhere.exe"
-        if (-not (Test-Path $vswhere)) { Write-Host "vswhere not found, skipping"; exit 0 }
-        $vsPath = & $vswhere -latest -property installationPath
-        # Sorted descending so this is the newest toolset, the one that built
-        # the binaries, rather than whichever side-by-side version sorts first.
-        $dumpbin = Get-ChildItem "$vsPath\VC\Tools\MSVC" -Filter 'dumpbin.exe' -Recurse -ErrorAction SilentlyContinue |
-                   Where-Object { $_.FullName -like '*\Hostx64\x64\*' } |
-                   Sort-Object FullName -Descending | Select-Object -First 1
-        if (-not $dumpbin) { Write-Host "dumpbin not found under $vsPath, skipping"; exit 0 }
-        Write-Host "dumpbin : $($dumpbin.FullName)"
-        Write-Host "bin dir : $((Get-Location).Path) ($((Get-ChildItem -Filter '*.dll').Count) DLLs)"
-
-        # The loader looks in the executable's own directory first, then
-        # System32, then PATH. api-ms-win-* / ext-ms-* are virtual API sets
-        # resolved by the loader with no file on disk, so they never count as
-        # missing.
-        $searchDirs = @((Get-Location).Path, "$env:SystemRoot\System32") +
-                      ($env:PATH -split ';' | Where-Object { $_ -and (Test-Path $_) })
-
-        # Load-time and delay-load imports have to be told apart: only a
-        # missing load-time import kills the process with 0xc0000135. A missing
-        # delay-load one is resolved on first call, or never, so it is normal
-        # for the Windows security stack (HvsiFileTrust, wpaxholder) to show up
-        # there on a runner. dumpbin prints them in two sections.
-        function Get-Imports($file) {
-          $load = @(); $delay = @(); $mode = $null
-          foreach ($line in (& $dumpbin.FullName /dependents $file 2>$null)) {
-            if     ($line -match 'following delay load dependencies') { $mode = 'delay'; continue }
-            elseif ($line -match 'following dependencies')            { $mode = 'load';  continue }
-            elseif ($line -match '^\s*Summary')                       { $mode = $null;  continue }
-            if ($mode -and $line -match '^\s+(\S+\.dll)\s*$') {
-              if ($mode -eq 'load') { $load += $Matches[1] } else { $delay += $Matches[1] }
-            }
-          }
-          [pscustomobject]@{ Load = $load; Delay = $delay }
-        }
-
-        function Test-Resolvable($dll) {
-          $key = $dll.ToLower()
-          if ($key -like 'api-ms-*' -or $key -like 'ext-ms-*') { return $true }
-          [bool]($searchDirs | ForEach-Object { Join-Path $_ $dll } |
-                 Where-Object { Test-Path $_ } | Select-Object -First 1)
-        }
-
-        function Resolve-Dll($dll) {
-          $searchDirs | ForEach-Object { Join-Path $_ $dll } |
-            Where-Object { Test-Path $_ } | Select-Object -First 1
-        }
-
-        # Recurses through load-time imports only, which is the graph the
-        # loader must satisfy before main() runs. Delay-load imports of each
-        # visited binary are checked but not followed.
-        function Walk($file, $seen, $missing, $missingDelay) {
-          $imports = Get-Imports $file
-          foreach ($dll in $imports.Delay) {
-            if (-not (Test-Resolvable $dll)) { [void]$missingDelay.Add($dll) }
-          }
-          foreach ($dll in $imports.Load) {
-            if (-not $seen.Add($dll.ToLower())) { continue }
-            if ($dll.ToLower() -like 'api-ms-*' -or $dll.ToLower() -like 'ext-ms-*') { continue }
-            $hit = Resolve-Dll $dll
-            if ($hit) { Walk $hit $seen $missing $missingDelay }
-            else      { [void]$missing.Add("$dll  <- imported by $(Split-Path $file -Leaf)") }
-          }
-        }
-
-        # test_ulogger passes today and rtabmap_core is what every failing test
-        # has in common, so the three together separate "this executable is
-        # broken" from "the dependency bundle is incomplete".
-        foreach ($exe in @('test_ulogger.exe', 'test_corelib.exe', 'rtabmap-console.exe')) {
-          if (-not (Test-Path $exe)) { Write-Host "--- $exe : not built"; continue }
-          $seen         = [System.Collections.Generic.HashSet[string]]::new()
-          $missing      = [System.Collections.Generic.HashSet[string]]::new()
-          $missingDelay = [System.Collections.Generic.HashSet[string]]::new()
-          Walk (Resolve-Path $exe).Path $seen $missing $missingDelay
-          if ($missing.Count) {
-            Write-Host "--- $exe : $($missing.Count) of $($seen.Count) LOAD-TIME imports MISSING (these fail the loader)"
-            $missing | Sort-Object | ForEach-Object { Write-Host "        $_" }
-          } else {
-            Write-Host "--- $exe : all $($seen.Count) load-time imports resolve"
-          }
-          if ($missingDelay.Count) {
-            Write-Host "      (delay-load, resolved on first call, not a loader failure: $(($missingDelay | Sort-Object) -join ', '))"
-          }
-        }
     - name: Test
       # Not run on the CUDA build, which is a build+package job only.
       #

.github/workflows/coverage.yml

@@ -4,16 +4,9 @@ on:
   push:
     branches:
       - master
-    paths-ignore: &platform_only
-      - '.github/workflows/android.yml'
-      - '.github/workflows/ios.yml'
-      - 'app/android/**'
-      - 'app/ios/**'
-      - 'docker/noble/android/**'
   pull_request:
     branches:
       - '**'
-    paths-ignore: *platform_only
   workflow_dispatch:
 
 concurrency:

.github/workflows/docker-ros.yml

@@ -1,227 +0,0 @@
-name: docker-ros
-
-# ROS images: focal/noetic (ROS1) and jammy/humble, noble/jazzy, noble-kilted,
-# resolute (ROS2). Android images live in android.yml.
-#
-# Every arch is built natively: amd64 on an x86 runner, arm64 on a GitHub
-# arm64 runner, so no QEMU emulation is involved. Because a single Docker Hub
-# tag cannot hold two independently pushed architectures, each build pushes an
-# arch-suffixed tag (e.g. :resolute-amd64 / :resolute-arm64) and a final job
-# joins them into the real multi-arch tag (:resolute) with `imagetools create`.
-#
-# The runner image only hosts the build; it does not have to match the Ubuntu
-# release inside the image, so ubuntu-26.04{,-arm} is used for all of them
-# (ubuntu-22.04{,-arm} and ubuntu-24.04{,-arm} also exist, if ever needed).
-# That is also why focal can sit in this matrix: a 20.04 userland runs fine on
-# a newer host kernel, and ros:noetic-perception publishes a native arm64 image.
-
-on:
-  push:
-    branches:
-      - 'master'
-    paths-ignore: &platform_only
-      - '.github/workflows/android.yml'
-      - '.github/workflows/ios.yml'
-      - 'app/android/**'
-      - 'app/ios/**'
-      - 'docker/noble/android/**'
-  pull_request:
-    branches:
-      - '**'
-    paths-ignore: *platform_only
-  workflow_dispatch:
-
-concurrency:
-  group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }}
-  cancel-in-progress: true
-
-jobs:
-  docker_deps:
-
-    # The ###-deps images used to be too flaky to build here at all (seg faults,
-    # arm64 build timeouts under QEMU -- see
-    # https://github.com/introlab/rtabmap/issues/1454) and had to be built by
-    # hand from a 20.04 box with an upgraded qemu-user-static. Building each
-    # arch natively removes that cause.
-    # Skipped on pull requests; built and pushed only from master (push or
-    # manual dispatch), since it pushes the :*-deps tags to Docker Hub.
-    #
-    # focal is deliberately absent: its ROS1/noetic deps are frozen, so
-    # :focal-deps is built by hand on the rare occasion it changes. The focal
-    # runtime image below still builds here, FROM the published :focal-deps.
-    if: github.ref == 'refs/heads/master'
-
-    runs-on: ${{ matrix.runner }}
-
-    strategy:
-      fail-fast: false
-      matrix:
-        docker_dir: [jammy, noble, noble-kilted, resolute]
-        arch: [amd64, arm64]
-        include:
-        - arch: amd64
-          runner: ubuntu-26.04
-          docker_platform: linux/amd64
-        - arch: arm64
-          runner: ubuntu-26.04-arm
-          docker_platform: linux/arm64
-
-    steps:
-      -
-        name: Checkout
-        uses: actions/checkout@v4
-      -
-        name: Set up Docker Buildx
-        uses: docker/setup-buildx-action@v3
-      -
-        name: Login to DockerHub
-        uses: docker/login-action@v3
-        with:
-          username: ${{ secrets.DOCKERHUB_USERNAME }}
-          password: ${{ secrets.DOCKERHUB_TOKEN }}
-      -
-        name: Build and push
-        uses: docker/build-push-action@v6
-        with:
-          context: .
-          push: true
-          platforms: ${{ matrix.docker_platform }}
-          file: ./docker/${{ matrix.docker_dir }}/deps/Dockerfile
-          tags: introlab3it/rtabmap:${{ matrix.docker_dir }}-deps-${{ matrix.arch }}
-          cache-from: type=registry,ref=introlab3it/rtabmap:${{ matrix.docker_dir }}-deps-${{ matrix.arch }}
-          cache-to: type=inline
-
-  docker_deps_manifest:
-    needs: docker_deps
-    # Same gate as docker_deps, so both are skipped together on pull requests
-    # (github.ref is refs/pull/<n>/merge there): the per-arch -deps tags this
-    # joins are only ever pushed from master.
-    if: ${{ !cancelled() && !failure() && github.ref == 'refs/heads/master' }}
-    runs-on: ubuntu-26.04
-
-    strategy:
-      fail-fast: false
-      matrix:
-        docker_dir: [jammy, noble, noble-kilted, resolute]
-
-    steps:
-      -
-        name: Login to DockerHub
-        uses: docker/login-action@v3
-        with:
-          username: ${{ secrets.DOCKERHUB_USERNAME }}
-          password: ${{ secrets.DOCKERHUB_TOKEN }}
-      -
-        name: Create multi-arch manifest
-        run: |
-          docker buildx imagetools create \
-            -t introlab3it/rtabmap:${{ matrix.docker_dir }}-deps \
-            introlab3it/rtabmap:${{ matrix.docker_dir }}-deps-amd64 \
-            introlab3it/rtabmap:${{ matrix.docker_dir }}-deps-arm64
-
-  docker:
-    needs: docker_deps_manifest
-    # Run even when the deps jobs are skipped (they are, on pull requests):
-    # the runtime Dockerfiles then pull the :*-deps manifest already on Docker Hub.
-    # A manual dispatch is honored only on master, the only ref we push from.
-    if: ${{ !cancelled() && !failure() && (github.event_name != 'workflow_dispatch' || github.ref == 'refs/heads/master') }}
-    runs-on: ${{ matrix.runner }}
-
-    strategy:
-      fail-fast: false
-      matrix:
-        docker_dir: [focal, jammy, noble, noble-kilted, resolute]
-        arch: [amd64, arm64]
-        include:
-        - arch: amd64
-          runner: ubuntu-26.04
-          docker_platform: linux/amd64
-        - arch: arm64
-          runner: ubuntu-26.04-arm
-          docker_platform: linux/arm64
-
-    steps:
-      -
-        name: Checkout
-        uses: actions/checkout@v4
-      -
-        name: Set up Docker Buildx
-        uses: docker/setup-buildx-action@v3
-      -
-        name: Login to DockerHub
-        # Only needed when pushing; skipped on pull requests (secrets are
-        # unavailable for fork PRs and we don't push there anyway).
-        if: github.event_name != 'pull_request'
-        uses: docker/login-action@v3
-        with:
-          username: ${{ secrets.DOCKERHUB_USERNAME }}
-          password: ${{ secrets.DOCKERHUB_TOKEN }}
-      -
-        name: Build and push
-        uses: docker/build-push-action@v6
-        with:
-          context: .
-          push: ${{ github.event_name != 'pull_request' }}
-          platforms: ${{ matrix.docker_platform }}
-          file: ./docker/${{ matrix.docker_dir }}/Dockerfile
-          build-args: |
-            RUN_TESTS=1
-          tags: introlab3it/rtabmap:${{ matrix.docker_dir }}-${{ matrix.arch }}
-          cache-from: type=registry,ref=introlab3it/rtabmap:${{ matrix.docker_dir }}-${{ matrix.arch }}
-          cache-to: type=inline
-
-  docker_manifest:
-    needs: docker
-    # Nothing to join on pull requests, where the per-arch tags are never pushed.
-    if: ${{ !cancelled() && !failure() && github.event_name != 'pull_request' && (github.event_name != 'workflow_dispatch' || github.ref == 'refs/heads/master') }}
-    runs-on: ubuntu-26.04
-
-    strategy:
-      fail-fast: false
-      matrix:
-        docker_dir: [focal, jammy, noble, noble-kilted, resolute]
-        include:
-        - docker_dir: focal
-          docker_tags: |
-            introlab3it/rtabmap:focal
-            introlab3it/rtabmap:20.04
-        - docker_dir: jammy
-          docker_tags: |
-            introlab3it/rtabmap:jammy
-            introlab3it/rtabmap:22.04
-        - docker_dir: noble
-          docker_tags: |
-            introlab3it/rtabmap:noble
-            introlab3it/rtabmap:24.04
-        - docker_dir: noble-kilted
-          docker_tags: |
-            introlab3it/rtabmap:noble-kilted
-        # :latest tracks the newest ROS2 image (currently resolute, ROS2 lyrical
-        # on ubuntu 26.04) -- move it along with the next distro bump.
-        - docker_dir: resolute
-          docker_tags: |
-            introlab3it/rtabmap:resolute
-            introlab3it/rtabmap:26.04
-            introlab3it/rtabmap:latest
-
-    steps:
-      -
-        name: Login to DockerHub
-        uses: docker/login-action@v3
-        with:
-          username: ${{ secrets.DOCKERHUB_USERNAME }}
-          password: ${{ secrets.DOCKERHUB_TOKEN }}
-      -
-        name: Create multi-arch manifest
-        env:
-          DOCKER_TAGS: ${{ matrix.docker_tags }}
-        run: |
-          tag_args=()
-          while read -r tag; do
-            if [ -n "$tag" ]; then
-              tag_args+=(-t "$tag")
-            fi
-          done <<< "$DOCKER_TAGS"
-          docker buildx imagetools create "${tag_args[@]}" \
-            introlab3it/rtabmap:${{ matrix.docker_dir }}-amd64 \
-            introlab3it/rtabmap:${{ matrix.docker_dir }}-arm64

.github/workflows/docker.yml

@@ -0,0 +1,243 @@
+name: docker
+
+on:
+  push:
+    branches:
+      - 'master'
+  pull_request:
+    branches:
+      - '**'
+  workflow_dispatch:
+
+concurrency:
+  group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }}
+  cancel-in-progress: ${{ github.event_name == 'pull_request' }}
+
+jobs:
+  docker_deps:
+
+    # Disabling ###-deps step from CI because it is too flaky (seg faults, arm64 build timeout...)
+    # Only way I was able to build all images is to do it from a ubuntu 20.04 computer with:
+    #    $ sudo add-apt-repository ppa:canonical-server/server-backports
+    #    $ sudo apt-get update
+    #    $ sudo apt-get upgrade qemu-user-static
+    #    $ docker run --rm --privileged multiarch/qemu-user-static --reset -p yes -c yes
+    # More info: https://github.com/introlab/rtabmap/issues/1454
+    # Skipped on pull requests; built and pushed only from master (push or
+    # manual dispatch), since it pushes the :*-deps tags to Docker Hub.
+    if: github.ref == 'refs/heads/master'
+
+    runs-on: ubuntu-latest
+    
+    strategy:
+      fail-fast: false
+      matrix:
+        docker_tag: [focal-deps, jammy-deps, noble-deps, noble-kilted-deps, resolute-deps]
+        include:
+        - docker_tag: focal-deps
+          docker_tags: |
+            introlab3it/rtabmap:focal-deps
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'focal/deps'
+        - docker_tag: jammy-deps
+          docker_tags: |
+            introlab3it/rtabmap:jammy-deps
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'jammy/deps'
+        - docker_tag: noble-deps
+          docker_tags: |
+            introlab3it/rtabmap:noble-deps
+          docker_platforms: |
+            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'
+        - docker_tag: resolute-deps
+          docker_tags: |
+            introlab3it/rtabmap:resolute-deps
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'resolute/deps'
+    
+    steps:
+      -
+        name: Checkout
+        uses: actions/checkout@v2
+      - 
+        name: Set up QEMU
+        uses: docker/setup-qemu-action@v3
+        with:
+          platforms: all
+      -
+        name: Set up Docker Buildx
+        uses: docker/setup-buildx-action@v3
+      -
+        name: Login to DockerHub
+        uses: docker/login-action@v3
+        with:
+          username: ${{ secrets.DOCKERHUB_USERNAME }}
+          password: ${{ secrets.DOCKERHUB_TOKEN }}
+      -
+        name: Build and push
+        uses: docker/build-push-action@v6
+        with:
+          context: .
+          push: true
+          platforms: ${{ matrix.docker_platforms }}
+          file: ./docker/${{ matrix.docker_path }}/Dockerfile
+          tags: ${{ matrix.docker_tags }}
+          cache-from: type=registry,ref=introlab3it/rtabmap:${{ matrix.docker_tag }}
+          cache-to: type=inline
+  
+  docker:
+    needs: docker_deps
+    # Run even when docker_deps is skipped (it is, on pull requests).
+    # A manual dispatch is honored only on master, the only ref we push from.
+    if: ${{ !cancelled() && !failure() && (github.event_name != 'workflow_dispatch' || github.ref == 'refs/heads/master') }}
+    runs-on: ubuntu-latest
+    
+    strategy:
+      fail-fast: false
+      matrix:
+        docker_tag: [bionic, focal, jammy, noble, noble-kilted, resolute, android23, android24, android26, android30]
+        include:
+        - docker_tag: bionic
+          docker_tags: |
+            introlab3it/rtabmap:bionic
+            introlab3it/rtabmap:18.04
+          docker_args: |
+            NOT_USED=0
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'bionic'
+        - docker_tag: focal
+          docker_tags: |
+            introlab3it/rtabmap:focal
+            introlab3it/rtabmap:20.04
+            introlab3it/rtabmap:latest
+          docker_args: |
+            NOT_USED=0
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'focal'
+        - docker_tag: jammy
+          docker_tags: |
+            introlab3it/rtabmap:jammy
+            introlab3it/rtabmap:22.04
+          docker_args: |
+            NOT_USED=0
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'jammy'
+        - docker_tag: noble
+          docker_tags: |
+            introlab3it/rtabmap:noble
+            introlab3it/rtabmap:24.04
+          docker_args: |
+            NOT_USED=0
+          docker_platforms: |
+            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: resolute
+          docker_tags: |
+            introlab3it/rtabmap:resolute
+            introlab3it/rtabmap:26.04
+          docker_args: |
+            NOT_USED=0
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'resolute'
+        - docker_tag: android23
+          docker_tags: |
+            introlab3it/rtabmap:android23
+            introlab3it/rtabmap:tango
+          docker_args: |
+            API_VERSION=23
+          docker_platforms: |
+            linux/amd64
+          docker_path: 'noble/android/rtabmap_apiXX'
+        - docker_tag: android24
+          docker_tags: |
+            introlab3it/rtabmap:android24
+          docker_args: |
+            API_VERSION=24
+          docker_platforms: |
+            linux/amd64
+          docker_path: 'noble/android/rtabmap_apiXX'
+        - docker_tag: android26
+          docker_tags: |
+            introlab3it/rtabmap:android26
+          docker_args: |
+            API_VERSION=26
+          docker_platforms: |
+            linux/amd64
+          docker_path: 'noble/android/rtabmap_apiXX'
+        - docker_tag: android30
+          docker_tags: |
+            introlab3it/rtabmap:android30
+          docker_args: |
+            API_VERSION=30
+          docker_platforms: |
+            linux/amd64
+          docker_path: 'noble/android/rtabmap_apiXX'
+    
+    steps:
+      -
+        name: Checkout
+        uses: actions/checkout@v2
+      - 
+        name: Set up QEMU
+        uses: docker/setup-qemu-action@v3
+        with:
+          platforms: all
+      -
+        name: Set up Docker Buildx
+        uses: docker/setup-buildx-action@v3
+      -
+        name: Login to DockerHub
+        # Only needed when pushing; skipped on pull requests (secrets are
+        # unavailable for fork PRs and we don't push there anyway).
+        if: github.event_name != 'pull_request'
+        uses: docker/login-action@v3
+        with:
+          username: ${{ secrets.DOCKERHUB_USERNAME }}
+          password: ${{ secrets.DOCKERHUB_TOKEN }}
+      -
+        name: Build and push
+        uses: docker/build-push-action@v6
+        with:
+          context: .
+          push: ${{ github.event_name != 'pull_request' }}
+          platforms: ${{ github.event_name == 'pull_request' && 'linux/amd64' || matrix.docker_platforms }}
+          file: ./docker/${{ matrix.docker_path }}/Dockerfile
+          build-args: |
+            ${{ matrix.docker_args }}
+          tags: ${{ matrix.docker_tags }}
+          cache-from: type=registry,ref=introlab3it/rtabmap:${{ matrix.docker_tag }}
+          cache-to: type=inline
+

CMakeLists.txt

@@ -21,8 +21,8 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 # VERSION
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
-SET(RTABMAP_MINOR_VERSION 24)
-SET(RTABMAP_PATCH_VERSION 0)
+SET(RTABMAP_MINOR_VERSION 23)
+SET(RTABMAP_PATCH_VERSION 11)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
 
@@ -620,7 +620,14 @@ IF(WITH_GTSAM)
     # Force config mode to ignore PCL's findGTSAM.cmake file
     FIND_PACKAGE(GTSAM CONFIG QUIET)
     IF(GTSAM_FOUND)
-      INCLUDE(${CMAKE_CURRENT_SOURCE_DIR}/cmake_modules/CheckGTSAMFeatures.cmake)
+      # For issue https://github.com/introlab/rtabmap/pull/1626
+      FIND_FILE(GTSAM_NOISE_MODEL_FACTOR_N_FILE gtsam/nonlinear/NoiseModelFactorN.h
+         PATHS ${GTSAM_INCLUDE_DIR}
+         NO_DEFAULT_PATH)
+      IF(GTSAM_NOISE_MODEL_FACTOR_N_FILE)
+         MESSAGE(STATUS "GTSAM with NoiseModelFactorN.h")
+         ADD_DEFINITIONS("-DGTSAM_WITH_NOISE_MODEL_FACTOR_N")
+      ENDIF(GTSAM_NOISE_MODEL_FACTOR_N_FILE)
     ENDIF(GTSAM_FOUND)
 ENDIF(WITH_GTSAM)
 
@@ -1057,7 +1064,6 @@ IF(NOT MSVC)
 ENDIF()
 
 
-
 ####### OSX BUNDLE CMAKE_INSTALL_PREFIX #######
 IF(APPLE AND BUILD_AS_BUNDLE)
   IF(Qt6_FOUND OR Qt5_FOUND OR (QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND))

README.md

@@ -8,7 +8,7 @@ rtabmap
 [![codecov](https://codecov.io/gh/introlab/rtabmap/graph/badge.svg?token=mPwvfZMOia)](https://codecov.io/gh/introlab/rtabmap)
 [![License][license-image]][license]
 
-[release-image]: https://img.shields.io/github/v/release/introlab/rtabmap?color=green&style=flat
+[release-image]: https://img.shields.io/badge/release-0.23.1-green.svg?style=flat
 [releases]: https://github.com/introlab/rtabmap/releases
 
 [downloads-image]: https://img.shields.io/github/downloads/introlab/rtabmap/total?label=downloads
@@ -34,26 +34,58 @@ This project is supported by [IntRoLab - Intelligent / Interactive / Integrated
 
 #### CI Latest
 
-| | Build |
-|---|---|
-| Desktop | [![Linux](https://github.com/introlab/rtabmap/actions/workflows/cmake-linux.yml/badge.svg)](https://github.com/introlab/rtabmap/actions/workflows/cmake-linux.yml) [![Windows](https://github.com/introlab/rtabmap/actions/workflows/cmake-windows.yml/badge.svg)](https://github.com/introlab/rtabmap/actions/workflows/cmake-windows.yml) [![macOS](https://github.com/introlab/rtabmap/actions/workflows/cmake-macos.yml/badge.svg)](https://github.com/introlab/rtabmap/actions/workflows/cmake-macos.yml) |
-| ROS | [![CMake ROS](https://github.com/introlab/rtabmap/actions/workflows/cmake-ros.yml/badge.svg)](https://github.com/introlab/rtabmap/actions/workflows/cmake-ros.yml) [![Docker ROS](https://github.com/introlab/rtabmap/actions/workflows/docker-ros.yml/badge.svg)](https://github.com/introlab/rtabmap/actions/workflows/docker-ros.yml) |
-| Mobile | [![Android](https://github.com/introlab/rtabmap/actions/workflows/android.yml/badge.svg)](https://github.com/introlab/rtabmap/actions/workflows/android.yml) [![iOS](https://github.com/introlab/rtabmap/actions/workflows/ios.yml/badge.svg)](https://github.com/introlab/rtabmap/actions/workflows/ios.yml) |
-| Quality | [![Coverage](https://github.com/introlab/rtabmap/actions/workflows/coverage.yml/badge.svg)](https://github.com/introlab/rtabmap/actions/workflows/coverage.yml) [![Documentation](https://github.com/introlab/rtabmap/actions/workflows/docs.yml/badge.svg)](https://github.com/introlab/rtabmap/actions/workflows/docs.yml) |
+  <table>
+    <tbody>
+        <tr>
+           <td>
+           <a href="https://github.com/introlab/rtabmap/actions/workflows/cmake-linux.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/cmake-linux.yml/badge.svg" alt="CMake Linux Build Status"/> <br> 
+           <a href="https://github.com/introlab/rtabmap/actions/workflows/cmake-windows.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/cmake-windows.yml/badge.svg" alt="CMake Windows Build Status"/> <br> 
+           <a href="https://github.com/introlab/rtabmap/actions/workflows/cmake-macos.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/cmake-macos.yml/badge.svg" alt="CMake MaCOS Build Status"/> <br> 
+           <a href="https://github.com/introlab/rtabmap/actions/workflows/cmake-ros.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/cmake-ros.yml/badge.svg" alt="CMake ROS Build Status"/> <br> 
+           <a href="https://github.com/introlab/rtabmap/actions/workflows/docker.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/docker.yml/badge.svg" alt="Docker Build Status"/>
+           </td>
+        </tr>
+     </tbody>
+  </table>
  
  #### ROS Binaries
  
  `ros-$ROS_DISTRO-rtabmap`
  
-| | Distro | Ubuntu | Released | In apt | Build |
-|---|---|---|---|---|---|
-| ROS 1 | Noetic (EOL) | 20.04 | [![released](https://img.shields.io/badge/dynamic/yaml?url=https%3A%2F%2Fraw.githubusercontent.com%2Fros%2Frosdistro%2Fmaster%2Fnoetic%2Fdistribution.yaml&query=%24.repositories.rtabmap.release.version&label=%20)](https://github.com/ros/rosdistro/blob/master/noetic/distribution.yaml) | [![apt](https://img.shields.io/ros/v/noetic/rtabmap?label=%20)](https://index.ros.org/p/rtabmap/#noetic) |  |
-| ROS 2 | Humble | 22.04 | [![released](https://img.shields.io/badge/dynamic/yaml?url=https%3A%2F%2Fraw.githubusercontent.com%2Fros%2Frosdistro%2Fmaster%2Fhumble%2Fdistribution.yaml&query=%24.repositories.rtabmap.release.version&label=%20)](https://github.com/ros/rosdistro/blob/master/humble/distribution.yaml) | [![apt](https://img.shields.io/ros/v/humble/rtabmap?label=%20)](https://index.ros.org/p/rtabmap/#humble) | [![build](http://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary)](http://build.ros2.org/job/Hbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary/) |
-| ROS 2 | Iron (EOL) | 22.04 | [![released](https://img.shields.io/badge/dynamic/yaml?url=https%3A%2F%2Fraw.githubusercontent.com%2Fros%2Frosdistro%2Fmaster%2Firon%2Fdistribution.yaml&query=%24.repositories.rtabmap.release.version&label=%20)](https://github.com/ros/rosdistro/blob/master/iron/distribution.yaml) | [![apt](https://img.shields.io/ros/v/iron/rtabmap?label=%20)](https://index.ros.org/p/rtabmap/#iron) |  |
-| ROS 2 | Jazzy | 24.04 | [![released](https://img.shields.io/badge/dynamic/yaml?url=https%3A%2F%2Fraw.githubusercontent.com%2Fros%2Frosdistro%2Fmaster%2Fjazzy%2Fdistribution.yaml&query=%24.repositories.rtabmap.release.version&label=%20)](https://github.com/ros/rosdistro/blob/master/jazzy/distribution.yaml) | [![apt](https://img.shields.io/ros/v/jazzy/rtabmap?label=%20)](https://index.ros.org/p/rtabmap/#jazzy) | [![build](http://build.ros2.org/buildStatus/icon?job=Jbin_uN64__rtabmap__ubuntu_noble_amd64__binary)](http://build.ros2.org/job/Jbin_uN64__rtabmap__ubuntu_noble_amd64__binary/) |
-| ROS 2 | Kilted | 24.04 | [![released](https://img.shields.io/badge/dynamic/yaml?url=https%3A%2F%2Fraw.githubusercontent.com%2Fros%2Frosdistro%2Fmaster%2Fkilted%2Fdistribution.yaml&query=%24.repositories.rtabmap.release.version&label=%20)](https://github.com/ros/rosdistro/blob/master/kilted/distribution.yaml) | [![apt](https://img.shields.io/ros/v/kilted/rtabmap?label=%20)](https://index.ros.org/p/rtabmap/#kilted) | [![build](http://build.ros2.org/buildStatus/icon?job=Kbin_uN64__rtabmap__ubuntu_noble_amd64__binary)](http://build.ros2.org/job/Kbin_uN64__rtabmap__ubuntu_noble_amd64__binary/) |
-| ROS 2 | Lyrical | 26.04 | [![released](https://img.shields.io/badge/dynamic/yaml?url=https%3A%2F%2Fraw.githubusercontent.com%2Fros%2Frosdistro%2Fmaster%2Flyrical%2Fdistribution.yaml&query=%24.repositories.rtabmap.release.version&label=%20)](https://github.com/ros/rosdistro/blob/master/lyrical/distribution.yaml) | [![apt](https://img.shields.io/ros/v/lyrical/rtabmap?label=%20)](https://index.ros.org/p/rtabmap/#lyrical) | [![build](http://build.ros2.org/buildStatus/icon?job=Lbin_uR64__rtabmap__ubuntu_resolute_amd64__binary)](http://build.ros2.org/job/Lbin_uR64__rtabmap__ubuntu_resolute_amd64__binary/) |
-| ROS 2 | Rolling | 26.04 | [![released](https://img.shields.io/badge/dynamic/yaml?url=https%3A%2F%2Fraw.githubusercontent.com%2Fros%2Frosdistro%2Fmaster%2Frolling%2Fdistribution.yaml&query=%24.repositories.rtabmap.release.version&label=%20)](https://github.com/ros/rosdistro/blob/master/rolling/distribution.yaml) | [![apt](https://img.shields.io/ros/v/rolling/rtabmap?label=%20)](https://index.ros.org/p/rtabmap/#rolling) | [![build](http://build.ros2.org/buildStatus/icon?job=Rbin_uR64__rtabmap__ubuntu_resolute_amd64__binary)](http://build.ros2.org/job/Rbin_uR64__rtabmap__ubuntu_resolute_amd64__binary/) |
-| Docker | [rtabmap](https://hub.docker.com/r/introlab3it/rtabmap) | | | ![Docker Pulls](https://img.shields.io/docker/pulls/introlab3it/rtabmap.svg?label=pulls) | |
-
-*Released* is the version bloomed into [rosdistro](https://github.com/ros/rosdistro); *In apt* is what `apt install` actually gives you today. They differ while a release is waiting on a buildfarm sync.
+ <table>
+    <tbody>
+        <tr>
+           <td rowspan="1">ROS 1</td>
+            <td>Noetic</td>
+            <td><a href="http://build.ros.org/job/Nbin_ufv8_uFv8__rtabmap__ubuntu_focal_arm64__binary/"><img src="http://build.ros.org/buildStatus/icon?job=Nbin_ufv8_uFv8__rtabmap__ubuntu_focal_arm64__binary" alt="Build Status"/></td>
+        </tr>
+        <tr>
+            <td rowspan="5">ROS 2</td>
+            <td>Humble</td>
+            <td><a href="http://build.ros2.org/job/Hbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary" alt="Build Status"/></td>
+        </tr>
+        <tr>
+            <td>Jazzy</td>
+            <td><a href="http://build.ros2.org/job/Jbin_uN64__rtabmap__ubuntu_noble_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Jbin_uN64__rtabmap__ubuntu_noble_amd64__binary" alt="Build Status"/></td>
+        </tr>
+        <tr>
+            <td>Kilted</td>
+            <td><a href="http://build.ros2.org/job/Lbin_uR64__rtabmap__ubuntu_resolute_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Lbin_uR64__rtabmap__ubuntu_resolute_amd64__binary" alt="Build Status"/></td>
+        </tr>
+        <tr>
+            <td>Lyrical</td>
+            <td><a href="http://build.ros2.org/job/Kbin_uN64__rtabmap__ubuntu_noble_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Kbin_uN64__rtabmap__ubuntu_noble_amd64__binary" alt="Build Status"/></td>
+        </tr>
+        <tr>
+            <td>Rolling</td>
+            <td><a href="http://build.ros2.org/job/Rbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary" alt="Build Status"/></td>
+        </tr>
+        <tr>
+           <td>Docker</td>
+           <td>
+             <a href="https://hub.docker.com/r/introlab3it/rtabmap">rtabmap</a>
+           </td>
+           <td><img src="https://img.shields.io/docker/pulls/introlab3it/rtabmap" alt="Docker Pulls"/></td>
+        </tr>
+    </tbody>
+</table>

app/ios/RTABMapApp.xcodeproj/project.pbxproj

@@ -1078,7 +1078,7 @@
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include/eigen3\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include/pcl-1.15\"",
-					"\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.24\"",
+					"\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.23\"",
 					"\"$(SRCROOT)/../android/jni/tango-gl/include\"",
 					"\"$(SRCROOT)/../android/jni/third-party/include\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/lib/vtk.framework/Headers\"",
@@ -1139,7 +1139,7 @@
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include/eigen3\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include/pcl-1.15\"",
-					"\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.24\"",
+					"\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.23\"",
 					"\"$(SRCROOT)/../android/jni/tango-gl/include\"",
 					"\"$(SRCROOT)/../android/jni/third-party/include\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/lib/vtk.framework/Headers\"",

cmake_modules/CheckGTSAMFeatures.cmake

@@ -1,27 +0,0 @@
-# Detects GTSAM API variations that the version number alone can't tell
-# apart. Included after FIND_PACKAGE(GTSAM) succeeded.
-
-# Pose3AttitudeFactor has been replaced by AttitudeFactor<Pose3> in 4.3, but
-# the 4.3 ROS snapshots share a numeric version (4.3.0) while exposing either
-# API, so probe which one compiles. Older versions only have
-# Pose3AttitudeFactor. The probe links the imported gtsam target, so it
-# inherits GTSAM's usage requirements (including cxx_std_17 for 4.3) and
-# doesn't depend on the C++ standard selected later in the main CMakeLists.txt.
-IF(GTSAM_VERSION VERSION_GREATER_EQUAL "4.3.0")
-   INCLUDE(CheckCXXSourceCompiles)
-   INCLUDE(CMakePushCheckState)
-   CMAKE_PUSH_CHECK_STATE(RESET)
-   SET(CMAKE_REQUIRED_LIBRARIES gtsam)
-   UNSET(RTABMAP_GTSAM_HAS_ATTITUDE_FACTOR_TEMPLATE CACHE)
-   CHECK_CXX_SOURCE_COMPILES("
-      #include <gtsam/navigation/AttitudeFactor.h>
-      int main() {
-         gtsam::AttitudeFactor<gtsam::Pose3> factor(1, gtsam::Unit3(0,0,1),
-            gtsam::noiseModel::Isotropic::Sigma(2, 1.0));
-         return factor.evaluateError(gtsam::Pose3()).size() != 2;
-      }" RTABMAP_GTSAM_HAS_ATTITUDE_FACTOR_TEMPLATE)
-   CMAKE_POP_CHECK_STATE()
-   IF(RTABMAP_GTSAM_HAS_ATTITUDE_FACTOR_TEMPLATE)
-      ADD_DEFINITIONS(-DRTABMAP_GTSAM_HAS_ATTITUDE_FACTOR_TEMPLATE)
-   ENDIF()
-ENDIF()

corelib/include/rtabmap/core/Compression.h

@@ -41,8 +41,7 @@ namespace rtabmap {
  * @brief Background thread to compress or uncompress images and generic matrices.
  *
  * In compress mode, pass a source matrix to the constructor with an optional image
- * format (".png", ".jpg", ".rvl", or empty for zlib data, see @ref compressImage() for
- * depth options). In uncompress mode, pass
+ * format (".png", ".jpg", ".rvl", or empty for zlib data). In uncompress mode, pass
  * compressed bytes and set @c isImage accordingly. Call @ref UThread::start() then
  * @ref UThread::join() to obtain the result from @ref getCompressedData() or
  * @ref getUncompressedData().
@@ -71,7 +70,7 @@ public:
 	/**
 	 * @brief Constructs a thread in compress mode.
 	 * @param mat Source image or data matrix to compress.
-	 * @param format Image format: @c ".png", @c ".jpg", @c ".rvl" (see @ref compressImage()), or empty for zlib (@ref compressData2).
+	 * @param format Image format: @c ".png", @c ".jpg", @c ".rvl", or empty for zlib (@ref compressData2).
 	 */
 	CompressionThread(const cv::Mat & mat, const std::string & format = "");
 	/**
@@ -94,63 +93,7 @@ private:
 	bool compressMode_;
 };
 
-/*
- * Compressed depth image layouts (all values little-endian). They are stable: they are
- * saved in databases, and rtabmap_ros converts them to and from ROS's
- * compressed_depth_image_transport messages without decompressing the images.
- *
- *  - ".png": a standard PNG file. 16UC1 depth images are 16 bits grayscale PNGs,
- *    32FC1 depth images (legacy) are 4-channel 8 bits PNGs holding the float bytes.
- *  - ".rvl" (16UC1):
- *      [0..7]   "DEPTHRVL"
- *      [8..11]  uint32 cols
- *      [12..15] uint32 rows
- *      [16..]   RVL data (see RvlCodec)
- *  - ".png:<maxDepth>:<quantization>" or ".rvl:<maxDepth>:<quantization>" (32FC1):
- *      [0..7]   "DEPTHINV"
- *      [8..11]  float depthQuantA = quantization*(quantization+1)
- *      [12..15] float depthQuantB = 1 - depthQuantA/maxDepth
- *      [16..]   the 16UC1 inverse depth image in ".png" or ".rvl" layout above, where
- *               0 is invalid and v>0 is the depth depthQuantA/(v-depthQuantB).
- */
-
-/** @brief Signature of the ".rvl" layout (8 bytes, not null-terminated). */
-const char kCompressedDepthRvlSignature[8] = {'D', 'E', 'P', 'T', 'H', 'R', 'V', 'L'};
-/** @brief Size of the ".rvl" header: signature, uint32 cols, uint32 rows. */
-const size_t kCompressedDepthRvlHeaderSize = 16;
-/** @brief Signature of the inverse depth layout (8 bytes, not null-terminated). */
-const char kCompressedDepthInvSignature[8] = {'D', 'E', 'P', 'T', 'H', 'I', 'N', 'V'};
-/** @brief Size of the inverse depth header: signature, float depthQuantA, float depthQuantB. */
-const size_t kCompressedDepthInvHeaderSize = 16;
-
-/**
- * @brief Parses an image compression format "<codec>[:<maxDepth>[:<quantization>]]".
- *
- * @param format Format, e.g., @c ".jpg", @c ".png", @c ".rvl", @c ".png:10" or @c ".rvl:20:100".
- *        The empty format is valid (general zlib compression, see @ref CompressionThread).
- * @param codec Output codec (e.g., @c ".png").
- * @param maxDepth Output maximum depth (m) of the inverse depth format, 0 if not set.
- * @param quantization Output depth quantization of the inverse depth format
- *        (100 if not set but @p maxDepth is), 0 if @p maxDepth is not set.
- * @return false if the format is invalid. The inverse depth parameters are only
- *        valid with @c ".png" and @c ".rvl", and should be positive.
- */
-bool RTABMAP_CORE_EXPORT parseImageCompressionFormat(const std::string & format, std::string & codec, float & maxDepth, float & quantization);
-
-/**
- * @brief Encodes @p image to a byte buffer (OpenCV @c imencode or RVL for depth).
- *
- * @param format @c ".png", @c ".jpg" or @c ".rvl" (16UC1 only), optionally followed by
- *        @c ":<maxDepth>[:<quantization>]" (see @ref parseImageCompressionFormat()).
- *        For 32FC1 depth images, if @c maxDepth is set, depth is quantized on 16 bits
- *        as inverse depth (as ROS's @c compressed_depth_image_transport) and compressed
- *        with the codec. With A=quantization*(quantization+1) and B=1-A/maxDepth, the
- *        precision is ~d^2/(2A), and depth values over @c maxDepth or under A/(65535-B)
- *        are lost (set to 0). For example, ".png:10:100" keeps depth between 0.15 and
- *        10 m with errors of 0.05 mm at 1 m and 5 mm at 10 m. Otherwise, 32FC1 depth
- *        images are compressed losslessly as 4-channel 8 bits PNG (legacy format). Other
- *        image types ignore the depth parameters.
- */
+/** @brief Encodes @p image to a byte buffer (OpenCV @c imencode or RVL for depth). */
 std::vector<unsigned char> RTABMAP_CORE_EXPORT compressImage(const cv::Mat & image, const std::string & format = ".png");
 /** @brief Same as @ref compressImage() but returns a @c CV_8UC1 row matrix. */
 cv::Mat RTABMAP_CORE_EXPORT compressImage2(const cv::Mat & image, const std::string & format = ".png");
@@ -159,8 +102,6 @@ cv::Mat RTABMAP_CORE_EXPORT compressImage2(const cv::Mat & image, const std::str
 cv::Mat RTABMAP_CORE_EXPORT uncompressImage(const cv::Mat & bytes);
 /** @brief Decodes compressed image bytes to a @cv::Mat. */
 cv::Mat RTABMAP_CORE_EXPORT uncompressImage(const std::vector<unsigned char> & bytes);
-/** @brief Decodes compressed image bytes to a @cv::Mat. */
-cv::Mat RTABMAP_CORE_EXPORT uncompressImage(const unsigned char * bytes, size_t size);
 
 /** @brief Compresses a matrix with zlib; appends rows, cols and type at the end. */
 std::vector<unsigned char> RTABMAP_CORE_EXPORT compressData(const cv::Mat & data);
@@ -181,10 +122,7 @@ std::string RTABMAP_CORE_EXPORT uncompressString(const cv::Mat & bytes);
 
 /**
  * @brief Detects the compression format of depth image bytes.
- * @return @c ".rvl" if the buffer has an RVL signature, @c ".png:<maxDepth>:<quantization>"
- *         or @c ".rvl:<maxDepth>:<quantization>" for inverse depth images (see
- *         @ref compressImage()), otherwise @c ".png". The returned format can be passed
- *         back to @ref compressImage() to compress in the same format.
+ * @return @c ".rvl" if the buffer has an RVL signature, otherwise @c ".png".
  */
 std::string RTABMAP_CORE_EXPORT compressedDepthFormat(const cv::Mat & bytes);
 /** @overload */
@@ -192,6 +130,5 @@ std::string RTABMAP_CORE_EXPORT compressedDepthFormat(const std::vector<unsigned
 /** @overload */
 std::string RTABMAP_CORE_EXPORT compressedDepthFormat(const unsigned char * bytes, size_t size);
 
-
 } /* namespace rtabmap */
 #endif /* COMPRESSION_H_ */

corelib/include/rtabmap/core/DBDriver.h

@@ -331,12 +331,6 @@ protected:
 	/**
 	 * @name Backend implementation (subclass responsibility)
 	 * @brief Pure virtual SQL/backend hooks invoked by public wrappers above.
-	 *
-	 * These are called with \c _dbSafeAccessMutex locked. Implementations must not
-	 * call public methods that look in the trash (they lock \c _trashesMutex), as it
-	 * would invert the lock order used by emptyTrashes() and could deadlock. Call the
-	 * corresponding \c *Query() method directly instead (e.g., getLastIdQuery("Word", id)
-	 * instead of getLastWordId(id)).
 	 * @{*/
 	virtual bool connectDatabaseQuery(const std::string & url, bool overwritten = false, bool readOnly = false) = 0;
 	virtual void disconnectDatabaseQuery(bool save = true, const std::string & outputUrl = "") = 0;
@@ -463,9 +457,6 @@ private:
 	UMutex _transactionMutex;
 	std::map<int, Signature *> _trashSignatures;//<id, Signature*>
 	std::map<int, VisualWord *> _trashVisualWords; //<id, VisualWord*>
-	// Lock order: _trashesMutex -> _dbSafeAccessMutex -> _transactionMutex.
-	// emptyTrashes() locks _dbSafeAccessMutex before releasing _trashesMutex, so that
-	// an item not found in the trash is guaranteed to be readable from the database.
 	UMutex _trashesMutex;
 	UMutex _dbSafeAccessMutex;
 	USemaphore _addSem;

corelib/include/rtabmap/core/DBDriverSqlite3.h

@@ -156,40 +156,6 @@ public:
 	void setTempStore(int tempStore);
 
 protected:
-	/**
-	 * @name Trash-checking DBDriver methods, hidden on purpose
-	 * @brief These public DBDriver methods lock the trash mutex. They are hidden here so that
-	 * *Query() implementations, which are called with the database mutex already locked,
-	 * cannot call them by mistake (it would invert the lock order with DBDriver::emptyTrashes()
-	 * and could deadlock). Call the corresponding *Query() method instead.
-	 *
-	 * To call them from outside, use a DBDriver pointer or reference (e.g., DBDriver::create()).
-	 * @{*/
-	void asyncSave(Signature * s) = delete;
-	void asyncSave(VisualWord * vw) = delete;
-	void loadSignatures(const std::list<int> & ids, std::list<Signature *> & signatures, std::set<int> * loadedFromTrash = 0, bool loadWordIdsOnly = false) = delete;
-	void loadWords(const std::set<int> & wordIds, std::list<VisualWord *> & vws) = delete;
-	void loadNodeData(Signature & signature, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const = delete;
-	void loadNodeData(std::list<Signature *> & signatures, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const = delete;
-	void getNodeData(int signatureId, SensorData & data, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const = delete;
-	bool getCalibration(int signatureId, std::vector<CameraModel> & models, std::vector<StereoCameraModel> & stereoModels) const = delete;
-	bool getLaserScanInfo(int signatureId, LaserScan & info) const = delete;
-	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 = delete;
-	void getLocalFeatures(int signatureId, std::multimap<int, int> & words, std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> & points, cv::Mat & descriptors) const = delete;
-	void loadLinks(int signatureId, std::multimap<int, Link> & links, Link::Type type = Link::kUndef) const = delete;
-	void getWeight(int signatureId, int & weight) const = delete;
-	void getAllNodeIds(std::set<int> & ids, bool ignoreChildren = false, bool ignoreBadSignatures = false, bool ignoreIntermediateNodes = false) const = delete;
-	void getAllOdomPoses(std::map<int, Transform> & poses, bool ignoreChildren = false, bool ignoreIntermediateNodes = false) const = delete;
-	void getAllLinks(std::multimap<int, Link> & links, bool ignoreNullLinks = true, bool withLandmarks = false) const = delete;
-	void getLastNodeId(int & id) const = delete;
-	void getLastMapId(int & mapId) const = delete;
-	void getLastWordId(int & id) const = delete;
-	void getInvertedIndexNi(int signatureId, int & ni) const = delete;
-	void getNodesObservingLandmark(int landmarkId, std::map<int, Link> & nodes) const = delete;
-	void getNodeIdByLabel(const std::string & label, int & id) const = delete;
-	void getAllLabels(std::map<int, std::string> & labels) const = delete;
-	/** @} */
-
 	virtual bool connectDatabaseQuery(const std::string & url, bool overwritten = false, bool readOnly = false);
 	virtual void disconnectDatabaseQuery(bool save = true, const std::string & outputUrl = "");
 	virtual bool isConnectedQuery() const;

corelib/include/rtabmap/core/FlannIndex.h

@@ -201,7 +201,6 @@ public:
 	 *        structures ignoring it
 	 * @param eps Search for eps-approximate neighbors
 	 * @param sorted Give the neighbors back by increasing distance
-	 * @param cores Threads for the batch search (0 = all available)
 	 */
 	void knnSearch(
 			const cv::Mat & query,
@@ -210,8 +209,8 @@ public:
 	        int knn,
 			int checks = 32,
 			float eps = 0.0,
-			bool sorted = true,
-			int cores = 1) const;
+			bool sorted = true) const;
+
 	/**
 	 * @brief Search the neighbors of each query within a radius
 	 * @param query One feature per row, of the type and dimension the index was
@@ -226,7 +225,6 @@ public:
 	 *        structures ignoring it
 	 * @param eps Search for eps-approximate neighbors
 	 * @param sorted Give the neighbors back by increasing distance
-	 * @param cores Threads for the batch search (0 = all available)
 	 */
 	void radiusSearch(
 			const cv::Mat & query,
@@ -236,8 +234,7 @@ public:
 			int maxNeighbors = 0,
 			int checks = 32,
 			float eps = 0.0,
-			bool sorted = true,
-			int cores = 1) const;
+			bool sorted = true) const;
 
 private:
 	void * index_;               // rtflann backend

corelib/include/rtabmap/core/Graph.h

@@ -490,29 +490,6 @@ std::list<std::pair<int, Transform> > RTABMAP_CORE_EXPORT computePath(
 			int to,
 			bool updateNewCosts = false);
 
-/**
- * @brief Single-source graph depth via BFS.
- *
- * Runs one breadth-first search from @p from and returns, for every reached
- * node, its depth from @p from (i.e., the number of links on the shortest
- * path, @p from itself having depth 0).
- *
- * @note The depth is a number of hops, not a distance: it counts links, and the poses
- *       of the nodes play no part in it. The path it stands for is thus not the one
- *       @ref computePath() returns, which minimizes the Euclidean length instead and
- *       can walk more links to save meters.
- *
- * @param links Directed edges (`from` → `to`) keyed by source id.
- * @param from Start node id.
- * @param maxDepth If &gt; 0, only nodes with depth ≤ this value are returned (the
- *                 frontier is not expanded further); `0` explores the whole component.
- * @return Node id → depth mapping.
- */
-std::map<int, int> RTABMAP_CORE_EXPORT computePathDepths(
-			const std::multimap<int, int> & links,
-			int from,
-			int maxDepth = 0);
-
 /**
  * @brief Dijkstra shortest path on link constraints.
  *

corelib/include/rtabmap/core/Memory.h

@@ -847,8 +847,6 @@ private:
 	bool _stereoFromMotion;
 	unsigned int _imagePreDecimation;
 	unsigned int _imagePostDecimation;
-	bool _legacyDecimatedOctave;
-	bool _inverseDepthCompressionAllowed; // database version >= 0.24
 	bool _compressionParallelized;
 	float _laserScanDownsampleStepSize;
 	float _laserScanVoxelSize;

corelib/include/rtabmap/core/Parameters.h

@@ -222,7 +222,7 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(Mem, NotLinkedNodesKept,          bool, true,     "Keep not linked nodes in db (rehearsed nodes and deleted nodes).");
     RTABMAP_PARAM(Mem, IntermediateNodeDataKept,    bool, false,    "Keep intermediate node data in db.");
     RTABMAP_PARAM_STR(Mem, ImageCompressionFormat,   ".jpg",        "RGB image compression format. It should be \".jpg\" or \".png\".");
-    RTABMAP_PARAM_STR(Mem, DepthCompressionFormat,   ".rvl",        "Depth image compression format. It should be \".png\" or \".rvl\", optionally followed by \":maxDepth[:quantization]\" (e.g., \".rvl:10:100\", quantization is 100 by default) to compress 32FC1 depth images as 16 bits inverse depth with that codec (same quantization than ROS's compressed_depth_image_transport). 16UC1 depth images are always compressed losslessly with the codec. Warning: the inverse depth format is lossy, the precision is ~d^2/(2*q*(q+1)) (q=quantization, e.g., 0.05 mm at 1 m and 5 mm at 10 m with q=100) and depth values over maxDepth or under ~q*(q+1)/65535 meters (0.15 m with q=100) are lost. Without depth parameters, 32FC1 depth images are compressed losslessly in \".png\" format (4 channels 8 bits). Databases with depth images compressed in inverse depth format cannot be opened by rtabmap versions under 0.24.");
+    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, LocalizationReadOnly,        bool, false,    uFormat("In localization mode, open the database in read-only mode (ignored if %s=true). Currrenty incompatible with memory management (%s and %s cannot be used) and if there are disjoint sessions in working memory. Last localization pose won't be saved back in the database at the end of the session, so the robot will always restart to original last localization pose, unless %s is used or an external initial pose is provided on initialization.", kMemIncrementalMemory().c_str(), kRtabmapLoopThr().c_str(), kRtabmapMemoryThr().c_str(), kRGBDStartAtOrigin().c_str()).c_str());
@@ -256,7 +256,6 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(Kp, IncrementalDictionary,    bool, true,   "");
     RTABMAP_PARAM(Kp, IncrementalFlann,         bool, true,   uFormat("When using FLANN based strategy, add/remove points to its index without always rebuilding the index (the index is only rebuilt when too many of its features have been removed, see \"%s\").", kKpFlannRebalancingFactor().c_str()));
     RTABMAP_PARAM(Kp, FlannRebalancingFactor,   float, 2.0,   uFormat("Rebuild the incremental FLANN index (see \"%s\") once the ratio (factor-1)/factor of its features has been removed, e.g. half of them for a factor of 2. Rebuilding frees the memory of the removed features and speeds up the searches. Features are mostly removed when memory management is enabled (\"%s\" or \"%s\"). Set to 1 to never rebuild, which also uses less memory as the features don't have to be referenced one by one.", kKpIncrementalFlann().c_str(), kRtabmapTimeThr().c_str(), kRtabmapMemoryThr().c_str()));
-    RTABMAP_PARAM(Kp, FlannThreads,             int, 1,       "Number of threads used for FLANN kNN search (batched queries). Set to 0 for all available.");
     RTABMAP_PARAM(Kp, ByteToFloat,              bool, false,  uFormat("For %s=1, binary descriptors are converted to float by converting each byte to float instead of converting each bit to float. When converting bytes instead of bits, less memory is used and search is faster at the cost of slightly less accurate matching.", kKpNNStrategy().c_str()));
     RTABMAP_PARAM(Kp, MaxDepth,                 float, 0,     "Filter extracted keypoints by depth (0=inf).");
     RTABMAP_PARAM(Kp, MinDepth,                 float, 0,     "Filter extracted keypoints by depth.");
@@ -472,7 +471,7 @@ class RTABMAP_CORE_EXPORT Parameters
 #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, PriorsIgnored,   bool, true,      "Ignore prior constraints (global pose, GPS or marker priors) while optimizing. Currently only g2o and gtsam optimization supports this.");
+    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)
     RTABMAP_PARAM(Optimizer, GravitySigma,    float, 0.3,      uFormat("Gravity sigma value (>=0, typically between 0.1 and 0.3). Optimization is done while preserving gravity orientation of the poses. This should be used only with visual/lidar inertial odometry approaches, for which we assume that all odometry poses are aligned with gravity. Set to 0 to disable gravity constraints. Currently supported only with g2o and GTSAM optimization strategies (see %s).", kOptimizerStrategy().c_str()));
@@ -961,7 +960,7 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(Marker, VarianceOrientationIgnored, bool, false, uFormat("When this setting is false, the landmark's orientation is optimized during graph optimization. When this setting is true, only the position of the landmark is optimized. This can be useful when the landmark's orientation estimation is not reliable. Note that for %s=1 (g2o), only %s needs be set if we ignore orientation. For %s=2 (GTSAM), instead of optimizing the landmark's position directly, a bearing/range factor is used, with %s as the variance of the range factor (with 9999 to optimize the position with only a bearing factor) and %s as the variance of the bearing factor (pitch/yaw).", kOptimizerStrategy().c_str(), kMarkerVarianceLinear().c_str(), kOptimizerStrategy().c_str(), kMarkerVarianceLinear().c_str(), kMarkerVarianceAngular().c_str()));
     RTABMAP_PARAM(Marker, MaxRange,               float, 0.0,   "Maximum range in which markers will be detected. <=0 for unlimited range.");
     RTABMAP_PARAM(Marker, MinRange,               float, 0.0,   "Miniminum range in which markers will be detected. <=0 for unlimited range.");
-    RTABMAP_PARAM_STR(Marker, Priors,             "",           uFormat("World prior locations of the markers. The map will be transformed in marker's world frame when a tag is detected. Format is the marker's ID followed by its position (angles in rad), multiple markers are separated by vertical line (\"id1 x y z roll pitch yaw|id2 x y z roll pitch yaw\"). Example:  \"1 0 0 1 0 0 0|2 1 0 1 0 0 1.57\" (marker 2 is 1 meter forward than marker 1 with 90 deg yaw rotation). The priors are used only if %s is false.", kOptimizerPriorsIgnored().c_str()));
+    RTABMAP_PARAM_STR(Marker, Priors,             "",           "World prior locations of the markers. The map will be transformed in marker's world frame when a tag is detected. Format is the marker's ID followed by its position (angles in rad), multiple markers are separated by vertical line (\"id1 x y z roll pitch yaw|id2 x y z roll pitch yaw\"). Example:  \"1 0 0 1 0 0 0|2 1 0 1 0 0 1.57\" (marker 2 is 1 meter forward than marker 1 with 90 deg yaw rotation).");
     RTABMAP_PARAM(Marker, PriorsVarianceLinear,   float, 0.001, "Linear variance to set on marker priors.");
     RTABMAP_PARAM(Marker, PriorsVarianceAngular,  float, 0.001, "Angular variance to set on marker priors.");
 

corelib/include/rtabmap/core/SensorData.h

@@ -470,33 +470,21 @@ public:
 	 * @brief Checks if the sensor data is valid
 	 * 
 	 * Returns true if the sensor data contains at least one of:
-	 * - Valid ID (> 0)
+	 * - Valid ID (> 0) or non-zero stamp
 	 * - Images (raw or compressed)
 	 * - Depth/right images (raw or compressed)
 	 * - Depth confidence (raw or compressed)
 	 * - Laser scan (raw or compressed)
-	 * - Camera models (mono or stereo), at least one valid for projection
-	 *   (see CameraModel::isValidForProjection() and StereoCameraModel::isValidForProjection())
+	 * - Camera models (mono or stereo)
 	 * - User data (raw or compressed)
 	 * - Keypoints and descriptors
-	 * - Occupancy grid cells (ground, obstacles or empty)
 	 * - IMU data
-	 *
-	 * @note The stamp is not considered: a SensorData with only a stamp set is not valid
-	 *       (e.g., an empty message converted from ROS still has its header stamp
-	 *       and a camera model created from an empty camera info).
-	 *
+	 * 
 	 * @return True if the sensor data contains any valid information, false otherwise
 	 */
 	bool isValid() const {
-		bool hasCameraModel = false;
-		for (size_t i=0; i < _cameraModels.size() && !hasCameraModel; ++i)
-			hasCameraModel = _cameraModels[i].isValidForProjection();
-		if (!hasCameraModel)
-			for (size_t i=0; i < _stereoCameraModels.size() && !hasCameraModel; ++i)
-				hasCameraModel = _stereoCameraModels[i].isValidForProjection();
-
 		return !(_id == 0 &&
+			_stamp == 0.0 &&
 			_imageRaw.empty() &&
 			_imageCompressed.empty() &&
 			_depthOrRightRaw.empty() &&
@@ -505,7 +493,8 @@ public:
 			_depthConfidenceCompressed.empty() &&
 			_laserScanRaw.isEmpty() &&
 			_laserScanCompressed.isEmpty() &&
-			!hasCameraModel &&
+			_cameraModels.empty() &&
+			_stereoCameraModels.empty() &&
 			_userDataRaw.empty() &&
 			_userDataCompressed.empty() &&
 			_keypoints.size() == 0 &&
@@ -598,8 +587,6 @@ public:
 	/**
 	 * Set image data. Detect automatically if raw or compressed.
 	 * A matrix of type CV_8UC1 with 1 row is considered as compressed.
-	 * An invalid @p model (not CameraModel::isValidForProjection()) without any image is
-	 * a placeholder (e.g., scan-only data): it is not added, so cameraModels() is empty.
 	 * @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);
@@ -744,15 +731,11 @@ public:
 
 	/**
 	 * Set user data. Detect automatically if raw or compressed. If raw, the data is
-	 * compressed too, unless compressed user data is already set (only possible with
-	 * @p clearPreviousData=false), which is then assumed to be that raw data compressed
-	 * and kept as is. A matrix of type CV_8UC1 with 1 row is considered as compressed.
+	 * compressed too. A matrix of type CV_8UC1 with 1 row is considered as compressed.
 	 * If you have one dimension unsigned 8 bits raw data, make sure to transpose it
 	 * (to have multiple rows instead of multiple columns) in order to be detected as
 	 * not compressed.
 	 * @param clearPreviousData, clear previous raw and compressed user data before setting the new one.
-	 *        With false, setting the raw data of compressed user data already set keeps
-	 *        the compressed one, like setLaserScan() and setRGBDImage() do.
 	 */
 	void setUserData(const cv::Mat & userData, bool clearPreviousData = true);
 	const cv::Mat & userDataRaw() const {return _userDataRaw;}
@@ -1027,9 +1010,6 @@ public:
 #endif
 
 private:
-	/// Whether setRGBDImage() keeps @p model: not an invalid model without any image.
-	bool keepCameraModel(const CameraModel & model, const cv::Mat & rgb, const cv::Mat & depth, bool clearPreviousData) const;
-
 	int _id; ///< Unique sensor data ID (0 if invalid)
 	double _stamp; ///< Timestamp in seconds
 

corelib/include/rtabmap/core/StereoCameraModel.h

@@ -427,51 +427,6 @@ public:
 	 */
 	float computeDisparity(unsigned short depth) const; // mm
 
-	/**
-	 * @brief Reprojects a 3D point of the left camera frame into both image planes (floating-point).
-	 *
-	 * The point is given in the rectified left camera frame (/camera_link), the same frame used
-	 * by CameraModel::reproject() of left(). The baseline is taken from the Tx of the rectified
-	 * projection matrices, so the horizontal shift between uLeft and uRight is the disparity of
-	 * that point. On a rectified stereo pair the rows are aligned, thus vRight equals vLeft.
-	 *
-	 * @note Unlike this function, CameraModel::reproject() ignores Tx, because a Tx set on a
-	 *       single camera model is also used to tag a left camera having stereo observations
-	 *       (see the stereo edges built by the BA optimizers).
-	 *
-	 * @param x X coordinate in the left camera space.
-	 * @param y Y coordinate in the left camera space.
-	 * @param z Z coordinate in the left camera space (must be non-zero).
-	 * @param[out] uLeft Output horizontal image coordinate in the left image (float).
-	 * @param[out] vLeft Output vertical image coordinate in the left image (float).
-	 * @param[out] uRight Output horizontal image coordinate in the right image (float).
-	 * @param[out] vRight Output vertical image coordinate in the right image (float).
-	 *
-	 * @pre `z != 0`
-	 *
-	 * @see CameraModel::reproject(), reproject(int&, int&, int&, int&)
-	 */
-	void reproject(float x, float y, float z, float & uLeft, float & vLeft, float & uRight, float & vRight) const;
-
-	/**
-	 * @brief Reprojects a 3D point of the left camera frame into both image planes (rounded to int).
-	 *
-	 * This version of `reproject()` returns integer pixel indices, computed from the 3D position.
-	 *
-	 * @param x X coordinate in the left camera space.
-	 * @param y Y coordinate in the left camera space.
-	 * @param z Z coordinate in the left camera space (must be non-zero).
-	 * @param[out] uLeft Output horizontal image coordinate in the left image (integer pixel).
-	 * @param[out] vLeft Output vertical image coordinate in the left image (integer pixel).
-	 * @param[out] uRight Output horizontal image coordinate in the right image (integer pixel).
-	 * @param[out] vRight Output vertical image coordinate in the right image (integer pixel).
-	 *
-	 * @pre `z != 0`
-	 *
-	 * @see CameraModel::reproject(), reproject(float&, float&, float&, float&)
-	 */
-	void reproject(float x, float y, float z, int & uLeft, int & vLeft, int & uRight, int & vRight) const;
-
 	const cv::Mat & R() const {return R_;} ///< Stereo extrinsic rotation matrix.
 	const cv::Mat & T() const {return T_;} ///< Stereo extrinsic translation vector.
 	const cv::Mat & E() const {return E_;} ///< Essential matrix.

corelib/include/rtabmap/core/VWDictionary.h

@@ -495,11 +495,6 @@ private:
 	 */
 	float _rebalancingFactor;
 	
-	/**
-	 * @brief Threads for FLANN batched kNN search (0 = all available)
-	 */
-	int _flannThreads;
-	
 	/**
 	 * @brief Whether to convert descriptors from byte to float format
 	 */

corelib/include/rtabmap/core/util3d_registration.h

@@ -41,6 +41,10 @@ namespace rtabmap
 namespace util3d
 {
 
+int RTABMAP_CORE_EXPORT getCorrespondencesCount(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
+							const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
+							float maxDistance);
+
 /**
  * @brief Estimates the rigid 3D transformation between two point clouds using SVD.
  *

corelib/include/rtabmap/core/util3d_surface.h

@@ -150,8 +150,7 @@ pcl::TextureMesh::Ptr RTABMAP_CORE_EXPORT createTextureMesh(
 		const std::vector<float> & roiRatios = std::vector<float>(), // [left, right, top, bottom] region of interest (in ratios) of the image projected.
 		const ProgressState * state = 0,
 		std::vector<std::map<int, pcl::PointXY> > * vertexToPixels = 0, // For each point, we have a list of cameras with corresponding pixel in it. Beware that the camera ids don't correspond to pose ids, they are indexes from 0 to total camera models and texture's materials.
-		bool distanceToCamPolicy = false,
-		int numThreads = 1); // number of threads used to compute the visible faces of the cameras (1=sequential)
+		bool distanceToCamPolicy = false);
 pcl::TextureMesh::Ptr RTABMAP_CORE_EXPORT createTextureMesh(
 		const pcl::PolygonMesh::Ptr & mesh,
 		const std::map<int, Transform> & poses,
@@ -164,8 +163,7 @@ pcl::TextureMesh::Ptr RTABMAP_CORE_EXPORT createTextureMesh(
 		const std::vector<float> & roiRatios = std::vector<float>(), // [left, right, top, bottom] region of interest (in ratios) of the image projected.
 		const ProgressState * state = 0,
 		std::vector<std::map<int, pcl::PointXY> > * vertexToPixels = 0, // For each point, we have a list of cameras with corresponding pixel in it. Beware that the camera ids don't correspond to pose ids, they are indexes from 0 to total camera models and texture's materials.
-		bool distanceToCamPolicy = false,
-		int numThreads = 1); // number of threads used to compute the visible faces of the cameras (1=sequential)
+		bool distanceToCamPolicy = false);
 
 /**
  * Remove not textured polygon clusters. If minClusterSize<0, only the largest cluster is kept.

corelib/src/CMakeLists.txt

@@ -689,42 +689,16 @@ IF(grid_map_core_FOUND)
             ${LIBRARIES}
             grid_map_core::grid_map_core
         )
-        # ${grid_map_core_INCLUDE_DIRS} is only ${EIGEN3_INCLUDE_DIR} on an
-        # ament install; the path to grid_map's own headers lives solely on
-        # the imported target.
-        GET_TARGET_PROPERTY(grid_map_core_PUBLIC_INCLUDE_DIRS
-                            grid_map_core::grid_map_core
-                            INTERFACE_INCLUDE_DIRECTORIES)
     ELSE()
-        SET(grid_map_core_PUBLIC_INCLUDE_DIRS ${grid_map_core_INCLUDE_DIRS})
+        SET(INCLUDE_DIRS
+            ${INCLUDE_DIRS}
+            ${grid_map_core_INCLUDE_DIRS}
+        )
         SET(LIBRARIES
             ${LIBRARIES}
             ${grid_map_core_LIBRARIES}
         )
     ENDIF()
-
-    # grid_map_core links PRIVATE (only global_map/GridMap.cpp includes it),
-    # but its Eigen plugins aren't private: FIND_PACKAGE(grid_map_core) injects
-    # -DEIGEN_FUNCTORS_PLUGIN / -DEIGEN_DENSEBASE_PLUGIN with a directory-scope
-    # ADD_DEFINITIONS, adding members to Eigen::MatrixBase and DenseBase in
-    # every translation unit. grid_map never pairs those with the include
-    # directory holding the headers they name (still true on master), so any
-    # target inheriting them without linking grid_map_core -- corelib/test, for
-    # one -- fails on its first Eigen include.
-    #
-    # Keep the two halves together on the public interface: everything here
-    # reaches Eigen through rtabmap_core, and installed consumers then get an
-    # Eigen matching the one rtabmap_core was built with.
-    SET(PUBLIC_INCLUDE_DIRS
-        ${PUBLIC_INCLUDE_DIRS}
-        ${grid_map_core_PUBLIC_INCLUDE_DIRS}
-    )
-    SET(PUBLIC_DEFINITIONS
-        ${PUBLIC_DEFINITIONS}
-        "EIGEN_FUNCTORS_PLUGIN=\"${EIGEN_FUNCTORS_PLUGIN_PATH}\""
-        "EIGEN_DENSEBASE_PLUGIN=\"${EIGEN_DENSEBASE_PLUGIN_PATH}\""
-    )
-
 	SET(SRC_FILES
     	${SRC_FILES}
 		global_map/GridMap.cpp
@@ -924,12 +898,6 @@ target_include_directories(rtabmap_core SYSTEM PUBLIC
   "$<BUILD_INTERFACE:${PUBLIC_INCLUDE_DIRS};${INCLUDE_DIRS}>"
   "$<INSTALL_INTERFACE:${PUBLIC_INCLUDE_DIRS}>")
 
-# Definitions that change how a dependency's headers compile, so consumers of
-# rtabmap_core's headers have to see them too (see grid_map_core above).
-IF(PUBLIC_DEFINITIONS)
-  target_compile_definitions(rtabmap_core PUBLIC ${PUBLIC_DEFINITIONS})
-ENDIF()
-
 # GCC 12 false positives from PCL/Eigen template instantiations (SSE codepath
 # unaligned-loads 16 bytes from a 3-element Eigen vector). Eigen knows the
 # over-read is safe; GCC 12 doesn't. Fixed in GCC 13. PCL itself doesn't

corelib/src/Compression.cpp

@@ -28,12 +28,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Compression.h"
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UConversion.h>
-#include <rtabmap/utilite/UStl.h>
 #include <opencv2/opencv.hpp>
 
 #include <zlib.h>
-#include <cmath>
-#include <cstring>
 
 namespace rtabmap {
 
@@ -70,117 +67,16 @@ int deserializeMatType(int serializedType)
 			((serializedType >> kSerializedCnShift) & 511) + 1);
 }
 
-// Default quantization when only the maximum depth is set in the format.
-const float kDefaultDepthQuantization = 100.0f;
-
-bool hasSignature(const unsigned char * bytes, size_t size, const void * signature)
-{
-	return bytes && size >= 8 && memcmp(bytes, signature, 8) == 0;
-}
-
-// Values over maxDepth, NaN, inf, 0 and negative values are set to 0 (invalid),
-// as well as values too close to be represented on 16 bits (under
-// depthQuantA / (65535 - depthQuantB) meters).
-cv::Mat depthToInvDepth(const cv::Mat & depth, float maxDepth, float quantization, float & depthQuantA, float & depthQuantB)
-{
-	UASSERT(depth.type() == CV_32FC1);
-	depthQuantA = quantization * (quantization + 1.0f);
-	depthQuantB = 1.0f - depthQuantA / maxDepth;
-	cv::Mat invDepth(depth.size(), CV_16UC1);
-	for(int i=0; i<depth.rows; ++i)
-	{
-		const float * in = depth.ptr<float>(i);
-		uint16_t * out = invDepth.ptr<uint16_t>(i);
-		for(int j=0; j<depth.cols; ++j)
-		{
-			const float d = in[j];
-			if(d > 0.0f && d < maxDepth) // false for NaN
-			{
-				// Rounded (ROS truncates), the decoding is the same.
-				const float v = depthQuantA / d + depthQuantB + 0.5f;
-				out[j] = v < 65536.0f ? (uint16_t)v : 0;
-			}
-			else
-			{
-				out[j] = 0;
-			}
-		}
-	}
-	return invDepth;
-}
-
-cv::Mat invDepthToDepth(const cv::Mat & invDepth, float depthQuantA, float depthQuantB)
-{
-	UASSERT(invDepth.type() == CV_16UC1);
-	cv::Mat depth(invDepth.size(), CV_32FC1);
-	for(int i=0; i<invDepth.rows; ++i)
-	{
-		const uint16_t * in = invDepth.ptr<uint16_t>(i);
-		float * out = depth.ptr<float>(i);
-		for(int j=0; j<invDepth.cols; ++j)
-		{
-			out[j] = in[j] ? depthQuantA / (float(in[j]) - depthQuantB) : 0.0f;
-		}
-	}
-	return depth;
-}
-
-void invDepthParameters(float depthQuantA, float depthQuantB, float & maxDepth, float & quantization)
-{
-	// inverse of depthQuantA = q*(q+1) and depthQuantB = 1 - depthQuantA/maxDepth
-	quantization = (std::sqrt(1.0f + 4.0f*depthQuantA) - 1.0f) / 2.0f;
-	maxDepth = depthQuantA / (1.0f - depthQuantB);
-}
-
 }  // namespace
 
-bool parseImageCompressionFormat(const std::string & format, std::string & codec, float & maxDepth, float & quantization)
-{
-	codec.clear();
-	maxDepth = 0.0f;
-	quantization = 0.0f;
-	std::vector<std::string> fields = uListToVector(uSplit(format, ':'));
-	if(fields.empty())
-	{
-		return format.empty(); // empty is general (zlib)
-	}
-	if(fields[0].size() < 2 || fields[0][0] != '.')
-	{
-		return false;
-	}
-	if(fields.size() > 1)
-	{
-		// Inverse depth parameters only for formats supporting 16UC1
-		if((fields[0] != ".png" && fields[0] != ".rvl") || fields.size() > 3)
-		{
-			return false;
-		}
-		for(size_t i=1; i<fields.size(); ++i)
-		{
-			if(!uIsNumber(fields[i]) || uStr2Float(fields[i]) <= 0.0f)
-			{
-				return false;
-			}
-		}
-		maxDepth = uStr2Float(fields[1]);
-		quantization = fields.size() == 3 ? uStr2Float(fields[2]) : kDefaultDepthQuantization;
-	}
-	codec = fields[0];
-	return true;
-}
-
-// format : ".jpg" ".png" ".rvl" "" (empty is general), see parseImageCompressionFormat()
+// format : ".jpg" ".png" ".rvl" "" (empty is general)
 CompressionThread::CompressionThread(const cv::Mat & mat, const std::string & format) :
 	uncompressedData_(mat),
 	format_(format),
 	image_(!format.empty()),
 	compressMode_(true)
 {
-	std::string codec;
-	float maxDepth, quantization;
-	UASSERT_MSG(parseImageCompressionFormat(format, codec, maxDepth, quantization) &&
-			(codec.empty() || codec == ".jpg" || codec == ".png" || codec == ".rvl"),
-			uFormat("Invalid compression format \"%s\"", format.c_str()).c_str());
+	UASSERT(format.empty() || format.compare(".jpg") == 0 || format.compare(".png") == 0 || format.compare(".rvl") == 0);
 }
 // assume image
 CompressionThread::CompressionThread(const cv::Mat & bytes, bool isImage) :
@@ -235,43 +131,21 @@ void CompressionThread::mainLoop()
 	this->kill();
 }
 
-// ".jpg" or ".png" or ".rvl", with optional ":maxDepth[:quantization]" for 32FC1 depth images
+// ".jpg" or ".png" or ".rvl"
 std::vector<unsigned char> compressImage(const cv::Mat & image, const std::string & format)
 {
 	std::vector<unsigned char> bytes;
 	if(!image.empty())
 	{
-		std::string codec;
-		float maxDepth, quantization;
-		if(!parseImageCompressionFormat(format, codec, maxDepth, quantization) || codec.empty())
-		{
-			UERROR("Invalid image compression format \"%s\"", format.c_str());
-			return bytes;
-		}
-
-		if(image.type() == CV_32FC1 && maxDepth > 0.0f)
-		{
-			float depthQuantA, depthQuantB;
-			cv::Mat invDepth = depthToInvDepth(image, maxDepth, quantization, depthQuantA, depthQuantB);
-			std::vector<unsigned char> invDepthBytes = compressImage(invDepth, codec);
-			if(!invDepthBytes.empty())
-			{
-				bytes.resize(kCompressedDepthInvHeaderSize + invDepthBytes.size());
-				memcpy(&bytes[0], kCompressedDepthInvSignature, 8);
-				memcpy(&bytes[8], &depthQuantA, 4);
-				memcpy(&bytes[12], &depthQuantB, 4);
-				memcpy(&bytes[kCompressedDepthInvHeaderSize], invDepthBytes.data(), invDepthBytes.size());
-			}
-		}
-		else if(image.type() == CV_32FC1)
+		if(image.type() == CV_32FC1)
 		{
 			//save in 8bits-4channel
 			cv::Mat bgra(image.size(), CV_8UC4, image.data);
 			cv::imencode(".png", bgra, bytes);
 		}
-		else if(codec == ".rvl")
+		else if(format == ".rvl")
 		{
-			bytes.assign(kCompressedDepthRvlSignature, kCompressedDepthRvlSignature+8);
+			bytes = {'D', 'E', 'P', 'T', 'H', 'R', 'V', 'L'};
 			int numPixels = image.rows * image.cols;
         	// In the worst case, RVL compression results in ~1.5x larger data.
         	bytes.resize(3 * numPixels + 20);
@@ -280,18 +154,18 @@ std::vector<unsigned char> compressImage(const cv::Mat & image, const std::strin
         	memcpy(&bytes[8], &cols, 4);
         	memcpy(&bytes[12], &rows, 4);
         	RvlCodec rvl;
-        	int compressedSize = rvl.CompressRVL(image.ptr<uint16_t>(), &bytes[kCompressedDepthRvlHeaderSize], numPixels);
-        	bytes.resize(kCompressedDepthRvlHeaderSize + compressedSize);
+        	int compressedSize = rvl.CompressRVL(image.ptr<uint16_t>(), &bytes[16], numPixels);
+        	bytes.resize(16 + compressedSize);
 		}
 		else
 		{
-			cv::imencode(codec, image, bytes);
+			cv::imencode(format, image, bytes);
 		}
 	}
 	return bytes;
 }
 
-// ".jpg" or ".png" or ".rvl", with optional ":maxDepth[:quantization]" for 32FC1 depth images
+// ".jpg" or ".png" or ".rvl"
 cv::Mat compressImage2(const cv::Mat & image, const std::string & format)
 {
 	std::vector<unsigned char> bytes = compressImage(image, format);
@@ -303,65 +177,25 @@ cv::Mat compressImage2(const cv::Mat & image, const std::string & format)
 }
 
 cv::Mat uncompressImage(const cv::Mat & bytes)
-{
-	if(bytes.empty())
-	{
-		return cv::Mat();
-	}
-	return uncompressImage(bytes.data, bytes.total()*bytes.elemSize());
-}
-
-cv::Mat uncompressImage(const std::vector<unsigned char> & bytes)
-{
-	return uncompressImage(bytes.data(), bytes.size());
-}
-
-cv::Mat uncompressImage(const unsigned char * bytes, size_t size)
 {
 	cv::Mat image;
-	if(bytes && size)
+	if(!bytes.empty())
 	{
-		if(hasSignature(bytes, size, kCompressedDepthInvSignature))
+		if (compressedDepthFormat(bytes) == ".rvl")
 		{
-			if(size <= kCompressedDepthInvHeaderSize)
-			{
-				UERROR("Inverse depth image is truncated (%d bytes).", (int)size);
-				return image;
-			}
-			float depthQuantA, depthQuantB;
-			memcpy(&depthQuantA, &bytes[8], 4);
-			memcpy(&depthQuantB, &bytes[12], 4);
-			cv::Mat invDepth = uncompressImage(&bytes[kCompressedDepthInvHeaderSize], size - kCompressedDepthInvHeaderSize);
-			if(invDepth.type() == CV_16UC1)
-			{
-				image = invDepthToDepth(invDepth, depthQuantA, depthQuantB);
-			}
-			else if(!invDepth.empty())
-			{
-				UERROR("Inverse depth image should be 16UC1 (type=%d).", invDepth.type());
-			}
-		}
-		else if(hasSignature(bytes, size, kCompressedDepthRvlSignature))
-		{
-			if(size < kCompressedDepthRvlHeaderSize)
-			{
-				UERROR("RVL depth image is truncated (%d bytes).", (int)size);
-				return image;
-			}
 			uint32_t cols, rows;
-        	memcpy(&cols, &bytes[8], 4);
-        	memcpy(&rows, &bytes[12], 4);
+        	memcpy(&cols, &bytes.data[8], 4);
+        	memcpy(&rows, &bytes.data[12], 4);
 			image = cv::Mat(rows, cols, CV_16UC1);
 			RvlCodec rvl;
-        	rvl.DecompressRVL(&bytes[kCompressedDepthRvlHeaderSize], image.ptr<uint16_t>(), cols * rows);
+        	rvl.DecompressRVL(&bytes.data[16], image.ptr<uint16_t>(), cols * rows);
 		}
 		else
 		{
-			const cv::Mat buf(1, (int)size, CV_8UC1, (void *)bytes);
 #if CV_MAJOR_VERSION>2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4)
-			image = cv::imdecode(buf, cv::IMREAD_UNCHANGED);
+			image = cv::imdecode(bytes, cv::IMREAD_UNCHANGED);
 #else
-			image = cv::imdecode(buf, -1);
+			image = cv::imdecode(bytes, -1);
 #endif
 			if(image.type() == CV_8UC4)
 			{
@@ -376,6 +210,36 @@ cv::Mat uncompressImage(const unsigned char * bytes, size_t size)
 	return image;
 }
 
+cv::Mat uncompressImage(const std::vector<unsigned char> & bytes)
+{
+	cv::Mat image;
+	if(bytes.size())
+	{
+		if (compressedDepthFormat(bytes) == ".rvl")
+		{
+			uint32_t cols, rows;
+        	memcpy(&cols, &bytes[8], 4);
+        	memcpy(&rows, &bytes[12], 4);
+			image = cv::Mat(rows, cols, CV_16UC1);
+			RvlCodec rvl;
+        	rvl.DecompressRVL(&bytes[16], image.ptr<uint16_t>(), cols * rows);
+		}
+		else
+		{
+#if CV_MAJOR_VERSION>2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4)
+			image = cv::imdecode(bytes, cv::IMREAD_UNCHANGED);
+#else
+			image = cv::imdecode(bytes, -1);
+#endif
+			if(image.type() == CV_8UC4)
+			{
+				image = cv::Mat(image.size(), CV_32FC1, image.data).clone();
+			}
+		}
+	}
+	return image;
+}
+
 std::vector<unsigned char> compressData(const cv::Mat & data)
 {
 	std::vector<unsigned char> bytes;
@@ -517,17 +381,10 @@ std::string compressedDepthFormat(const unsigned char * bytes, size_t size)
 	std::string format;
 	if(bytes && size)
 	{
-		if(hasSignature(bytes, size, kCompressedDepthInvSignature) && size > kCompressedDepthInvHeaderSize)
-		{
-			float depthQuantA, depthQuantB, maxDepth, quantization;
-			memcpy(&depthQuantA, &bytes[8], 4);
-			memcpy(&depthQuantB, &bytes[12], 4);
-			invDepthParameters(depthQuantA, depthQuantB, maxDepth, quantization);
-			format = uFormat("%s:%g:%g",
-					compressedDepthFormat(&bytes[kCompressedDepthInvHeaderSize], size - kCompressedDepthInvHeaderSize).c_str(),
-					maxDepth, quantization);
-		}
-		else if(hasSignature(bytes, size, kCompressedDepthRvlSignature))
+		size_t maxlen = std::min(size, size_t(8));
+		std::vector<unsigned char> signature(maxlen);
+		memcpy(&signature[0], bytes, maxlen);
+		if (std::string(signature.begin(), signature.end()) == "DEPTHRVL")
 		{
 			format = ".rvl";
 		}

corelib/src/DBDriver.cpp

@@ -707,10 +707,7 @@ void DBDriver::getNodeData(
 			((!images || !s->sensorData().imageCompressed().empty()) &&
 			 (!scan || !s->sensorData().laserScanCompressed().isEmpty()) &&
 			 (!userData || !s->sensorData().userDataCompressed().empty()) &&
-			 (!occupancyGrid ||
-				!s->sensorData().gridGroundCellsCompressed().empty() ||
-				!s->sensorData().gridObstacleCellsCompressed().empty() ||
-				!s->sensorData().gridEmptyCellsCompressed().empty()))))
+			 (!occupancyGrid || s->sensorData().gridCellSize() != 0.0f))))
 		{
 			data = (SensorData)s->sensorData();
 			if(!images)

corelib/src/DBDriverSqlite3.cpp

@@ -3624,8 +3624,8 @@ void DBDriverSqlite3::loadQuery(VWDictionary & dictionary, bool lastStateOnly, b
 		rc = sqlite3_finalize(ppStmt);
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 
-		// Get Last word id (query directly: _dbSafeAccessMutex is already locked by DBDriver::load())
-		getLastIdQuery("Word", id);
+		// Get Last word id
+		getLastWordId(id);
 		dictionary.setLastWordId(id);
 
 		if(!idsOnly && uStrNumCmp(_version, "0.23.0") >= 0) {

corelib/src/Features2d.cpp

@@ -2282,6 +2282,20 @@ std::vector<cv::KeyPoint> GFTT::generateKeypointsImpl(const cv::Mat & image, con
 		_gftt->detect(imgRoi, keypoints, maskRoi); // Opencv keypoints
 	}
 
+	if(!_useHarrisDetector && _qualityLevel>0.0)
+	{
+		std::vector<cv::KeyPoint> bestKeypoints;
+		bestKeypoints.reserve(keypoints.size());
+		for(size_t i=0; i<keypoints.size(); ++i)
+		{
+			if(keypoints[i].response > _qualityLevel)
+			{
+				bestKeypoints.push_back(keypoints[i]);
+			}
+		}
+		
+		return bestKeypoints;
+	}
 	return keypoints;
 }
 

corelib/src/FlannIndex.cpp

@@ -36,33 +36,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtflann/flann.hpp"
 #include "nanoflann/NanoFlannIndex.h"
 #include <boost/crc.hpp>
-#ifdef _OPENMP
-#include <omp.h>
-#endif
 
 namespace rtabmap {
 
-namespace {
-// A count of 0 means one thread per core, as Kp/FlannThreads spells it.
-// rtflann would reach the same place by leaving num_threads(0) to OpenMP, but
-// only where it is compiled with it: resolving the count here makes 0 mean the
-// same thing in both builds, and keeps a negative count from reaching
-// num_threads(), where it wraps around to an unsigned and asks the runtime for
-// billions of threads.
-int resolveCores(int cores)
-{
-	if(cores > 0)
-	{
-		return cores;
-	}
-#ifdef _OPENMP
-	return omp_get_max_threads();
-#else
-	return 1;
-#endif
-}
-}
-
 FlannIndex::FlannIndex():
 		index_(0),
 		nanoIndex_(0),
@@ -934,8 +910,7 @@ void FlannIndex::knnSearch(
 		int knn,
 		int checks,
 		float eps,
-		bool sorted,
-		int cores) const
+		bool sorted) const
 {
 	if(nanoIndex_)
 	{
@@ -955,7 +930,6 @@ void FlannIndex::knnSearch(
 	rtflann::Matrix<size_t> indicesF((size_t*)indicesBuffer.data(), query.rows, knn);
 
 	rtflann::SearchParams params = rtflann::SearchParams(checks, eps, sorted);
-	params.cores = resolveCores(cores);
 
 	if(featuresType_ == CV_8UC1)
 	{
@@ -1000,12 +974,11 @@ void FlannIndex::radiusSearch(
 		int maxNeighbors,
 		int checks,
 		float eps,
-		bool sorted,
-		int cores) const
+		bool sorted) const
 {
 	if(nanoIndex_)
 	{
-		// "checks" and "cores" don't apply, it searches on one core
+		// "checks" doesn't apply
 		nanoIndex_->radiusSearch(query, indices, dists, radius, maxNeighbors, eps, sorted);
 		return;
 	}
@@ -1017,7 +990,6 @@ void FlannIndex::radiusSearch(
 
 	rtflann::SearchParams params = rtflann::SearchParams(checks, eps, sorted);
 	params.max_neighbors = maxNeighbors<=0?-1:maxNeighbors; // -1 is all in radius
-	params.cores = resolveCores(cores);
 
 	if(featuresType_ == CV_8UC1)
 	{

corelib/src/Graph.cpp

@@ -1073,7 +1073,7 @@ std::multimap<int, Link>::iterator findLink(
 		bool checkBothWays,
 		Link::Type type)
 {
-	std::multimap<int, Link>::iterator iter = links.lower_bound(from);
+	std::multimap<int, Link>::iterator iter = links.find(from);
 	while(iter != links.end() && iter->first == from)
 	{
 		if(iter->second.to() == to && (type==Link::kUndef || type == iter->second.type()))
@@ -1086,7 +1086,7 @@ std::multimap<int, Link>::iterator findLink(
 	if(checkBothWays)
 	{
 		// let's try to -> from
-		iter = links.lower_bound(to);
+		iter = links.find(to);
 		while(iter != links.end() && iter->first == to)
 		{
 			if(iter->second.to() == from && (type==Link::kUndef || type == iter->second.type()))
@@ -1106,7 +1106,7 @@ std::multimap<int, std::pair<int, Link::Type> >::iterator findLink(
 		bool checkBothWays,
 		Link::Type type)
 {
-	std::multimap<int, std::pair<int, Link::Type> >::iterator iter = links.lower_bound(from);
+	std::multimap<int, std::pair<int, Link::Type> >::iterator iter = links.find(from);
 	while(iter != links.end() && iter->first == from)
 	{
 		if(iter->second.first == to && (type==Link::kUndef || type == iter->second.second))
@@ -1119,7 +1119,7 @@ std::multimap<int, std::pair<int, Link::Type> >::iterator findLink(
 	if(checkBothWays)
 	{
 		// let's try to -> from
-		iter = links.lower_bound(to);
+		iter = links.find(to);
 		while(iter != links.end() && iter->first == to)
 		{
 			if(iter->second.first == from && (type==Link::kUndef || type == iter->second.second))
@@ -1138,7 +1138,7 @@ std::multimap<int, int>::iterator findLink(
 		int to,
 		bool checkBothWays)
 {
-	std::multimap<int, int>::iterator iter = links.lower_bound(from);
+	std::multimap<int, int>::iterator iter = links.find(from);
 	while(iter != links.end() && iter->first == from)
 	{
 		if(iter->second == to)
@@ -1151,7 +1151,7 @@ std::multimap<int, int>::iterator findLink(
 	if(checkBothWays)
 	{
 		// let's try to -> from
-		iter = links.lower_bound(to);
+		iter = links.find(to);
 		while(iter != links.end() && iter->first == to)
 		{
 			if(iter->second == from)
@@ -1170,7 +1170,7 @@ std::multimap<int, Link>::const_iterator findLink(
 		bool checkBothWays,
 		Link::Type type)
 {
-	std::multimap<int, Link>::const_iterator iter = links.lower_bound(from);
+	std::multimap<int, Link>::const_iterator iter = links.find(from);
 	while(iter != links.end() && iter->first == from)
 	{
 		if(iter->second.to() == to && (type==Link::kUndef || type == iter->second.type()))
@@ -1183,7 +1183,7 @@ std::multimap<int, Link>::const_iterator findLink(
 	if(checkBothWays)
 	{
 		// let's try to -> from
-		iter = links.lower_bound(to);
+		iter = links.find(to);
 		while(iter != links.end() && iter->first == to)
 		{
 			if(iter->second.to() == from && (type==Link::kUndef || type == iter->second.type()))
@@ -1203,7 +1203,7 @@ std::multimap<int, std::pair<int, Link::Type> >::const_iterator findLink(
 		bool checkBothWays,
 		Link::Type type)
 {
-	std::multimap<int, std::pair<int, Link::Type> >::const_iterator iter = links.lower_bound(from);
+	std::multimap<int, std::pair<int, Link::Type> >::const_iterator iter = links.find(from);
 	while(iter != links.end() && iter->first == from)
 	{
 		if(iter->second.first == to && (type==Link::kUndef || type == iter->second.second))
@@ -1216,7 +1216,7 @@ std::multimap<int, std::pair<int, Link::Type> >::const_iterator findLink(
 	if(checkBothWays)
 	{
 		// let's try to -> from
-		iter = links.lower_bound(to);
+		iter = links.find(to);
 		while(iter != links.end() && iter->first == to)
 		{
 			if(iter->second.first == from && (type==Link::kUndef || type == iter->second.second))
@@ -1235,7 +1235,7 @@ std::multimap<int, int>::const_iterator findLink(
 		int to,
 		bool checkBothWays)
 {
-	std::multimap<int, int>::const_iterator iter = links.lower_bound(from);
+	std::multimap<int, int>::const_iterator iter = links.find(from);
 	while(iter != links.end() && iter->first == from)
 	{
 		if(iter->second == to)
@@ -1248,7 +1248,7 @@ std::multimap<int, int>::const_iterator findLink(
 	if(checkBothWays)
 	{
 		// let's try to -> from
-		iter = links.lower_bound(to);
+		iter = links.find(to);
 		while(iter != links.end() && iter->first == to)
 		{
 			if(iter->second == from)
@@ -1611,7 +1611,7 @@ void reduceGraph(
 					posesToHyperNodes.insert(std::make_pair(id, hyperNodeId));
 					hyperNodes.insert(std::make_pair(hyperNodeId, id));
 
-					for(std::multimap<int, Link>::const_iterator jter=bidirectionalLoopClosureLinks.lower_bound(id); jter!=bidirectionalLoopClosureLinks.end() && jter->first==id; ++jter)
+					for(std::multimap<int, Link>::const_iterator jter=bidirectionalLoopClosureLinks.find(id); jter!=bidirectionalLoopClosureLinks.end() && jter->first==id; ++jter)
 					{
 						if(posesToHyperNodes.find(jter->second.to()) == posesToHyperNodes.end() &&
 						   loopClosuresAdded.find(jter->second.to()) == loopClosuresAdded.end())
@@ -1904,39 +1904,6 @@ std::list<std::pair<int, Transform> > computePath(
 	return path;
 }
 
-std::map<int, int> computePathDepths(
-			const std::multimap<int, int> & links,
-			int from,
-			int maxDepth)
-{
-	std::map<int, int> pathDepths;
-	pathDepths.insert(std::make_pair(from, 0));
-	std::list<int> frontier;
-	frontier.push_back(from);
-	while(!frontier.empty())
-	{
-		int currentId = frontier.front();
-		frontier.pop_front();
-		int currentDepth = pathDepths.at(currentId);
-		if(maxDepth > 0 && currentDepth >= maxDepth)
-		{
-			continue;
-		}
-		for(std::multimap<int, int>::const_iterator iter = links.find(currentId);
-			iter!=links.end() && iter->first == currentId;
-			++iter)
-		{
-			int nextId = iter->second;
-			if(pathDepths.find(nextId) == pathDepths.end())
-			{
-				pathDepths.insert(std::make_pair(nextId, currentDepth+1));
-				frontier.push_back(nextId);
-			}
-		}
-	}
-	return pathDepths;
-}
-
 // Dijksta
 std::list<int> computePath(
 			const std::multimap<int, Link> & links,

corelib/src/Memory.cpp

@@ -101,8 +101,6 @@ Memory::Memory(const ParametersMap & parameters) :
 	_stereoFromMotion(Parameters::defaultMemStereoFromMotion()),
     _imagePreDecimation(Parameters::defaultMemImagePreDecimation()),
 	_imagePostDecimation(Parameters::defaultMemImagePostDecimation()),
-	_legacyDecimatedOctave(false),
-	_inverseDepthCompressionAllowed(true),
 	_compressionParallelized(Parameters::defaultMemCompressionParallelized()),
 	_laserScanDownsampleStepSize(Parameters::defaultMemLaserScanDownsampleStepSize()),
 	_laserScanVoxelSize(Parameters::defaultMemLaserScanVoxelSize()),
@@ -222,28 +220,6 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
 		if(_dbDriver->openConnection(dbUrl, dbOverwritten, isReadOnly()))
 		{
 			success = true;
-
-			// Before 0.23.12 the octave of a keypoint scaled into a decimated image was
-			// moved the wrong way, which changes the pyramid level its descriptor is
-			// taken from. A map filled that way stays self-consistent only if we keep
-			// filling it that way; a new one gets the corrected scaling.
-			_legacyDecimatedOctave =
-					uStrNumCmp(_dbDriver->getDatabaseVersion(), "0.23.12") < 0;
-			// Depth images compressed as inverse depth cannot be read before 0.24, which
-			// would still open databases created with Db/TargetVersion < 0.24 or by an
-			// older version.
-			_inverseDepthCompressionAllowed =
-					uStrNumCmp(_dbDriver->getDatabaseVersion(), "0.24.0") >= 0;
-			// Only where the descriptors stored in the map end up different: keypoints
-			// from odometry, scaled into the pre-decimated image before being described.
-			if(_legacyDecimatedOctave && _useOdometryFeatures && _imagePreDecimation > 1)
-			{
-				UWARN("Database \"%s\" was created by version %s, before the octave of "
-						"decimated keypoints was corrected (0.23.12). Its features keep "
-						"being described the old way so that they stay comparable with "
-						"those already in it.",
-						dbUrl.c_str(), _dbDriver->getDatabaseVersion().c_str());
-			}
 			if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Connecting to database \"") + dbUrl + "\", done!"));
 		}
 		else
@@ -832,19 +808,6 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(params, Parameters::kMemIntermediateNodeDataKept(), _saveIntermediateNodeData);
 	Parameters::parse(params, Parameters::kMemImageCompressionFormat(), _rgbCompressionFormat);
 	Parameters::parse(params, Parameters::kMemDepthCompressionFormat(), _depthCompressionFormat);
-	{
-		std::string codec;
-		float maxDepth, quantization;
-		if(!parseImageCompressionFormat(_depthCompressionFormat, codec, maxDepth, quantization) ||
-		   (codec != ".png" && codec != ".rvl"))
-		{
-			UWARN("Invalid %s=\"%s\", using default \"%s\".",
-					Parameters::kMemDepthCompressionFormat().c_str(),
-					_depthCompressionFormat.c_str(),
-					Parameters::defaultMemDepthCompressionFormat().c_str());
-			_depthCompressionFormat = Parameters::defaultMemDepthCompressionFormat();
-		}
-	}
 	Parameters::parse(params, Parameters::kMemRehearsalIdUpdatedToNewOne(), _idUpdatedToNewOneRehearsal);
 	Parameters::parse(params, Parameters::kMemGenerateIds(), _generateIds);
 	Parameters::parse(params, Parameters::kMemBadSignaturesIgnored(), _badSignaturesIgnored);
@@ -4814,10 +4777,7 @@ SensorData Memory::getNodeData(int locationId, bool images, bool scan, bool user
 			((!images || !s->sensorData().imageCompressed().empty()) &&
 			 (!scan || !s->sensorData().laserScanCompressed().isEmpty()) &&
 			 (!userData || !s->sensorData().userDataCompressed().empty()) &&
-			 (!occupancyGrid ||
-				!s->sensorData().gridGroundCellsCompressed().empty() ||
-				!s->sensorData().gridObstacleCellsCompressed().empty() ||
-				!s->sensorData().gridEmptyCellsCompressed().empty()))))
+			 (!occupancyGrid || s->sensorData().gridCellSize() != 0.0f))))
 	{
 		r = s->sensorData();
 		if(!images)
@@ -5246,7 +5206,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 	if(!isIntermediateNode)
 	{
 		// We need raw images if we need to extract features and/or do tag detection
-		bool needRawImages = (_feature2D->getMaxFeatures() >= 0 &&
+		bool needRawImages = _feature2D->getMaxFeatures() >= 0 && 
 			(!_useOdometryFeatures ||
 			 data.keypoints().empty() ||
 			 (int)data.keypoints().size() != data.descriptors().rows ||
@@ -5254,9 +5214,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			 _detectMarkers ||
 			 _rotateImagesUpsideUp ||
 			 _imagePostDecimation > 1 ||
-			 (_createOccupancyGrid && _localMapMaker->isGridFromDepth()))) ||
-			// Images rectified below: stereo always, RGB-D unless only its features are
-			(!_imagesAlreadyRectified && !(_rectifyOnlyFeatures && data.stereoCameraModels().empty()));
+			 (_createOccupancyGrid && _localMapMaker->isGridFromDepth()));
 
 		// Note: we could avoid uncompressing scan if we don't do any filtering
 		// and if we don't use it for local occupancy grid
@@ -5702,13 +5660,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
                 if(_imagePreDecimation > 1 || useProvided3dPoints)
                 {
                     float decimationRatio = 1.0f / float(_imagePreDecimation);
-                    // The octave a feature was found at moves with the image it is
-                    // expressed in, by the same ratio as its position: a decimated
-                    // image is already that many pyramid levels down, so scaling the
-                    // keypoints into it lowers their octave. Databases older than
-                    // 0.23.12 were filled with it raised instead; see _legacyDecimatedOctave.
-                    double log2value = log(double(_legacyDecimatedOctave?
-                            double(_imagePreDecimation):double(decimationRatio)))/log(2.0);
+                    double log2value = log(double(_imagePreDecimation))/log(2.0);
                     for(unsigned int i=0; i < keypoints.size(); ++i)
                     {
                         cv::KeyPoint & kpt = keypoints[i];
@@ -5717,10 +5669,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
                             kpt.pt.x *= decimationRatio;
                             kpt.pt.y *= decimationRatio;
                             kpt.size *= decimationRatio;
-                            // Never below the finest level of the image it is now
-                            // expressed in: the detail it was found at is not in there
-                            // any more, and ORB refuses a negative octave outright.
-                            kpt.octave = std::max(0, int(kpt.octave + log2value));
+                            kpt.octave += log2value;
                         }
                         if(useProvided3dPoints)
                         {
@@ -6297,7 +6246,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		UASSERT(keypoints3D.size() == 0 || keypoints3D.size() == wordIds.size());
 		unsigned int i=0;
 		float decimationRatio = float(preDecimation) / float(_imagePostDecimation);
-		double log2value = log(double(decimationRatio))/log(2.0);
+		double log2value = log(double(preDecimation))/log(2.0);
 		for(std::list<int>::iterator iter=wordIds.begin(); iter!=wordIds.end() && i < keypoints.size(); ++iter, ++i)
 		{
 			cv::KeyPoint kpt = keypoints[i];
@@ -6307,7 +6256,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 				kpt.pt.x *= decimationRatio;
 				kpt.pt.y *= decimationRatio;
 				kpt.size *= decimationRatio;
-				kpt.octave = std::max(0, int(kpt.octave + log2value));
+				kpt.octave += log2value;
 			}
 			words.insert(std::make_pair(*iter, words.size()));
 			wordsKpts.push_back(kpt);
@@ -6646,44 +6595,6 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		std::vector<unsigned char> imageBytes;
 		std::vector<unsigned char> depthBytes;
 
-		std::string depthCompressionFormat = _depthCompressionFormat;
-		bool reuseCompressedDepth =
-				depthOrRightImage.data == data.depthOrRightRaw().data &&
-				!data.depthOrRightCompressed().empty();
-		if(!_inverseDepthCompressionAllowed)
-		{
-			std::string codec;
-			float maxDepth, quantization;
-			if(parseImageCompressionFormat(depthCompressionFormat, codec, maxDepth, quantization) && maxDepth > 0.0f)
-			{
-				static bool warned = false;
-				if(!warned)
-				{
-					UWARN("%s=\"%s\": inverse depth compression format is not compatible with database "
-						  "version %s (requires >= 0.24, see %s), \"%s\" format is used instead. This "
-						  "warning is only printed once.",
-						Parameters::kMemDepthCompressionFormat().c_str(),
-						depthCompressionFormat.c_str(),
-						_dbDriver?_dbDriver->getDatabaseVersion().c_str():"",
-						Parameters::kDbTargetVersion().c_str(),
-						codec.c_str());
-					warned = true;
-				}
-				depthCompressionFormat = codec;
-			}
-			if(reuseCompressedDepth &&
-			   compressedDepthFormat(data.depthOrRightCompressed()).find(':') != std::string::npos)
-			{
-				// Already compressed as inverse depth (e.g., received from ROS's
-				// compressed_depth_image_transport), re-compress it.
-				reuseCompressedDepth = false;
-				if(depthOrRightImage.empty())
-				{
-					depthOrRightImage = uncompressImage(data.depthOrRightCompressed());
-				}
-			}
-		}
-
 		if(!depthOrRightImage.empty() && depthOrRightImage.type() == CV_32FC1)
 		{
 			if(_saveDepth16Format)
@@ -6699,7 +6610,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 				}
 				depthOrRightImage = util2d::cvtDepthFromFloat(depthOrRightImage);
 			}
-			else if(depthCompressionFormat == ".rvl")
+			else if(_depthCompressionFormat == ".rvl")
 			{
 				static bool warned = false;
 				if(!warned)
@@ -6709,35 +6620,19 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 						  "images will be compressed in \".png\" format instead. Explicitly "
 						  "set %s to true to keep using \"%s\" format and images will be "
 						  "converted to 16bits for convenience (warning: that would "
-						  "remove all depth values over 65 meters). Set %s=\".rvl:<maxDepth>:<quantization>\" "
-						  "(e.g., \".rvl:10:100\") to compress them in RVL as 16 bits inverse depth "
-						  "(lossy, see parameter's description). Explicitly set %s=\".png\" "
+						  "remove all depth values over 65 meters). Explicitly set %s=\".png\" "
 						  "to suppress this warning. This warning is only printed once.",
 						Parameters::kMemSaveDepth16Format().c_str(),
 						Parameters::kMemDepthCompressionFormat().c_str(),
-						depthCompressionFormat.c_str(),
+						_depthCompressionFormat.c_str(),
 						Parameters::kMemSaveDepth16Format().c_str(),
-						depthCompressionFormat.c_str(),
-						Parameters::kMemDepthCompressionFormat().c_str(),
+						_depthCompressionFormat.c_str(),
 						Parameters::kMemDepthCompressionFormat().c_str());
 					warned = true;
 				}
 			}
 		}
 
-		bool reuseCompressedImage =
-				image.data == data.imageRaw().data &&
-				!data.imageCompressed().empty();
-		reuseCompressedDepth = reuseCompressedDepth &&
-				depthOrRightImage.data == data.depthOrRightRaw().data;
-		bool reuseCompressedDepthConfidence =
-				depthConfidence.data == data.depthConfidenceRaw().data &&
-				!data.depthConfidenceCompressed().empty();
-		bool reuseCompressedUserData = !data.userDataCompressed().empty();
-		bool reuseCompressedScan =
-				laserScan.data().data == data.laserScanRaw().data().data &&
-				!data.laserScanCompressed().isEmpty();
-
 		cv::Mat compressedImage;
 		cv::Mat compressedDepth;
 		cv::Mat compressedDepthConfidence;
@@ -6746,27 +6641,27 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		if(_compressionParallelized)
 		{
 			rtabmap::CompressionThread ctImage(image, _rgbCompressionFormat);
-			rtabmap::CompressionThread ctDepth(depthOrRightImage, depthOrRightImage.type() == CV_32FC1 || depthOrRightImage.type() == CV_16UC1?depthCompressionFormat:_rgbCompressionFormat);
+			rtabmap::CompressionThread ctDepth(depthOrRightImage, depthOrRightImage.type() == CV_32FC1 || depthOrRightImage.type() == CV_16UC1?_depthCompressionFormat:_rgbCompressionFormat);
 			rtabmap::CompressionThread ctDepthConfidence(depthConfidence);
 			rtabmap::CompressionThread ctLaserScan(laserScan.data());
 			rtabmap::CompressionThread ctUserData(data.userDataRaw());
-			if(!image.empty() && !reuseCompressedImage)
+			if(!image.empty())
 			{
 				ctImage.start();
 			}
-			if(!depthOrRightImage.empty() && !reuseCompressedDepth)
+			if(!depthOrRightImage.empty())
 			{
 				ctDepth.start();
 			}
-			if(!depthConfidence.empty() && !reuseCompressedDepthConfidence)
+			if(!depthConfidence.empty())
 			{
 				ctDepthConfidence.start();
 			}
-			if(!laserScan.isEmpty() && !reuseCompressedScan)
+			if(!laserScan.isEmpty())
 			{
 				ctLaserScan.start();
 			}
-			if(!data.userDataRaw().empty() && !reuseCompressedUserData)
+			if(!data.userDataRaw().empty())
 			{
 				ctUserData.start();
 			}
@@ -6779,16 +6674,16 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			compressedImage = ctImage.getCompressedData();
 			compressedDepth = ctDepth.getCompressedData();
 			compressedDepthConfidence = ctDepthConfidence.getCompressedData();
-			compressedScan = reuseCompressedScan?data.laserScanCompressed().data():ctLaserScan.getCompressedData();
-			compressedUserData = reuseCompressedUserData?data.userDataCompressed():ctUserData.getCompressedData();
+			compressedScan = ctLaserScan.getCompressedData();
+			compressedUserData = ctUserData.getCompressedData();
 		}
 		else
 		{
-			compressedImage = reuseCompressedImage?cv::Mat():compressImage2(image, _rgbCompressionFormat);
-			compressedDepth = reuseCompressedDepth?cv::Mat():compressImage2(depthOrRightImage, depthOrRightImage.type() == CV_32FC1 || depthOrRightImage.type() == CV_16UC1?depthCompressionFormat:_rgbCompressionFormat);
-			compressedDepthConfidence = reuseCompressedDepthConfidence?cv::Mat():compressData2(depthConfidence);
-			compressedScan = reuseCompressedScan?data.laserScanCompressed().data():compressData2(laserScan.data());
-			compressedUserData = reuseCompressedUserData?data.userDataCompressed():compressData2(data.userDataRaw());
+			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());
 		}
 
 		s = new Signature(id,
@@ -6852,32 +6747,28 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		// just compress user data and laser scan (scans can be used for local scan matching)
 		cv::Mat compressedScan;
 		cv::Mat compressedUserData;
-		bool reuseCompressedUserData = !data.userDataCompressed().empty();
-		bool reuseCompressedScan =
-				laserScan.data().data == data.laserScanRaw().data().data &&
-				!data.laserScanCompressed().isEmpty();
 		if(_compressionParallelized)
 		{
 			rtabmap::CompressionThread ctUserData(data.userDataRaw());
 			rtabmap::CompressionThread ctLaserScan(laserScan.data());
-			if(!data.userDataRaw().empty() && !isIntermediateNode && !reuseCompressedUserData)
+			if(!data.userDataRaw().empty() && !isIntermediateNode)
 			{
 				ctUserData.start();
 			}
-			if(!laserScan.isEmpty() && !isIntermediateNode && !reuseCompressedScan)
+			if(!laserScan.isEmpty() && !isIntermediateNode)
 			{
 				ctLaserScan.start();
 			}
 			ctUserData.join();
 			ctLaserScan.join();
 
-			compressedScan = reuseCompressedScan?data.laserScanCompressed().data():ctLaserScan.getCompressedData();
-			compressedUserData = reuseCompressedUserData && !isIntermediateNode?data.userDataCompressed():ctUserData.getCompressedData();
+			compressedScan = ctLaserScan.getCompressedData();
+			compressedUserData = ctUserData.getCompressedData();
 		}
 		else
 		{
-			compressedScan = reuseCompressedScan?data.laserScanCompressed().data():compressData2(laserScan.data());
-			compressedUserData = reuseCompressedUserData?data.userDataCompressed():compressData2(data.userDataRaw());
+			compressedScan = compressData2(laserScan.data());
+			compressedUserData = compressData2(data.userDataRaw());
 		}
 
 		s = new Signature(id,

corelib/src/Odometry.cpp

@@ -779,26 +779,6 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 		}
 
 
-		// Features that came with the frame are placed in the full size image, while what
-		// is about to be registered is the decimated one and the calibration that goes
-		// with it, so bring them along. They are scaled back below with whatever the
-		// registration returns, leaving the caller its own frame of reference.
-		if(!decimatedData.keypoints().empty())
-		{
-			std::vector<cv::KeyPoint> decimatedKpts = decimatedData.keypoints();
-			double log2value = log(double(_imageDecimation))/log(2.0);
-			for(unsigned int i=0; i<decimatedKpts.size(); ++i)
-			{
-				decimatedKpts[i].pt.x /= _imageDecimation;
-				decimatedKpts[i].pt.y /= _imageDecimation;
-				decimatedKpts[i].size /= _imageDecimation;
-				// Never below the finest level of the decimated image, which is as fine
-				// as its detail goes; ORB refuses a negative octave outright.
-				decimatedKpts[i].octave = std::max(0, int(decimatedKpts[i].octave - log2value));
-			}
-			decimatedData.setFeatures(decimatedKpts, decimatedData.keypoints3D(), decimatedData.descriptors());
-		}
-
 		// compute transform
 		t = this->computeTransform(decimatedData, guess, info);
 
@@ -837,14 +817,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 			}
 		}
 	}
-	// A frame that brings its own features carries no image, and a frame whose scene was
-	// empty carries no feature either, so neither says whether there is a frame at all.
-	// The calibration does: it is there when a camera produced this data.
-	else if(!data.imageRaw().empty() ||
-			!data.cameraModels().empty() ||
-			!data.stereoCameraModels().empty() ||
-			!data.laserScanRaw().isEmpty() ||
-			(this->canProcessAsyncIMU() && !data.imu().empty()))
+	else if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty() || (this->canProcessAsyncIMU() && !data.imu().empty()))
 	{
 		t = this->computeTransform(data, guess, info);
 	}

corelib/src/Optimizer.cpp

@@ -277,7 +277,7 @@ void Optimizer::getConnectedGraph(
 		posesOut.insert(std::make_pair(currentId, currentPose));
 
 		// add prior links
-		for(std::multimap<int, Link>::const_iterator pter=linksIn.lower_bound(currentId); pter!=linksIn.end() && pter->first==currentId; ++pter)
+		for(std::multimap<int, Link>::const_iterator pter=linksIn.find(currentId); pter!=linksIn.end() && pter->first==currentId; ++pter)
 		{
 			if(pter->second.from() == pter->second.to() && (!priorsIgnored() || pter->second.type() != Link::kPosePrior))
 			{
@@ -285,7 +285,7 @@ void Optimizer::getConnectedGraph(
 			}
 		}
 
-		for(std::multimap<int, std::pair<int, Link::Type> >::const_iterator iter=biLinks.lower_bound(currentId); iter!=biLinks.end() && iter->first==currentId; ++iter)
+		for(std::multimap<int, std::pair<int, Link::Type> >::const_iterator iter=biLinks.find(currentId); iter!=biLinks.end() && iter->first==currentId; ++iter)
 		{
 			int toId = iter->second.first;
 			Link::Type type = iter->second.second;

corelib/src/Rtabmap.cpp

@@ -2746,6 +2746,7 @@ bool Rtabmap::process(
 				std::map<int, Transform> nearestPoses;
 				std::map<int, Transform> optimizedPosesWithOdomCache;
 				std::multimap<int, int> links;
+				std::map<int, Transform> * refPoses = &_optimizedPoses;
 				if(_memory->isIncremental() && _proximityMaxGraphDepth>0)
 				{
 					// get bidirectional links
@@ -2764,6 +2765,7 @@ bool Rtabmap::process(
 						// mapping mode while being localized on the previous session.
 						optimizedPosesWithOdomCache = _optimizedPoses;
 						optimizedPosesWithOdomCache.insert(_odomCachePoses.begin(), _odomCachePoses.end());
+						refPoses = &optimizedPosesWithOdomCache;
 						for(std::multimap<int, Link>::iterator iter=_odomCacheConstraints.begin(); iter!=_odomCacheConstraints.end(); ++iter)
 						{
 							if(uContains(optimizedPosesWithOdomCache, iter->second.from()) && 
@@ -2776,23 +2778,18 @@ bool Rtabmap::process(
 						}
 					}
 				}
-				std::map<int, int> proximityPathDepths;
-				if(_memory->isIncremental() && _proximityMaxGraphDepth > 0)
-				{
-					proximityPathDepths = graph::computePathDepths(links, signature->id(), _proximityMaxGraphDepth);
-				}
 				for(std::map<int, float>::iterator iter=nearestIds.lower_bound(1); iter!=nearestIds.end(); ++iter)
 				{
 					if(_memory->getStMem().find(iter->first) == _memory->getStMem().end())
 					{
 						if(_memory->isIncremental() && _proximityMaxGraphDepth > 0)
 						{
-							std::map<int, int>::const_iterator depthIter = proximityPathDepths.find(iter->first);
-							if(depthIter == proximityPathDepths.end())
+							std::list<std::pair<int, Transform> > path = graph::computePath(*refPoses, links, signature->id(), iter->first);
+							UDEBUG("Graph depth to %d = %ld", iter->first, path.size());
+							if(!path.empty() && (int)path.size() <= _proximityMaxGraphDepth)
 							{
-								continue;
+								nearestPoses.insert(std::make_pair(iter->first, _optimizedPoses.at(iter->first)));
 							}
-							nearestPoses.insert(std::make_pair(iter->first, _optimizedPoses.at(iter->first)));
 						}
 						else
 						{
@@ -4664,7 +4661,7 @@ bool Rtabmap::process(
 				int lastId = signaturesRemoved.front();
 				UDEBUG("Detected that only last signature has been removed (lastId=%d)", lastId);
 				_optimizedPoses.erase(lastId);
-				for(std::multimap<int, Link>::iterator iter=_constraints.lower_bound(lastId); iter!=_constraints.end() && iter->first==lastId;++iter)
+				for(std::multimap<int, Link>::iterator iter=_constraints.find(lastId); iter!=_constraints.end() && iter->first==lastId;++iter)
 				{
 					if(iter->second.to() != iter->second.from())
 					{
@@ -5920,11 +5917,6 @@ Signature Rtabmap::getSignatureCopy(int id, bool images, bool scan, bool userDat
 				s.sensorData().setGlobalDescriptors(globalDescriptors);
 			}
 		}
-		if(!withGlobalDescriptors)
-		{
-			// Node data taken from memory comes with its global descriptors.
-			s.sensorData().clearGlobalDescriptors();
-		}
 		if(velocity.size()==6)
 		{
 			s.setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]);

corelib/src/SensorCaptureThread.cpp

@@ -700,7 +700,7 @@ void SensorCaptureThread::postUpdate(SensorData * dataPtr, SensorCaptureInfo * i
 				kpts[i].pt.x /= _imageDecimation;
 				kpts[i].pt.y /= _imageDecimation;
 				kpts[i].size /= _imageDecimation;
-				kpts[i].octave = std::max(0, int(kpts[i].octave - log2value));
+				kpts[i].octave -= log2value;
 			}
 			data.setFeatures(kpts, data.keypoints3D(), data.descriptors());
 		}

corelib/src/SensorData.cpp

@@ -313,24 +313,6 @@ SensorData::~SensorData()
 {
 }
 
-bool SensorData::keepCameraModel(
-		const CameraModel & model,
-		const cv::Mat & rgb,
-		const cv::Mat & depth,
-		bool clearPreviousData) const
-{
-	// An invalid model without any image is only a placeholder (e.g., scan-only data
-	// created with CameraModel()): it is not kept, so that cameraModels() is empty when
-	// there is no camera. An invalid model with an image is kept: images can be used
-	// without calibration, and they are split per camera model.
-	return model.isValidForProjection() ||
-			!rgb.empty() ||
-			!depth.empty() ||
-			(!clearPreviousData && (
-					!_imageRaw.empty() || !_imageCompressed.empty() ||
-					!_depthOrRightRaw.empty() || !_depthOrRightCompressed.empty()));
-}
-
 void SensorData::setRGBDImage(
 		const cv::Mat & rgb,
 		const cv::Mat & depth,
@@ -338,10 +320,7 @@ void SensorData::setRGBDImage(
 		bool clearPreviousData)
 {
 	std::vector<CameraModel> models;
-	if(keepCameraModel(model, rgb, depth, clearPreviousData))
-	{
-		models.push_back(model);
-	}
+	models.push_back(model);
 	setRGBDImage(rgb, depth, models, clearPreviousData);
 }
 void SensorData::setRGBDImage(
@@ -352,10 +331,7 @@ void SensorData::setRGBDImage(
 	bool clearPreviousData)
 {
 	std::vector<CameraModel> models;
-	if(keepCameraModel(model, rgb, depth, clearPreviousData))
-	{
-		models.push_back(model);
-	}
+	models.push_back(model);
 	setRGBDImage(rgb, depth, depthConfidence, models, clearPreviousData);
 }
 void SensorData::setRGBDImage(
@@ -592,7 +568,7 @@ void SensorData::setUserData(const cv::Mat & userData, bool clearPreviousData)
 	else
 	{
 		_userDataRaw = userData;
-		if(!userData.empty() && _userDataCompressed.empty())
+		if(!userData.empty())
 		{
 			_userDataCompressed = compressData2(userData);
 		}

corelib/src/Signature.cpp

@@ -144,7 +144,7 @@ bool Signature::hasLink(int idTo, Link::Type type) const
 	}
 	else
 	{
-		for(std::multimap<int, Link>::const_iterator iter=_links.lower_bound(idTo); iter!=_links.end() && iter->first == idTo; ++iter)
+		for(std::multimap<int, Link>::const_iterator iter=_links.find(idTo); iter!=_links.end() && iter->first == idTo; ++iter)
 		{
 			if(type == iter->second.type())
 			{
@@ -157,7 +157,7 @@ bool Signature::hasLink(int idTo, Link::Type type) const
 
 void Signature::changeLinkIds(int idFrom, int idTo)
 {
-	std::multimap<int, Link>::iterator iter = _links.lower_bound(idFrom);
+	std::multimap<int, Link>::iterator iter = _links.find(idFrom);
 	while(iter != _links.end() && iter->first == idFrom)
 	{
 		Link link = iter->second;

corelib/src/StereoCameraModel.cpp

@@ -598,30 +598,6 @@ float StereoCameraModel::computeDisparity(unsigned short depth) const
 	return baseline() * left().fx() / (float(depth)/1000.0f) - right().cx() + left().cx();
 }
 
-void StereoCameraModel::reproject(float x, float y, float z, float & uLeft, float & vLeft, float & uRight, float & vRight) const
-{
-	UASSERT(z!=0.0f);
-	float invZ = 1.0f/z;
-	// CameraModel::reproject() doesn't apply Tx, as a camera model with a Tx set is
-	// also used to tag a left camera having stereo observations (see the stereo edges
-	// of the BA optimizers). Here Tx is the baseline of the rectified projection
-	// matrices (0 for the left camera, -fx*baseline for the right one), so that
-	// (uLeft-uRight) is the disparity of the point.
-	uLeft = (left_.fx()*x + left_.Tx())*invZ + left_.cx();
-	vLeft = (left_.fy()*y)*invZ + left_.cy();
-	uRight = (right_.fx()*x + right_.Tx())*invZ + right_.cx();
-	vRight = (right_.fy()*y)*invZ + right_.cy();
-}
-void StereoCameraModel::reproject(float x, float y, float z, int & uLeft, int & vLeft, int & uRight, int & vRight) const
-{
-	float uLeftF, vLeftF, uRightF, vRightF;
-	this->reproject(x, y, z, uLeftF, vLeftF, uRightF, vRightF);
-	uLeft = uLeftF;
-	vLeft = vLeftF;
-	uRight = uRightF;
-	vRight = vRightF;
-}
-
 Transform StereoCameraModel::stereoTransform() const
 {
 	if(!R_.empty() && !T_.empty())

corelib/src/VWDictionary.cpp

@@ -101,7 +101,6 @@ VWDictionary::VWDictionary(const ParametersMap & parameters) :
 	_incrementalDictionary(Parameters::defaultKpIncrementalDictionary()),
 	_incrementalFlann(Parameters::defaultKpIncrementalFlann()),
 	_rebalancingFactor(Parameters::defaultKpFlannRebalancingFactor()),
-	_flannThreads(Parameters::defaultKpFlannThreads()),
 	_byteToFloat(Parameters::defaultKpByteToFloat()),
 	_nndrRatio(Parameters::defaultKpNndrRatio()),
 	_newDictionaryPath(Parameters::defaultKpDictionaryPath()),
@@ -131,7 +130,6 @@ void VWDictionary::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kKpSerializeWithChecksum(), _serializeWithChecksum);
 	Parameters::parse(parameters, Parameters::kKpIncrementalFlann(), _incrementalFlann);
 	Parameters::parse(parameters, Parameters::kKpFlannRebalancingFactor(), _rebalancingFactor);
-	Parameters::parse(parameters, Parameters::kKpFlannThreads(), _flannThreads);
 	bool byteToFloat = _byteToFloat;
 	Parameters::parse(parameters, Parameters::kKpByteToFloat(), _byteToFloat);
 
@@ -1076,7 +1074,7 @@ std::list<int> VWDictionary::addNewWords(
 
 		if(isFlannStrategy(_strategy))
 		{
-			_flannIndex->knnSearch(descriptors, results, dists, k, KNN_CHECKS, 0.0f, true, _flannThreads);
+			_flannIndex->knnSearch(descriptors, results, dists, k, KNN_CHECKS);
 		}
 		else if(_strategy == kNNBruteForce)
 		{
@@ -1398,7 +1396,7 @@ std::vector<int> VWDictionary::findNN(const cv::Mat & queryIn) const
 
 			if(isFlannStrategy(_strategy))
 			{
-				_flannIndex->knnSearch(query, results, dists, k, KNN_CHECKS, 0.0f, true, _flannThreads);
+				_flannIndex->knnSearch(query, results, dists, k, KNN_CHECKS);
 			}
 			else if(_strategy == kNNBruteForce)
 			{

corelib/src/VisualWord.cpp

@@ -73,7 +73,7 @@ unsigned long VisualWord::getMemoryUsed() const
 {
 	unsigned long memoryUsage = sizeof(VisualWord);
 	memoryUsage += _references.size() * (sizeof(int)*2+sizeof(std::map<int ,int>::iterator)) + sizeof(std::map<int ,int>);
-	memoryUsage += _descriptor.empty()?0:_descriptor.total() * _descriptor.elemSize();
+	memoryUsage += _descriptor.total() * _descriptor.elemSize();
 	return memoryUsage;
 }
 

corelib/src/global_map/GridMap.cpp

@@ -237,11 +237,6 @@ void GridMap::assemble(const std::list<std::pair<int, Transform> > & newPoses)
 	if(!cache().empty())
 	{
 		UDEBUG("Updating from cache");
-		int not3DCount = 0;
-		int not3DFirstId = 0;
-		int not3DGroundType = 0;
-		int not3DObstaclesType = 0;
-		int not3DEmptyType = 0;
 		for(std::list<std::pair<int, Transform> >::const_iterator iter = newPoses.begin(); iter!=newPoses.end(); ++iter)
 		{
 			if(uContains(cache(), iter->first))
@@ -250,13 +245,8 @@ void GridMap::assemble(const std::list<std::pair<int, Transform> > & newPoses)
 
 				if(!localGrid.is3D())
 				{
-					if(++not3DCount == 1)
-					{
-						not3DFirstId = iter->first;
-						not3DGroundType = localGrid.groundCells.type();
-						not3DObstaclesType = localGrid.obstacleCells.type();
-						not3DEmptyType = localGrid.emptyCells.type();
-					}
+					UWARN("It seems the local occupancy grids are not 3d, cannot update GridMap! (ground type=%d, obstacles type=%d, empty type=%d)",
+							localGrid.groundCells.type(), localGrid.obstacleCells.type(), localGrid.emptyCells.type());
 					continue;
 				}
 
@@ -349,12 +339,6 @@ void GridMap::assemble(const std::list<std::pair<int, Transform> > & newPoses)
 				uInsert(occupiedLocalMaps, std::make_pair(iter->first, occupied));
 			}
 		}
-		if(not3DCount)
-		{
-			UWARN("It seems the local occupancy grids are not 3d, cannot update GridMap! "
-					"(%d local grid(s) ignored, first one (id=%d) had ground type=%d, obstacles type=%d, empty type=%d)",
-					not3DCount, not3DFirstId, not3DGroundType, not3DObstaclesType, not3DEmptyType);
-		}
 	}
 
 	if(minX != maxX && minY != maxY)

corelib/src/global_map/OctoMap.cpp

@@ -479,11 +479,6 @@ void OctoMap::assemble(const std::list<std::pair<int, Transform> > & newPoses)
 	{
 		float rangeMaxSqrd = rangeMax_*rangeMax_;
 		float cellSize = octree_->getResolution();
-		int not3DCount = 0;
-		int not3DFirstId = 0;
-		int not3DGroundType = 0;
-		int not3DObstaclesType = 0;
-		int not3DEmptyType = 0;
 		for(std::list<std::pair<int, Transform> >::const_iterator iter=newPoses.begin(); iter!=newPoses.end(); ++iter)
 		{
 			std::map<int, LocalGrid>::const_iterator localGridIter;
@@ -496,13 +491,8 @@ void OctoMap::assemble(const std::list<std::pair<int, Transform> > & newPoses)
 
 				if(!localGridIter->second.is3D())
 				{
-					if(++not3DCount == 1)
-					{
-						not3DFirstId = iter->first;
-						not3DGroundType = ground.type();
-						not3DObstaclesType = obstacles.type();
-						not3DEmptyType = emptyCells.type();
-					}
+					UWARN("It seems the local occupancy grids are not 3d, cannot update OctoMap! (ground type=%d, obstacles type=%d, empty type=%d)",
+											ground.type(), obstacles.type(), emptyCells.type());
 					continue;
 				}
 
@@ -772,12 +762,6 @@ void OctoMap::assemble(const std::list<std::pair<int, Transform> > & newPoses)
 				UDEBUG("Did not find %d in cache", iter->first);
 			}
 		}
-		if(not3DCount)
-		{
-			UWARN("It seems the local occupancy grids are not 3d, cannot update OctoMap! "
-					"(%d local grid(s) ignored, first one (id=%d) had ground type=%d, obstacles type=%d, empty type=%d)",
-					not3DCount, not3DFirstId, not3DGroundType, not3DObstaclesType, not3DEmptyType);
-		}
 	}
 
     if(emptyFloodFillDepth_>0)

corelib/src/odometry/OdometryF2M.cpp

@@ -1469,25 +1469,6 @@ Transform OdometryF2M::computeTransform(
 						}
 					}
 
-					const int scanMaxPoints = lastFrame_->sensorData().laserScanRaw().maxPoints();
-					if(frameValid && scanMaxPoints > 0)
-					{
-						float correspondenceRatio = Parameters::defaultIcpCorrespondenceRatio();
-						Parameters::parse(parameters_, Parameters::kIcpCorrespondenceRatio(), correspondenceRatio);
-						if(float(lastFrame_->sensorData().laserScanRaw().size()) <
-								float(scanMaxPoints) * correspondenceRatio)
-						{
-							UWARN("Scan has %d points of the %d of a full sweep, under the %s=%f "
-									"that a registration against it would have to reach, so no "
-									"later scan could be matched to it. Not initializing on it.",
-									(int)lastFrame_->sensorData().laserScanRaw().size(),
-									scanMaxPoints,
-									Parameters::kIcpCorrespondenceRatio().c_str(),
-									correspondenceRatio);
-							frameValid = false;
-						}
-					}
-
 					if(frameValid)
 					{
 						if (scanMapMaxRange_ > 0 ){

corelib/src/optimizer/OptimizerGTSAM.cpp

@@ -504,7 +504,9 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 					gtsam::Unit3 nZ(0,0,1);
 					gtsam::Unit3 bGMeas = nRbMeas.unrotate(nZ);
 					gtsam::SharedNoiseModel model = gtsam::noiseModel::Isotropic::Sigma(2, gravitySigma());
-#ifndef RTABMAP_GTSAM_HAS_ATTITUDE_FACTOR_TEMPLATE
+#if GTSAM_VERSION_NUMERIC <= 40300
+					// Note: till 40301 is officially released, version 40300 with "4.3a1" would fail here.
+					//       Just replace "<=" above by "<" to use AttitudeFactor<Pose3> below.
 					graph.add(gtsam::Pose3AttitudeFactor(iter->first, nZ, model, bGMeas));
 #else
 					graph.add(gtsam::AttitudeFactor<gtsam::Pose3>(iter->first, nZ, model, bGMeas));

corelib/src/optimizer/vertigo/gtsam/betweenFactorSwitchable.h

@@ -100,8 +100,7 @@ namespace vertigo {
         // handle derivatives
         if (H1) *H1 = *H1 * w;
         if (H2) *H2 = *H2 * w;
-        // error already includes w; sigmoid's derivative is w*(1-w).
-        if (H3) *H3 = error * (1.0-w);
+        if (H3) *H3 = error /* (w*(1.0-w))*/;  // sig(x)*(1-sig(x)) is the derivative of sig(x) wrt. x
 
         return error;
       };

corelib/src/optimizer/vertigo/gtsam/switchVariableLinear.h

@@ -13,7 +13,6 @@
 // DerivedValue2.h removed from gtsam repo (Dec 2018): https://github.com/borglab/gtsam/commit/e550f4f2aec423cb3f2791b81cb5858b8826ebac
 #include "DerivedValue.h"
 #include <gtsam/base/Lie.h>
-#include <gtsam/base/Manifold.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 
 namespace vertigo {
@@ -46,7 +45,6 @@ namespace vertigo {
     }
 
     // Manifold requirements
-    static constexpr int dimension = 1;
 
     /** Returns dimensionality of the tangent space */
     inline size_t dim() const { return 1; }
@@ -63,13 +61,7 @@ namespace vertigo {
     }
 
     /** @return the local coordinates of another object */
-    inline gtsam::Vector1 localCoordinates(const SwitchVariableLinear& t2,
-        gtsam::OptionalJacobian<1, 1> H1 = {},
-        gtsam::OptionalJacobian<1, 1> H2 = {}) const {
-      if (H1) *H1 = -gtsam::Matrix11::Identity();
-      if (H2) *H2 = gtsam::Matrix11::Identity();
-      return gtsam::Vector1(t2.value() - value());
-    }
+    inline gtsam::Vector localCoordinates(const SwitchVariableLinear& t2) const { return gtsam::Vector1(t2.value() - value()); }
 
     // Group requirements
 
@@ -116,9 +108,36 @@ namespace vertigo {
 }
 
 namespace gtsam {
-// Use the scalar manifold's dimension, category and chart operations.
-template<> struct traits<vertigo::SwitchVariableLinear>
-    : internal::Manifold<vertigo::SwitchVariableLinear> {};
+// Define Key to be Testable by specializing gtsam::traits
+template<typename T> struct traits;
+template<> struct traits<vertigo::SwitchVariableLinear> {
+  static void Print(const vertigo::SwitchVariableLinear& key, const std::string& str = "") {
+    key.print(str);
+  }
+  static bool Equals(const vertigo::SwitchVariableLinear& key1, const vertigo::SwitchVariableLinear& key2, double tol = 1e-8) {
+    return key1.equals(key2, tol);
+  }
+  static int GetDimension(const vertigo::SwitchVariableLinear & key) {return key.Dim();}
+
+  typedef OptionalJacobian<3, 3> ChartJacobian;
+  typedef gtsam::Vector TangentVector;
+  static TangentVector Local(const vertigo::SwitchVariableLinear& origin, const vertigo::SwitchVariableLinear& other,
+#if GTSAM_VERSION_NUMERIC >= 40300
+      ChartJacobian Horigin = {}, ChartJacobian Hother = {}) {
+#else
+      ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
+#endif
+    return origin.localCoordinates(other);
+  }
+  static vertigo::SwitchVariableLinear Retract(const vertigo::SwitchVariableLinear& g, const TangentVector& v,
+#if GTSAM_VERSION_NUMERIC >= 40300
+      ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
+#else
+      ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
+#endif
+      return g.retract(v);
+    }
+};
 }
 
 

corelib/src/optimizer/vertigo/gtsam/switchVariableSigmoid.h

@@ -13,7 +13,6 @@
 // DerivedValue.h removed from gtsam repo (Dec 2018): https://github.com/borglab/gtsam/commit/e550f4f2aec423cb3f2791b81cb5858b8826ebac
 #include "DerivedValue.h"
 #include <gtsam/base/Lie.h>
-#include <gtsam/base/Manifold.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 
 namespace vertigo {
@@ -46,7 +45,6 @@ namespace vertigo {
     }
 
     // Manifold requirements
-    static constexpr int dimension = 1;
 
     /** Returns dimensionality of the tangent space */
     inline size_t dim() const { return 1; }
@@ -63,13 +61,7 @@ namespace vertigo {
     }
 
     /** @return the local coordinates of another object */
-    inline gtsam::Vector1 localCoordinates(const SwitchVariableSigmoid& t2,
-        gtsam::OptionalJacobian<1, 1> H1 = {},
-        gtsam::OptionalJacobian<1, 1> H2 = {}) const {
-      if (H1) *H1 = -gtsam::Matrix11::Identity();
-      if (H2) *H2 = gtsam::Matrix11::Identity();
-      return gtsam::Vector1(t2.value() - value());
-    }
+    inline gtsam::Vector localCoordinates(const SwitchVariableSigmoid& t2) const { return gtsam::Vector1(t2.value() - value()); }
 
     // Group requirements
 
@@ -117,9 +109,36 @@ namespace vertigo {
 
 
 namespace gtsam {
-// Use the scalar manifold's dimension, category and chart operations.
-template<> struct traits<vertigo::SwitchVariableSigmoid>
-    : internal::Manifold<vertigo::SwitchVariableSigmoid> {};
+// Define Key to be Testable by specializing gtsam::traits
+template<typename T> struct traits;
+template<> struct traits<vertigo::SwitchVariableSigmoid> {
+  static void Print(const vertigo::SwitchVariableSigmoid& key, const std::string& str = "") {
+    key.print(str);
+  }
+  static bool Equals(const vertigo::SwitchVariableSigmoid& key1, const vertigo::SwitchVariableSigmoid& key2, double tol = 1e-8) {
+    return key1.equals(key2, tol);
+  }
+  static int GetDimension(const vertigo::SwitchVariableSigmoid & key) {return key.Dim();}
+
+  typedef OptionalJacobian<3, 3> ChartJacobian;
+  typedef gtsam::Vector TangentVector;
+  static TangentVector Local(const vertigo::SwitchVariableSigmoid& origin, const vertigo::SwitchVariableSigmoid& other,
+#if GTSAM_VERSION_NUMERIC >= 40300
+	  ChartJacobian Horigin = {}, ChartJacobian Hother = {}) {
+#else
+	  ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
+#endif
+    return origin.localCoordinates(other);
+  }
+  static vertigo::SwitchVariableSigmoid Retract(const vertigo::SwitchVariableSigmoid& g, const TangentVector& v,
+#if GTSAM_VERSION_NUMERIC >= 40300
+        ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
+#else
+        ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
+#endif
+      return g.retract(v);
+    }
+};
 }
 
 #endif /* SWITCHVARIABLESIGMOID_H_ */

corelib/src/pcl18/surface/impl/texture_mapping.hpp

@@ -42,9 +42,6 @@
 #include <pcl18/surface/texture_mapping.h>
 #include <pcl/search/octree.h>
 #include <pcl/common/common.h> // for getAngle3D
-#ifdef _OPENMP
-#include <omp.h>
-#endif
 
 ///////////////////////////////////////////////////////////////////////////////////////////////
 template<typename PointInT> std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> >
@@ -1054,8 +1051,7 @@ pcl::TextureMapping<PointInT>::textureMeshwithMultipleCameras2 (
 		const pcl::texture_mapping::CameraVector &cameras,
 		const rtabmap::ProgressState * state,
 		std::vector<std::map<int, pcl::PointXY> > * vertexToPixels,
-		bool distanceToCamPolicy,
-		int numThreads)
+		bool distanceToCamPolicy)
 {
 
 	if (mesh.tex_polygons.size () != 1)
@@ -1085,17 +1081,7 @@ pcl::TextureMapping<PointInT>::textureMeshwithMultipleCameras2 (
 		UWARN("Texturing cancelled!");
 		return false;
 	}
-	// Visible faces of a camera don't depend on the other cameras, so they are computed by batch
-	// in parallel below, then merged sequentially (in camera order) to keep the same output.
-	struct CameraVisibility
-	{
-		std::vector<int> keptFaces; // faces kept for that camera, in increasing face index
-		int occludedFaces = 0;
-		int spuriousFaces = 0;
-		int projectedFaces = 0;
-	};
-
-	auto computeVisibleFaces = [&](unsigned int current_cam, CameraVisibility & out)
+	for (unsigned int current_cam = 0; current_cam < cameras.size(); ++current_cam)
 	{
 		UDEBUG("Texture camera %d...", current_cam);
 
@@ -1273,6 +1259,7 @@ pcl::TextureMapping<PointInT>::textureMeshwithMultipleCameras2 (
 			for(std::list<int>::iterator jter=iter->begin(); jter!=iter->end(); ++jter)
 			{
 				polygonsKept.insert(polygon_to_face_index[*jter]);
+				faceCameras[polygon_to_face_index[*jter]].push_back(current_cam);
 			}
 		}
 
@@ -1289,55 +1276,14 @@ pcl::TextureMapping<PointInT>::textureMeshwithMultipleCameras2 (
 			}
 		}
 
-		out.keptFaces = std::vector<int>(polygonsKept.begin(), polygonsKept.end());
-		out.occludedFaces = (int)occludedFaces.size();
-		out.spuriousFaces = clusterFaces;
-		out.projectedFaces = (int)visibilityIndices.size();
-	};
-
-#ifdef _OPENMP
-	const int usedThreads = numThreads>1?numThreads:1;
-#else
-	const int usedThreads = 1;
-#endif
-	// More cameras than threads are batched so that a thread getting a cheap camera can pick up
-	// more work. With a single thread, cameras are processed one by one.
-	const size_t chunkSize = usedThreads>1?usedThreads*4:1;
-	std::vector<CameraVisibility> chunkVisibility;
-	bool canceled = false;
-	for(size_t chunkStart=0; chunkStart<cameras.size() && !canceled; chunkStart+=chunkSize)
-	{
-		size_t chunkCams = std::min(chunkSize, cameras.size()-chunkStart);
-		chunkVisibility.assign(chunkCams, CameraVisibility());
-
-		#pragma omp parallel for schedule(dynamic) num_threads(usedThreads)
-		for(int i=0; i<(int)chunkCams; ++i)
+		msg = uFormat("Processed camera %d/%d: %d occluded and %d spurious polygons out of %d", (int)current_cam+1, (int)cameras.size(), (int)occludedFaces.size(), clusterFaces, (int)visibilityIndices.size());
+		UINFO("%s", msg.c_str());
+		if(state && !state->callback(msg))
 		{
-			computeVisibleFaces((unsigned int)(chunkStart+i), chunkVisibility[i]);
+			//cancelled!
+			UWARN("Texturing cancelled!");
+			return false;
 		}
-
-		for(size_t i=0; i<chunkCams && !canceled; ++i)
-		{
-			unsigned int current_cam = (unsigned int)(chunkStart+i);
-			const CameraVisibility & visibility = chunkVisibility[i];
-			for(size_t j=0; j<visibility.keptFaces.size(); ++j)
-			{
-				faceCameras[visibility.keptFaces[j]].push_back(current_cam);
-			}
-
-			msg = uFormat("Processed camera %d/%d: %d occluded and %d spurious polygons out of %d", (int)current_cam+1, (int)cameras.size(), visibility.occludedFaces, visibility.spuriousFaces, visibility.projectedFaces);
-			UINFO("%s", msg.c_str());
-			if(state && !state->callback(msg))
-			{
-				//cancelled!
-				canceled = true;
-			}
-		}
-	}
-	if(canceled)
-	{
-		UWARN("Texturing cancelled!");
-		return false;
 	}
 
 	msg = uFormat("Texturing %d polygons...", (int)faces.size());

corelib/src/pcl18/surface/texture_mapping.h

@@ -368,8 +368,7 @@ namespace pcl
                                       const pcl::texture_mapping::CameraVector &cameras,
 									  const rtabmap::ProgressState * callback = 0,
 									  std::vector<std::map<int, pcl::PointXY> > * vertexToPixels = 0,
-									  bool distanceToCamPolicy = false,
-									  int numThreads = 1); // number of threads used to compute the visible faces of the cameras (1=sequential)
+									  bool distanceToCamPolicy = false);
 
     protected:
       /** \brief mesh scale control. */

corelib/src/util2d.cpp

@@ -1029,9 +1029,8 @@ float getDepth(
 						}
 						else
 						{
-							float mean = tmp/float(count);
-							float depthError = depthErrorRatio * mean;
-							if(fabs(d - mean) < depthError)
+							float depthError = depthErrorRatio * tmp;
+							if(fabs(d - tmp/float(count)) < depthError)
 
 							{
 								tmp += d;

corelib/src/util3d.cpp

@@ -2328,59 +2328,10 @@ pcl::PCLPointCloud2::Ptr laserScanToPointCloud2(const LaserScan & laserScan, con
 	{
 		pcl::toPCLPointCloud2(*laserScanToPointCloud(laserScan, transform), *cloud);
 	}
-	else if(laserScan.format() == LaserScan::kXYI || laserScan.format() == LaserScan::kXYZI)
+	else if(laserScan.format() == LaserScan::kXYI || laserScan.format() == LaserScan::kXYZI || laserScan.format() == LaserScan::kXYZIT || laserScan.format() == LaserScan::kXYZIRT)
 	{
 		pcl::toPCLPointCloud2(*laserScanToPointCloudI(laserScan, transform), *cloud);
 	}
-	else if(laserScan.format() == LaserScan::kXYZIT || laserScan.format() == LaserScan::kXYZIRT)
-	{
-		// PCL has no point type with time (and ring): append them to the XYZI fields, with
-		// the types laserScanFromPointCloud() reads back (time FLOAT32, ring UINT16).
-		pcl::PCLPointCloud2 xyzi;
-		pcl::toPCLPointCloud2(*laserScanToPointCloudI(laserScan, transform), xyzi);
-		const bool hasRing = laserScan.format() == LaserScan::kXYZIRT;
-
-		cloud->header = xyzi.header;
-		cloud->height = xyzi.height;
-		cloud->width = xyzi.width;
-		cloud->is_bigendian = xyzi.is_bigendian;
-		cloud->is_dense = xyzi.is_dense;
-		cloud->fields = xyzi.fields;
-		pcl::PCLPointField time;
-		time.name = "time";
-		time.offset = xyzi.point_step;
-		time.datatype = pcl::PCLPointField::FLOAT32;
-		time.count = 1;
-		cloud->fields.push_back(time);
-		cloud->point_step = xyzi.point_step + 4;
-		pcl::PCLPointField ring;
-		if(hasRing)
-		{
-			ring.name = "ring";
-			ring.offset = cloud->point_step;
-			ring.datatype = pcl::PCLPointField::UINT16;
-			ring.count = 1;
-			cloud->fields.push_back(ring);
-			cloud->point_step += 4; // keep points 4-byte aligned
-		}
-		cloud->row_step = cloud->point_step * cloud->width;
-		cloud->data.resize(size_t(cloud->row_step) * cloud->height, 0);
-
-		const int cols = laserScan.data().cols;
-		const size_t points = size_t(cloud->width) * cloud->height;
-		for(size_t i=0; i<points; ++i)
-		{
-			unsigned char * dst = &cloud->data[i * cloud->point_step];
-			memcpy(dst, &xyzi.data[i * xyzi.point_step], xyzi.point_step);
-			const float * src = laserScan.data().ptr<float>(int(i) / cols, int(i) % cols);
-			memcpy(dst + time.offset, src + laserScan.getTimeOffset(), sizeof(float));
-			if(hasRing)
-			{
-				const std::uint16_t r = (std::uint16_t)src[laserScan.getRingOffset()];
-				memcpy(dst + ring.offset, &r, sizeof(r));
-			}
-		}
-	}
 	else if(laserScan.format() == LaserScan::kXYNormal || laserScan.format() == LaserScan::kXYZNormal)
 	{
 		pcl::toPCLPointCloud2(*laserScanToPointCloudNormal(laserScan, transform), *cloud);

corelib/src/util3d_surface.cpp

@@ -738,8 +738,7 @@ pcl::TextureMesh::Ptr createTextureMesh(
 		const std::vector<float> & roiRatios,
 		const ProgressState * state,
 		std::vector<std::map<int, pcl::PointXY> > * vertexToPixels,
-		bool distanceToCamPolicy,
-		int numThreads)
+		bool distanceToCamPolicy)
 {
 	std::map<int, std::vector<CameraModel> > cameraSubModels;
 	for(std::map<int, CameraModel>::const_iterator iter=cameraModels.begin(); iter!=cameraModels.end(); ++iter)
@@ -761,8 +760,7 @@ pcl::TextureMesh::Ptr createTextureMesh(
 			roiRatios,
 			state,
 			vertexToPixels,
-			distanceToCamPolicy,
-			numThreads);
+			distanceToCamPolicy);
 }
 
 pcl::TextureMesh::Ptr createTextureMesh(
@@ -777,8 +775,7 @@ pcl::TextureMesh::Ptr createTextureMesh(
 		const std::vector<float> & roiRatios,
 		const ProgressState * state,
 		std::vector<std::map<int, pcl::PointXY> > * vertexToPixels,
-		bool distanceToCamPolicy,
-		int numThreads)
+		bool distanceToCamPolicy)
 {
 	UASSERT(mesh->polygons.size());
 	pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh);
@@ -840,7 +837,7 @@ pcl::TextureMesh::Ptr createTextureMesh(
 	tm.setMaxAngle(maxAngle);
 	tm.setMaxDepthError(maxDepthError);
 	tm.setMinClusterSize(minClusterSize);
-	if(tm.textureMeshwithMultipleCameras2(*textureMesh, cameras, state, vertexToPixels, distanceToCamPolicy, numThreads))
+	if(tm.textureMeshwithMultipleCameras2(*textureMesh, cameras, state, vertexToPixels, distanceToCamPolicy))
 	{
 		// compute normals for the mesh if not already here
 		bool hasNormals = false;

corelib/test/CMakeLists.txt

@@ -68,20 +68,9 @@ set(corelib_test_sources
 	test_sensorcapturethread.cpp        #SensorCaptureThread.h
 )
 
-# test_optimizer_gtsam.cpp includes the Vertigo GTSAM factors from
-# corelib/src/optimizer directly, so it needs GTSAM itself (rtabmap_core links
-# it PRIVATE).
-IF(GTSAM_FOUND)
-	list(APPEND corelib_test_sources test_optimizer_gtsam.cpp)  #OptimizerGTSAM.h (Vertigo factors, gravity)
-ENDIF(GTSAM_FOUND)
-
 add_executable(test_corelib ${corelib_test_sources})
 target_link_libraries(test_corelib gtest_main rtabmap_core)
 
-IF(GTSAM_FOUND)
-	target_link_libraries(test_corelib gtsam)
-ENDIF(GTSAM_FOUND)
-
 # test_registrationicp.cpp includes corelib/src/icp/libpointmatcher.h directly
 # to test the LaserScan <-> DataPoints conversions, so it needs the library
 # itself (rtabmap_core links it PRIVATE and only re-exports its include dirs).
@@ -148,34 +137,6 @@ IF(BUILD_PERF_TESTS)
 	set_tests_properties(test_bayesfilter_perf PROPERTIES
 			TIMEOUT ${_perf_timeout}
 			LABELS  "performance")
-
-	# Comparison of the two ways of getting the graph depth of every node of a map, which
-	# is what the RGBD/ProximityMaxGraphDepth filtering needs: one graph::computePath()
-	# (A*) per candidate against one graph::computePathDepths() (BFS) for all of them,
-	# over spiral graphs, where the straight line to the goal tells A* nothing:
-	#   bin/test_graph_perf
-	#   bin/test_graph_perf --gtest_filter=*ProximityLinks*
-	add_executable(test_graph_perf perf_graph.cpp)
-	target_link_libraries(test_graph_perf gtest_main rtabmap_core)
-
-	add_test(NAME test_graph_perf COMMAND test_graph_perf)
-	set_tests_properties(test_graph_perf PROPERTIES
-			TIMEOUT ${_perf_timeout}
-			LABELS  "performance")
-
-	# Comparison of the depth image compression approaches (sizes, times, errors) for
-	# 16UC1 and 32FC1 depth images: PNG, RVL, zlib, the legacy 4-channel PNG of 32FC1
-	# images, their conversion to 16UC1 millimeters, and their quantization as 16 bits
-	# inverse depth (Mem/DepthCompressionFormat=".png:max:q" or ".rvl:max:q"):
-	#   bin/test_compression_perf
-	#   bin/test_compression_perf --gtest_filter=*Synthetic*
-	add_executable(test_compression_perf perf_compression.cpp)
-	target_link_libraries(test_compression_perf gtest_main rtabmap_core)
-
-	add_test(NAME test_compression_perf COMMAND test_compression_perf)
-	set_tests_properties(test_compression_perf PROPERTIES
-			TIMEOUT ${_perf_timeout}
-			LABELS  "performance")
 ENDIF(BUILD_PERF_TESTS)
 
 # Rtabmap end-to-end replay of sample DBs (test data fetched by
@@ -194,12 +155,6 @@ target_link_libraries(test_rtabmap_integration gtest_main rtabmap_core)
 #  `ctest -L long`.
 add_test(NAME test_rtabmap_integration COMMAND test_rtabmap_integration)
 math(EXPR _integration_timeout "1800 * ${_test_timeout_scale}")
-# The Windows runners replay the sample DBs far slower than the Linux and macOS
-# ones (the rest of the suite takes ~3 min there, this test alone went over 30),
-# so they get twice the budget rather than lowering the bar for every platform.
-IF(WIN32)
-	math(EXPR _integration_timeout "${_integration_timeout} * 2")
-ENDIF(WIN32)
 set_tests_properties(test_rtabmap_integration PROPERTIES
         TIMEOUT ${_integration_timeout}
         LABELS  "long")

corelib/test/FlannIndexBackends.h

@@ -28,21 +28,12 @@ struct Backend
 	float rebalancingFactor = 2.0f;
 	// Not a FlannIndex at all: cv::BFMatcher, what the brute force strategies of
 	// VWDictionary and RegistrationVis use. Kept in the comparisons as the
-	// baseline every index has to beat.
+	// baseline every index has to beat. OpenCV threads its search where the
+	// indexes here search on one core, so it comes in two flavours: as the
+	// application gets it, and held to one core to compare the work done rather
+	// than the time it takes on an idle machine.
 	bool bruteForce = false;
-	// Threads the batch of queries is searched with, as Kp/FlannThreads sets it
-	// on VWDictionary: 1 to search on one core, 0 for one per core. It says the
-	// same thing on both sides of bruteForce, which is what makes the rows
-	// comparable: cv::BFMatcher threads its search too, so it appears in the
-	// same two flavours as the rtflann trees. A row named "threaded" is the one
-	// per core one, a row named without it searches on a single core, so that
-	// the tables compare the work done rather than the time it takes on an idle
-	// machine.
-	//
-	// Of the indexes only the rtflann ones read it, they are the ones searching
-	// a batch under an OpenMP loop; FlannIndex ignores it for the nanoflann
-	// ones, which always search on one core.
-	int cores = 1;
+	bool singleCore = false;
 };
 
 // Every algorithm that indexes float features. The exhaustive search comes
@@ -50,8 +41,9 @@ struct Backend
 // found and for the time taken.
 const Backend FLOAT_BACKENDS[] = {
 	{"linear    exhaustive                ", FlannIndex::FLANN_INDEX_LINEAR},
-	{"cv        BFMatcher                 ", FlannIndex::FLANN_INDEX_LINEAR, 1.0f, true, 1},
-	{"cv        BFMatcher threaded        ", FlannIndex::FLANN_INDEX_LINEAR, 1.0f, true, 0},
+	// No single core row for the float features: OpenCV doesn't thread that
+	// match at these sizes, it measures the same thing as the one above.
+	{"cv        BFMatcher                 ", FlannIndex::FLANN_INDEX_LINEAR, 1.0f, true},
 	{"rtflann   kd-tree (4 randomized)    ", FlannIndex::FLANN_INDEX_KDTREE},
 	{"rtflann   kd-tree single            ", FlannIndex::FLANN_INDEX_KDTREE_SINGLE},
 	{"nanoflann kd-tree single            ", FlannIndex::NANOFLANN_INDEX_KDTREE_SINGLE, 1.0f},
@@ -72,8 +64,8 @@ const Backend EXACT_BACKENDS[] = {
 // LSH is for.
 const Backend BINARY_BACKENDS[] = {
 	{"linear    exhaustive (hamming)      ", FlannIndex::FLANN_INDEX_LINEAR},
-	{"cv        BFMatcher hamming         ", FlannIndex::FLANN_INDEX_LINEAR, 1.0f, true, 1},
-	{"cv        BFMatcher hamming threaded", FlannIndex::FLANN_INDEX_LINEAR, 1.0f, true, 0},
+	{"cv        BFMatcher (hamming)       ", FlannIndex::FLANN_INDEX_LINEAR, 1.0f, true},
+	{"cv        BFMatcher (hamming,1 core)", FlannIndex::FLANN_INDEX_LINEAR, 1.0f, true, true},
 	{"rtflann   LSH                       ", FlannIndex::FLANN_INDEX_LSH},
 };
 
@@ -196,12 +188,11 @@ inline Result run(
 
 	if(backend.bruteForce)
 	{
-		// cv::setNumThreads() is global, put it back before leaving. Left alone
-		// for cores=0: OpenCV's own default is already one thread per core.
+		// cv::setNumThreads() is global, put it back before leaving.
 		const int threads = cv::getNumThreads();
-		if(backend.cores > 0)
+		if(backend.singleCore)
 		{
-			cv::setNumThreads(backend.cores);
+			cv::setNumThreads(1);
 		}
 
 		UTimer timer;
@@ -230,7 +221,10 @@ inline Result run(
 			result.radiusTime = timer.ticks();
 		}
 		result.memory = 0; // it indexes nothing
-		cv::setNumThreads(threads);
+		if(backend.singleCore)
+		{
+			cv::setNumThreads(threads);
+		}
 		return result;
 	}
 
@@ -239,7 +233,7 @@ inline Result run(
 	index.buildIndex(backend.algorithm, data, false, rebalancingFactor);
 	result.buildTime = timer.ticks();
 
-	index.knnSearch(queries, result.indices, dists, knn, 32, 0.0f, true, backend.cores);
+	index.knnSearch(queries, result.indices, dists, knn);
 	result.knnTime = timer.ticks();
 
 	if(radius > 0.0f)

corelib/test/perf_compression.cpp

@@ -1,342 +0,0 @@
-// Comparison of the depth image compression approaches of Compression.h, for each
-// depth type rtabmap receives:
-//
-//   16UC1 (millimeters):
-//     - ".png"                  lossless, 16 bits grayscale PNG
-//     - ".rvl"                  lossless, RVL (Mem/DepthCompressionFormat default)
-//     - zlib                    lossless, compressData2(), as a reference
-//   32FC1 (meters):
-//     - ".png"                  lossless, float bytes as a 4-channel 8 bits PNG (legacy)
-//     - zlib                    lossless, compressData2(), as a reference
-//     - 16UC1 mm + ".png/.rvl"  lossy, util2d::cvtDepthFromFloat() then 16 bits codec,
-//                               what Mem/SaveDepth16Format=true does
-//     - ".png:max:q/.rvl:max:q" lossy, 16 bits quantized inverse depth (same
-//                               quantization than ROS's compressed_depth_image_transport)
-//
-// over the depth images of data/rgbd/depth (a structured light camera, millimeters),
-// the same images converted to meters in 32FC1 (as many drivers publish them), and a
-// synthetic 32FC1 image with continuous values, like stereo or lidar projected depth.
-//
-// Its own executable, run by ctest under the "performance" label, so that its seconds
-// of benchmarking stay out of the unit test shards:
-//   ctest -L performance    to run them
-//   ctest -LE performance   to skip them
-//   bin/test_compression_perf --gtest_filter=*Synthetic*
-//
-// The times are reported rather than asserted on, as they depend on the machine. What
-// is asserted is that the lossless approaches give back the same image, and that the
-// lossy ones stay within their error bounds for the depth range they keep.
-#include <gtest/gtest.h>
-#include <rtabmap/core/Compression.h>
-#include <rtabmap/core/util2d.h>
-#include <rtabmap/utilite/ULogger.h>
-#include <rtabmap/utilite/UConversion.h>
-#include <rtabmap/utilite/UTimer.h>
-#include <opencv2/imgcodecs.hpp>
-#include <algorithm>
-#include <cmath>
-#include <cstdio>
-#include <functional>
-#include <string>
-#include <vector>
-
-using namespace rtabmap;
-
-namespace {
-
-static const int ITERATIONS = 15;
-
-struct Approach
-{
-	std::string name;
-	std::function<std::vector<unsigned char>(const cv::Mat &)> encode;
-	std::function<cv::Mat(const std::vector<unsigned char> &)> decode;
-	bool lossless;
-	float maxDepth;     // meters, lossy approaches only: depth kept under it
-	float minDepth;     // meters, lossy approaches only: depth kept over it
-	std::function<float(float)> tolerance; // meters, lossy approaches only, for a depth in meters
-};
-
-struct Result
-{
-	size_t bytes = 0;
-	double encodeMs = 0.0;
-	double decodeMs = 0.0;
-	double maxError = 0.0;   // mm, over the depth range kept
-	double rmse = 0.0;       // mm, over the depth range kept
-	double lost = 0.0;       // % of the valid pixels set to 0
-	int outOfTolerance = 0;  // pixels with an error over the tolerance
-};
-
-double median(std::vector<double> v)
-{
-	std::sort(v.begin(), v.end());
-	return v[v.size()/2];
-}
-
-float toMeters(const cv::Mat & depth, int r, int c)
-{
-	return depth.type() == CV_16UC1 ? float(depth.at<uint16_t>(r, c)) * 0.001f : depth.at<float>(r, c);
-}
-
-Result run(const cv::Mat & depth, const Approach & approach)
-{
-	Result result;
-	std::vector<unsigned char> bytes;
-	cv::Mat restored;
-	std::vector<double> encodeTimes, decodeTimes;
-	for(int i=0; i<ITERATIONS; ++i)
-	{
-		UTimer timer;
-		bytes = approach.encode(depth);
-		encodeTimes.push_back(timer.restart() * 1000.0);
-		restored = approach.decode(bytes);
-		decodeTimes.push_back(timer.ticks() * 1000.0);
-	}
-	result.bytes = bytes.size();
-	result.encodeMs = median(encodeTimes);
-	result.decodeMs = median(decodeTimes);
-
-	EXPECT_EQ(restored.size(), depth.size());
-	EXPECT_EQ(restored.type(), approach.lossless ? depth.type() : restored.type());
-	if(restored.size() != depth.size())
-	{
-		return result;
-	}
-
-	if(approach.lossless)
-	{
-		EXPECT_EQ(memcmp(restored.data, depth.data, depth.total()*depth.elemSize()), 0);
-		return result;
-	}
-
-	int valid = 0, lost = 0, kept = 0;
-	double sumSq = 0.0;
-	for(int r=0; r<depth.rows; ++r)
-	{
-		for(int c=0; c<depth.cols; ++c)
-		{
-			const float d = toMeters(depth, r, c);
-			if(!(std::isfinite(d) && d > 0.0f))
-			{
-				continue;
-			}
-			++valid;
-			const float out = toMeters(restored, r, c);
-			if(out == 0.0f)
-			{
-				++lost;
-				// Only allowed outside the kept range
-				if(d >= approach.minDepth && d < approach.maxDepth)
-				{
-					++result.outOfTolerance;
-				}
-				continue;
-			}
-			const double err = std::fabs(out - d);
-			result.maxError = std::max(result.maxError, err*1000.0);
-			sumSq += err*err*1e6;
-			++kept;
-			if(err > approach.tolerance(d))
-			{
-				++result.outOfTolerance;
-			}
-		}
-	}
-	result.rmse = kept ? std::sqrt(sumSq / kept) : 0.0;
-	result.lost = valid ? 100.0 * lost / valid : 0.0;
-	EXPECT_EQ(result.outOfTolerance, 0) << approach.name;
-	return result;
-}
-
-void report(const std::string & title, const cv::Mat & depth, const std::vector<Approach> & approaches)
-{
-	const size_t raw = depth.total() * depth.elemSize();
-	std::printf("\n%s: %dx%d %s, %zu bytes raw\n", title.c_str(), depth.cols, depth.rows,
-			depth.type() == CV_16UC1 ? "16UC1" : "32FC1", raw);
-	std::printf("  %-22s %10s %7s %10s %10s %11s %10s %8s\n",
-			"approach", "bytes", "ratio", "encode ms", "decode ms", "max err mm", "rmse mm", "lost %");
-	for(const Approach & approach : approaches)
-	{
-		SCOPED_TRACE(title + " " + approach.name);
-		const Result r = run(depth, approach);
-		if(approach.lossless)
-		{
-			std::printf("  %-22s %10zu %6.1fx %10.2f %10.2f %11s %10s %8s\n",
-					approach.name.c_str(), r.bytes, double(raw)/double(r.bytes), r.encodeMs, r.decodeMs,
-					"lossless", "-", "-");
-		}
-		else
-		{
-			std::printf("  %-22s %10zu %6.1fx %10.2f %10.2f %11.3f %10.3f %8.2f\n",
-					approach.name.c_str(), r.bytes, double(raw)/double(r.bytes), r.encodeMs, r.decodeMs,
-					r.maxError, r.rmse, r.lost);
-		}
-	}
-	std::fflush(stdout);
-}
-
-std::vector<unsigned char> encode(const cv::Mat & depth, const std::string & format)
-{
-	return compressImage(depth, format);
-}
-
-cv::Mat decode(const std::vector<unsigned char> & bytes)
-{
-	return uncompressImage(bytes);
-}
-
-std::vector<unsigned char> encodeZlib(const cv::Mat & depth)
-{
-	return compressData(depth);
-}
-
-cv::Mat decodeZlib(const std::vector<unsigned char> & bytes)
-{
-	return uncompressData(bytes);
-}
-
-std::vector<Approach> approaches16U()
-{
-	using namespace std::placeholders;
-	return {
-		{".png", std::bind(encode, _1, ".png"), decode, true, 0, 0, nullptr},
-		{".rvl", std::bind(encode, _1, ".rvl"), decode, true, 0, 0, nullptr},
-		{"zlib", encodeZlib, decodeZlib, true, 0, 0, nullptr}};
-}
-
-Approach invDepth(const std::string & codec, float maxDepth, float quantization)
-{
-	using namespace std::placeholders;
-	const float A = quantization * (quantization + 1.0f);
-	const float B = 1.0f - A / maxDepth;
-	const std::string format = uFormat("%s:%g:%g", codec.c_str(), maxDepth, quantization);
-	return {format, std::bind(encode, _1, format), decode, false,
-		maxDepth,
-		A / (65535.0f - B) * 1.001f,
-		[A](float d) { return 0.51f * d * d / A + 1e-6f; }};
-}
-
-Approach depth16(const std::string & codec)
-{
-	return {"16UC1 mm + " + codec,
-		[codec](const cv::Mat & depth) { return compressImage(util2d::cvtDepthFromFloat(depth), codec); },
-		[](const std::vector<unsigned char> & bytes) { return util2d::cvtDepthToFloat(uncompressImage(bytes)); },
-		false,
-		65.535f,
-		0.0f,
-		[](float) { return 0.001f + 1e-6f; }}; // truncated to millimeters
-}
-
-std::vector<Approach> approaches32F()
-{
-	using namespace std::placeholders;
-	return {
-		{".png (legacy RGBA)", std::bind(encode, _1, ".png"), decode, true, 0, 0, nullptr},
-		{"zlib", encodeZlib, decodeZlib, true, 0, 0, nullptr},
-		depth16(".png"),
-		depth16(".rvl"),
-		invDepth(".png", 10.0f, 100.0f),
-		invDepth(".rvl", 10.0f, 100.0f),
-		invDepth(".png", 40.0f, 100.0f),
-		invDepth(".rvl", 40.0f, 100.0f),
-		invDepth(".rvl", 40.0f, 200.0f)};
-}
-
-std::vector<cv::Mat> loadSampleDepths()
-{
-	std::vector<cv::Mat> depths;
-	for(const std::string & name : {"17.png", "154.png"})
-	{
-		const std::string path = std::string(RTABMAP_TEST_DATA_ROOT) + "/rgbd/depth/" + name;
-		cv::Mat depth = cv::imread(path, cv::IMREAD_UNCHANGED);
-		if(depth.type() == CV_16UC1)
-		{
-			depths.push_back(depth);
-		}
-		else
-		{
-			std::printf("Cannot load 16UC1 depth image \"%s\", skipped.\n", path.c_str());
-		}
-	}
-	return depths;
-}
-
-// Ground plane, walls and boxes seen by a 640x480 camera, with continuous
-// values up to ~35 m, noise growing with depth (as stereo) and holes.
-cv::Mat makeSyntheticDepth(int cols = 640, int rows = 480)
-{
-	cv::RNG rng(42);
-	const float fx = 0.75f * cols, cx = cols / 2.0f, cy = rows / 2.0f;
-	const float cameraHeight = 1.0f;
-	cv::Mat depth(rows, cols, CV_32FC1);
-	for(int v=0; v<rows; ++v)
-	{
-		for(int u=0; u<cols; ++u)
-		{
-			const float x = (u - cx) / fx; // ray direction, z = 1
-			const float y = (v - cy) / fx;
-			float d = 35.0f; // far wall
-			if(y > 0.0f)
-			{
-				d = std::min(d, cameraHeight / y); // ground
-			}
-			if(x < 0.0f)
-			{
-				d = std::min(d, 3.0f / -x); // left wall, 3 m away
-			}
-			// boxes
-			if(x > 0.05f && x < 0.25f && y > -0.1f && y < cameraHeight / 2.5f)
-			{
-				d = std::min(d, 2.5f - 1.5f * x);
-			}
-			if(x > -0.35f && x < -0.15f && y > -0.2f && y < cameraHeight / 12.0f)
-			{
-				d = std::min(d, 12.0f);
-			}
-			d += (float)rng.gaussian(0.002 * d * d); // stereo-like noise
-			depth.at<float>(v, u) = d;
-		}
-	}
-	// Holes
-	for(int i=0; i<40; ++i)
-	{
-		const int u = rng.uniform(0, cols - 20), v = rng.uniform(0, rows - 20);
-		depth(cv::Rect(u, v, rng.uniform(2, 20), rng.uniform(2, 20))).setTo(0.0f);
-	}
-	return depth;
-}
-
-} // namespace
-
-TEST(CompressionPerf, SampleDepth16UC1)
-{
-	const std::vector<cv::Mat> depths = loadSampleDepths();
-	if(depths.empty())
-	{
-		GTEST_SKIP() << "No sample depth images in " << RTABMAP_TEST_DATA_ROOT << "/rgbd/depth";
-	}
-	for(size_t i=0; i<depths.size(); ++i)
-	{
-		report(uFormat("Sample depth %d", (int)i), depths[i], approaches16U());
-	}
-}
-
-TEST(CompressionPerf, SampleDepth32FC1)
-{
-	const std::vector<cv::Mat> depths = loadSampleDepths();
-	if(depths.empty())
-	{
-		GTEST_SKIP() << "No sample depth images in " << RTABMAP_TEST_DATA_ROOT << "/rgbd/depth";
-	}
-	for(size_t i=0; i<depths.size(); ++i)
-	{
-		report(uFormat("Sample depth %d in meters", (int)i), util2d::cvtDepthToFloat(depths[i]), approaches32F());
-	}
-}
-
-TEST(CompressionPerf, Synthetic32FC1)
-{
-	report("Synthetic continuous depth", makeSyntheticDepth(), approaches32F());
-	report("Synthetic continuous depth HD", makeSyntheticDepth(1280, 720), approaches32F());
-}

corelib/test/perf_flann_index.cpp

@@ -11,10 +11,6 @@
 // version can be compared to what it replaces.
 #include "FlannIndexBackends.h"
 
-#ifdef _OPENMP
-#include <omp.h>
-#endif
-
 // The times are reported rather than asserted on: which backend is the fastest
 // depends on the machine. They are here so that a change of backend, of
 // parameters or of nanoflann version can be compared to what it replaces.
@@ -521,31 +517,16 @@ TEST(FlannIndexPerfTest, RegistrationGuessMatching)
 
 	// A factor of 1 for the rtflann rows keeps their per-point bookkeeping out
 	// of the measurement, and picks the nanoflann tree that is built once.
-	std::vector<Backend> backends = {
-		{"cv        BFMatcher                 ", FlannIndex::FLANN_INDEX_LINEAR, 1.0f, true, 1},
-		{"cv        BFMatcher threaded        ", FlannIndex::FLANN_INDEX_LINEAR, 1.0f, true, 0},
+	const Backend backends[] = {
+		{"cv        BFMatcher                 ", FlannIndex::FLANN_INDEX_LINEAR, 1.0f, true},
 		{"rtflann   kd-tree (4 randomized)    ", FlannIndex::FLANN_INDEX_KDTREE, 1.0f},
 		{"rtflann   kd-tree single            ", FlannIndex::FLANN_INDEX_KDTREE_SINGLE, 1.0f},
 		{"nanoflann kd-tree single            ", FlannIndex::NANOFLANN_INDEX_KDTREE_SINGLE, 1.0f},
 		{"nanoflann kd-tree single incremental", FlannIndex::NANOFLANN_INDEX_KDTREE_SINGLE, 2.0f},
 	};
 
-#ifdef _OPENMP
-	// rtflann threads a radius search over its batch of queries the same way it
-	// threads a kNN one, so the two trees come back with one thread per core.
-	// The tree is still built on one core, and here it is rebuilt every frame,
-	// which caps what threading can take off the total: the times say how much
-	// of a frame is the search rather than the build.
-	backends.push_back({"rtflann   kd-tree (4 rand.) threaded", FlannIndex::FLANN_INDEX_KDTREE,        1.0f, false, 0});
-	backends.push_back({"rtflann   kd-tree single threaded   ", FlannIndex::FLANN_INDEX_KDTREE_SINGLE, 1.0f, false, 0});
-#endif
-
 	std::cout << "[          ] " << keypoints << " keypoints indexed and as many looked up in a "
 			  << radius << " px radius, per frame" << std::endl;
-#ifdef _OPENMP
-	std::cout << "[          ] the threaded rows search with " << omp_get_max_threads()
-			  << " threads, the others with one" << std::endl;
-#endif
 
 	for(const Backend & backend: backends)
 	{
@@ -557,7 +538,7 @@ TEST(FlannIndexPerfTest, RegistrationGuessMatching)
 		{
 			FlannIndex index;
 			index.buildIndex(backend.algorithm, points, false, backend.rebalancingFactor);
-			index.radiusSearch(projected, indices, dists, radius, 0, 32, 0.0f, false, backend.cores);
+			index.radiusSearch(projected, indices, dists, radius, 0, 32, 0.0f, false);
 		}
 		const double perFrame = timer.ticks()/double(frames);
 
@@ -590,31 +571,15 @@ void compareDictionaryMatching(int indexedCount, int queriedCount)
 	// built once, so it is neither kept ready to be added to nor rebuilt. The
 	// incremental nanoflann tree is kept in the comparison to show what asking
 	// for one costs here.
-	std::vector<Backend> backends = {
+	const Backend backends[] = {
 		{"linear    exhaustive                ", FlannIndex::FLANN_INDEX_LINEAR, 1.0f},
-		{"cv        BFMatcher                 ", FlannIndex::FLANN_INDEX_LINEAR, 1.0f, true, 1},
-		{"cv        BFMatcher threaded        ", FlannIndex::FLANN_INDEX_LINEAR, 1.0f, true, 0},
+		{"cv        BFMatcher                 ", FlannIndex::FLANN_INDEX_LINEAR, 1.0f, true},
 		{"rtflann   kd-tree (4 randomized)    ", FlannIndex::FLANN_INDEX_KDTREE, 1.0f},
 		{"rtflann   kd-tree single            ", FlannIndex::FLANN_INDEX_KDTREE_SINGLE, 1.0f},
 		{"nanoflann kd-tree single            ", FlannIndex::NANOFLANN_INDEX_KDTREE_SINGLE, 1.0f},
 		{"nanoflann kd-tree single incremental", FlannIndex::NANOFLANN_INDEX_KDTREE_SINGLE, 2.0f},
 	};
 
-#ifdef _OPENMP
-	// The same two rtflann trees searched with Kp/FlannThreads=0, one thread per
-	// core: rtflann is the only backend here threading a batch of queries, over
-	// an OpenMP loop. Only the search half of the times can improve, the trees
-	// are still built on one core. Queries are independent of each other, so
-	// threading doesn't change what is found: the exact tree holds its recall to
-	// the digit. The randomized one moves by a tenth of a percent from one run to
-	// the next whether threaded or not, it randomizes its splits on every build.
-	backends.push_back({"rtflann   kd-tree (4 rand.) threaded", FlannIndex::FLANN_INDEX_KDTREE,        1.0f, false, 0});
-	backends.push_back({"rtflann   kd-tree single threaded   ", FlannIndex::FLANN_INDEX_KDTREE_SINGLE, 1.0f, false, 0});
-
-	std::cout << "[          ] the threaded rows search with " << omp_get_max_threads()
-			  << " threads, the others with one" << std::endl;
-#endif
-
 	for(int dim: {32, 64, 128, 256})
 	{
 		const cv::Mat from = makeDescriptors(indexedCount, dim, clusterCount(indexedCount), 150);
@@ -634,7 +599,7 @@ void compareDictionaryMatching(int indexedCount, int queriedCount)
 			{
 				FlannIndex index;
 				index.buildIndex(backend.algorithm, from, false, backend.rebalancingFactor);
-				index.knnSearch(to, indices, dists, KNN, 32, 0.0f, true, backend.cores);
+				index.knnSearch(to, indices, dists, KNN);
 			}
 			const double perFrame = timer.ticks()/double(frames);
 

corelib/test/perf_graph.cpp

@@ -1,315 +0,0 @@
-// Comparison of the two ways of getting the graph depth of every node of a map,
-// which is what Rtabmap::process() needs to reject proximity candidates that are
-// too far in the graph (Parameters::kRGBDProximityMaxGraphDepth()):
-//
-//   - one graph::computePath() (A*) per candidate, which is what it did before, each
-//     search paying for the whole graph again;
-//   - one graph::computePathDepths() (BFS) for all of them, which is what it does now.
-//
-// The graph is a spiral walked inward, a pose every 30 cm: the shape a robot draws
-// covering a room, and the worst case for the A* heuristic. Two nodes on neighboring
-// turns are ~50 cm apart in space but a whole turn apart in the graph, so the straight
-// line to the goal says nothing about the path to it and each A* expands nearly the
-// whole graph. That is exactly the situation proximity detection is called for.
-//
-// Its own executable, run by ctest under the "performance" label, so that its seconds
-// of benchmarking stay out of the unit test shards:
-//   ctest -L performance    to run them
-//   ctest -LE performance   to skip them
-//   bin/test_graph_perf --gtest_filter=*Spiral*
-//
-// Each spiral it builds is written to the temp directory as a g2o file (the path is
-// printed with the results), so that the graph a number was measured on can be looked at
-// with rtabmap-graphViewer or g2o_viewer, or replayed by another tool.
-//
-// The times are reported rather than asserted on, as they depend on the machine. What
-// is asserted is that both approaches answer the same thing on the spiral, so that the
-// numbers below compare two ways of computing the same depths.
-#include <gtest/gtest.h>
-#include "TestUtils.h"
-#include <rtabmap/core/Graph.h>
-#include <rtabmap/core/Link.h>
-#include <rtabmap/core/Transform.h>
-#include <rtabmap/utilite/UConversion.h>
-#include <rtabmap/utilite/ULogger.h>
-#include <rtabmap/utilite/UTimer.h>
-#include <algorithm>
-#include <cmath>
-#include <cstdio>
-#include <iostream>
-#include <list>
-#include <map>
-#include <vector>
-
-using namespace rtabmap;
-
-namespace {
-
-// All the spirals have their turns PITCH apart and a pose every SPACING meters, walked
-// from their own radius in to RADIUS_END.
-static const float RADIUS_END = 1.0f;
-static const float PITCH = 0.5f;
-static const float SPACING = 0.3f;
-
-// An Archimedean spiral r(theta) = radiusStart - pitch*theta/(2*pi), walked from
-// radiusStart inward to radiusEnd with one pose every `spacing` meters of arc length,
-// linked as a chain in the order it was walked. Ids are 1..n, so the last id is the
-// innermost pose: the one a session ends on, and the one the depths are computed from.
-struct Spiral
-{
-	std::map<int, Transform> poses;
-	std::multimap<int, int> links;   // bidirectional, as Rtabmap builds them
-	std::vector<int> ids;            // in the order they were walked
-	float length = 0.0f;             // walked arc length, meters
-};
-
-Spiral makeSpiral(float radiusStart, float radiusEnd, float pitch, float spacing)
-{
-	UASSERT(radiusStart > radiusEnd && pitch > 0.0f && spacing > 0.0f);
-	Spiral spiral;
-	const float b = pitch/(2.0f*M_PI);          // -dr/dtheta
-	float theta = 0.0f;
-	float r = radiusStart;
-	while(r >= radiusEnd)
-	{
-		const int id = (int)spiral.ids.size()+1;
-		// Heading along the tangent, so that the poses are what a robot would have.
-		const float tangent = theta + M_PI_2 - std::atan2(b, r);
-		spiral.poses.insert(std::make_pair(id,
-				Transform(r*std::cos(theta), r*std::sin(theta), 0.0f, 0.0f, 0.0f, tangent)));
-		spiral.ids.push_back(id);
-		if(id > 1)
-		{
-			spiral.links.insert(std::make_pair(id-1, id));
-			spiral.links.insert(std::make_pair(id, id-1));
-			spiral.length += spacing;
-		}
-		// Arc length ds = sqrt(r^2 + (dr/dtheta)^2) dtheta, stepped by `spacing`.
-		theta += spacing/std::sqrt(r*r + b*b);
-		r = radiusStart - b*theta;
-	}
-	return spiral;
-}
-
-// The links between two poses closer than `maxDistance` in space but more than
-// `minTrajectoryGap` meters apart along the trajectory: the proximity links a session
-// would have added between neighboring turns, and the shortcuts that make the
-// fewest-links path and the shortest-in-meters path two different paths. The gap is what
-// makes them proximity links rather than trajectory ones: two poses a few steps apart are
-// within `maxDistance` of each other as well, but linking them adds no shortcut, it just
-// short-circuits the chain.
-std::multimap<int, int> proximityLinks(
-		const Spiral & spiral,
-		float maxDistance,
-		float minTrajectoryGap = 2.0f,
-		int * added = 0)
-{
-	std::multimap<int, int> links = spiral.links;
-	const size_t minStep = (size_t)std::ceil(minTrajectoryGap/SPACING);
-	int count = 0;
-	for(size_t i=0; i<spiral.ids.size(); ++i)
-	{
-		const Transform & a = spiral.poses.at(spiral.ids[i]);
-		for(size_t j=i+minStep; j<spiral.ids.size(); ++j)
-		{
-			const Transform & b = spiral.poses.at(spiral.ids[j]);
-			if(a.getDistance(b) <= maxDistance)
-			{
-				links.insert(std::make_pair(spiral.ids[i], spiral.ids[j]));
-				links.insert(std::make_pair(spiral.ids[j], spiral.ids[i]));
-				++count;
-			}
-		}
-	}
-	if(added)
-	{
-		*added = count;
-	}
-	return links;
-}
-
-// The links as constraints, one per pair (the multimap above holds both directions),
-// with the transform the poses give between the two nodes. Only needed to write the
-// graph to disk: exportPoses() needs Link objects, the searches only need the ids.
-std::multimap<int, Link> constraints(const Spiral & spiral, const std::multimap<int, int> & links)
-{
-	std::multimap<int, Link> constraints;
-	const cv::Mat information = cv::Mat::eye(6, 6, CV_64FC1);
-	for(std::multimap<int, int>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
-	{
-		const int from = iter->first, to = iter->second;
-		if(from > to)
-		{
-			continue;   // the other direction of a pair already written
-		}
-		const Transform & a = spiral.poses.at(from);
-		const Transform & b = spiral.poses.at(to);
-		// Consecutive poses are the trajectory, the rest are the proximity detections.
-		const Link::Type type = (to == from+1) ? Link::kNeighbor : Link::kLocalSpaceClosure;
-		constraints.insert(std::make_pair(from, Link(from, to, type, a.inverse()*b, information)));
-	}
-	return constraints;
-}
-
-// The graph these numbers were measured on, written next to the results so that it can be
-// looked at (rtabmap-graphViewer, g2o_viewer) or replayed by another tool. Overwritten on
-// every run, under a stable name rather than a pid-suffixed one: the file is there to be
-// opened, and makeSpiral() builds the same graph every time anyway.
-void saveG2o(const Spiral & spiral, const std::multimap<int, int> & links, const std::string & name)
-{
-	const std::string path = test::tempPath(uFormat("rtabmap_spiral_%s.g2o", name.c_str()));
-	if(graph::exportPoses(path, /*format=*/4, spiral.poses, constraints(spiral, links)))
-	{
-		std::cout << "[          ]   graph saved to " << path << std::endl;
-	}
-	else
-	{
-		std::cout << "[          ]   could not save the graph to " << path << std::endl;
-	}
-}
-
-// What Rtabmap::process() did before: one A* per candidate, from the last node, and the
-// candidate is kept when the path it found is short enough. Returns the ids each path
-// walks through, `from` first: its node count is what the old code compared against
-// RGBD/ProximityMaxGraphDepth, one more than the depth graph::computePathDepths() gives.
-std::map<int, std::vector<std::pair<int, Transform> > > pathsWithAStar(
-		const std::map<int, Transform> & poses,
-		const std::multimap<int, int> & links,
-		int from,
-		const std::vector<int> & targets)
-{
-	std::map<int, std::vector<std::pair<int, Transform> > > paths;
-	for(size_t i=0; i<targets.size(); ++i)
-	{
-		const std::list<std::pair<int, Transform> > path =
-				graph::computePath(poses, links, from, targets[i]);
-		if(!path.empty())
-		{
-			// As a vector, which is what the checks below (and graph::computePathLength()) take.
-			paths.insert(std::make_pair(targets[i],
-					std::vector<std::pair<int, Transform> >(path.begin(), path.end())));
-		}
-	}
-	return paths;
-}
-
-// A* needs one search per node, BFS answers for every node in the one search.
-void report(size_t nodes, double aStarTime, double bfsTime)
-{
-	printf("[          ]   A* %8.2f ms (%ld searches), BFS %6.2f ms (1 search), speedup x%.0f\n",
-			aStarTime*1000.0, (long)nodes, bfsTime*1000.0,
-			bfsTime > 0.0 ? aStarTime/bfsTime : 0.0);
-}
-
-// The spirals compared. All of them have a pose every 30 cm and turns 50 cm apart; what
-// changes is how far out they start, and so how many nodes they hold.
-struct SpiralSize
-{
-	float radiusStart;
-	const char * name;
-	const char * fileName;
-};
-static const SpiralSize SPIRAL_SIZES[] = {
-	{2.0f,  "2 m to 1 m",  "2m_to_1m"},
-	{5.0f,  "5 m to 1 m",  "5m_to_1m"},
-	{10.0f, "10 m to 1 m", "10m_to_1m"},
-};
-static const size_t SPIRAL_COUNT = sizeof(SPIRAL_SIZES)/sizeof(SPIRAL_SIZES[0]);
-
-}
-
-// The depths of every node of the spiral, from its last node, both ways. The spiral is a
-// chain, so there is only one path between two of its nodes and both approaches have to
-// agree: the A* path holds one more node than the BFS depth, the start node itself.
-TEST(GraphPerfTest, PathDepthsOnSpiral)
-{
-	for(size_t s=0; s<SPIRAL_COUNT; ++s)
-	{
-		const Spiral spiral = makeSpiral(SPIRAL_SIZES[s].radiusStart, RADIUS_END, PITCH, SPACING);
-		const int from = spiral.ids.back();
-		std::cout << "[          ] spiral " << SPIRAL_SIZES[s].name << ", turns "
-				<< PITCH << " m apart, a pose every " << SPACING << " m: "
-				<< spiral.ids.size() << " nodes, " << spiral.length << " m walked, depths from "
-				<< from << " (the innermost pose) to all of them" << std::endl;
-		saveG2o(spiral, spiral.links, SPIRAL_SIZES[s].fileName);
-
-		UTimer timer;
-		const std::map<int, std::vector<std::pair<int, Transform> > > aStarPaths =
-				pathsWithAStar(spiral.poses, spiral.links, from, spiral.ids);
-		const double aStarTime = timer.ticks();
-
-		const std::map<int, int> depths = graph::computePathDepths(spiral.links, from);
-		const double bfsTime = timer.ticks();
-
-		report(spiral.ids.size(), aStarTime, bfsTime);
-
-		ASSERT_EQ(depths.size(), spiral.ids.size());
-		ASSERT_EQ(aStarPaths.size(), spiral.ids.size());
-		EXPECT_EQ(depths.at(from), 0);
-		for(size_t i=0; i<spiral.ids.size(); ++i)
-		{
-			const int id = spiral.ids[i];
-			// The chain gives the depth in closed form: the number of links back to `from`.
-			EXPECT_EQ(depths.at(id), from-id) << "node " << id;
-
-			// Same path, not only the same count: the only way from `from` to `id` walks the
-			// chain, and A* walks it node by node, each step one deeper than the one before.
-			const std::vector<std::pair<int, Transform> > & path = aStarPaths.at(id);
-			ASSERT_EQ((int)path.size(), depths.at(id)+1) << "node " << id;
-			for(size_t j=0; j<path.size(); ++j)
-			{
-				ASSERT_EQ(path[j].first, from-(int)j) << "node " << id << ", step " << j;
-				ASSERT_EQ(depths.at(path[j].first), (int)j) << "node " << id << ", step " << j;
-			}
-		}
-	}
-}
-
-// The same spiral once the proximity links between neighboring turns are added, which is
-// what the graph looks like after a session closed on itself. Beyond the timings, this is
-// where the two approaches stop answering the same thing: A* minimizes meters, so the path
-// it returns is not always the one with the fewest links, and the node count it reports is
-// then larger than the depth. Rejecting candidates on it rejected some that were within
-// RGBD/ProximityMaxGraphDepth links of the current node.
-TEST(GraphPerfTest, PathDepthsOnSpiralWithProximityLinks)
-{
-	for(size_t s=0; s<SPIRAL_COUNT; ++s)
-	{
-		const Spiral spiral = makeSpiral(SPIRAL_SIZES[s].radiusStart, RADIUS_END, PITCH, SPACING);
-		int added = 0;
-		const std::multimap<int, int> links = proximityLinks(spiral, /*maxDistance=*/0.6f,
-				/*minTrajectoryGap=*/2.0f, &added);
-		const int from = spiral.ids.back();
-		std::cout << "[          ] spiral " << SPIRAL_SIZES[s].name << ": " << spiral.ids.size()
-				<< " nodes, " << added << " proximity links added between turns" << std::endl;
-		saveG2o(spiral, links, uFormat("%s_proximity", SPIRAL_SIZES[s].fileName));
-
-		UTimer timer;
-		const std::map<int, std::vector<std::pair<int, Transform> > > aStarPaths =
-				pathsWithAStar(spiral.poses, links, from, spiral.ids);
-		const double aStarTime = timer.ticks();
-
-		const std::map<int, int> depths = graph::computePathDepths(links, from);
-		const double bfsTime = timer.ticks();
-
-		report(spiral.ids.size(), aStarTime, bfsTime);
-
-		ASSERT_EQ(depths.size(), spiral.ids.size());
-		ASSERT_EQ(aStarPaths.size(), spiral.ids.size());
-		int overestimated = 0, maxOverestimation = 0;
-		for(size_t i=0; i<spiral.ids.size(); ++i)
-		{
-			const int id = spiral.ids[i];
-			const int over = (int)aStarPaths.at(id).size() - (depths.at(id)+1);
-			// A* cannot beat the BFS depth, it can only walk more links to save meters.
-			EXPECT_GE(over, 0) << "node " << id;
-			if(over > 0)
-			{
-				++overestimated;
-				maxOverestimation = std::max(maxOverestimation, over);
-			}
-		}
-		printf("[          ]   A* counted more links than the depth on %d of the %ld nodes"
-				" (up to %d more)\n", overestimated, (long)spiral.ids.size(), maxOverestimation);
-	}
-}

corelib/test/test_cameramodel.cpp

@@ -298,45 +298,6 @@ TEST_F(CameraModelTest, ReprojectInt)
     EXPECT_NEAR(v, static_cast<int>(cy_), 1);
 }
 
-TEST_F(CameraModelTest, ReprojectIgnoresTx)
-{
-    // A Tx set on a single camera model tags a left camera having stereo
-    // observations (the BA optimizers read the baseline from it to build their
-    // stereo edges), so reprojection stays that of the camera itself. Use
-    // StereoCameraModel::reproject() to get both images of a stereo pair.
-    double baseline = 0.12;
-    CameraModel withTx(fx_, fy_, cx_, cy_, CameraModel::opticalRotation(), -baseline*fx_, imageSize_);
-    CameraModel withoutTx(fx_, fy_, cx_, cy_, CameraModel::opticalRotation(), 0.0, imageSize_);
-    EXPECT_DOUBLE_EQ(withTx.Tx(), -baseline*fx_);
-
-    float x = 0.3f, y = -0.2f, z = 2.0f;
-
-    float u, v, uNoTx, vNoTx;
-    withTx.reproject(x, y, z, u, v);
-    withoutTx.reproject(x, y, z, uNoTx, vNoTx);
-
-    EXPECT_FLOAT_EQ(u, uNoTx);
-    EXPECT_FLOAT_EQ(v, vNoTx);
-    EXPECT_FLOAT_EQ(u, static_cast<float>(fx_*x/z + cx_));
-    EXPECT_FLOAT_EQ(v, static_cast<float>(fy_*y/z + cy_));
-}
-
-TEST_F(CameraModelTest, ReprojectProjectRoundTripNoTx)
-{
-    CameraModel model(fx_, fy_, cx_, cy_, CameraModel::opticalRotation(), 0.0, imageSize_);
-
-    float x = 0.35f, y = -0.15f, z = 2.5f;
-
-    float u, v;
-    model.reproject(x, y, z, u, v);
-
-    float x2, y2, z2;
-    model.project(u, v, z, x2, y2, z2);
-    EXPECT_NEAR(x2, x, 0.001f);
-    EXPECT_NEAR(y2, y, 0.001f);
-    EXPECT_FLOAT_EQ(z2, z);
-}
-
 // Field of View Tests
 
 TEST_F(CameraModelTest, FieldOfView)

corelib/test/test_compression.cpp

@@ -1,9 +1,6 @@
 #include <gtest/gtest.h>
 #include <rtabmap/core/Compression.h>
-#include <rtabmap/utilite/UException.h>
 #include <opencv2/core.hpp>
-#include <cstring>
-#include <limits>
 
 using namespace rtabmap;
 
@@ -186,228 +183,3 @@ TEST(CompressionTest, CompressionThreadDataRoundTrip)
 
 	expectMatEqual(uncompressThread.getUncompressedData(), data);
 }
-
-namespace {
-
-// 32FC1 depth image covering [minDepth, maxDepth[ with sub-millimeter values,
-// and the invalid values of the inverse depth format on the first row.
-cv::Mat makeFloatDepth(int rows, int cols, float minDepth, float maxDepth)
-{
-	cv::Mat depth(rows, cols, CV_32FC1);
-	for(int r = 0; r < rows; ++r)
-	{
-		for(int c = 0; c < cols; ++c)
-		{
-			depth.at<float>(r, c) = minDepth + (maxDepth - minDepth) * float(r * cols + c) / float(rows * cols);
-		}
-	}
-	return depth;
-}
-
-// Error bound of the inverse depth format: half a quantization step.
-float invDepthTolerance(float d, float quantization)
-{
-	// (with some margin for the float rounding of A/d + B, up to ~66000)
-	return 0.51f * d * d / (quantization * (quantization + 1.0f)) + 1e-6f;
-}
-
-} // namespace
-
-TEST(CompressionTest, ParseImageCompressionFormat)
-{
-	std::string codec;
-	float maxDepth, quantization;
-
-	EXPECT_TRUE(parseImageCompressionFormat("", codec, maxDepth, quantization));
-	EXPECT_TRUE(codec.empty());
-	EXPECT_EQ(maxDepth, 0.0f);
-
-	EXPECT_TRUE(parseImageCompressionFormat(".jpg", codec, maxDepth, quantization));
-	EXPECT_EQ(codec, ".jpg");
-	EXPECT_EQ(maxDepth, 0.0f);
-	EXPECT_EQ(quantization, 0.0f);
-
-	EXPECT_TRUE(parseImageCompressionFormat(".rvl", codec, maxDepth, quantization));
-	EXPECT_EQ(codec, ".rvl");
-	EXPECT_EQ(maxDepth, 0.0f);
-
-	EXPECT_TRUE(parseImageCompressionFormat(".png:20", codec, maxDepth, quantization));
-	EXPECT_EQ(codec, ".png");
-	EXPECT_FLOAT_EQ(maxDepth, 20.0f);
-	EXPECT_FLOAT_EQ(quantization, 100.0f);
-
-	EXPECT_TRUE(parseImageCompressionFormat(".rvl:10.5:50", codec, maxDepth, quantization));
-	EXPECT_EQ(codec, ".rvl");
-	EXPECT_FLOAT_EQ(maxDepth, 10.5f);
-	EXPECT_FLOAT_EQ(quantization, 50.0f);
-
-	EXPECT_FALSE(parseImageCompressionFormat("png", codec, maxDepth, quantization));
-	EXPECT_FALSE(parseImageCompressionFormat(".jpg:10:100", codec, maxDepth, quantization));
-	EXPECT_FALSE(parseImageCompressionFormat(".png:abc", codec, maxDepth, quantization));
-	EXPECT_FALSE(parseImageCompressionFormat(".png:0:100", codec, maxDepth, quantization));
-	EXPECT_FALSE(parseImageCompressionFormat(".png:-10:100", codec, maxDepth, quantization));
-	EXPECT_FALSE(parseImageCompressionFormat(".png:10:0", codec, maxDepth, quantization));
-	EXPECT_FALSE(parseImageCompressionFormat(".png:10:100:1", codec, maxDepth, quantization));
-}
-
-TEST(CompressionTest, InvalidFormatReturnsEmpty)
-{
-	const cv::Mat depth = makeFloatDepth(4, 4, 1.0f, 2.0f);
-	EXPECT_TRUE(compressImage(depth, ".jpg:10").empty());
-	EXPECT_TRUE(compressImage(depth, ".png:x").empty());
-}
-
-TEST(CompressionTest, InverseDepthRoundTrip)
-{
-	const float maxDepth = 10.0f;
-	const float quantization = 100.0f;
-	const float minDepth = quantization * (quantization + 1.0f) / (65535.0f + quantization * (quantization + 1.0f) / maxDepth);
-	cv::Mat depth = makeFloatDepth(48, 64, minDepth * 1.001f, maxDepth * 0.999f);
-	const float invalid[] = {
-			0.0f, -1.0f, maxDepth, maxDepth * 2.0f, minDepth * 0.9f,
-			std::numeric_limits<float>::quiet_NaN(),
-			std::numeric_limits<float>::infinity(),
-			-std::numeric_limits<float>::infinity()};
-	const int nInvalid = sizeof(invalid) / sizeof(float);
-	for(int i = 0; i < nInvalid; ++i)
-	{
-		depth.at<float>(0, i) = invalid[i];
-	}
-
-	for(const std::string codec : {".png", ".rvl"})
-	{
-		SCOPED_TRACE(codec);
-		const std::string format = codec + ":10:100";
-		const std::vector<unsigned char> bytes = compressImage(depth, format);
-		ASSERT_FALSE(bytes.empty());
-		EXPECT_LT(bytes.size(), depth.total() * depth.elemSize() / 2);
-		EXPECT_EQ(compressedDepthFormat(bytes), format);
-
-		const cv::Mat restored = uncompressImage(bytes);
-		ASSERT_EQ(restored.type(), CV_32FC1);
-		ASSERT_EQ(restored.size(), depth.size());
-		for(int r = 0; r < depth.rows; ++r)
-		{
-			for(int c = 0; c < depth.cols; ++c)
-			{
-				const float d = depth.at<float>(r, c);
-				if(r == 0 && c < nInvalid)
-				{
-					EXPECT_EQ(restored.at<float>(r, c), 0.0f) << "input=" << d;
-				}
-				else
-				{
-					ASSERT_NEAR(restored.at<float>(r, c), d, invDepthTolerance(d, quantization)) << "r=" << r << " c=" << c;
-				}
-			}
-		}
-
-		// Re-compressing with the detected format gives back the same bytes
-		// (e.g., DatabaseViewer saving an edited depth image).
-		EXPECT_EQ(compressImage(restored, compressedDepthFormat(bytes)), compressImage(restored, format));
-
-		// Same through cv::Mat and thread overloads
-		CompressionThread compressThread(depth, format);
-		compressThread.start();
-		compressThread.join();
-		const cv::Mat bytesMat = compressThread.getCompressedData();
-		ASSERT_EQ(bytesMat.total(), bytes.size());
-		EXPECT_EQ(memcmp(bytesMat.data, bytes.data(), bytes.size()), 0);
-		CompressionThread uncompressThread(bytesMat, true);
-		uncompressThread.start();
-		uncompressThread.join();
-		expectMatEqual(uncompressThread.getUncompressedData(), restored);
-	}
-}
-
-TEST(CompressionTest, InverseDepthQuantizationParameters)
-{
-	const cv::Mat depth = makeFloatDepth(32, 32, 1.0f, 39.0f);
-	const std::vector<unsigned char> bytes = compressImage(depth, ".png:40:50");
-	EXPECT_EQ(compressedDepthFormat(bytes), ".png:40:50");
-	const cv::Mat restored = uncompressImage(bytes);
-	ASSERT_EQ(restored.type(), CV_32FC1);
-	for(int r = 0; r < depth.rows; ++r)
-	{
-		for(int c = 0; c < depth.cols; ++c)
-		{
-			const float d = depth.at<float>(r, c);
-			ASSERT_NEAR(restored.at<float>(r, c), d, invDepthTolerance(d, 50.0f));
-		}
-	}
-}
-
-TEST(CompressionTest, InverseDepthNonContinuousImage)
-{
-	const cv::Mat depth = makeFloatDepth(20, 30, 1.0f, 5.0f);
-	const cv::Mat roi = depth(cv::Rect(3, 2, 10, 8));
-	ASSERT_FALSE(roi.isContinuous());
-	const cv::Mat restored = uncompressImage(compressImage(roi, ".rvl:10:100"));
-	ASSERT_EQ(restored.size(), roi.size());
-	for(int r = 0; r < roi.rows; ++r)
-	{
-		for(int c = 0; c < roi.cols; ++c)
-		{
-			const float d = roi.at<float>(r, c);
-			ASSERT_NEAR(restored.at<float>(r, c), d, invDepthTolerance(d, 100.0f));
-		}
-	}
-}
-
-TEST(CompressionTest, DepthParametersIgnoredFor16UC1)
-{
-	cv::Mat depth(24, 32, CV_16UC1);
-	cv::randu(depth, 0, 20000); // includes values over the max depth below
-	for(const std::string codec : {".png", ".rvl"})
-	{
-		SCOPED_TRACE(codec);
-		const std::vector<unsigned char> bytes = compressImage(depth, codec + ":10:100");
-		EXPECT_EQ(bytes, compressImage(depth, codec));
-		EXPECT_EQ(compressedDepthFormat(bytes), codec);
-		expectMatEqual(uncompressImage(bytes), depth);
-	}
-}
-
-TEST(CompressionTest, LegacyFloatDepthIsLossless)
-{
-	const cv::Mat depth = makeFloatDepth(16, 16, 0.01f, 100.0f);
-	for(const std::string format : {".png", ".rvl"})
-	{
-		SCOPED_TRACE(format);
-		const std::vector<unsigned char> bytes = compressImage(depth, format);
-		EXPECT_EQ(compressedDepthFormat(bytes), ".png");
-		const cv::Mat restored = uncompressImage(bytes);
-		ASSERT_EQ(restored.type(), CV_32FC1);
-		EXPECT_EQ(memcmp(restored.data, depth.data, depth.total() * depth.elemSize()), 0);
-	}
-}
-
-TEST(CompressionTest, MalformedDepthFormatsDecodeToEmpty)
-{
-	// Signature and header only, no payload
-	std::vector<unsigned char> invDepth = {'D', 'E', 'P', 'T', 'H', 'I', 'N', 'V'};
-	invDepth.resize(16, 0);
-	EXPECT_TRUE(uncompressImage(invDepth).empty());
-	EXPECT_EQ(compressedDepthFormat(invDepth), ".png") << "too short to be inverse depth";
-
-	// Inverse depth header followed by an 8 bits image instead of a 16 bits one
-	const std::vector<unsigned char> png8 = compressImage(cv::Mat(4, 4, CV_8UC1, cv::Scalar(1)), ".png");
-	invDepth.insert(invDepth.end(), png8.begin(), png8.end());
-	EXPECT_TRUE(uncompressImage(invDepth).empty());
-
-	// RVL signature without its size
-	const std::vector<unsigned char> rvl = {'D', 'E', 'P', 'T', 'H', 'R', 'V', 'L', 4, 0};
-	EXPECT_TRUE(uncompressImage(rvl).empty());
-	EXPECT_EQ(compressedDepthFormat(rvl), ".rvl");
-
-	EXPECT_TRUE(uncompressImage(nullptr, 0).empty());
-}
-
-TEST(CompressionTest, CompressionThreadRejectsInvalidFormat)
-{
-	// std::string: a string literal would select the (bytes, isImage) constructor
-	const cv::Mat depth(4, 4, CV_32FC1, cv::Scalar(1.0f));
-	EXPECT_THROW(CompressionThread(depth, std::string(".jpg:10")), UException);
-	EXPECT_THROW(CompressionThread(depth, std::string(".bmp")), UException);
-	EXPECT_NO_THROW(CompressionThread(depth, std::string(".rvl:10:100")));
-}

corelib/test/test_dbdriver.cpp

@@ -956,53 +956,6 @@ TEST_F(DbDriverFixture, LabelAndGraphQueries)
 	EXPECT_TRUE(lastNodeIds.count(4));
 }
 
-// getNodeData() answers from the trash -- signatures waiting to be written -- when it can.
-// For a saved signature, that is only when the compressed payload asked for is still in
-// it: saving drops a signature's compressed occupancy grid but keeps the raw cells, and so
-// the cell size, which alone does not mean the compressed grid is there.
-TEST_F(DbDriverFixture, GetNodeDataTakesTheGridFromTheTrashOnlyIfCompressed)
-{
-	cv::Mat obstacles(1, 3, CV_32FC3);
-	for(int i = 0; i < 3; ++i)
-	{
-		obstacles.at<cv::Vec3f>(0, i) = cv::Vec3f(float(i) * 0.1f, 0.0f, 0.0f);
-	}
-
-	// Node 1 in the database, with its compressed grid.
-	Signature * written = new Signature(1);
-	attachSensorDataForDatabaseSave(*written);
-	written->sensorData().setOccupancyGrid(cv::Mat(), obstacles, cv::Mat(), 0.05f, cv::Point3f());
-	saveSignature(written);
-
-	// Node 2, saved but only in the trash, with its compressed grid: taken from there.
-	Signature * pending = new Signature(2);
-	pending->sensorData().setOccupancyGrid(cv::Mat(), obstacles, cv::Mat(), 0.05f, cv::Point3f());
-	pending->setSaved(true);
-	driver_->asyncSave(pending);
-	{
-		SensorData data;
-		driver_->getNodeData(2, data, false, false, false, true);
-		EXPECT_FALSE(data.gridObstacleCellsCompressed().empty());
-		EXPECT_FLOAT_EQ(0.05f, data.gridCellSize());
-	}
-
-	// A copy of node 1 in the trash, its compressed grid dropped as saving does, the raw
-	// cells and the cell size kept: the grid comes from the database instead.
-	Signature * stale = new Signature(1);
-	stale->sensorData().setOccupancyGrid(cv::Mat(), obstacles, cv::Mat(), 0.05f, cv::Point3f());
-	stale->sensorData().clearCompressedData(false, false, false, true);
-	stale->setSaved(true);
-	ASSERT_TRUE(stale->sensorData().gridObstacleCellsCompressed().empty());
-	ASSERT_FLOAT_EQ(0.05f, stale->sensorData().gridCellSize());
-	driver_->asyncSave(stale);
-	{
-		SensorData data;
-		driver_->getNodeData(1, data, false, false, false, true);
-		EXPECT_FALSE(data.gridObstacleCellsCompressed().empty());
-		EXPECT_FLOAT_EQ(0.05f, data.gridCellSize());
-	}
-}
-
 TEST_F(DbDriverFixture, GetNodeDataAndLocalFeatures)
 {
 	Signature * sig = new Signature(1);

corelib/test/test_dbdriversqlite3.cpp

@@ -50,12 +50,9 @@ protected:
 		}
 	}
 
-	// Trash-checking methods are hidden in DBDriverSqlite3, call them through the base class
-	DBDriver * db() const { return driver_; }
-
 	void saveSignature(Signature * s)
 	{
-		db()->asyncSave(s);
+		driver_->asyncSave(s);
 		driver_->emptyTrashes(false);
 	}
 
@@ -106,7 +103,7 @@ TEST(DBDriverSqlite3Test, ParseParametersEnablesInMemory)
 	EXPECT_TRUE(driver.isInMemory());
 	EXPECT_TRUE(driver.isConnected());
 
-	static_cast<DBDriver &>(driver).asyncSave(new Signature(1));
+	driver.asyncSave(new Signature(1));
 	driver.emptyTrashes(false);
 	EXPECT_EQ(driver.getTotalNodesSize(), 1);
 
@@ -124,7 +121,7 @@ TEST(DBDriverSqlite3Test, InMemorySaveToFileOnClose)
 	ASSERT_TRUE(driver.openConnection(path, true));
 	EXPECT_TRUE(driver.isInMemory());
 
-	static_cast<DBDriver &>(driver).asyncSave(new Signature(1, 5, 1, 50.0, "sqlite_mem", Transform(1.f, 0.f, 0.f, 0.f, 0.f, 0.f)));
+	driver.asyncSave(new Signature(1, 5, 1, 50.0, "sqlite_mem", Transform(1.f, 0.f, 0.f, 0.f, 0.f, 0.f)));
 	driver.emptyTrashes(false);
 	driver.closeConnection(true, path);
 
@@ -236,7 +233,7 @@ TEST_F(DBDriverSqlite3Fixture, SavesAndLoadsRichSensorData)
 	saveSignature(s);
 
 	std::list<Signature *> loaded;
-	db()->loadSignatures(std::list<int>(1, 10), loaded);
+	driver_->loadSignatures(std::list<int>(1, 10), loaded);
 	ASSERT_EQ(1u, loaded.size());
 	Signature * back = loaded.front();
 	EXPECT_EQ(10, back->id());
@@ -247,7 +244,7 @@ TEST_F(DBDriverSqlite3Fixture, SavesAndLoadsRichSensorData)
 
 	// Payloads come back compressed; ask the driver to fill them in.
 	std::list<Signature *> toFill(1, back);
-	db()->loadNodeData(toFill);
+	driver_->loadNodeData(toFill);
 	back->sensorData().uncompressData();
 	EXPECT_FALSE(back->sensorData().imageRaw().empty()) << "image blob did not round-trip";
 	EXPECT_FALSE(back->sensorData().depthRaw().empty()) << "depth blob did not round-trip";
@@ -323,9 +320,9 @@ TEST_F(DBDriverSqlite3Fixture, RawOnlySensorDataIsNotPersisted)
 	saveSignature(new Signature(42, 0, 1, 1.0, "", Transform::getIdentity(), Transform(), raw));
 
 	std::list<Signature *> loaded;
-	db()->loadSignatures(std::list<int>(1, 42), loaded);
+	driver_->loadSignatures(std::list<int>(1, 42), loaded);
 	ASSERT_EQ(1u, loaded.size());
-	db()->loadNodeData(loaded);
+	driver_->loadNodeData(loaded);
 	loaded.front()->sensorData().uncompressData();
 	EXPECT_TRUE(loaded.front()->sensorData().imageRaw().empty())
 			<< "raw-only image unexpectedly survived a save/load round trip";
@@ -366,12 +363,9 @@ protected:
 		UFile::erase(dbPath_.c_str());
 	}
 
-	// Trash-checking methods are hidden in DBDriverSqlite3, call them through the base class
-	DBDriver * db() const { return driver_; }
-
 	void saveSignature(Signature * s)
 	{
-		db()->asyncSave(s);
+		driver_->asyncSave(s);
 		driver_->emptyTrashes(false);
 	}
 
@@ -406,11 +400,11 @@ TEST_P(DBSchemaVersionTest, NodesAndLinksSurviveARoundTrip)
 	EXPECT_FALSE(driver_->getDatabaseVersion().empty());
 
 	std::list<Signature *> loaded;
-	db()->loadSignatures(std::list<int>{1, 2}, loaded);
+	driver_->loadSignatures(std::list<int>{1, 2}, loaded);
 	ASSERT_EQ(2u, loaded.size()) << "nodes did not survive the round trip";
 
 	// Payloads
-	db()->loadNodeData(loaded);
+	driver_->loadNodeData(loaded);
 	for(Signature * s : loaded)
 	{
 		s->sensorData().uncompressData();
@@ -421,7 +415,7 @@ TEST_P(DBSchemaVersionTest, NodesAndLinksSurviveARoundTrip)
 	// Links: the second node must still point back at the first, with the
 	// variances recovered from whatever columns this schema uses.
 	std::multimap<int, Link> links;
-	db()->loadLinks(2, links);
+	driver_->loadLinks(2, links);
 	ASSERT_FALSE(links.empty()) << "link did not survive the round trip";
 	const Link & link = links.begin()->second;
 	EXPECT_EQ(1, link.to());

corelib/test/test_graph.cpp

@@ -124,29 +124,6 @@ TEST(GraphTest, FindLinkForwardAndReverse)
 	EXPECT_NE(graph::findLink(links, 1, 2, true, Link::kNeighbor), links.end());
 }
 
-TEST(GraphTest, FindLinkWithManyLinksPerNode)
-{
-	// multimap::find() may return any element with the key (recent libc++ does),
-	// so lookups must start from lower_bound() to see every link of a node.
-	std::multimap<int, Link> links;
-	std::multimap<int, std::pair<int, Link::Type> > biLinks;
-	std::multimap<int, int> intLinks;
-	for(int to=2; to<=40; ++to)
-	{
-		insertLink(links, Link(1, to, to%2?Link::kGlobalClosure:Link::kNeighbor, Transform::getIdentity()));
-		biLinks.insert(std::make_pair(1, std::make_pair(to, to%2?Link::kGlobalClosure:Link::kNeighbor)));
-		intLinks.insert(std::make_pair(1, to));
-	}
-	for(int to=2; to<=40; ++to)
-	{
-		Link::Type type = to%2?Link::kGlobalClosure:Link::kNeighbor;
-		EXPECT_NE(graph::findLink(links, 1, to, false, type), links.end()) << "to=" << to;
-		EXPECT_NE(graph::findLink(links, to, 1, true, type), links.end()) << "to=" << to;
-		EXPECT_NE(graph::findLink(biLinks, 1, to, false, type), biLinks.end()) << "to=" << to;
-		EXPECT_NE(graph::findLink(intLinks, 1, to), intLinks.end()) << "to=" << to;
-	}
-}
-
 TEST(GraphTest, FindLinkIntMultimap)
 {
 	std::multimap<int, int> links;

corelib/test/test_memory.cpp

@@ -10,7 +10,6 @@
 #include <rtabmap/core/RegistrationInfo.h>
 #include <rtabmap/core/util3d_transforms.h>
 #include <rtabmap/core/SensorData.h>
-#include <rtabmap/core/StereoCameraModel.h>
 #include <rtabmap/core/Signature.h>
 #include <rtabmap/core/Transform.h>
 #include <rtabmap/core/VWDictionary.h>
@@ -2844,144 +2843,6 @@ TEST_F(MemoryFixture, CreateSignatureAutoIncrementsIdWhenGenerateIdsOn)
 	EXPECT_EQ(memory_->getLastSignatureId(), id1 + 1);
 }
 
-namespace {
-
-// Mem/ImagePreDecimation with keypoints provided by odometry: createSignature scales them
-// into the decimated image it describes them in, and back to the final image size after.
-// These parameters and this frame are what the four tests below vary the surroundings of.
-ParametersMap decimatedOctaveParams()
-{
-	ParametersMap params = defaultMemoryParams();
-	params[Parameters::kKpMaxFeatures()] = "100";                  // let descriptors be extracted
-	params[Parameters::kMemUseOdomFeatures()] = "true";
-	params[Parameters::kMemImagePreDecimation()] = "2";
-	params[Parameters::kMemImagePostDecimation()] = "1";
-	params[Parameters::kRtabmapImagesAlreadyRectified()] = "true"; // skip rectification
-	return params;
-}
-
-// One keypoint at @p octave, with its 3D point but no descriptor -- the missing descriptor
-// is what sends createSignature down the branch that describes provided keypoints from the
-// image. The image is big enough that the keypoint stays far from the border of the
-// decimated one, where a descriptor cannot be computed and the keypoint would be dropped.
-SensorData decimatedOctaveFrame(int octave)
-{
-	cv::Mat image(256, 256, CV_8UC1);
-	cv::RNG rng(7);
-	rng.fill(image, cv::RNG::UNIFORM, 0, 255);
-	const CameraModel model(100.0, 100.0, 128.0, 128.0,
-			CameraModel::opticalRotation(), 0.0, cv::Size(256, 256));
-
-	SensorData data;
-	data.setRGBDImage(image, cv::Mat(), std::vector<CameraModel>{model});
-	data.setId(0);
-
-	cv::KeyPoint kpt(128.0f, 120.0f, 8.0f);
-	kpt.octave = octave;
-	data.setFeatures(std::vector<cv::KeyPoint>(1, kpt),
-			std::vector<cv::Point3f>(1, cv::Point3f(0.0f, 0.0f, 1.0f)),
-			cv::Mat());
-	return data;
-}
-
-const cv::KeyPoint & theOnlyWord(const Memory & memory)
-{
-	const Signature * s = memory.getSignature(memory.getLastSignatureId());
-	UASSERT(s != 0 && s->getWordsKpts().size() == 1);
-	return s->getWordsKpts()[0];
-}
-
-} // namespace
-
-TEST(MemoryTest, PreDecimationGivesBackProvidedKeypointsAsTheyCameIn)
-{
-	// With no post-decimation the two conversions undo each other, which is the whole of
-	// what this test knows: what comes out is what went in, the octave included -- it
-	// moves down with the image and back up again, a decimated image being that many
-	// pyramid levels down already.
-	Memory memory(decimatedOctaveParams());
-	const SensorData sent = decimatedOctaveFrame(2);
-	SensorData data = sent;
-
-	ASSERT_TRUE(memory.update(data, Transform(0, 0, 0, 0, 0, 0),
-			cv::Mat::eye(6, 6, CV_64FC1) * 0.01));
-	const cv::KeyPoint & word = theOnlyWord(memory);
-	EXPECT_FLOAT_EQ(word.pt.x, sent.keypoints()[0].pt.x);
-	EXPECT_FLOAT_EQ(word.pt.y, sent.keypoints()[0].pt.y);
-	EXPECT_FLOAT_EQ(word.size, sent.keypoints()[0].size);
-	EXPECT_EQ(word.octave, sent.keypoints()[0].octave);
-}
-
-TEST(MemoryTest, PreDecimationKeepsProvidedKeypointsAtTheFinestLevelAvailable)
-{
-	// The same for a keypoint found at the finest level there is. Scaling it into a
-	// decimated image would put it below level 0, which does not exist -- the detail it
-	// was found at was decimated away -- and which ORB rejects outright rather than
-	// describing. It stays at 0 instead, and so cannot come back at 0: the level it would
-	// need to return to is the one that was lost.
-	Memory memory(decimatedOctaveParams());
-	const SensorData sent = decimatedOctaveFrame(0);
-	SensorData data = sent;
-
-	ASSERT_TRUE(memory.update(data, Transform(0, 0, 0, 0, 0, 0),
-			cv::Mat::eye(6, 6, CV_64FC1) * 0.01));
-	const cv::KeyPoint & word = theOnlyWord(memory);
-	// Where it is and how big it is are unaffected, those having room to scale.
-	EXPECT_FLOAT_EQ(word.pt.x, sent.keypoints()[0].pt.x);
-	EXPECT_FLOAT_EQ(word.pt.y, sent.keypoints()[0].pt.y);
-	EXPECT_FLOAT_EQ(word.size, sent.keypoints()[0].size);
-	EXPECT_GE(word.octave, 0);
-}
-
-TEST(MemoryTest, PreDecimationOnANewDatabaseUsesTheCorrectedOctaveScaling)
-{
-	// A database this version created is filled the corrected way. Worth its own test
-	// because the choice is made from the database's version string: were that to come
-	// back empty or unreadable, every map would silently be treated as an old one.
-	const std::string dbPath = uniqueDbPath();
-	Memory memory(decimatedOctaveParams());
-	ASSERT_TRUE(memory.init(dbPath, true));
-
-	SensorData data = decimatedOctaveFrame(2);
-	ASSERT_TRUE(memory.update(data, Transform(0, 0, 0, 0, 0, 0),
-			cv::Mat::eye(6, 6, CV_64FC1) * 0.01));
-	EXPECT_EQ(theOnlyWord(memory).octave, 2);
-
-	memory.close(false);
-	UFile::erase(dbPath);
-}
-
-TEST(MemoryTest, PreDecimationOnAnOlderDatabaseKeepsTheScalingItWasFilledWith)
-{
-	// A map made before 0.23.12 holds features described one pyramid level too coarse.
-	// Adding to it keeps doing that, so that what goes in now can still be matched
-	// against what is already there; the corrected scaling starts with a new map. Here
-	// the octave comes back at 2+1+1 rather than 2-1+1.
-	const std::string source =
-			std::string(RTABMAP_TEST_DATA_ROOT) + "/tests/pr2_scan2d_corridor_50s.db";
-	if(!UFile::exists(source))
-	{
-		GTEST_SKIP() << "Test data not found: " << source
-				<< " (run scripts/fetch_test_data.sh to populate)";
-	}
-	const std::string dbPath = uniqueDbPath();
-	UFile::copy(source, dbPath);
-
-	Memory memory(decimatedOctaveParams());
-	ASSERT_TRUE(memory.init(dbPath));
-	ASSERT_LT(uStrNumCmp(memory.getDatabaseVersion(), "0.23.12"), 0)
-			<< "this fixture is supposed to predate the correction";
-
-	SensorData data = decimatedOctaveFrame(2);
-	ASSERT_TRUE(memory.update(data, Transform(0, 0, 0, 0, 0, 0),
-			cv::Mat::eye(6, 6, CV_64FC1) * 0.01));
-	EXPECT_EQ(theOnlyWord(memory).octave, 4)
-			<< "an older map has to keep being filled the way it was";
-
-	memory.close(false);
-	UFile::erase(dbPath);
-}
-
 TEST(MemoryTest, CreateSignaturePostDecimatesImageWhenPostDecimationGreaterThanOne)
 {
 	// kMemImagePostDecimation > 1 causes createSignature to downsample the RGB image
@@ -3131,142 +2992,6 @@ TEST(MemoryTest, GetNodeDataReturnsInMemoryPayloadsWhenSignatureNotSaved)
 	EXPECT_EQ(r.imageCompressed().cols, s->sensorData().imageCompressed().cols);
 }
 
-TEST(MemoryTest, UpdateKeepsUserDataThatArrivesCompressed)
-{
-	// User data can reach update() already compressed, with no raw copy -- as it does
-	// from a serialized SensorData (e.g., rtabmap_ros's SensorData messages). It must be
-	// stored as it is, like already-compressed images, rather than dropped for lack of
-	// raw data to compress. Checked with and without Mem/BinDataKept, which build the
-	// signature in two different branches, and with and without parallel compression.
-	const cv::Mat userData = (cv::Mat_<float>(1, 4) << 1.0f, 2.0f, 3.0f, 4.0f);
-	for(const char * binDataKept : {"true", "false"})
-	{
-		for(const char * parallel : {"true", "false"})
-		{
-			SCOPED_TRACE(std::string("Mem/BinDataKept=") + binDataKept +
-					" Mem/CompressionParallelized=" + parallel);
-			ParametersMap params = defaultMemoryParams();
-			params[Parameters::kMemBinDataKept()] = binDataKept;
-			params[Parameters::kMemCompressionParallelized()] = parallel;
-			Memory memory(params);
-
-			SensorData data(cv::Mat(8, 8, CV_8UC1, cv::Scalar(128)));
-			data.setUserData(compressData2(userData)); // bytes: taken as already compressed
-			ASSERT_TRUE(data.userDataRaw().empty());
-			ASSERT_FALSE(data.userDataCompressed().empty());
-
-			ASSERT_TRUE(memory.update(data, Transform(0, 0, 0, 0, 0, 0), cv::Mat::eye(6, 6, CV_64FC1) * 0.01));
-			const Signature * s = memory.getSignature(memory.getLastSignatureId());
-			ASSERT_NE(s, nullptr);
-			ASSERT_FALSE(s->sensorData().userDataCompressed().empty());
-			const cv::Mat stored = uncompressData(s->sensorData().userDataCompressed());
-			ASSERT_EQ(stored.size(), userData.size());
-			ASSERT_EQ(stored.type(), userData.type());
-			EXPECT_EQ(0.0, cv::norm(stored, userData, cv::NORM_INF));
-		}
-	}
-}
-
-TEST(MemoryTest, UpdateReusesTheGivenCompressedData)
-{
-	// Data given both raw and compressed is not compressed again: the compressed copy
-	// given is stored as is, sharing its buffer. For the scan, only while Memory has not
-	// filtered it, since a filtered scan no longer matches the compressed one given.
-	cv::Mat points(1, 10, CV_32FC3);
-	for(int i = 0; i < points.cols; ++i)
-	{
-		points.at<cv::Vec3f>(0, i) = cv::Vec3f(1.0f + i, 0.5f * i, 0.0f);
-	}
-	for(const char * binDataKept : {"true", "false"})
-	{
-		for(const char * parallel : {"true", "false"})
-		{
-			for(const char * downsample : {"1", "2"})
-			{
-				SCOPED_TRACE(std::string("Mem/BinDataKept=") + binDataKept +
-						" Mem/CompressionParallelized=" + parallel +
-						" Mem/LaserScanDownsampleStepSize=" + downsample);
-				ParametersMap params = defaultMemoryParams();
-				params[Parameters::kMemBinDataKept()] = binDataKept;
-				params[Parameters::kMemCompressionParallelized()] = parallel;
-				params[Parameters::kMemLaserScanDownsampleStepSize()] = downsample;
-				Memory memory(params);
-
-				SensorData data(cv::Mat(8, 8, CV_8UC1, cv::Scalar(128)));
-				const LaserScan compressedScan(compressData2(points), points.cols, 10.0f, LaserScan::kXYZ);
-				data.setLaserScan(compressedScan);
-				data.setLaserScan(LaserScan(points, points.cols, 10.0f, LaserScan::kXYZ), false);
-				data.setUserData(points.t()); // raw, several rows: compressed by setUserData()
-				ASSERT_FALSE(data.userDataCompressed().empty());
-				ASSERT_FALSE(data.laserScanRaw().isEmpty());
-				ASSERT_FALSE(data.laserScanCompressed().isEmpty());
-
-				ASSERT_TRUE(memory.update(data, Transform(0, 0, 0, 0, 0, 0), cv::Mat::eye(6, 6, CV_64FC1) * 0.01));
-				const Signature * s = memory.getSignature(memory.getLastSignatureId());
-				ASSERT_NE(s, nullptr);
-
-				EXPECT_EQ(s->sensorData().userDataCompressed().data, data.userDataCompressed().data);
-
-				const LaserScan & stored = s->sensorData().laserScanCompressed();
-				ASSERT_FALSE(stored.isEmpty());
-				if(std::string(downsample) == "1")
-				{
-					EXPECT_EQ(stored.data().data, compressedScan.data().data);
-				}
-				else
-				{
-					EXPECT_NE(stored.data().data, compressedScan.data().data);
-					EXPECT_EQ(uncompressData(stored.data()).cols, points.cols / 2);
-				}
-			}
-		}
-	}
-}
-
-TEST(MemoryTest, GetNodeDataLoadsTheGridOfASavedSignatureFromDatabase)
-{
-	// Once a signature still in WM is saved (Rtabmap::process() does it right after
-	// adding it, when the database is not in memory), saveLocationData() drops its
-	// compressed data but keeps the raw grid cells, so gridCellSize() stays set. A
-	// request for the grid alone must not be answered from memory on the strength of
-	// that cell size: it would return the raw cells only, and callers that only read
-	// the compressed ones (e.g., rtabmap_ros's conversion to messages) would get an
-	// empty grid. It has to be loaded from the database, like the other payloads.
-	const std::string dbPath = uniqueDbPath();
-	ParametersMap params = defaultMemoryParams();
-	params[Parameters::kMemBinDataKept()] = "true";
-	params[Parameters::kRGBDCreateOccupancyGrid()] = "true";
-	Memory memory(params);
-	ASSERT_TRUE(memory.init(dbPath));
-
-	SensorData data(cv::Mat(8, 8, CV_8UC1, cv::Scalar(128)));
-	cv::Mat obstacles(1, 3, CV_32FC3);
-	for(int i = 0; i < 3; ++i)
-	{
-		obstacles.at<cv::Vec3f>(0, i) = cv::Vec3f(float(i) * 0.1f, 0.0f, 0.0f);
-	}
-	const float kCellSize = 0.05f;
-	data.setOccupancyGrid(cv::Mat(), obstacles, cv::Mat(), kCellSize, cv::Point3f(0, 0, 0));
-	ASSERT_TRUE(memory.update(data, Transform(0, 0, 0, 0, 0, 0), cv::Mat::eye(6, 6, CV_64FC1) * 0.01));
-	const int id = memory.getLastSignatureId();
-
-	memory.saveLocationData(id);
-	const Signature * s = memory.getSignature(id);
-	ASSERT_NE(s, nullptr);
-	ASSERT_TRUE(s->isSaved());
-	ASSERT_TRUE(s->sensorData().gridObstacleCellsCompressed().empty()); // dropped by the save
-	ASSERT_FLOAT_EQ(s->sensorData().gridCellSize(), kCellSize);         // but still set
-	memory.emptyTrash(); // flush the async writer so the row can be read back
-
-	SensorData r = memory.getNodeData(id, /*images=*/false, /*scan=*/false, /*userData=*/false, /*occupancyGrid=*/true);
-	EXPECT_FALSE(r.gridObstacleCellsCompressed().empty());
-	EXPECT_FLOAT_EQ(r.gridCellSize(), kCellSize);
-	EXPECT_EQ(r.imageCompressed().rows, 0);
-
-	memory.close(false);
-	UFile::erase(dbPath);
-}
-
 TEST(MemoryTest, GetNodeDataMasksFieldsThatWereNotRequested)
 {
 	// Even when a signature has all payloads populated, getNodeData must clear the
@@ -3406,7 +3131,6 @@ TEST(MemoryTest, GetNodeDataLoadsEachPayloadTypeFromDatabase)
 		expectScanEmpty(r.laserScanCompressed());
 		EXPECT_EQ(r.userDataCompressed().rows, 0);
 		EXPECT_FLOAT_EQ(r.gridCellSize(), kCellSize);
-		EXPECT_FALSE(r.gridObstacleCellsCompressed().empty()); // the cells, not just the cell size
 	}
 
 	// All four together.
@@ -4436,373 +4160,3 @@ TEST_F(MemoryFixture, ComputeIcpTransformMultiRejectsScansTooFarApart)
 	EXPECT_NE(info.rejectedMsg.find("Too far"), std::string::npos)
 			<< "unexpected reason: " << info.rejectedMsg;
 }
-
-// ---------------------------------------------------------------------------
-// createSignature() reuses the caller's compressed blob instead of
-// re-compressing, but only while the pixels it would store are provably the
-// ones that blob already encodes. Two separate mechanisms keep that true, and
-// these tests pin both:
-//   - decimation leaves `data` untouched and is caught by a buffer-identity
-//     check on the local image,
-//   - rectification/rotation go through SensorData::setRGBDImage(), which
-//     clears the compressed blob so there is nothing left to reuse.
-// A regression in either one stores pixels that don't match the signature.
-
-namespace {
-
-// A SensorData carrying both the raw image and the blob that encodes it, the
-// shape produced by SensorData::uncompressData() when reprocessing a database.
-SensorData dataWithRawAndCompressed(const cv::Mat & raw, const cv::Mat & blob)
-{
-	SensorData data(blob);     // 1-row CV_8UC1 is detected as compressed
-	data.setImageRaw(raw);     // setImageRaw() does not clear the blob
-	return data;
-}
-
-SensorData dataWithRawAndCompressed(const cv::Mat & raw, const cv::Mat & blob, const CameraModel & model)
-{
-	SensorData data(blob, model);
-	data.setImageRaw(raw);
-	return data;
-}
-
-cv::Mat texture(int rows, int cols)
-{
-	cv::Mat image(rows, cols, CV_8UC1);
-	cv::randu(image, cv::Scalar(0), cv::Scalar(255));
-	return image;
-}
-
-bool sameBytes(const cv::Mat & a, const cv::Mat & b)
-{
-	return a.size() == b.size() && a.type() == b.type() && cv::countNonZero(a != b) == 0;
-}
-
-// What createSignature() ended up storing for `data`.
-SensorData storedData(Memory & memory, SensorData & data)
-{
-	const cv::Mat covariance = cv::Mat::eye(6, 6, CV_64FC1) * 0.01;
-	if(!memory.update(data, Transform(0, 0, 0, 0, 0, 0), covariance))
-	{
-		return SensorData();
-	}
-	const Signature * s = memory.getSignature(memory.getLastSignatureId());
-	return s ? s->sensorData() : SensorData();
-}
-
-cv::Mat storedBlob(Memory & memory, SensorData & data)
-{
-	return storedData(memory, data).imageCompressed();
-}
-
-}  // namespace
-
-TEST(MemoryTest, CreateSignatureReusesCompressedImageWhenPixelsUnchanged)
-{
-	// Nothing decimates, rectifies or rotates the image, so the blob the caller
-	// supplied still encodes exactly what gets stored: it must be passed through
-	// byte for byte rather than re-compressed. Both compression paths are
-	// exercised -- the reuse flags gate the threaded branch and the serial one
-	// separately.
-	for(int parallelized = 0; parallelized <= 1; ++parallelized)
-	{
-		SCOPED_TRACE(std::string(Parameters::kMemCompressionParallelized()) +
-				"=" + (parallelized ? "true" : "false"));
-
-		ParametersMap params = defaultMemoryParams();
-		params[Parameters::kMemBinDataKept()] = "true";
-		params[Parameters::kMemImagePostDecimation()] = "1";
-		params[Parameters::kMemCompressionParallelized()] = parallelized ? "true" : "false";
-		Memory memory(params);
-
-		const cv::Mat raw = texture(32, 32);
-		const cv::Mat blob = compressImage2(raw, ".png");
-		ASSERT_FALSE(blob.empty());
-
-		SensorData data = dataWithRawAndCompressed(raw, blob);
-		ASSERT_FALSE(data.imageRaw().empty());
-		ASSERT_FALSE(data.imageCompressed().empty());
-
-		const cv::Mat stored = storedBlob(memory, data);
-		ASSERT_FALSE(stored.empty()) << "no compressed image was kept";
-		EXPECT_TRUE(sameBytes(stored, blob))
-				<< "the caller's blob was re-compressed instead of reused ("
-				<< blob.cols << " bytes in, " << stored.cols << " bytes stored)";
-	}
-}
-
-TEST(MemoryTest, CreateSignatureRecompressesWhenPostDecimationChangesPixels)
-{
-	// Decimation never touches the SensorData, so its blob is still there and
-	// still non-empty; only the buffer-identity check stands between it and a
-	// signature whose stored image is twice the size of its own pixels.
-	ParametersMap params = defaultMemoryParams();
-	params[Parameters::kMemBinDataKept()] = "true";
-	params[Parameters::kMemImagePostDecimation()] = "2";
-	Memory memory(params);
-
-	const cv::Mat raw = texture(32, 32);
-	const cv::Mat blob = compressImage2(raw, ".png");
-	SensorData data = dataWithRawAndCompressed(raw, blob);
-
-	const cv::Mat stored = storedBlob(memory, data);
-	ASSERT_FALSE(stored.empty()) << "no compressed image was kept";
-	EXPECT_FALSE(sameBytes(stored, blob))
-			<< "the full-resolution blob was stored for a decimated signature";
-
-	const cv::Mat decoded = uncompressImage(stored);
-	EXPECT_EQ(decoded.cols, raw.cols / 2);
-	EXPECT_EQ(decoded.rows, raw.rows / 2);
-}
-
-TEST(MemoryTest, CreateSignatureRecompressesAfterRectification)
-{
-	// Rectification replaces the raw image through setRGBDImage(), whose
-	// clearPreviousData argument defaults to true and drops the blob. Were that
-	// default to change, the buffer-identity check would not save us: the local
-	// image is read back out of the SensorData after rectification, so the
-	// pointers would match and the unrectified blob would be stored against
-	// rectified pixels.
-	const int size = 32;
-	const double f = 16.0, c = 16.0;
-	const cv::Mat K = (cv::Mat_<double>(3, 3) << f, 0.0, c, 0.0, f, c, 0.0, 0.0, 1.0);
-	const cv::Mat D = (cv::Mat_<double>(1, 4) << -0.3, 0.1, 0.001, -0.001);
-	const cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
-	const cv::Mat P = (cv::Mat_<double>(3, 4) << f, 0.0, c, 0.0, 0.0, f, c, 0.0, 0.0, 0.0, 1.0, 0.0);
-	const CameraModel model("rectifiable", cv::Size(size, size), K, D, R, P);
-	ASSERT_TRUE(model.isValidForRectification());
-
-	ParametersMap params = defaultMemoryParams();
-	params[Parameters::kMemBinDataKept()] = "true";
-	params[Parameters::kMemImagePostDecimation()] = "1";
-	params[Parameters::kRtabmapImagesAlreadyRectified()] = "false";
-	Memory memory(params);
-
-	const cv::Mat raw = texture(size, size);
-	const cv::Mat blob = compressImage2(raw, ".png");
-	SensorData data = dataWithRawAndCompressed(raw, blob, model);
-
-	const cv::Mat stored = storedBlob(memory, data);
-	ASSERT_FALSE(stored.empty()) << "no compressed image was kept";
-	EXPECT_FALSE(sameBytes(stored, blob))
-			<< "the unrectified blob was stored for a rectified signature";
-
-	const cv::Mat decoded = uncompressImage(stored);
-	ASSERT_EQ(decoded.size(), raw.size());
-	EXPECT_GT(cv::countNonZero(decoded != raw), 0)
-			<< "stored image still holds the unrectified pixels";
-}
-
-TEST(MemoryTest, CreateSignatureRecompressesAfterUpsideUpRotation)
-{
-	// Same setter, different caller: rotating the image upright also replaces it
-	// through setRGBDImage() and so drops the blob. Rectification is left on
-	// (already rectified) so only the rotation can account for the difference.
-	ParametersMap params = defaultMemoryParams();
-	params[Parameters::kMemBinDataKept()] = "true";
-	params[Parameters::kMemImagePostDecimation()] = "1";
-	params[Parameters::kMemRotateImagesUpsideUp()] = "true";
-	params[Parameters::kRtabmapImagesAlreadyRectified()] = "true";
-	Memory memory(params);
-
-	// 8 rows x 16 cols, with the camera rolled +pi/2: the upright correction is a
-	// 90 deg rotation, so the stored image must come back 16 rows x 8 cols. That
-	// swap is what makes a reused blob unmistakable here -- it would still decode
-	// at the original 8x16.
-	const cv::Mat raw = texture(8, 16);
-	const cv::Mat blob = compressImage2(raw, ".png");
-	const Transform rolled(0.0f, 0.0f, 0.0f, (float)M_PI / 2.0f, 0.0f, 0.0f);
-	const CameraModel model(10.0, 10.0, 8.0, 4.0,
-			rolled * CameraModel::opticalRotation(), 0.0, cv::Size(16, 8));
-
-	SensorData data = dataWithRawAndCompressed(raw, blob, model);
-
-	const cv::Mat stored = storedBlob(memory, data);
-	ASSERT_FALSE(stored.empty()) << "no compressed image was kept";
-	EXPECT_FALSE(sameBytes(stored, blob))
-			<< "the unrotated blob was stored for a rotated signature";
-
-	const cv::Mat decoded = uncompressImage(stored);
-	EXPECT_EQ(decoded.rows, raw.cols);
-	EXPECT_EQ(decoded.cols, raw.rows);
-}
-
-TEST(MemoryTest, CreateSignatureRecompressesStereoPairAfterRectification)
-{
-	// The stereo branch rectifies both images and hands them to setStereoImage(),
-	// which clears the left blob AND the right one. This is the only test that
-	// covers reuseCompressedDepth, since for a stereo pair the "depth" slot
-	// carries the right image.
-	const int size = 32;
-	const double f = 16.0, c = 16.0, baseline = 0.1;
-	const cv::Mat K = (cv::Mat_<double>(3, 3) << f, 0.0, c, 0.0, f, c, 0.0, 0.0, 1.0);
-	const cv::Mat D = (cv::Mat_<double>(1, 4) << -0.3, 0.1, 0.001, -0.001);
-	const cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
-	const cv::Mat Pleft = (cv::Mat_<double>(3, 4) <<
-			f, 0.0, c, baseline * f, 0.0, f, c, 0.0, 0.0, 0.0, 1.0, 0.0);
-	const cv::Mat Pright = (cv::Mat_<double>(3, 4) <<
-			f, 0.0, c, 0.0, 0.0, f, c, 0.0, 0.0, 0.0, 1.0, 0.0);
-	const cv::Mat T = (cv::Mat_<double>(3, 1) << -baseline, 0.0, 0.0);
-	const StereoCameraModel model("stereo",
-			CameraModel("left", cv::Size(size, size), K, D, R, Pleft),
-			CameraModel("right", cv::Size(size, size), K, D, R, Pright),
-			cv::Mat::eye(3, 3, CV_64FC1), T);
-	ASSERT_TRUE(model.isValidForRectification());
-
-	ParametersMap params = defaultMemoryParams();
-	params[Parameters::kMemBinDataKept()] = "true";
-	params[Parameters::kMemImagePostDecimation()] = "1";
-	params[Parameters::kRtabmapImagesAlreadyRectified()] = "false";
-	Memory memory(params);
-
-	const cv::Mat left = texture(size, size);
-	const cv::Mat right = texture(size, size);
-	const cv::Mat leftBlob = compressImage2(left, ".png");
-	const cv::Mat rightBlob = compressImage2(right, ".png");
-
-	SensorData data;
-	data.setStereoImage(leftBlob, rightBlob, std::vector<StereoCameraModel>{model});
-	data.setImageRaw(left);          // neither setter clears the blobs
-	data.setDepthOrRightRaw(right);
-	ASSERT_FALSE(data.imageCompressed().empty());
-	ASSERT_FALSE(data.depthOrRightCompressed().empty());
-
-	const SensorData stored = storedData(memory, data);
-	ASSERT_FALSE(stored.imageCompressed().empty()) << "no left image was kept";
-	ASSERT_FALSE(stored.depthOrRightCompressed().empty()) << "no right image was kept";
-
-	EXPECT_FALSE(sameBytes(stored.imageCompressed(), leftBlob))
-			<< "the unrectified left blob was stored for a rectified signature";
-	EXPECT_FALSE(sameBytes(stored.depthOrRightCompressed(), rightBlob))
-			<< "the unrectified right blob was stored for a rectified signature";
-
-	EXPECT_GT(cv::countNonZero(uncompressImage(stored.imageCompressed()) != left), 0)
-			<< "stored left image still holds the unrectified pixels";
-	EXPECT_GT(cv::countNonZero(uncompressImage(stored.depthOrRightCompressed()) != right), 0)
-			<< "stored right image still holds the unrectified pixels";
-}
-
-// ---------------------------------------------------------------------------
-// Mem/DepthCompressionFormat with inverse depth (".rvl:max:q"), which databases
-// older than 0.24 cannot hold: rtabmap 0.23 would still open them (e.g., created
-// with Db/TargetVersion=0.23.0) but could not decode their depth images.
-// ---------------------------------------------------------------------------
-
-namespace {
-
-enum DepthInput
-{
-	kRawDepth,
-	kCompressedDepthWithRaw,   // e.g., received from ROS and decoded
-	kCompressedDepthOnly       // raw depth not needed (no features extracted here)
-};
-
-struct InverseDepthCase
-{
-	const char * targetVersion;
-	DepthInput input;
-	const char * depthCompressionFormat;
-	bool parallelCompression;
-	const char * expectedFormat;
-};
-
-class MemoryInverseDepthTest : public ::testing::TestWithParam<InverseDepthCase> {};
-
-} // namespace
-
-TEST_P(MemoryInverseDepthTest, StoredDepthFormatFollowsDatabaseVersion)
-{
-	const InverseDepthCase & cs = GetParam();
-	ParametersMap params = defaultMemoryParams();
-	params[Parameters::kMemBinDataKept()] = "true";
-	params[Parameters::kMemDepthCompressionFormat()] = cs.depthCompressionFormat;
-	params[Parameters::kMemCompressionParallelized()] = cs.parallelCompression ? "true" : "false";
-	params[Parameters::kDbTargetVersion()] = cs.targetVersion;
-	Memory memory(params);
-	const std::string dbPath = uniqueDbPath();
-	ASSERT_TRUE(memory.init(dbPath, true, params));
-
-	const cv::Mat rgb(16, 16, CV_8UC3, cv::Scalar(10, 20, 30));
-	cv::Mat depth(16, 16, CV_32FC1);
-	cv::randu(depth, 0.5f, 8.0f);
-	const CameraModel model(10.0, 10.0, 8.0, 8.0, CameraModel::opticalRotation());
-	SensorData data;
-	if(cs.input == kRawDepth)
-	{
-		data = SensorData(rgb, depth, model);
-	}
-	else
-	{
-		data = SensorData(compressImage2(rgb, ".png"), compressImage2(depth, ".png:10:100"), model);
-		if(cs.input == kCompressedDepthWithRaw)
-		{
-			data.uncompressData();
-			ASSERT_FALSE(data.depthRaw().empty());
-		}
-	}
-
-	ASSERT_TRUE(memory.update(data, Transform(0, 0, 0, 0, 0, 0), cv::Mat::eye(6, 6, CV_64FC1) * 0.01));
-	const Signature * s = memory.getSignature(memory.getLastSignatureId());
-	ASSERT_NE(s, nullptr);
-	const cv::Mat & stored = s->sensorData().depthOrRightCompressed();
-	ASSERT_FALSE(stored.empty());
-	EXPECT_EQ(compressedDepthFormat(stored), cs.expectedFormat);
-
-	const cv::Mat restored = uncompressImage(stored);
-	ASSERT_EQ(restored.type(), CV_32FC1);
-	ASSERT_EQ(restored.size(), depth.size());
-	EXPECT_LT(cv::norm(restored, depth, cv::NORM_INF), 0.01);
-
-	memory.close(false);
-	UFile::erase(dbPath);
-}
-
-INSTANTIATE_TEST_SUITE_P(
-		DatabaseVersions,
-		MemoryInverseDepthTest,
-		::testing::Values(
-				InverseDepthCase{"", kRawDepth, ".rvl:10:100", true, ".rvl:10:100"},
-				InverseDepthCase{"", kRawDepth, ".rvl:10:100", false, ".rvl:10:100"},
-				InverseDepthCase{"", kCompressedDepthWithRaw, ".rvl:10:100", true, ".png:10:100"},  // reused as is
-				InverseDepthCase{"", kCompressedDepthOnly, ".rvl:10:100", true, ".png:10:100"},     // reused as is
-				InverseDepthCase{"0.23.0", kRawDepth, ".rvl:10:100", true, ".png"},                // legacy 32FC1 format
-				InverseDepthCase{"0.23.0", kCompressedDepthWithRaw, ".rvl:10:100", true, ".png"},  // re-compressed
-				InverseDepthCase{"0.23.0", kCompressedDepthOnly, ".rvl:10:100", true, ".png"},     // decompressed, re-compressed
-				InverseDepthCase{"", kRawDepth, ".rvl", true, ".png"},                             // RVL is 16UC1 only: legacy
-				InverseDepthCase{"", kRawDepth, ".jpg", true, ".png"}));                           // invalid: default ".rvl"
-
-// Compressed images that Memory rectifies (Rtabmap/ImagesAlreadyRectified=false) are
-// decoded for it, even when nothing else needs them (no feature extraction here): they
-// are stored rectified, not as received.
-TEST(MemoryTest, DecodesCompressedImagesToRectifyThem)
-{
-	for(bool alreadyRectified : {true, false})
-	{
-		SCOPED_TRACE(alreadyRectified ? "already rectified" : "rectified by Memory");
-		ParametersMap params = defaultMemoryParams();
-		params[Parameters::kMemBinDataKept()] = "true";
-		params[Parameters::kRtabmapImagesAlreadyRectified()] = alreadyRectified ? "true" : "false";
-		Memory memory(params);
-		ASSERT_TRUE(memory.init(""));
-
-		cv::Mat rgb(48, 64, CV_8UC3);
-		cv::randu(rgb, 0, 255);
-		const cv::Mat K = (cv::Mat_<double>(3, 3) << 50, 0, 32, 0, 50, 24, 0, 0, 1);
-		const cv::Mat D = (cv::Mat_<double>(1, 5) << -0.3, 0.1, 0, 0, 0);
-		const cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
-		const cv::Mat P = (cv::Mat_<double>(3, 4) << 50, 0, 32, 0, 0, 50, 24, 0, 0, 0, 1, 0);
-		const CameraModel model("cam", cv::Size(64, 48), K, D, R, P, CameraModel::opticalRotation());
-		ASSERT_TRUE(model.isValidForRectification());
-		const cv::Mat compressed = compressImage2(rgb, ".png");
-		SensorData data(compressed, cv::Mat(), model);
-
-		ASSERT_TRUE(memory.update(data, Transform(0, 0, 0, 0, 0, 0), cv::Mat::eye(6, 6, CV_64FC1) * 0.01));
-		const Signature * s = memory.getSignature(memory.getLastSignatureId());
-		ASSERT_NE(s, nullptr);
-		const cv::Mat & stored = s->sensorData().imageCompressed();
-		ASSERT_FALSE(stored.empty());
-		const bool sameBytes = stored.total() == compressed.total() &&
-				memcmp(stored.data, compressed.data, compressed.total()) == 0;
-		EXPECT_EQ(sameBytes, alreadyRectified);
-	}
-}

corelib/test/test_odometry.cpp

@@ -543,44 +543,3 @@ TEST_P(OdometryStrategyTest, IcpConvergesFromOffsetGuess)
 			<< guess.prettyPrint() << "); ICP did not converge";
 	expectPoseNear(pose, motion, 0.002f, 0.2f, "2D corner, offset guess");
 }
-
-// A sweep that arrives far short of the points it should have cannot be
-// registered against: Icp/CorrespondenceRatio is measured against a full sweep,
-// so even matching every point it does have would leave it under the ratio.
-// F2M refuses such a scan as the first keyframe rather than starting a map that
-// nothing can be matched to. maxPoints is what says how big a full sweep is, so
-// the check is only possible when a driver reported it.
-TEST(OdometryTest, RefusesAFirstScanTooSmallForTheCorrespondenceRatio)
-{
-	const ParametersMap parameters =
-			icpOdometryParameters(Odometry::kTypeF2M, /*force3DoF=*/false, /*guessMotion=*/false);
-	const LaserScan corner = makeCorner3D();   // 1200 points
-
-	// The same points three ways. First as a twentieth of the sweep they should be,
-	// which no registration could reach the 0.1 ratio against.
-	const LaserScan tooFewPoints(
-			corner.data(), 20*(int)corner.size(), /*maxRange=*/0.0f, corner.format());
-	std::unique_ptr<Odometry> refusing(Odometry::create(parameters));
-	ASSERT_TRUE(refusing.get() != 0);
-	SensorData refusingData = makeScanData(tooFewPoints, 1, 0.0);
-	EXPECT_TRUE(refusing->process(refusingData).isNull())
-			<< "a map was started on a scan no later one could be registered to";
-
-	// Then as everything a sweep has, which is what a full one looks like.
-	const LaserScan wholeSweep(
-			corner.data(), (int)corner.size(), /*maxRange=*/0.0f, corner.format());
-	std::unique_ptr<Odometry> accepting(Odometry::create(parameters));
-	ASSERT_TRUE(accepting.get() != 0);
-	SensorData acceptingData = makeScanData(wholeSweep, 1, 0.0);
-	EXPECT_FALSE(accepting->process(acceptingData).isNull())
-			<< "a full sweep was refused as the first keyframe";
-
-	// And last as makeCorner3D() builds it, with maxPoints left at 0: a driver that
-	// never said how big a sweep is, so there is no ratio to fall under and the check
-	// does not apply.
-	std::unique_ptr<Odometry> unchecked(Odometry::create(parameters));
-	ASSERT_TRUE(unchecked.get() != 0);
-	SensorData uncheckedData = makeScanData(corner, 1, 0.0);
-	EXPECT_FALSE(unchecked->process(uncheckedData).isNull())
-			<< "a scan of unknown sweep size was refused";
-}

corelib/test/test_optimizer_gtsam.cpp

@@ -1,285 +0,0 @@
-// Tests for the GTSAM pieces used by rtabmap::OptimizerGTSAM, independent of
-// the optimizer itself:
-//   - Vertigo switch variables (linear and sigmoid): scalar manifold traits,
-//     1x1 Local Jacobians, priors, and constructor/retract clamping.
-//   - Switchable between factors: residuals and Jacobians against the
-//     release's BetweenFactor, the switch derivative against finite
-//     differences, and linearization.
-//   - Gravity (attitude) factor API selected by CMake
-//     (RTABMAP_GTSAM_HAS_ATTITUDE_FACTOR_TEMPLATE).
-//
-// Background: Optimizer/Robust=true adds, for every loop closure, a switch
-// variable s_ij with a prior, and replaces the loop closure's BetweenFactor
-// by a switchable one whose residual is weighted by s_ij (linear switch) or
-// sigmoid(s_ij) (sigmoid switch). The optimizer can then turn off outlier
-// loop closures by driving their weight to 0. These factors live in
-// corelib/src/optimizer/vertigo/gtsam and depend on GTSAM internals (traits,
-// OptionalJacobian, PriorFactor), which changed across 4.0/4.2/4.3, hence the
-// focused checks below. They are meant to pass on every GTSAM version
-// rtabmap supports.
-
-#include <gtest/gtest.h>
-#include <gtsam/config.h>
-#include <gtsam/base/numericalDerivative.h>
-#include <gtsam/geometry/Pose2.h>
-#include <gtsam/geometry/Pose3.h>
-#include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/slam/PriorFactor.h>
-#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/nonlinear/Values.h>
-#include <gtsam/navigation/AttitudeFactor.h>
-
-#include "../src/optimizer/vertigo/gtsam/betweenFactorSwitchable.h"
-
-#include <cmath>
-#include <sstream>
-
-using namespace gtsam;
-
-namespace {
-
-// Compares matrices (or vectors) with the same tolerance everywhere. A size
-// mismatch is reported separately: a Jacobian of the wrong size is exactly the
-// kind of regression these tests target (e.g., the 3x3 Jacobians the switch
-// traits used to declare for a 1-D variable, which made JacobianFactor throw
-// InvalidMatrixBlock during linearization).
-::testing::AssertionResult near(const Matrix& actual, const Matrix& expected)
-{
-	if(actual.rows() != expected.rows() || actual.cols() != expected.cols())
-	{
-		return ::testing::AssertionFailure() << "dimensions " << actual.rows() << "x" << actual.cols()
-				<< " instead of " << expected.rows() << "x" << expected.cols();
-	}
-	if(!actual.allFinite() || (actual - expected).norm() >= 1e-6)
-	{
-		std::stringstream ss;
-		ss << "Actual:\n" << actual << "\nExpected:\n" << expected;
-		return ::testing::AssertionFailure() << ss.str();
-	}
-	return ::testing::AssertionSuccess();
-}
-
-// Checks that a switch variable behaves as a 1-D manifold for GTSAM:
-//  1. traits<Switch>::dimension is 1 (required by noiseModel::Unit::Create()
-//     and fixed-size code paths in GTSAM >= 4.3).
-//  2. localCoordinates(y) = y - x, with analytical Jacobians -1 (wrt x) and
-//     +1 (wrt y).
-//  3. A PriorFactor on the switch (what OptimizerGTSAM adds for every switch)
-//     gives a scalar residual x - prior and a 1x1 identity Jacobian, matching
-//     a numerical derivative, and can be linearized. Depending on the GTSAM
-//     version/configuration (GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR), the prior
-//     takes its Jacobian from traits<Switch>::Local(), so this is where
-//     incomplete traits show up.
-//  4. On GTSAM >= 4.3, the same with a prior built without a noise model,
-//     which goes through noiseModel::Unit::Create(value) and so needs the
-//     full manifold traits (dimension, structure_category, ManifoldType).
-template<class Switch>
-void checkSwitch(double value, double other)
-{
-	static_assert(traits<Switch>::dimension == 1, "scalar tangent");
-	const Switch x(value), y(other);
-
-	// Chart and its Jacobians
-	Matrix11 h1, h2;
-	EXPECT_TRUE(near(x.localCoordinates(y, h1, h2), Vector1(other - value)));
-	EXPECT_TRUE(near(h1, -Matrix11::Identity()));
-	EXPECT_TRUE(near(h2, Matrix11::Identity()));
-
-	// Prior with an explicit noise model, as created by OptimizerGTSAM
-	const PriorFactor<Switch> prior(1, y, noiseModel::Isotropic::Sigma(1, 1));
-	Matrix h;
-	EXPECT_TRUE(near(prior.evaluateError(x, h), Vector1(value - other)));
-	EXPECT_TRUE(near(h, Matrix11::Identity()));
-	EXPECT_TRUE(near(h, numericalDerivative11<Vector, Switch>(
-		[&prior](const Switch& s) { return prior.evaluateError(s); }, x)));
-	Values values;
-	values.insert(1, x);
-	EXPECT_TRUE(bool(prior.linearize(values))) << "explicit prior linearization";
-
-#if GTSAM_VERSION_NUMERIC >= 40300
-	// Prior with the default (unit) noise model. Default noise was added in
-	// 4.3; 4.2 requires the explicit model above.
-	const PriorFactor<Switch> unitPrior(1, y);
-	EXPECT_TRUE(near(unitPrior.evaluateError(x, h), Vector1(value - other)));
-	EXPECT_TRUE(near(h, Matrix11::Identity()));
-	EXPECT_TRUE(bool(unitPrior.linearize(values))) << "default prior linearization";
-#endif
-}
-
-// Checks BetweenFactorSwitchableLinear, the robust loop closure factor used
-// by OptimizerGTSAM: residual = s * BetweenFactor residual.
-//  - The pose Jacobians must be the regular BetweenFactor Jacobians scaled by
-//    the switch weight. They are compared against the BetweenFactor of the
-//    installed GTSAM rather than against numerical derivatives, because older
-//    GTSAM (without GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR) uses an approximate
-//    Local Jacobian for poses. When the exact one is enabled, they are also
-//    compared against numerical derivatives.
-//  - The switch Jacobian (d residual / d s = raw residual) is always compared
-//    against a numerical derivative.
-//  - The factor, with the two poses and the switch in a Values, must
-//    linearize (which checks that all Jacobian sizes agree with the residual).
-template<class Pose>
-void checkBetweenLinear(const Pose& first, const Pose& second, double switchValue)
-{
-	const vertigo::SwitchVariableLinear s(switchValue);
-	const auto model = noiseModel::Isotropic::Sigma(traits<Pose>::dimension, 1);
-	const vertigo::BetweenFactorSwitchableLinear<Pose> factor(1, 2, 3, Pose(), model);
-	Matrix h1, h2, h3;
-#if GTSAM_VERSION_NUMERIC >= 40300
-	const Vector error = factor.evaluateError(first, second, s, &h1, &h2, &h3);
-#else
-	const Vector error = factor.evaluateError(first, second, s, h1, h2, h3);
-#endif
-	// Same measurement without switch: its residual ratio gives the weight
-	// actually applied, which must also scale the pose Jacobians.
-	Matrix rawH1, rawH2;
-	const BetweenFactor<Pose> raw(1, 2, Pose(), model);
-	const Vector rawError = raw.evaluateError(first, second, rawH1, rawH2);
-	const double weight = error.norm() / rawError.norm();
-	EXPECT_TRUE(near(h1, rawH1 * weight));
-	EXPECT_TRUE(near(h2, rawH2 * weight));
-#ifdef GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR
-	EXPECT_TRUE(near(h1, numericalDerivative11<Vector, Pose>(
-		[&](const Pose& p) { return factor.evaluateError(p, second, s); }, first)));
-	EXPECT_TRUE(near(h2, numericalDerivative11<Vector, Pose>(
-		[&](const Pose& p) { return factor.evaluateError(first, p, s); }, second)));
-#endif
-	EXPECT_TRUE(near(h3, numericalDerivative11<Vector, vertigo::SwitchVariableLinear>(
-		[&](const vertigo::SwitchVariableLinear& x) { return factor.evaluateError(first, second, x); }, s)));
-	EXPECT_TRUE(error.allFinite());
-
-	Values values;
-	values.insert(1, first); values.insert(2, second); values.insert(3, s);
-	EXPECT_TRUE(bool(factor.linearize(values))) << "switchable linearization";
-}
-
-// Checks BetweenFactorSwitchableSigmoid: residual = w * BetweenFactor
-// residual, with w = sigmoid(s) = 1/(1+exp(-s)).
-//  - Residual and pose Jacobians must be the BetweenFactor ones scaled by w.
-//  - The switch Jacobian must be raw residual * dw/ds = raw * w*(1-w), both
-//    analytically and by central finite differences. This catches the bug
-//    where it returned the weighted residual (raw * w), missing the (1-w)
-//    factor.
-//  - The factor must linearize.
-template<class Pose>
-void checkBetweenSigmoid(const Pose& second, double switchValue)
-{
-	const Pose first;
-	const vertigo::SwitchVariableSigmoid s(switchValue);
-	const auto model = noiseModel::Isotropic::Sigma(traits<Pose>::dimension, 1);
-	const vertigo::BetweenFactorSwitchableSigmoid<Pose> factor(1, 2, 3, Pose(), model);
-	Matrix h1, h2, h3;
-#if GTSAM_VERSION_NUMERIC >= 40300
-	const Vector error = factor.evaluateError(first, second, s, &h1, &h2, &h3);
-#else
-	const Vector error = factor.evaluateError(first, second, s, h1, h2, h3);
-#endif
-	Matrix rawH1, rawH2;
-	const BetweenFactor<Pose> raw(1, 2, Pose(), model);
-	const Vector rawError = raw.evaluateError(first, second, rawH1, rawH2);
-	const double weight = 1.0 / (1.0 + std::exp(-switchValue));
-	EXPECT_TRUE(near(error, rawError * weight));
-	EXPECT_TRUE(near(h1, rawH1 * weight));
-	EXPECT_TRUE(near(h2, rawH2 * weight));
-	// Analytical: d(sigmoid)/ds = w*(1-w)
-	EXPECT_TRUE(near(h3, rawError * (weight * (1.0 - weight))));
-	// Numerical: central difference over the switch value. The switch is
-	// re-constructed rather than retracted, as retract() clamps it.
-	const double step = 1e-5;
-	const Vector plus = factor.evaluateError(first, second,
-		vertigo::SwitchVariableSigmoid(switchValue + step));
-	const Vector minus = factor.evaluateError(first, second,
-		vertigo::SwitchVariableSigmoid(switchValue - step));
-	EXPECT_TRUE(near(h3, (plus - minus) / (2.0 * step)));
-
-	Values values;
-	values.insert(1, first); values.insert(2, second); values.insert(3, s);
-	EXPECT_TRUE(bool(factor.linearize(values))) << "sigmoid linearization";
-}
-
-} // namespace
-
-// Linear switch (the one OptimizerGTSAM uses) as a 1-D GTSAM manifold.
-TEST(OptimizerGTSAM, SwitchVariableLinearManifold)
-{
-	checkSwitch<vertigo::SwitchVariableLinear>(0.4, 0.7);
-}
-
-// Sigmoid switch as a 1-D GTSAM manifold (negative values are valid for it).
-TEST(OptimizerGTSAM, SwitchVariableSigmoidManifold)
-{
-	checkSwitch<vertigo::SwitchVariableSigmoid>(-0.4, 0.7);
-}
-
-// Switch bounds: retract() (applied at each optimization step) projects the
-// linear switch to [0,1] and the sigmoid switch to [-10,10]. The constructor
-// doesn't clamp the linear switch, but does clamp the sigmoid one. Moving the
-// traits to internal::Manifold must not change this behavior.
-TEST(OptimizerGTSAM, SwitchVariableClamping)
-{
-	EXPECT_DOUBLE_EQ(vertigo::SwitchVariableLinear(0.4).retract(Vector1(2)).value(), 1.0);
-	EXPECT_DOUBLE_EQ(vertigo::SwitchVariableLinear(0.4).retract(Vector1(-2)).value(), 0.0);
-	EXPECT_DOUBLE_EQ(vertigo::SwitchVariableLinear(2).value(), 2.0);
-	EXPECT_DOUBLE_EQ(vertigo::SwitchVariableSigmoid(0).retract(Vector1(20)).value(), 10.0);
-	EXPECT_DOUBLE_EQ(vertigo::SwitchVariableSigmoid(0).retract(Vector1(-20)).value(), -10.0);
-	EXPECT_DOUBLE_EQ(vertigo::SwitchVariableSigmoid(20).value(), 10.0);
-	EXPECT_DOUBLE_EQ(vertigo::SwitchVariableSigmoid(-20).value(), -10.0);
-}
-
-// Robust loop closure factor with a linear switch, for 2D (Optimizer/Slam2D)
-// and 3D graphs, with a partially-on switch (0.4).
-TEST(OptimizerGTSAM, BetweenFactorSwitchableLinear)
-{
-	checkBetweenLinear(Pose2(), Pose2(1, 2, 0.2), 0.4);
-	checkBetweenLinear(Pose3(), Pose3(Rot3::RzRyRx(0.1, 0.2, 0.3), Point3(1, 2, 3)), 0.4);
-}
-
-// Robust loop closure factor with a sigmoid switch, for 2D and 3D graphs.
-// Switch values -2, 0 and 2 give weights ~0.12, 0.5 and ~0.88, staying away
-// from the [-10,10] clamps.
-TEST(OptimizerGTSAM, BetweenFactorSwitchableSigmoid)
-{
-	for(double s : {-2.0, 0.0, 2.0})
-	{
-		SCOPED_TRACE(s);
-		checkBetweenSigmoid(Pose2(1, 2, 0.2), s);
-		checkBetweenSigmoid(Pose3(Rot3::RzRyRx(0.1, 0.2, 0.3), Point3(1, 2, 3)), s);
-	}
-}
-
-// Gravity constraints (Link::kGravity, Optimizer/GravitySigma > 0) are added
-// as a Pose3 attitude factor. Its class changed name in GTSAM 4.3
-// (Pose3AttitudeFactor -> AttitudeFactor<Pose3>), and some ROS 4.3 snapshots
-// report the same version number with either API, so CMake detects which one
-// compiles. This checks the detected API is usable as OptimizerGTSAM uses it:
-//  - Jacobian matches a numerical derivative on a tilted pose.
-//  - Residual is zero when the pose is aligned with gravity.
-//  - Optimizing a tilted pose (with a loose prior to fix the yaw and the
-//    translation, which gravity doesn't observe) removes the tilt.
-TEST(OptimizerGTSAM, GravityFactor)
-{
-#ifdef RTABMAP_GTSAM_HAS_ATTITUDE_FACTOR_TEMPLATE
-	using GravityFactor = AttitudeFactor<Pose3>;
-#else
-	using GravityFactor = Pose3AttitudeFactor;
-#endif
-	// Reference direction: world z axis; measured in body frame: also z (pose
-	// should be level).
-	const GravityFactor gravity(1, Unit3(0,0,1), noiseModel::Isotropic::Sigma(2, 0.1));
-	const Pose3 initial(Rot3::RzRyRx(0.1, -0.2, 0.3), Point3(1, 2, 3));
-	Matrix h;
-	const Vector error = gravity.evaluateError(initial, h);
-	EXPECT_TRUE(near(h, numericalDerivative11<Vector, Pose3>(
-		[&](const Pose3& p) { return gravity.evaluateError(p); }, initial)));
-	EXPECT_TRUE(near(gravity.evaluateError(Pose3()), Vector2::Zero()));
-
-	NonlinearFactorGraph graph;
-	graph.add(gravity);
-	graph.add(PriorFactor<Pose3>(1, Pose3(), noiseModel::Isotropic::Sigma(6, 1)));
-	Values values;
-	values.insert(1, initial);
-	const Values result = LevenbergMarquardtOptimizer(graph, values).optimize();
-	EXPECT_LT(gravity.evaluateError(result.at<Pose3>(1)).norm(), error.norm()*0.01)
-		<< "gravity optimization reduces tilt";
-}

corelib/test/test_rtabmap.cpp

@@ -1,6 +1,5 @@
 #include <gtest/gtest.h>
 #include <rtabmap/core/Rtabmap.h>
-#include <rtabmap/core/GlobalDescriptor.h>
 #include <rtabmap/core/GPS.h>
 #include <rtabmap/core/Landmark.h>
 #include <rtabmap/core/Memory.h>
@@ -1338,49 +1337,6 @@ TEST(RtabmapTest, GetSignatureCopyReturnsRequestedPayloads)
 	rtabmap.close(false);
 }
 
-TEST(RtabmapTest, GetSignatureCopyReturnsOnlyTheSavedGridWhenAskedAlone)
-{
-	// With a database on disk, process() saves each new node right away, which drops its
-	// compressed data from memory but keeps its global descriptors and its raw grid. A
-	// copy asking for the grid alone must still return the compressed grid, and must not
-	// return global descriptors that were not asked for.
-	const std::string dbPath = uniqueDbPath();
-	ParametersMap params = defaultRtabmapParams();
-	params[Parameters::kMemBinDataKept()] = "true";
-	params[Parameters::kRGBDCreateOccupancyGrid()] = "true";
-	Rtabmap rtabmap;
-	rtabmap.init(params, dbPath);
-
-	SensorData data(cv::Mat(8, 8, CV_8UC1, cv::Scalar(128)));
-	data.setId(1);
-	cv::Mat obstacles(1, 3, CV_32FC3);
-	for(int i = 0; i < 3; ++i)
-	{
-		obstacles.at<cv::Vec3f>(0, i) = cv::Vec3f(float(i) * 0.1f, 0.0f, 0.0f);
-	}
-	data.setOccupancyGrid(cv::Mat(), obstacles, cv::Mat(), 0.05f, cv::Point3f(0, 0, 0));
-	data.setGlobalDescriptors(std::vector<GlobalDescriptor>(1, GlobalDescriptor(1, cv::Mat::ones(1, 8, CV_32FC1))));
-	ASSERT_TRUE(rtabmap.process(data, Transform(0, 0, 0, 0, 0, 0), cv::Mat::eye(6, 6, CV_64FC1) * 0.01));
-	const int id = rtabmap.getLastLocationId();
-	ASSERT_NE(rtabmap.getMemory()->getSignature(id), nullptr);
-	ASSERT_TRUE(rtabmap.getMemory()->getSignature(id)->isSaved());
-
-	const Signature grid = rtabmap.getSignatureCopy(id, /*images=*/false,
-			/*scan=*/false, /*userData=*/false, /*occupancyGrid=*/true,
-			/*withWords=*/false, /*withGlobalDescriptors=*/false);
-	EXPECT_FALSE(grid.sensorData().gridObstacleCellsCompressed().empty());
-	EXPECT_FLOAT_EQ(grid.sensorData().gridCellSize(), 0.05f);
-	EXPECT_TRUE(grid.sensorData().globalDescriptors().empty());
-
-	const Signature descriptors = rtabmap.getSignatureCopy(id, /*images=*/false,
-			/*scan=*/false, /*userData=*/false, /*occupancyGrid=*/true,
-			/*withWords=*/false, /*withGlobalDescriptors=*/true);
-	EXPECT_EQ(descriptors.sensorData().globalDescriptors().size(), 1u);
-
-	rtabmap.close(false);
-	UFile::erase(dbPath);
-}
-
 TEST(RtabmapTest, GetSignatureCopyOmitsImageWhenNotRequested)
 {
 	ParametersMap params = defaultRtabmapParams();

corelib/test/test_rtabmap_integration.cpp

@@ -2957,9 +2957,8 @@ TEST_F(RtabmapIntegrationFixture, AppearanceOnly_PrecisionRecall)
 		const bool xfeatures2dDescriptor = freakOrBriefDescriptor || daisyDescriptor;
 
 		const bool kazeDescriptor = detectorType == Feature2D::kFeatureKaze;
-		//We saw FAST+FREAK sat at 0.84375 on a macOS CI run with 0.85.
 		const float kMinPrecision = tfIdfUsed ? 0.70f :
-				(looseFloors || kazeDescriptor ? 0.80f : 0.9f);
+				(looseFloors || kazeDescriptor ? 0.85f : 0.9f);
 		const float kMinRecall    = xfeatures2dDescriptor ? 0.5f :
 				(looseFloors ? 0.7f : 0.85f);
 		EXPECT_GE(acceptedPrec, kMinPrecision)

corelib/test/test_sensordata.cpp

@@ -1,6 +1,5 @@
 #include <gtest/gtest.h>
 #include <rtabmap/core/SensorData.h>
-#include <rtabmap/core/Compression.h>
 #include <rtabmap/core/CameraModel.h>
 #include <rtabmap/core/StereoCameraModel.h>
 #include <rtabmap/core/LaserScan.h>
@@ -187,43 +186,10 @@ TEST(SensorDataTest, IsValidWithId)
 
 TEST(SensorDataTest, IsValidWithStamp)
 {
-    // A stamp alone doesn't make the data valid
     SensorData data;
     EXPECT_FALSE(data.isValid());
-
+    
     data.setStamp(12345.0);
-    EXPECT_FALSE(data.isValid());
-}
-
-TEST(SensorDataTest, IsValidWithCameraModel)
-{
-    SensorData data;
-    data.setRGBDImage(cv::Mat(), cv::Mat(), CameraModel());
-    EXPECT_TRUE(data.cameraModels().empty()); // invalid without image: placeholder not kept
-    EXPECT_FALSE(data.isValid()); // not valid for projection
-
-    data.setRGBDImage(cv::Mat(), cv::Mat(), CameraModel(525.0, 525.0, 320.0, 240.0));
-    EXPECT_TRUE(data.isValid());
-
-    // At least one valid model among multiple cameras
-    std::vector<CameraModel> models;
-    models.push_back(CameraModel());
-    models.push_back(CameraModel(525.0, 525.0, 320.0, 240.0));
-    data.setRGBDImage(cv::Mat(), cv::Mat(), models);
-    EXPECT_TRUE(data.isValid());
-}
-
-TEST(SensorDataTest, IsValidWithStereoCameraModel)
-{
-    SensorData data;
-    data.setStereoImage(cv::Mat(), cv::Mat(), StereoCameraModel());
-    EXPECT_FALSE(data.stereoCameraModels().empty());
-    EXPECT_FALSE(data.isValid()); // not valid for projection
-
-    data.setStereoImage(cv::Mat(), cv::Mat(), StereoCameraModel(525.0, 525.0, 320.0, 240.0, 0.0));
-    EXPECT_FALSE(data.isValid()); // null baseline
-
-    data.setStereoImage(cv::Mat(), cv::Mat(), StereoCameraModel(525.0, 525.0, 320.0, 240.0, 0.12));
     EXPECT_TRUE(data.isValid());
 }
 
@@ -629,43 +595,6 @@ TEST(SensorDataTest, SetUserData)
     EXPECT_EQ(data.userDataRaw().cols, 100);
 }
 
-TEST(SensorDataTest, SetUserDataCompressesRawData)
-{
-    SensorData data;
-    const cv::Mat userData = (cv::Mat_<float>(1, 4) << 1.0f, 2.0f, 3.0f, 4.0f);
-
-    data.setUserData(userData);
-
-    ASSERT_FALSE(data.userDataCompressed().empty());
-    EXPECT_EQ(0.0, cv::norm(uncompressData(data.userDataCompressed()), userData, cv::NORM_INF));
-}
-
-// Without clearing, the raw data of compressed user data already set is added to it:
-// the compressed copy is kept rather than compressed again, as setLaserScan() and
-// setRGBDImage() do. With nothing compressed yet, the raw data is still compressed.
-TEST(SensorDataTest, SetUserDataWithoutClearingKeepsTheCompressedCopy)
-{
-    const cv::Mat userData = (cv::Mat_<float>(1, 4) << 1.0f, 2.0f, 3.0f, 4.0f);
-    const cv::Mat compressed = compressData2(userData);
-
-    SensorData data;
-    data.setUserData(compressed);
-    ASSERT_TRUE(data.userDataRaw().empty());
-    data.setUserData(userData, false);
-    EXPECT_EQ(data.userDataRaw().data, userData.data);
-    EXPECT_EQ(data.userDataCompressed().data, compressed.data);
-
-    SensorData fresh;
-    fresh.setUserData(userData, false);
-    ASSERT_FALSE(fresh.userDataCompressed().empty());
-    EXPECT_EQ(0.0, cv::norm(uncompressData(fresh.userDataCompressed()), userData, cv::NORM_INF));
-
-    // Clearing, the default, compresses the new data again.
-    data.setUserData(userData);
-    EXPECT_NE(data.userDataCompressed().data, compressed.data);
-    EXPECT_EQ(0.0, cv::norm(uncompressData(data.userDataCompressed()), userData, cv::NORM_INF));
-}
-
 // Occupancy Grid Tests
 
 TEST(SensorDataTest, SetOccupancyGrid)
@@ -986,32 +915,3 @@ TEST(SensorDataTest, DifferentDepthTypes)
     EXPECT_EQ(data.depthOrRightRaw().type(), CV_32FC1);
 }
 
-
-// An invalid CameraModel without any image is a placeholder (e.g., lidar odometry
-// creating scan-only data with CameraModel()): it is not kept. With an image, it is kept,
-// as images can be used without calibration.
-TEST(SensorDataTest, InvalidCameraModelIsKeptOnlyWithImages)
-{
-	const LaserScan scan(cv::Mat(1, 3, CV_32FC2, cv::Scalar(1.0f, 0.0f)), 0, 10.0f, LaserScan::kXY);
-	const SensorData scanOnly(scan, cv::Mat(), cv::Mat(), CameraModel(), 1, 1.0);
-	EXPECT_TRUE(scanOnly.cameraModels().empty());
-	EXPECT_TRUE(scanOnly.isValid());
-
-	const cv::Mat image(4, 6, CV_8UC1, cv::Scalar(1));
-	const SensorData uncalibrated(image, CameraModel(), 1, 1.0);
-	EXPECT_EQ(uncalibrated.cameraModels().size(), 1u);
-
-	const SensorData compressedOnly(compressImage2(image, ".png"), CameraModel(), 1, 1.0);
-	EXPECT_EQ(compressedOnly.cameraModels().size(), 1u);
-
-	const CameraModel valid(10.0, 10.0, 3.0, 2.0);
-	const SensorData calibratedNoImage(scan, cv::Mat(), cv::Mat(), valid, 1, 1.0);
-	EXPECT_EQ(calibratedNoImage.cameraModels().size(), 1u) << "valid models are always kept";
-
-	// Keeping the images already there: they still need their model
-	SensorData data(image, CameraModel(), 1, 1.0);
-	data.setRGBDImage(cv::Mat(), cv::Mat(), CameraModel(), false);
-	EXPECT_EQ(data.cameraModels().size(), 1u);
-	data.setRGBDImage(cv::Mat(), cv::Mat(), CameraModel(), true);
-	EXPECT_TRUE(data.cameraModels().empty()) << "images cleared, nothing left to describe";
-}

corelib/test/test_stereocameramodel.cpp

@@ -7,7 +7,6 @@
 #include "rtabmap/utilite/UDirectory.h"
 #include "rtabmap/utilite/UFile.h"
 #include <cmath>
-#include <limits>
 
 using namespace rtabmap;
 
@@ -237,94 +236,6 @@ TEST_F(StereoCameraModelTest, ComputeDisparityZeroDepth)
     EXPECT_EQ(disparityMM, 0.0f);
 }
 
-// Reprojection Tests
-
-TEST_F(StereoCameraModelTest, Reproject)
-{
-    StereoCameraModel model(fx_, fy_, cx_, cy_, baseline_);
-
-    // a point 2 m in front of the left camera, off its optical axis
-    float x = 0.3f, y = -0.2f, z = 2.0f;
-
-    float uLeft, vLeft, uRight, vRight;
-    model.reproject(x, y, z, uLeft, vLeft, uRight, vRight);
-
-    // the left camera has no Tx, so its image point is the one of the left model alone
-    float u, v;
-    model.left().reproject(x, y, z, u, v);
-    EXPECT_DOUBLE_EQ(model.left().Tx(), 0.0);
-    EXPECT_FLOAT_EQ(uLeft, u);
-    EXPECT_FLOAT_EQ(vLeft, v);
-    EXPECT_FLOAT_EQ(uLeft, static_cast<float>(fx_*x/z + cx_));
-    EXPECT_FLOAT_EQ(vLeft, static_cast<float>(fy_*y/z + cy_));
-
-    // rectified pair: same row in both images, right point shifted by the disparity
-    EXPECT_FLOAT_EQ(vRight, vLeft);
-    EXPECT_NEAR(uLeft - uRight, model.computeDisparity(z), 0.001f);
-    EXPECT_NEAR(uLeft - uRight, static_cast<float>(baseline_*fx_/z), 0.001f);
-}
-
-TEST_F(StereoCameraModelTest, ReprojectDisparityDecreasesWithDepth)
-{
-    StereoCameraModel model(fx_, fy_, cx_, cy_, baseline_);
-
-    float previousDisparity = std::numeric_limits<float>::max();
-    for(float z=1.0f; z<=10.0f; z+=1.0f)
-    {
-        float uLeft, vLeft, uRight, vRight;
-        model.reproject(0.0f, 0.0f, z, uLeft, vLeft, uRight, vRight);
-
-        // on the optical axis, the left point is the principal point
-        EXPECT_FLOAT_EQ(uLeft, static_cast<float>(cx_));
-        EXPECT_FLOAT_EQ(vLeft, static_cast<float>(cy_));
-
-        float disparity = uLeft - uRight;
-        EXPECT_GT(disparity, 0.0f); // right camera on the right of the left one
-        EXPECT_LT(disparity, previousDisparity);
-        EXPECT_NEAR(model.computeDepth(disparity), z, 0.001f);
-        previousDisparity = disparity;
-    }
-}
-
-TEST_F(StereoCameraModelTest, ReprojectInt)
-{
-    StereoCameraModel model(fx_, fy_, cx_, cy_, baseline_);
-
-    float x = 0.3f, y = -0.2f, z = 2.0f;
-
-    float uLeftF, vLeftF, uRightF, vRightF;
-    model.reproject(x, y, z, uLeftF, vLeftF, uRightF, vRightF);
-
-    int uLeft, vLeft, uRight, vRight;
-    model.reproject(x, y, z, uLeft, vLeft, uRight, vRight);
-
-    EXPECT_EQ(uLeft, static_cast<int>(uLeftF));
-    EXPECT_EQ(vLeft, static_cast<int>(vLeftF));
-    EXPECT_EQ(uRight, static_cast<int>(uRightF));
-    EXPECT_EQ(vRight, static_cast<int>(vRightF));
-}
-
-TEST_F(StereoCameraModelTest, ReprojectProjectRoundTrip)
-{
-    StereoCameraModel model(fx_, fy_, cx_, cy_, baseline_);
-
-    float x = -0.45f, y = 0.25f, z = 3.7f;
-
-    float uLeft, vLeft, uRight, vRight;
-    model.reproject(x, y, z, uLeft, vLeft, uRight, vRight);
-
-    // the disparity of the reprojected pair gives the depth back...
-    float depth = model.computeDepth(uLeft - uRight);
-    EXPECT_NEAR(depth, z, 0.001f);
-
-    // ... and the left image point gives the 3D point back
-    float x2, y2, z2;
-    model.left().project(uLeft, vLeft, depth, x2, y2, z2);
-    EXPECT_NEAR(x2, x, 0.001f);
-    EXPECT_NEAR(y2, y, 0.001f);
-    EXPECT_NEAR(z2, z, 0.001f);
-}
-
 // Getter Tests
 
 TEST_F(StereoCameraModelTest, Baseline)

corelib/test/test_util2d.cpp

@@ -508,31 +508,6 @@ TEST(Util2dTest, GetDepthEstimationFromNeighbors16U) {
     EXPECT_NEAR(result, 1.5f, 1e-3f);
 }
 
-TEST(Util2dTest, GetDepthEstimationFromNeighborsRejectsOutlier) {
-    // Neighbors are visited as (2,1), (1,2), (3,2), (2,3). The last one is
-    // 25% away from the mean of the first three and must be ignored.
-    cv::Mat depth = cv::Mat::zeros(5, 5, CV_32FC1);
-    depth.at<float>(2, 1) = 1.00f;
-    depth.at<float>(1, 2) = 1.02f;
-    depth.at<float>(3, 2) = 0.98f;
-    depth.at<float>(2, 3) = 1.25f;
-
-    float result = util2d::getDepth(depth, 2.0f, 2.0f, false, 0.1f, true);
-    EXPECT_NEAR(result, 1.0f, 1e-5f);
-
-    cv::Mat depth16U = util2d::cvtDepthFromFloat(depth);
-    result = util2d::getDepth(depth16U, 2.0f, 2.0f, false, 0.1f, true);
-    EXPECT_NEAR(result, 1.0f, 1e-3f);
-
-    // Same with the default ratio (0.02): the last neighbor is 5% away.
-    depth.at<float>(2, 1) = 1.00f;
-    depth.at<float>(1, 2) = 1.01f;
-    depth.at<float>(3, 2) = 0.99f;
-    depth.at<float>(2, 3) = 1.05f;
-    result = util2d::getDepth(depth, 2.0f, 2.0f, false, 0.02f, true);
-    EXPECT_NEAR(result, 1.0f, 1e-5f);
-}
-
 TEST(Util2dTest, GetDepthOutOfBounds) {
     cv::Mat depth = cv::Mat::ones(5, 5, CV_32FC1);
 

corelib/test/test_util3d.cpp

@@ -1024,47 +1024,6 @@ TEST(Util3dTest, LaserScanFromPointCloudXYZINormal) {
     EXPECT_FLOAT_EQ(pt.normal_z, 1.0f);
 }
 
-// laserScanToPointCloud2() and laserScanFromPointCloud() are each other's inverse, in
-// every format: the per-point time and ring included, which no PCL point type holds, and
-// 2D scans, which come back 2D when is2D is set.
-TEST(Util3dTest, LaserScanPointCloud2RoundTripEveryFormat) {
-    for(int f = LaserScan::kXY; f <= LaserScan::kXYZIRT; ++f)
-    {
-        const LaserScan::Format format = (LaserScan::Format)f;
-        SCOPED_TRACE(LaserScan::formatName(format));
-        const int channels = LaserScan::channels(format);
-
-        // Small integers: exact through float and through the ring's UINT16 field.
-        cv::Mat points(1, 3, CV_32FC(channels));
-        for(int i = 0; i < points.cols; ++i)
-        {
-            float * p = points.ptr<float>(0, i);
-            for(int c = 0; c < channels; ++c)
-            {
-                p[c] = float(1 + i + c);
-            }
-        }
-        const LaserScan scan(points, 360, 10.0f, format);
-        if(scan.hasRGB())
-        {
-            for(int i = 0; i < points.cols; ++i)
-            {
-                const uint32_t rgb = 0x00102030u + i; // packed 0x00RRGGBB, as PCL stores it
-                memcpy(points.ptr<float>(0, i) + scan.getRGBOffset(), &rgb, sizeof(float));
-            }
-        }
-
-        pcl::PCLPointCloud2::Ptr cloud = util3d::laserScanToPointCloud2(scan);
-        ASSERT_EQ(cloud->width * cloud->height, (unsigned int)points.cols);
-        const LaserScan out = util3d::laserScanFromPointCloud(*cloud, true, scan.is2d());
-
-        EXPECT_EQ(out.format(), format);
-        ASSERT_EQ(out.data().size(), scan.data().size());
-        ASSERT_EQ(out.data().type(), scan.data().type());
-        EXPECT_EQ(0, memcmp(out.data().data, scan.data().data, scan.data().total() * scan.data().elemSize()));
-    }
-}
-
 TEST(Util3dTest, LaserScan2dFromPointCloudXYZ) {
     pcl::PointCloud<pcl::PointXYZ> cloud;
     cloud.push_back(pcl::PointXYZ(1.0f, 2.0f, 3.0f));

corelib/test/test_util3d_correspondences.cpp

@@ -2,7 +2,6 @@
 #include "rtabmap/core/util3d.h"
 #include "rtabmap/core/util3d_correspondences.h"
 #include "rtabmap/core/CameraModel.h"
-#include "rtabmap/core/StereoCameraModel.h"
 #include "rtabmap/utilite/UException.h"
 #include "rtabmap/utilite/UConversion.h"
 #include <pcl/io/pcd_io.h>
@@ -83,93 +82,44 @@ TEST(Util3dCorrespondencesTest, ExtractXYZCorrespondencesNoCommonIDs) {
     EXPECT_TRUE(cloud2.empty());
 }
 
-// Reprojects a fixed non-planar 3D scene in both images of a rectified stereo
-// camera. The two-view geometry must be generic: with a planar scene or a pure
-// image translation, all correspondences are related by a homography and the
-// fundamental matrix is then only defined up to a 1-parameter family
-// (F = [e']x * H for any epipole e'). RANSAC can pick a member of that family
-// which also fits an outlier, making the inlier count depend on floating-point
-// details of the platform and of the OpenCV version. Here the points span a
-// range of depths, so their disparities differ and the geometry is well
-// constrained.
-static void reprojectStereoPair(int index, pcl::PointXYZ & left, pcl::PointXYZ & right)
-{
-	static const float points3d[12][3] = {
-		{-0.50f, -0.40f, 2.0f}, { 0.40f, -0.30f, 3.5f}, {-0.20f,  0.50f, 2.8f},
-		{ 0.60f,  0.20f, 5.0f}, {-0.60f,  0.10f, 4.2f}, { 0.10f, -0.50f, 6.5f},
-		{ 0.30f,  0.45f, 3.0f}, {-0.35f, -0.15f, 7.5f}, { 0.50f, -0.05f, 2.2f},
-		{-0.10f,  0.30f, 5.8f}, { 0.25f,  0.35f, 4.6f}, {-0.45f,  0.20f, 3.3f}};
-
-	static const StereoCameraModel model(500.0, 500.0, 320.0, 240.0, 0.12);
-
-	float uLeft, vLeft, uRight, vRight;
-	model.reproject(points3d[index][0], points3d[index][1], points3d[index][2],
-			uLeft, vLeft, uRight, vRight);
-
-	// extractXYZCorrespondencesRANSAC() only uses x and y, as image coordinates
-	left = pcl::PointXYZ(uLeft, vLeft, 0.0f);
-	right = pcl::PointXYZ(uRight, vRight, 0.0f);
-}
-
 TEST(Util3dCorrespondencesTest, ExtractXYZCorrespondencesRANSACAcceptsCleanMatches) {
 	std::multimap<int, pcl::PointXYZ> words1;
 	std::multimap<int, pcl::PointXYZ> words2;
 
-	// 12 consistent matches
-	for (int i = 0; i < 12; ++i) {
-		pcl::PointXYZ left, right;
-		reprojectStereoPair(i, left, right);
-		words1.insert({i, left});
-		words2.insert({i, right});
+	// 10 consistent matches
+	for (int i = 0; i < 10; ++i) {
+		words1.insert({i, pcl::PointXYZ(i * 1.0f, exp2(i)/10.0f, 0.0f)});
+		words2.insert({i, pcl::PointXYZ(i * 1.0f + 1.1f, exp2(i)/10.0f + 1.1f, 0.0f)});  // Slight noise
 	}
 
 	pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
 	util3d::extractXYZCorrespondencesRANSAC(words1, words2, cloud1, cloud2);
 
 	EXPECT_EQ(cloud1.size(), cloud2.size());
-	EXPECT_EQ(cloud1.size(), 12); // every match is on its epipolar line
+	EXPECT_GE(cloud1.size(), 8); // At least 8 inliers from 10 consistent matches
 }
 
 TEST(Util3dCorrespondencesTest, ExtractXYZCorrespondencesRANSACRejectsOutliers) {
 	std::multimap<int, pcl::PointXYZ> words1;
 	std::multimap<int, pcl::PointXYZ> words2;
 
-	// 12 inliers
-	for (int i = 0; i < 12; ++i) {
-		pcl::PointXYZ left, right;
-		reprojectStereoPair(i, left, right);
-		words1.insert({i, left});
-		words2.insert({i, right});
+	// 8 inliers
+	for (int i = 0; i < 8; ++i) {
+		words1.insert({i, pcl::PointXYZ(i * 1.0f, exp2(i)/10.0f, 0.0f)});
+		words2.insert({i, pcl::PointXYZ(i * 1.0f + 1.1f, exp2(i)/10.0f + 1.1f, 0.0f)});  // Slight noise
 	}
 
-	// 3 outliers: correct point in the left image, right point moved far away from
-	// the corresponding epipolar line (horizontal on a rectified stereo camera)
-	const int outlierSources[3] = {0, 4, 8};
-	const float outlierOffsets[3][2] = {{0.0f, 120.0f}, {0.0f, -150.0f}, {40.0f, 90.0f}};
-	for (int i = 0; i < 3; ++i) {
-		pcl::PointXYZ left, right;
-		reprojectStereoPair(outlierSources[i], left, right);
-		right.x += outlierOffsets[i][0];
-		right.y += outlierOffsets[i][1];
-		words1.insert({100+i, left});
-		words2.insert({100+i, right});
-	}
+	// 2 outliers
+	words1.insert({100, pcl::PointXYZ(0.0f, 0.0f, 0.0f)});
+	words2.insert({100, pcl::PointXYZ(100.0f, 100.0f, 0.0f)});
+	words1.insert({101, pcl::PointXYZ(1.0f, 1.0f, 0.0f)});
+	words2.insert({101, pcl::PointXYZ(200.0f, -50.0f, 0.0f)});
 
 	pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
 	util3d::extractXYZCorrespondencesRANSAC(words1, words2, cloud1, cloud2);
 
 	EXPECT_EQ(cloud1.size(), cloud2.size());
-	EXPECT_EQ(cloud1.size(), 12);  // RANSAC should reject the 3 outliers
-
-	// none of the outliers should have survived
-	for (unsigned int i = 0; i < cloud2.size(); ++i) {
-		for (int j = 0; j < 3; ++j) {
-			pcl::PointXYZ left, right;
-			reprojectStereoPair(outlierSources[j], left, right);
-			EXPECT_FALSE(cloud2[i].x == right.x + outlierOffsets[j][0] &&
-						 cloud2[i].y == right.y + outlierOffsets[j][1]);
-		}
-	}
+	EXPECT_EQ(cloud1.size(), 8);  // RANSAC should reject 2 outliers
 }
 
 TEST(Util3dCorrespondencesTest, ExtractXYZCorrespondencesRANSACFailsGracefullyOnTooFewMatches) {

docker/README.md

@@ -1,36 +1,3 @@
 ### Docker
 
 * Go to the [wiki](https://github.com/introlab/rtabmap/wiki/Installation#docker) for usage examples and how to build locally the images.
-
-#### Tags
-
-All images are published to [introlab3it/rtabmap](https://hub.docker.com/r/introlab3it/rtabmap) as
-multi-arch manifests (`linux/amd64` and `linux/arm64`).
-
-| Tag | Alias | Base | ROS |
-| --- | --- | --- | --- |
-| `resolute` | `26.04`, `latest` | Ubuntu 26.04 | ROS2 Lyrical |
-| `noble-kilted` | | Ubuntu 24.04 | ROS2 Kilted |
-| `noble` | `24.04` | Ubuntu 24.04 | ROS2 Jazzy |
-| `jammy` | `22.04` | Ubuntu 22.04 | ROS2 Humble |
-| `focal` | `20.04` | Ubuntu 20.04 | ROS1 Noetic |
-
-Each image is built on top of a matching `<tag>-deps` image holding the third-party
-dependencies, so that a source change only rebuilds the top layer.
-
-> [!IMPORTANT]
-> **`latest` now points to the newest ROS2 image** (`resolute`), where it used to point
-> to `focal` (ROS1 Noetic). Pulling `introlab3it/rtabmap` without a tag therefore gets you
-> a different ROS version than before. Use `introlab3it/rtabmap:focal` to stay on ROS1, or
-> pin an explicit tag in general rather than relying on `latest`.
-
-`bionic` / `18.04` (ROS1 Melodic) is no longer built; the last published image stays on
-Docker Hub but will not be updated. Its Dockerfile is kept in [bionic/](bionic) for reference.
-
-The `<tag>-amd64` / `<tag>-arm64` tags are per-architecture build outputs that CI joins into
-the manifests above (see [.github/workflows/docker-ros.yml](../.github/workflows/docker-ros.yml));
-use the plain tags instead.
-
-Android build environments (`android23`, `android24`, `android26`, `android30`, `tango`) are
-`linux/amd64` only and are built by
-[.github/workflows/android.yml](../.github/workflows/android.yml).

docker/focal-foxy/deps/Dockerfile

@@ -165,10 +165,6 @@ RUN git clone --branch 4.2.0 https://github.com/opencv/opencv.git && \
     cd ../.. && \
     rm -rf opencv opencv_contrib
 
-# ld.so.conf.d is read in filename order: <arch>-linux-gnu.conf sorts before
-# libc.conf (/usr/local/lib) on arm64, so source builds lose to the distro ones.
-RUN echo /usr/local/lib > /etc/ld.so.conf.d/00-usr-local.conf && ldconfig
-
 RUN rm /bin/sh && ln -s /bin/bash /bin/sh
 
 COPY ./docker/focal-foxy/deps/ros_entrypoint.sh /ros_entrypoint.sh

docker/focal/Dockerfile

@@ -8,15 +8,11 @@ RUN mkdir -p /root/Documents/RTAB-Map && chmod 777 /root/Documents/RTAB-Map
 # Copy current source code
 COPY . /root/rtabmap
 
-ARG RUN_TESTS=0
-
 # Build RTAB-Map project
 RUN source /ros_entrypoint.sh && \
     cd rtabmap/build && \
-    cmake -DWITH_ALICE_VISION=ON -DWITH_OPENGV=ON -DBUILD_TESTING=$RUN_TESTS .. && \
+    cmake -DWITH_ALICE_VISION=ON -DWITH_OPENGV=ON .. && \
     make -j4 && \
-    # ldconfig first: source-installed deps (GTSAM) are not yet in the loader cache.
-    if [ "$RUN_TESTS" = "1" ]; then ldconfig && ctest --output-on-failure -LE "long|performance"; fi && \
     make install && \
     cd ../.. && \
     rm -rf rtabmap && \

docker/focal/deps/Dockerfile

@@ -190,10 +190,6 @@ RUN git clone https://github.com/laurentkneip/opengv.git && \
     cd && \
     rm -r opengv
 
-# ld.so.conf.d is read in filename order: <arch>-linux-gnu.conf sorts before
-# libc.conf (/usr/local/lib) on arm64, so source builds lose to the distro ones.
-RUN echo /usr/local/lib > /etc/ld.so.conf.d/00-usr-local.conf && ldconfig
-
 RUN rm /bin/sh && ln -s /bin/bash /bin/sh
 
 # for jetson (https://github.com/introlab/rtabmap/issues/776)

docker/jammy-iron/deps/Dockerfile

@@ -74,10 +74,6 @@ RUN git clone --branch 4.5.4 https://github.com/opencv/opencv.git && \
     cd ../.. && \
     rm -rf opencv opencv_contrib
 
-# ld.so.conf.d is read in filename order: <arch>-linux-gnu.conf sorts before
-# libc.conf (/usr/local/lib) on arm64, so source builds lose to the distro ones.
-RUN echo /usr/local/lib > /etc/ld.so.conf.d/00-usr-local.conf && ldconfig
-
 RUN rm /bin/sh && ln -s /bin/bash /bin/sh
 
 COPY ./docker/jammy-iron/deps/ros_entrypoint.sh /ros_entrypoint.sh

docker/jammy/Dockerfile

@@ -8,15 +8,11 @@ RUN mkdir -p /root/Documents/RTAB-Map && chmod 777 /root/Documents/RTAB-Map
 # Copy current source code
 COPY . /root/rtabmap
 
-ARG RUN_TESTS=0
-
 # Build RTAB-Map project
 RUN source /ros_entrypoint.sh && \
     cd rtabmap/build && \
-    cmake -DWITH_OPENGV=ON -DBUILD_TESTING=$RUN_TESTS .. && \
+    cmake -DWITH_OPENGV=ON .. && \
     make -j4 && \
-    # ldconfig first: source-installed deps (GTSAM) are not yet in the loader cache.
-    if [ "$RUN_TESTS" = "1" ]; then ldconfig && ctest --output-on-failure -LE "long|performance"; fi && \
     make install && \
     cd ../.. && \
     rm -rf rtabmap && \

docker/jammy/deps/Dockerfile

@@ -68,12 +68,9 @@ RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing zed-open-cap
     cd && \
     rm -r zed-open-capture; fi
     
-# Ubuntu 22.04 is the only release whose OpenCV carries an ABI suffix in its SONAME:
-# libopencv_core.so.4.5d. 
 RUN git clone --branch 4.5.4 https://github.com/opencv/opencv.git && \
     git clone --branch 4.5.4 https://github.com/opencv/opencv_contrib.git && \
     cd opencv && \
-    sed -i '/OPENCV_SOVERSION/s/}")/}d")/' cmake/OpenCVVersion.cmake && \
     mkdir build && \
     cd build && \
     cmake -DWITH_TBB=ON -DWITH_OPENMP=ON -DBUILD_opencv_python3=OFF -DBUILD_opencv_python_bindings_generator=OFF -DBUILD_opencv_python_tests=OFF -DBUILD_PERF_TESTS=OFF -DBUILD_TESTS=OFF -DOPENCV_ENABLE_NONFREE=ON -DOPENCV_EXTRA_MODULES_PATH=/root/opencv_contrib/modules .. && \
@@ -98,10 +95,6 @@ RUN git clone https://github.com/laurentkneip/opengv.git && \
     cd && \
     rm -r opengv
 
-# ld.so.conf.d is read in filename order: <arch>-linux-gnu.conf sorts before
-# libc.conf (/usr/local/lib) on arm64, so source builds lose to the distro ones.
-RUN echo /usr/local/lib > /etc/ld.so.conf.d/00-usr-local.conf && ldconfig
-
 RUN rm /bin/sh && ln -s /bin/bash /bin/sh
 
 RUN echo -e '#!/bin/bash\nset -e\n\n# setup ros2 environment\nsource "/opt/ros/humble/setup.bash" --\nexec "$@"' > /ros_entrypoint.sh

docker/noble-kilted/Dockerfile

@@ -8,15 +8,11 @@ RUN mkdir -p /root/Documents/RTAB-Map && chmod 777 /root/Documents/RTAB-Map
 # Copy current source code
 COPY . /root/rtabmap
 
-ARG RUN_TESTS=0
-
 # Build RTAB-Map project
 RUN source /ros_entrypoint.sh && \
     cd rtabmap/build && \
-    cmake -DWITH_OPENGV=ON -DBUILD_TESTING=$RUN_TESTS .. && \
+    cmake -DWITH_OPENGV=ON .. && \
     make -j4 && \
-    # ldconfig first: source-installed deps (GTSAM) are not yet in the loader cache.
-    if [ "$RUN_TESTS" = "1" ]; then ldconfig && ctest --output-on-failure -LE "long|performance"; fi && \
     make install && \
     cd ../.. && \
     rm -rf rtabmap && \

docker/noble-kilted/deps/Dockerfile

@@ -118,10 +118,6 @@ RUN git clone https://github.com/laurentkneip/opengv.git && \
     cd && \
     rm -r opengv
 
-# ld.so.conf.d is read in filename order: <arch>-linux-gnu.conf sorts before
-# libc.conf (/usr/local/lib) on arm64, so source builds lose to the distro ones.
-RUN echo /usr/local/lib > /etc/ld.so.conf.d/00-usr-local.conf && ldconfig
-
 RUN rm /bin/sh && ln -s /bin/bash /bin/sh
 
 RUN echo -e '#!/bin/bash\nset -e\n\n# setup ros2 environment\nsource "/opt/ros/kilted/setup.bash" --\nexec "$@"' > /ros_entrypoint.sh

docker/noble/Dockerfile

@@ -8,15 +8,11 @@ RUN mkdir -p /root/Documents/RTAB-Map && chmod 777 /root/Documents/RTAB-Map
 # Copy current source code
 COPY . /root/rtabmap
 
-ARG RUN_TESTS=0
-
 # Build RTAB-Map project
 RUN source /ros_entrypoint.sh && \
     cd rtabmap/build && \
-    cmake -DWITH_OPENGV=ON -DBUILD_TESTING=$RUN_TESTS .. && \
+    cmake -DWITH_OPENGV=ON .. && \
     make -j4 && \
-    # ldconfig first: source-installed deps (GTSAM) are not yet in the loader cache.
-    if [ "$RUN_TESTS" = "1" ]; then ldconfig && ctest --output-on-failure -LE "long|performance"; fi && \
     make install && \
     cd ../.. && \
     rm -rf rtabmap && \

docker/noble/deps/Dockerfile

@@ -117,10 +117,6 @@ RUN git clone https://github.com/laurentkneip/opengv.git && \
     cd && \
     rm -r opengv
 
-# ld.so.conf.d is read in filename order: <arch>-linux-gnu.conf sorts before
-# libc.conf (/usr/local/lib) on arm64, so source builds lose to the distro ones.
-RUN echo /usr/local/lib > /etc/ld.so.conf.d/00-usr-local.conf && ldconfig
-
 RUN rm /bin/sh && ln -s /bin/bash /bin/sh
 
 RUN echo -e '#!/bin/bash\nset -e\n\n# setup ros2 environment\nsource "/opt/ros/jazzy/setup.bash" --\nexec "$@"' > /ros_entrypoint.sh

docker/resolute/Dockerfile

@@ -8,15 +8,11 @@ RUN mkdir -p /root/Documents/RTAB-Map && chmod 777 /root/Documents/RTAB-Map
 # Copy current source code
 COPY . /root/rtabmap
 
-ARG RUN_TESTS=0
-
 # Build RTAB-Map project
 RUN source /ros_entrypoint.sh && \
     cd rtabmap/build && \
-    cmake -DWITH_OPENGV=ON -DBUILD_TESTING=$RUN_TESTS .. && \
+    cmake -DWITH_OPENGV=ON .. && \
     make -j4 && \
-    # ldconfig first: source-installed deps (GTSAM) are not yet in the loader cache.
-    if [ "$RUN_TESTS" = "1" ]; then ldconfig && ctest --output-on-failure -LE "long|performance"; fi && \
     make install && \
     cd ../.. && \
     rm -rf rtabmap && \

docker/resolute/deps/Dockerfile

@@ -107,10 +107,6 @@ RUN git clone https://github.com/laurentkneip/opengv.git && \
     cd && \
     rm -r opengv
 
-# ld.so.conf.d is read in filename order: <arch>-linux-gnu.conf sorts before
-# libc.conf (/usr/local/lib) on arm64, so source builds lose to the distro ones.
-RUN echo /usr/local/lib > /etc/ld.so.conf.d/00-usr-local.conf && ldconfig
-
 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/lyrical/setup.bash" --\nexec "$@"' > /ros_entrypoint.sh

guilib/include/rtabmap/gui/ExportCloudsDialog.h

@@ -32,7 +32,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <QDialog>
 #include <QMap>
-#include <QColor>
 #include <QtCore/QSettings>
 
 #include <rtabmap/core/Signature.h>
@@ -47,10 +46,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 class Ui_ExportCloudsDialog;
 class QAbstractButton;
 
-namespace clams {
-class DiscreteDepthDistortionModel;
-}
-
 namespace rtabmap {
 class ProgressDialog;
 class GainCompensator;
@@ -137,30 +132,7 @@ private Q_SLOTS:
 	void cancel();
 
 private:
-	int numThreads() const; // resolves the "Auto" value of the threads spin box
 	std::map<int, Transform> filterNodes(const std::map<int, Transform> & poses);
-	struct CloudGenResult
-	{
-		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud;
-		pcl::IndicesPtr indices;
-		bool hasScan = false;
-		bool scanHasRGB = false;
-		std::vector<std::pair<QString, QColor> > messages;
-	};
-	CloudGenResult generateCloudForNode(
-			int nodeId,
-			const Transform & pose,
-			int index,
-			int totalPoses,
-			const std::vector<float> & roiRatios,
-			const clams::DiscreteDepthDistortionModel * model,
-			const QMap<int, Signature> & cachedSignatures,
-			const std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> > & cachedClouds,
-			const std::map<int, LaserScan> & cachedScans,
-			const ParametersMap & parameters,
-			pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr * previousCloud,
-			pcl::IndicesPtr * previousIndices,
-			Transform * previousPose) const;
 	std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::IndicesPtr> > getClouds(
 			const std::map<int, Transform> & poses,
 			const QMap<int, Signature> & cachedSignatures,

guilib/src/DatabaseViewer.cpp

@@ -2029,7 +2029,7 @@ void DatabaseViewer::updateIds()
 		envSensors_.insert(std::make_pair(ids_[i], sensors));
 		if(w>=0)
 		{
-			for(std::multimap<int, Link>::iterator iter=links.lower_bound(ids_[i]); iter!=links.end() && iter->first==ids_[i]; ++iter)
+			for(std::multimap<int, Link>::iterator iter=links.find(ids_[i]); iter!=links.end() && iter->first==ids_[i]; ++iter)
 			{
 				// Make compatible with old databases, when "weight=-1" was not yet introduced to identify ignored nodes
 				if(iter->second.type() == Link::kNeighbor || iter->second.type() == Link::kNeighborMerged)
@@ -2070,7 +2070,7 @@ void DatabaseViewer::updateIds()
 		previousPose=p;
 
 		//links
-		for(std::multimap<int, Link>::iterator jter=links.lower_bound(ids_[i]); jter!=links.end() && jter->first == ids_[i]; ++jter)
+		for(std::multimap<int, Link>::iterator jter=links.find(ids_[i]); jter!=links.end() && jter->first == ids_[i]; ++jter)
 		{
 			if(jter->second.type() == Link::kNeighborMerged)
 			{
@@ -4852,7 +4852,7 @@ void DatabaseViewer::updateCovariances(const QList<Link> & links)
 					infMatrix.clone(),
 					currentLink.userDataCompressed());
 			bool updated = false;
-			std::multimap<int, Link>::iterator iter = linksRefined_.lower_bound(currentLink.from());
+			std::multimap<int, Link>::iterator iter = linksRefined_.find(currentLink.from());
 			while(iter != linksRefined_.end() && iter->first == currentLink.from())
 			{
 				if(iter->second.to() == currentLink.to() &&
@@ -6681,7 +6681,7 @@ void DatabaseViewer::editConstraint()
 			{
 				cv::Mat covariance = dialog.getCovariance();
 				Link newLink(link.from(), link.to(), link.type(), dialog.getTransform(), covariance.inv());
-				std::multimap<int, Link>::iterator iter = linksRefined_.lower_bound(link.from());
+				std::multimap<int, Link>::iterator iter = linksRefined_.find(link.from());
 				while(iter != linksRefined_.end() && iter->first == link.from())
 				{
 					if(iter->second.to() == link.to() &&
@@ -9462,7 +9462,7 @@ void DatabaseViewer::refineConstraint(int from, int to, Registration * reg, Regi
 		Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), transform, info.covariance.inv(), currentLink.userDataCompressed());
 
 		bool updated = false;
-		std::multimap<int, Link>::iterator iter = linksRefined_.lower_bound(currentLink.from());
+		std::multimap<int, Link>::iterator iter = linksRefined_.find(currentLink.from());
 		while(iter != linksRefined_.end() && iter->first == currentLink.from())
 		{
 			if(iter->second.to() == currentLink.to() &&

guilib/src/PreferencesDialog.cpp

@@ -1141,7 +1141,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->checkBox_kp_incrementalFlann->setObjectName(Parameters::kKpIncrementalFlann().c_str());
 	_ui->checkBox_kp_byteToFloat->setObjectName(Parameters::kKpByteToFloat().c_str());
 	_ui->surf_doubleSpinBox_rebalancingFactor->setObjectName(Parameters::kKpFlannRebalancingFactor().c_str());
-	_ui->spinBox_kp_flannThreads->setObjectName(Parameters::kKpFlannThreads().c_str());
 	_ui->comboBox_detector_strategy->setObjectName(Parameters::kKpDetectorStrategy().c_str());
 	_ui->surf_doubleSpinBox_nndrRatio->setObjectName(Parameters::kKpNndrRatio().c_str());
 	_ui->surf_doubleSpinBox_maxDepth->setObjectName(Parameters::kKpMaxDepth().c_str());

guilib/src/ui/exportCloudsDialog.ui

@@ -31,7 +31,7 @@
       <layout class="QVBoxLayout" name="verticalLayout_13">
        <item>
         <layout class="QGridLayout" name="gridLayout_8" columnstretch="0,1">
-         <item row="18" column="0">
+         <item row="17" column="0">
           <widget class="QCheckBox" name="checkBox_cameraProjection">
            <property name="text">
             <string/>
@@ -45,7 +45,7 @@
            </property>
           </widget>
          </item>
-         <item row="15" column="0">
+         <item row="14" column="0">
           <widget class="QCheckBox" name="checkBox_filtering">
            <property name="text">
             <string/>
@@ -59,7 +59,7 @@
            </property>
           </widget>
          </item>
-         <item row="19" column="1">
+         <item row="18" column="1">
           <widget class="QLabel" name="label_binaryFile_12">
            <property name="text">
             <string>Meshing.</string>
@@ -69,7 +69,7 @@
            </property>
           </widget>
          </item>
-         <item row="15" column="1">
+         <item row="14" column="1">
           <widget class="QLabel" name="label_binaryFile_9">
            <property name="text">
             <string>Cloud filtering.</string>
@@ -89,14 +89,14 @@
            </property>
           </widget>
          </item>
-         <item row="19" column="0">
+         <item row="18" column="0">
           <widget class="QCheckBox" name="checkBox_meshing">
            <property name="text">
             <string/>
            </property>
           </widget>
          </item>
-         <item row="17" column="1">
+         <item row="16" column="1">
           <widget class="QLabel" name="label_gainCompensation">
            <property name="text">
             <string>Gain compensation. Normalize brightness of images.</string>
@@ -208,7 +208,7 @@
            </property>
           </widget>
          </item>
-         <item row="16" column="0">
+         <item row="15" column="0">
           <widget class="QCheckBox" name="checkBox_smoothing">
            <property name="text">
             <string/>
@@ -229,7 +229,7 @@
            </item>
           </widget>
          </item>
-         <item row="16" column="1">
+         <item row="15" column="1">
           <widget class="QLabel" name="label_smoothing">
            <property name="text">
             <string>Cloud smoothing using Moving Least Squares algorithm (MLS).</string>
@@ -278,7 +278,7 @@
            </item>
           </widget>
          </item>
-         <item row="18" column="1">
+         <item row="17" column="1">
           <widget class="QLabel" name="label_cameraProjection">
            <property name="text">
             <string>Camera projection. This can be used to colorize point cloud created from scans and/or export camera IDs for each point of the cloud.</string>
@@ -329,7 +329,7 @@
            </property>
           </widget>
          </item>
-         <item row="17" column="0">
+         <item row="16" column="0">
           <widget class="QCheckBox" name="checkBox_gainCompensation">
            <property name="text">
             <string/>
@@ -403,32 +403,6 @@
            </property>
           </widget>
          </item>
-         <item row="14" column="0">
-          <widget class="QSpinBox" name="spinBox_numThreads">
-           <property name="specialValueText">
-            <string>Auto</string>
-           </property>
-           <property name="minimum">
-            <number>0</number>
-           </property>
-           <property name="maximum">
-            <number>128</number>
-           </property>
-           <property name="value">
-            <number>0</number>
-           </property>
-          </widget>
-         </item>
-         <item row="14" column="1">
-          <widget class="QLabel" name="label_numThreads">
-           <property name="text">
-            <string>Number of threads used to generate the clouds and to texture the mesh (Auto=one per core, 1=process them one by one).</string>
-           </property>
-           <property name="wordWrap">
-            <bool>true</bool>
-           </property>
-          </widget>
-         </item>
         </layout>
        </item>
        <item>

guilib/src/ui/preferencesDialog.ui

@@ -12185,35 +12185,6 @@ When set to false, no new words are added to dictionary, so no more updates are
                          </property>
                         </widget>
                        </item>
-                       <item row="12" column="0">
-                        <widget class="QSpinBox" name="spinBox_kp_flannThreads">
-                         <property name="specialValueText">
-                          <string>Auto</string>
-                         </property>
-                         <property name="minimum">
-                          <number>0</number>
-                         </property>
-                         <property name="maximum">
-                          <number>128</number>
-                         </property>
-                         <property name="value">
-                          <number>1</number>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="12" column="2">
-                        <widget class="QLabel" name="label_kp_flannThreads">
-                         <property name="text">
-                          <string>Number of threads used for FLANN kNN search of the batched queries (Auto=one per core, 1=single-threaded search).</string>
-                         </property>
-                         <property name="wordWrap">
-                          <bool>true</bool>
-                         </property>
-                         <property name="textInteractionFlags">
-                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                         </property>
-                        </widget>
-                       </item>
                       </layout>
                      </widget>
                     </item>
@@ -12391,7 +12362,7 @@ see Sqlite3 doc 'PRAGMA synchronous'.</string>
                     <item row="5" column="1">
                      <widget class="QLabel" name="label_760">
                       <property name="text">
-                       <string>Depth image compression format (should be &quot;.png&quot; or &quot;.rvl&quot;). Add &quot;:maxDepth[:quantization]&quot; (e.g., &quot;.rvl:10:100&quot;) to compress 32FC1 depth images as 16 bits inverse depth (lossy: precision ~d²/(2q(q+1)), depth over maxDepth is lost, databases cannot be opened by versions &lt; 0.24). 16UC1 depth images are always compressed losslessly.</string>
+                       <string>Depth image compression format (should be &quot;.png&quot; or &quot;.rvl&quot;).</string>
                       </property>
                       <property name="wordWrap">
                        <bool>true</bool>

package.xml

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

tools/CMakeLists.txt

@@ -17,7 +17,6 @@ ADD_SUBDIRECTORY( Info )
 ADD_SUBDIRECTORY( CleanupLocalGrids )
 ADD_SUBDIRECTORY( GlobalBundleAdjustment )
 ADD_SUBDIRECTORY( ReduceGraph )
-ADD_SUBDIRECTORY( LidarCameraCalibration )
 
 IF(OPENCV_NONFREE_FOUND)
 ADD_SUBDIRECTORY( VocabularyComparison )

tools/Export/main.cpp

@@ -35,7 +35,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/util3d_mapping.h>
 #include <rtabmap/core/optimizer/OptimizerG2O.h>
 #include <rtabmap/core/Graph.h>
-#include <rtabmap/core/Signature.h>
 #include <rtabmap/core/global_map/OccupancyGrid.h>
 #ifdef RTABMAP_OCTOMAP
 #include <rtabmap/core/global_map/OctoMap.h>
@@ -50,13 +49,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/common/common.h>
 #include <pcl/surface/poisson.h>
 #include <stdio.h>
-#include <algorithm>
 #include <fstream>
 
-#ifdef _OPENMP
-#include <omp.h>
-#endif
-
 #ifdef RTABMAP_PDAL
 #include <rtabmap/core/PDALWriter.h>
 #endif
@@ -190,8 +184,6 @@ void showUsage()
 			"    --density_angle #     Filter poses up to angle (deg) in the --density_radius.\n"
 			"    --filter_ceiling #    Filter points over a custom height (default 0 m, 0=disabled).\n"
 			"    --filter_floor #      Filter points below a custom height (default 0 m, 0=disabled).\n"
-			"    --threads       #     Number of threads used to generate the clouds and to texture the mesh\n"
-			"                              (default 0=one per core, 1=process them sequentially).\n"
 
 			"\n%s", Parameters::showUsage());
 	;
@@ -264,7 +256,6 @@ int main(int argc, char * argv[])
 	float poissonSize = 0.03;
 	int maxPolygons = 300000;
 	int decimation = -1;
-	int numThreads = 0;
 	float depthEdgeBleedingFilterError = 0.0f;
 	unsigned char depthConfidenceThr = 0;
 	float minRange = 0.0f;
@@ -828,23 +819,6 @@ int main(int argc, char * argv[])
 				showUsage();
 			}
 		}
-		else if(std::strcmp(argv[i], "--threads") == 0)
-		{
-			++i;
-			if(i<argc-1)
-			{
-				numThreads = uStr2Int(argv[i]);
-				if(numThreads < 0)
-				{
-					printf("--threads cannot be negative!\n");
-					showUsage();
-				}
-			}
-			else
-			{
-				showUsage();
-			}
-		}
 		else if(std::strcmp(argv[i], "--decimation") == 0)
 		{
 			++i;
@@ -1519,9 +1493,6 @@ int main(int argc, char * argv[])
 	}
 	int processedNodes = 0;
 	int lastPercent = 0;
-
-	std::vector<std::pair<int, Transform> > nodes;
-	nodes.reserve(optimizedPoses.size());
 	for(std::map<int, Transform>::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
 	{
 		if(iter->first<0)
@@ -1532,42 +1503,26 @@ int main(int argc, char * argv[])
 
 			landmarkPoses.insert(*iter);
 			landmarkStamps.insert(std::make_pair(iter->first, 0));
+			continue;
 		}
-		else
-		{
-			nodes.push_back(*iter);
-		}
-	}
-
-	struct NodeExportData
-	{
-		// node info, calibration, compressed data, uncompressed local occupancy grid
-		// and uncompressed depth image (only if texturing)
-		Signature node;
-		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
-		pcl::PointCloud<pcl::PointXYZI>::Ptr cloudI;
-	};
-
-	auto loadNode = [&](int nodeId, const Transform & pose, NodeExportData & out)
-	{
+		
 		Transform p, gt;
 		int m;
 		std::string l;
 		GPS gps;
 		std::vector<float> v;
 		EnvSensors s;
-		int weight;
-		double stamp;
-		dbDriver->getNodeInfo(nodeId, p, m, weight, l, stamp, gt, v, gps, s);
+		int weight = -1;
+		double stamp = 0.0;
+		dbDriver->getNodeInfo(iter->first, p, m, weight, l, stamp, gt, v, gps, s);
 
-		out.node = Signature(nodeId, m, weight, stamp, l, pose, gt);
-		SensorData & data = out.node.sensorData();
+		SensorData data;
 		bool loadImages = ((exportCloud || exportMesh) && (!cloudFromScan || texture || camProjection)) || exportImages;
 		bool loadScan = ((exportCloud || exportMesh) && cloudFromScan) || exportPosesScan;
 		if(loadImages || loadScan || export2DMap || exportOctomap)
 		{
 			dbDriver->getNodeData(
-				nodeId, 
+				iter->first, 
 				data, 
 				loadImages, 
 				loadScan,
@@ -1575,29 +1530,23 @@ int main(int argc, char * argv[])
 				export2DMap || exportOctomap);
 		}
 
-		data.setGPS(gps); // getNodeData() above overwrites the whole sensor data
-
 		// uncompress data
+		std::vector<CameraModel> models;
+		std::vector<StereoCameraModel> stereoModels;
 		if(loadImages || exportPosesCamera)
 		{
-			std::vector<CameraModel> models;
-			std::vector<StereoCameraModel> stereoModels;
-			dbDriver->getCalibration(nodeId, models, stereoModels);
-			data.setCameraModels(models);
-			data.setStereoCameraModels(stereoModels);
+			dbDriver->getCalibration(iter->first, models, stereoModels);
 		}
-		const std::vector<CameraModel> & models = data.cameraModels();
-		const std::vector<StereoCameraModel> & stereoModels = data.stereoCameraModels();
 
-		cv::Mat depth;
 		if(exportCloud || exportMesh || exportImages)
 		{
-			bool densityFiltered = !densityPoses.empty() && densityPoses.find(nodeId) == densityPoses.end();
+			bool densityFiltered = !densityPoses.empty() && densityPoses.find(iter->first) == densityPoses.end();
 			cv::Mat rgb;
+			cv::Mat depth;
 			cv::Mat confidence;
 			pcl::IndicesPtr indices(new std::vector<int>);
-			pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud = out.cloud;
-			pcl::PointCloud<pcl::PointXYZI>::Ptr & cloudI = out.cloudI;
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
+			pcl::PointCloud<pcl::PointXYZI>::Ptr cloudI;
 			if(weight != -1)
 			{
 				if(!densityFiltered && cloudFromScan && (exportCloud || exportMesh))
@@ -1606,7 +1555,7 @@ int main(int argc, char * argv[])
 					data.uncompressData(exportImages?&rgb:0, (texture||exportImages)&&!data.depthOrRightCompressed().empty()?&depth:0, &scan, 0, 0, 0, 0, exportImages?&confidence:0);
 					if(scan.empty())
 					{
-						printf("Node %d doesn't have scan data, empty cloud is created.\n", nodeId);
+						printf("Node %d doesn't have scan data, empty cloud is created.\n", iter->first);
 					}
 					if(decimation>1 || minRange>0.0f || maxRange)
 					{
@@ -1637,7 +1586,7 @@ int main(int argc, char * argv[])
 						if(depth.empty())
 						{
 							printf("Node %d doesn't have depth or stereo data, empty cloud is "
-									"created (if you want to create point cloud from scan, use --scan option).\n", nodeId);
+									"created (if you want to create point cloud from scan, use --scan option).\n", iter->first);
 						}
 						else if(!data.depthRaw().empty() && depthEdgeBleedingFilterError>0.0f)
 						{
@@ -1668,9 +1617,8 @@ int main(int argc, char * argv[])
 				if(!UDirectory::exists(dir)) {
 					UDirectory::makeDir(dir);
 				}
-				std::string outputPath=dir+"/"+(exportImagesId?uNumber2Str(nodeId):uFormat("%f", stamp))+".jpg";
+				std::string outputPath=dir+"/"+(exportImagesId?uNumber2Str(iter->first):uFormat("%f", stamp))+".jpg";
 				cv::imwrite(outputPath, rgb);
-				#pragma omp atomic
 				++imagesExported;
 				if(!depth.empty())
 				{
@@ -1694,7 +1642,7 @@ int main(int argc, char * argv[])
 						UDirectory::makeDir(dir);
 					}
 
-					outputPath=dir+"/"+(exportImagesId?uNumber2Str(nodeId):uFormat("%f", stamp))+ext;
+					outputPath=dir+"/"+(exportImagesId?uNumber2Str(iter->first):uFormat("%f", stamp))+ext;
 					cv::imwrite(outputPath, depthExported);
 				}
 				if(!confidence.empty())
@@ -1704,7 +1652,7 @@ int main(int argc, char * argv[])
 						UDirectory::makeDir(dir);
 					}
 
-					outputPath=dir+"/"+(exportImagesId?uNumber2Str(nodeId):uFormat("%f", stamp))+".png";
+					outputPath=dir+"/"+(exportImagesId?uNumber2Str(iter->first):uFormat("%f", stamp))+".png";
 					cv::imwrite(outputPath, confidence);
 				}
 
@@ -1712,7 +1660,7 @@ int main(int argc, char * argv[])
 				for(size_t i=0; i<models.size(); ++i)
 				{
 					CameraModel model = models[i];
-					std::string modelName = (exportImagesId?uNumber2Str(nodeId):uFormat("%f", stamp));
+					std::string modelName = (exportImagesId?uNumber2Str(iter->first):uFormat("%f", stamp));
 					if(models.size() > 1) {
 						modelName += "_" + uNumber2Str((int)i);
 					}
@@ -1726,7 +1674,7 @@ int main(int argc, char * argv[])
 				for(size_t i=0; i<stereoModels.size(); ++i)
 				{
 					StereoCameraModel model = stereoModels[i];
-					std::string modelName = (exportImagesId?uNumber2Str(nodeId):uFormat("%f", stamp));
+					std::string modelName = (exportImagesId?uNumber2Str(iter->first):uFormat("%f", stamp));
 					if(stereoModels.size() > 1) {
 						modelName += "_" + uNumber2Str((int)i);
 					}
@@ -1746,20 +1694,20 @@ int main(int argc, char * argv[])
 					if(cloud.get() && !cloud->empty()) {
 						cloud = rtabmap::util3d::voxelize(cloud, indices, voxelSize);
 						if(!cloud->empty())
-							cloud = rtabmap::util3d::transformPointCloud(cloud, pose);
+							cloud = rtabmap::util3d::transformPointCloud(cloud, iter->second);
 					}
 					else if(cloudI.get() && !cloudI->empty()) {
 						cloudI = rtabmap::util3d::voxelize(cloudI, indices, voxelSize);
 						if(!cloudI->empty())
-							cloudI = rtabmap::util3d::transformPointCloud(cloudI, pose);
+							cloudI = rtabmap::util3d::transformPointCloud(cloudI, iter->second);
 					}
 				}
 				else
 				{
 					if(cloud.get() && !cloud->empty())
-						cloud = rtabmap::util3d::transformPointCloud(cloud, indices, pose);
+						cloud = rtabmap::util3d::transformPointCloud(cloud, indices, iter->second);
 					else if(cloudI.get() && !cloudI->empty())
-						cloudI = rtabmap::util3d::transformPointCloud(cloudI, indices, pose);
+						cloudI = rtabmap::util3d::transformPointCloud(cloudI, indices, iter->second);
 				}
 
 				if(filter_ceiling != 0.0 || filter_floor != 0.0f)
@@ -1774,90 +1722,54 @@ int main(int argc, char * argv[])
 					}
 				}
 
-			}
-		}
-
-		if(weight != -1 && (export2DMap || exportOctomap))
-		{
-			cv::Mat ground, obstacles, empty;
-			data.uncompressData(0, 0, 0, 0, &ground, &obstacles, &empty);
-		}
-
-		data.clearRawData(true, true, true, false); // keep uncompressed occupancy grid
-		if(texture && !depth.empty() && (depth.type() == CV_16UC1 || depth.type() == CV_32FC1))
-		{
-			// Keep uncompressed depth for texturing, the compressed one is not needed anymore.
-			// The compressed image is passed back as is (rows==1), as flushNode() uses it to
-			// know if the node has an image.
-			data.setRGBDImage(data.imageCompressed(), depth, cv::Mat(), data.cameraModels());
-		}
-	};
-
-	auto flushNode = [&](NodeExportData & out)
-	{
-		const Signature & node = out.node;
-		const SensorData & data = node.sensorData();
-		const int nodeId = node.id();
-		const Transform & pose = node.getPose();
-		std::vector<CameraModel> models = data.cameraModels();
-		const std::vector<StereoCameraModel> & stereoModels = data.stereoCameraModels();
-		pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud = out.cloud;
-		pcl::PointCloud<pcl::PointXYZI>::Ptr & cloudI = out.cloudI;
-		const cv::Mat & depth = data.depthOrRightRaw();
-		double stamp = node.getStamp();
-		int weight = node.getWeight();
-		const GPS & gps = data.gps();
-		const Transform & gt = node.getGroundTruthPose();
-
-		if(exportCloud || exportMesh)
-		{
-			if(cloudFromScan)
-			{
-				Transform lidarViewpoint = pose * data.laserScanCompressed().localTransform();
-				rawViewpoints.insert(std::make_pair(nodeId, lidarViewpoint));
-			}
-			else if(!models.empty() && !models[0].localTransform().isNull())
-			{
-				Transform cameraViewpoint = pose * models[0].localTransform(); // take the first camera
-				rawViewpoints.insert(std::make_pair(nodeId, cameraViewpoint));
-			}
-			else if(!stereoModels.empty() && !stereoModels[0].localTransform().isNull())
-			{
-				Transform cameraViewpoint = pose * stereoModels[0].localTransform();
-				rawViewpoints.insert(std::make_pair(nodeId, cameraViewpoint));
-			}
-			else
-			{
-				rawViewpoints.insert(std::make_pair(nodeId, pose));
-			}
-
-			if(cloud.get() && !cloud->empty())
-			{
-				if(assembledCloud->empty())
+				if(cloudFromScan)
 				{
-					*assembledCloud = *cloud;
+					Transform lidarViewpoint = iter->second * data.laserScanRaw().localTransform();
+					rawViewpoints.insert(std::make_pair(iter->first, lidarViewpoint));
+				}
+				else if(!models.empty() && !models[0].localTransform().isNull())
+				{
+					Transform cameraViewpoint = iter->second * models[0].localTransform(); // take the first camera
+					rawViewpoints.insert(std::make_pair(iter->first, cameraViewpoint));
+				}
+				else if(!stereoModels.empty() && !stereoModels[0].localTransform().isNull())
+				{
+					Transform cameraViewpoint = iter->second * stereoModels[0].localTransform();
+					rawViewpoints.insert(std::make_pair(iter->first, cameraViewpoint));
 				}
 				else
 				{
-					*assembledCloud += *cloud;
+					rawViewpoints.insert(*iter);
 				}
-				rawViewpointIndices.resize(assembledCloud->size(), nodeId);
-			}
-			else if(cloudI.get() && !cloudI->empty())
-			{
-				if(assembledCloudI->empty())
+
+				if(cloud.get() && !cloud->empty())
 				{
-					*assembledCloudI = *cloudI;
+					if(assembledCloud->empty())
+					{
+						*assembledCloud = *cloud;
+					}
+					else
+					{
+						*assembledCloud += *cloud;
+					}
+					rawViewpointIndices.resize(assembledCloud->size(), iter->first);
 				}
-				else
+				else if(cloudI.get() && !cloudI->empty())
 				{
-					*assembledCloudI += *cloudI;
+					if(assembledCloudI->empty())
+					{
+						*assembledCloudI = *cloudI;
+					}
+					else
+					{
+						*assembledCloudI += *cloudI;
+					}
+					rawViewpointIndices.resize(assembledCloudI->size(), iter->first);
+				}
+				if(texture && !depth.empty() && (depth.type() == CV_16UC1 || depth.type() == CV_32FC1))
+				{
+					cameraDepths.insert(std::make_pair(iter->first, depth));
 				}
-				rawViewpointIndices.resize(assembledCloudI->size(), nodeId);
-			}
-			if(!depth.empty()) // depth is set only when texturing (see loadNode)
-			{
-				cameraDepths.insert(std::make_pair(nodeId, depth));
 			}
 		}
 
@@ -1869,8 +1781,8 @@ int main(int argc, char * argv[])
 			}
 		}
 
-		robotPoses.insert(std::make_pair(nodeId, pose));
-		robotStamps.insert(std::make_pair(nodeId, stamp));
+		robotPoses.insert(std::make_pair(iter->first, iter->second));
+		robotStamps.insert(std::make_pair(iter->first, stamp));
 		if(models.empty() && weight == -1 && !cameraModels.empty())
 		{
 			// For intermediate nodes, use latest models
@@ -1880,7 +1792,7 @@ int main(int argc, char * argv[])
 		{
 			if(!data.imageCompressed().empty())
 			{
-				cameraModels.insert(std::make_pair(nodeId, models));
+				cameraModels.insert(std::make_pair(iter->first, models));
 			}
 			if(exportPosesCamera)
 			{
@@ -1892,15 +1804,15 @@ int main(int argc, char * argv[])
 				UASSERT_MSG(models.size() == cameraPoses.size(), "Not all nodes have same number of cameras to export camera poses.");
 				for(size_t i=0; i<models.size(); ++i)
 				{
-					cameraPoses[i].insert(std::make_pair(nodeId, pose*models[i].localTransform()));
-					cameraStamps[i].insert(std::make_pair(nodeId, stamp));
+					cameraPoses[i].insert(std::make_pair(iter->first, iter->second*models[i].localTransform()));
+					cameraStamps[i].insert(std::make_pair(iter->first, stamp));
 				}
 			}
 		}
 		if(exportPosesScan && !data.laserScanCompressed().empty())
 		{
-			scanPoses.insert(std::make_pair(nodeId, pose*data.laserScanCompressed().localTransform()));
-			scanStamps.insert(std::make_pair(nodeId, stamp));
+			scanPoses.insert(std::make_pair(iter->first, iter->second*data.laserScanCompressed().localTransform()));
+			scanStamps.insert(std::make_pair(iter->first, stamp));
 		}
 
 		if(exportPosesGps || exportGps>=0)
@@ -1920,85 +1832,56 @@ int main(int argc, char * argv[])
 						gpsOrigin = gps;
 					}
 					Transform pose(p.x, p.y, p.z, 0.0f, 0.0f, (float)((-(gps.bearing()-90))*M_PI/180.0));
-					gpsPoses.insert(std::make_pair(nodeId, pose));
+					gpsPoses.insert(std::make_pair(iter->first, pose));
 				}
 				if(exportGps>=0)
 				{
-					gpsValues.insert(std::make_pair(nodeId, gps));
+					gpsValues.insert(std::make_pair(iter->first, gps));
 				}
-				gpsStamps.insert(std::make_pair(nodeId, gps.stamp()));
+				gpsStamps.insert(std::make_pair(iter->first, gps.stamp()));
 			}
 		}
 
 		if(exportPosesGt && !gt.isNull())
 		{
-			gtPoses.insert(std::make_pair(nodeId, gt));
-			gtStamps.insert(std::make_pair(nodeId, stamp));
+			gtPoses.insert(std::make_pair(iter->first, gt));
+			gtStamps.insert(std::make_pair(iter->first, stamp));
 		}
 
 		if(weight != -1 && (export2DMap || exportOctomap)) {
-			const cv::Mat & ground = data.gridGroundCellsRaw();
-			const cv::Mat & obstacles = data.gridObstacleCellsRaw();
-			const cv::Mat & empty = data.gridEmptyCellsRaw();
+			cv::Mat ground;
+			cv::Mat obstacles;
+			cv::Mat empty;
+			data.uncompressDataConst(0, 0, 0, 0, &ground, &obstacles, &empty);
 			if(ground.empty() && obstacles.empty() && empty.empty()) {
-				printf("Node %d doesn't have local occupancy grid, ignored!\n", nodeId);
+				printf("Node %d doesn't have local occupancy grid, ignored!\n", iter->first);
 			}
 			else {
-				addedPosesToMap.insert(std::make_pair(nodeId, pose));
-				localGridCache.add(nodeId, ground, obstacles, empty, data.gridCellSize(), data.gridViewPoint());
+				addedPosesToMap.insert(*iter);
+				localGridCache.add(iter->first, ground, obstacles, empty, data.gridCellSize(), data.gridViewPoint());
 				if(export2DMap && !grid.update(addedPosesToMap)) {
-					printf("Failed to assemble local grid %d to global occupancy grid!\n", nodeId);
+					printf("Failed to assemble local grid %d to global occupancy grid!\n", iter->first);
 				}
 #ifdef RTABMAP_OCTOMAP
 				if(exportOctomap && !octomap.update(addedPosesToMap)) {
-					printf("Failed to assemble local grid %d to OctoMap!\n", nodeId);
+					printf("Failed to assemble local grid %d to OctoMap!\n", iter->first);
 				}
 #endif
 				localGridCache.clear();
 			}
 		}
 
-	};
-
-#ifdef _OPENMP
-	const int usedThreads = numThreads>0?numThreads:omp_get_max_threads();
-#else
-	const int usedThreads = 1;
-#endif
-	// Nodes are loaded by batch, a batch is generated in parallel then assembled sequentially
-	// (in node order) to keep the output independent of the thread count. More nodes than
-	// threads are batched so that a thread getting cheap nodes can pick up more work. With a
-	// single thread, nodes are processed one by one, keeping only one node in memory.
-	const size_t chunkSize = usedThreads>1?usedThreads*4:1;
-	std::vector<NodeExportData> chunkData;
-	for(size_t chunkStart=0; chunkStart<nodes.size(); chunkStart+=chunkSize)
-	{
-		size_t chunkNodes = std::min(chunkSize, nodes.size()-chunkStart);
-		chunkData.assign(chunkNodes, NodeExportData());
-
-		#pragma omp parallel for schedule(dynamic) num_threads(usedThreads)
-		for(int i=0; i<(int)chunkNodes; ++i)
+		if(optimizedPoses.size() >= 500)
 		{
-			loadNode(nodes[chunkStart+i].first, nodes[chunkStart+i].second, chunkData[i]);
-		}
-
-		for(size_t i=0; i<chunkNodes; ++i)
-		{
-			flushNode(chunkData[i]);
-			chunkData[i] = NodeExportData();
-
-			if(optimizedPoses.size() >= 500)
+			++processedNodes;
+			int percent = processedNodes*100/(int)optimizedPoses.size();
+			if(percent != lastPercent)
 			{
-				++processedNodes;
-				int percent = processedNodes*100/(int)optimizedPoses.size();
-				if(percent != lastPercent)
-				{
-					printf("Processed %d/%d (%d%%) nodes...\n",
-						processedNodes,
-						(int)optimizedPoses.size(),
-						percent);
-					lastPercent = percent;
-				}
+				printf("Processed %d/%d (%d%%) nodes...\n",
+					processedNodes,
+					(int)optimizedPoses.size(),
+					percent);
+				lastPercent = percent;
 			}
 		}
 	}
@@ -2775,8 +2658,7 @@ int main(int argc, char * argv[])
 							textureRoiRatios,
 							&progressState,
 							&vertexToPixels,
-							distanceToCamPolicy,
-							usedThreads);
+							distanceToCamPolicy);
 					printf("Texturing... done (%fs).\n", timer.ticks());
 
 					// Remove occluded polygons (polygons with no texture)

tools/LidarCameraCalibration/CMakeLists.txt

@@ -1,33 +0,0 @@
-
-ADD_EXECUTABLE(lidarCameraCalibration
-  main.cpp
-  CalibrationProblem.cpp
-  CorrectionSolver.cpp
-  PatternSearchSolver.cpp
-  DownhillSimplexSolver.cpp
-  G2oSolver.cpp
-  GtsamSolver.cpp)
-  
-TARGET_LINK_LIBRARIES(lidarCameraCalibration rtabmap_core)
-
-# For the g2o solver: rtabmap_core links g2o privately.
-IF(G2O_FOUND)
-  IF(g2o_FOUND)
-    TARGET_LINK_LIBRARIES(lidarCameraCalibration g2o::core g2o::solver_eigen)
-  ELSE()
-    TARGET_INCLUDE_DIRECTORIES(lidarCameraCalibration PRIVATE ${G2O_INCLUDE_DIRS})
-    TARGET_LINK_LIBRARIES(lidarCameraCalibration ${G2O_LIBRARIES})
-  ENDIF()
-ENDIF(G2O_FOUND)
-
-# For the GTSAM solver: rtabmap_core links GTSAM privately.
-IF(GTSAM_FOUND)
-  TARGET_LINK_LIBRARIES(lidarCameraCalibration gtsam)
-ENDIF(GTSAM_FOUND)
-
-SET_TARGET_PROPERTIES( lidarCameraCalibration 
-  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-lidarCameraCalibration)
-
-INSTALL(TARGETS lidarCameraCalibration
-		RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
-		BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)

tools/LidarCameraCalibration/CalibrationProblem.cpp

@@ -1,130 +0,0 @@
-/*
-Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without
-modification, are permitted provided that the following conditions are met:
-    * Redistributions of source code must retain the above copyright
-      notice, this list of conditions and the following disclaimer.
-    * Redistributions in binary form must reproduce the above copyright
-      notice, this list of conditions and the following disclaimer in the
-      documentation and/or other materials provided with the distribution.
-    * Neither the name of the Universite de Sherbrooke nor the
-      names of its contributors may be used to endorse or promote products
-      derived from this software without specific prior written permission.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
-ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
-WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
-DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
-ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-*/
-
-#include "CalibrationProblem.h"
-
-#include <rtabmap/core/util3d_transforms.h>
-
-#include <algorithm>
-#include <cmath>
-
-using namespace rtabmap;
-
-Transform correctionFrom(const double p[6])
-{
-	return Transform(p[0], p[1], p[2], p[3]*M_PI/180.0, p[4]*M_PI/180.0, p[5]*M_PI/180.0);
-}
-
-Eigen::Isometry3d correctionFromDouble(const double p[6])
-{
-	// As Transform(x, y, z, roll, pitch, yaw): yaw * pitch * roll.
-	Eigen::Isometry3d C = Eigen::Isometry3d::Identity();
-	C.translation() = Eigen::Vector3d(p[0], p[1], p[2]);
-	C.linear() = (Eigen::AngleAxisd(p[5]*M_PI/180.0, Eigen::Vector3d::UnitZ()) *
-			Eigen::AngleAxisd(p[4]*M_PI/180.0, Eigen::Vector3d::UnitY()) *
-			Eigen::AngleAxisd(p[3]*M_PI/180.0, Eigen::Vector3d::UnitX())).toRotationMatrix();
-	return C;
-}
-
-CalibrationProblem::CalibrationProblem(const std::vector<Frame> & frames, const std::vector<int> & nodes, float minDepth) :
-	frames_(frames), nodes_(nodes), minDepth_(minDepth), evaluations_(0)
-{
-}
-
-void CalibrationProblem::project(const Transform & C,
-		const std::function<void(const Frame &, size_t, float, float)> & visit) const
-{
-	for(int k : nodes_)
-	{
-		const Frame & f = frames_[k];
-		const Transform scanInCam = (f.scanToCam * C).inverse();
-		const double fx = f.K.at<double>(0,0), fy = f.K.at<double>(1,1);
-		const double cx = f.K.at<double>(0,2), cy = f.K.at<double>(1,2);
-		for(size_t i = 0; i < f.edgePoints.size(); ++i)
-		{
-			const cv::Point3f pc = util3d::transformPoint(f.edgePoints[i], scanInCam);
-			if(pc.z < minDepth_)
-			{
-				continue;
-			}
-			const float u = fx * pc.x / pc.z + cx, v = fy * pc.y / pc.z + cy;
-			if(u < 0 || v < 0 || u >= f.gray.cols - 1 || v >= f.gray.rows - 1)
-			{
-				continue;
-			}
-			visit(f, i, u, v);
-		}
-	}
-}
-
-double CalibrationProblem::score(const Transform & C) const
-{
-	++evaluations_;
-	double sum = 0.0;
-	project(C, [&](const Frame & f, size_t i, float u, float v) {
-		const int u0 = int(u), v0 = int(v);
-		const float a = u - u0, b = v - v0;
-		const float s =
-				(1-a)*(1-b)*f.edgeScore.at<float>(v0, u0) + a*(1-b)*f.edgeScore.at<float>(v0, u0+1) +
-				(1-a)*b*f.edgeScore.at<float>(v0+1, u0) + a*b*f.edgeScore.at<float>(v0+1, u0+1);
-		sum += f.edgeWeights[i] * s;
-	});
-	// The frames' edge points are selected again between passes: not cached.
-	double weights = 0.0;
-	for(int k : nodes_)
-	{
-		for(float w : frames_[k].edgeWeights)
-		{
-			weights += w;
-		}
-	}
-	return weights > 0.0 ? sum / weights : 0.0;
-}
-
-double CalibrationProblem::edgeDistance(const Frame & f, size_t i, const double p[6], double cap) const
-{
-	const cv::Point3f & pt = f.edgePoints[i];
-	const Eigen::Vector3d pc =
-			(f.scanToCam.toEigen3d() * correctionFromDouble(p)).inverse() * Eigen::Vector3d(pt.x, pt.y, pt.z);
-	if(pc.z() < minDepth_)
-	{
-		return cap;
-	}
-	const double u = f.K.at<double>(0,0) * pc.x() / pc.z() + f.K.at<double>(0,2);
-	const double v = f.K.at<double>(1,1) * pc.y() / pc.z() + f.K.at<double>(1,2);
-	if(u < 0 || v < 0 || u >= f.edgeDistance.cols - 1 || v >= f.edgeDistance.rows - 1)
-	{
-		return cap;
-	}
-	const int u0 = int(u), v0 = int(v);
-	const double a = u - u0, b = v - v0;
-	const cv::Mat & d = f.edgeDistance;
-	const double distance =
-			(1-a)*(1-b)*d.at<float>(v0, u0) + a*(1-b)*d.at<float>(v0, u0+1) +
-			(1-a)*b*d.at<float>(v0+1, u0) + a*b*d.at<float>(v0+1, u0+1);
-	return std::min(distance, cap);
-}

tools/LidarCameraCalibration/CalibrationProblem.h

@@ -1,113 +0,0 @@
-/*
-Copyright (c) 2010-2026, 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 LIDARCAMERACALIBRATION_CALIBRATIONPROBLEM_H_
-#define LIDARCAMERACALIBRATION_CALIBRATIONPROBLEM_H_
-
-#include <rtabmap/core/Transform.h>
-
-#include <opencv2/core/core.hpp>
-#include <Eigen/Geometry>
-
-#include <functional>
-#include <vector>
-
-// What makes a lidar point an edge point, in order of precedence.
-enum EdgeType
-{
-	kEdgeDepth = 0,      // in front of a depth discontinuity
-	kEdgeCrease = 1,     // where the surface's orientation changes (e.g., wall and floor)
-	kEdgeIntensity = 2   // where the surface's reflectance changes
-};
-
-// One node of the database: its image, its lidar scan, and the lidar's edges.
-struct Frame
-{
-	int id;
-	cv::Mat gray;          // as stored, matching the camera model
-	cv::Mat edges;         // CV_8U: the image's edges (Canny), non-zero on an edge
-	cv::Mat edgeDistance;  // CV_32F: distance (pixels) to the image's nearest edge
-	cv::Mat edgeScore;     // CV_32F: 1 on the image's edges, decaying with the distance to them
-	cv::Mat K;             // CV_64F 3x3
-	rtabmap::Transform scanToCam;  // camera pose in the scan frame, before correction
-	cv::Mat cloud;         // Nx4 CV_32F, scan frame: x y z intensity (0 without intensity)
-	cv::Mat normals;       // Mx6 CV_32F, scan frame: voxelized points and their normals (for creases)
-	bool hasIntensity;
-	// The robot's speeds (m/s, deg/s; negative if unknown): instantaneous, odometry's when the
-	// node was added, and mean along odometry over the meanWindow (s) since the previous
-	// node, which is when an assembled scan was taken.
-	float linearSpeed = -1.0f, angularSpeed = -1.0f;
-	float meanLinearSpeed = -1.0f, meanAngularSpeed = -1.0f, meanWindow = 0.0f;
-	int meanPoses = 0;  // odometry steps the mean speed is from: more than 1 with intermediate nodes
-	std::vector<cv::Point3f> edgePoints;  // depth and intensity edge points, scan frame
-	std::vector<float> edgeWeights;
-	std::vector<unsigned char> edgeTypes; // EdgeType of each edge point
-};
-
-// The correction of parameters p = tx ty tz (m), rx ry rz (deg), in the camera frame.
-rtabmap::Transform correctionFrom(const double p[6]);
-// The same, in double precision: rtabmap's Transform is float, too coarse for the small
-// steps of numerical derivatives.
-Eigen::Isometry3d correctionFromDouble(const double p[6]);
-
-// The lidar edge points of some nodes, and how well a candidate correction C lines them
-// up with their images' edges. This is all a solver sees of the calibration.
-class CalibrationProblem
-{
-public:
-	// The frames' edge points are read at each call, so they can be selected again after
-	// the problem is made. minDepth: lidar points closer than this to the camera (m) are
-	// ignored.
-	CalibrationProblem(const std::vector<Frame> & frames, const std::vector<int> & nodes, float minDepth);
-
-	const std::vector<Frame> & frames() const {return frames_;}
-	const std::vector<int> & nodes() const {return nodes_;}
-	float minDepth() const {return minDepth_;}
-
-	// Calls visit(frame, point index, u, v) for each edge point that C projects in its
-	// image, at full resolution.
-	void project(const rtabmap::Transform & C,
-			const std::function<void(const Frame &, size_t, float, float)> & visit) const;
-
-	// What the direct solvers maximize: the weighted mean of the image edges' score under
-	// the projected lidar edge points, in [0, 1]. A point out of its image counts as 0.
-	double score(const rtabmap::Transform & C) const;
-	int evaluations() const {return evaluations_;}
-
-	// What the least-squares solvers minimize, per point: the distance (pixels) from
-	// where p projects edge point i of f to f's nearest image edge, capped; the cap where
-	// it does not project in the image. In double precision, for numerical derivatives.
-	double edgeDistance(const Frame & f, size_t i, const double p[6], double cap) const;
-
-private:
-	const std::vector<Frame> & frames_;
-	std::vector<int> nodes_;
-	float minDepth_;
-	mutable int evaluations_;
-};
-
-#endif /* LIDARCAMERACALIBRATION_CALIBRATIONPROBLEM_H_ */

tools/LidarCameraCalibration/CorrectionSolver.cpp

@@ -1,74 +0,0 @@
-/*
-Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without
-modification, are permitted provided that the following conditions are met:
-    * Redistributions of source code must retain the above copyright
-      notice, this list of conditions and the following disclaimer.
-    * Redistributions in binary form must reproduce the above copyright
-      notice, this list of conditions and the following disclaimer in the
-      documentation and/or other materials provided with the distribution.
-    * Neither the name of the Universite de Sherbrooke nor the
-      names of its contributors may be used to endorse or promote products
-      derived from this software without specific prior written permission.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
-ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
-WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
-DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
-ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-*/
-
-#include "CorrectionSolver.h"
-#include "DownhillSimplexSolver.h"
-#include "G2oSolver.h"
-#include "GtsamSolver.h"
-#include "PatternSearchSolver.h"
-
-#include <rtabmap/core/Version.h>
-
-const double kInitialSteps[6] = {0.04, 0.04, 0.04, 2.0, 2.0, 2.0};
-
-std::unique_ptr<CorrectionSolver> createSolver(const std::string & name, float sigma)
-{
-	(void)sigma;  // used only by the least-squares solvers
-	if(name == "pattern")
-	{
-		return std::unique_ptr<CorrectionSolver>(new PatternSearchSolver());
-	}
-	if(name == "simplex")
-	{
-		return std::unique_ptr<CorrectionSolver>(new DownhillSimplexSolver());
-	}
-#ifdef RTABMAP_G2O
-	if(name == "g2o")
-	{
-		return std::unique_ptr<CorrectionSolver>(new G2oSolver(sigma));
-	}
-#endif
-#ifdef RTABMAP_GTSAM
-	if(name == "gtsam")
-	{
-		return std::unique_ptr<CorrectionSolver>(new GtsamSolver(sigma));
-	}
-#endif
-	return std::unique_ptr<CorrectionSolver>();
-}
-
-std::vector<std::string> availableSolvers()
-{
-	std::vector<std::string> names = {"simplex", "pattern"};
-#ifdef RTABMAP_G2O
-	names.push_back("g2o");
-#endif
-#ifdef RTABMAP_GTSAM
-	names.push_back("gtsam");
-#endif
-	return names;
-}