Merge branch 'master' of github.com:introlab/rtabmap into kilted-devel

When estimating a missing depth from its 4-connected neighbors, a
neighbor is accepted if it is within depthErrorRatio of the mean of the
neighbors accepted so far. The tolerance was computed from the running
sum instead of the mean, so it grew to 2x and then 3x the ratio for the
third and fourth neighbor, letting inconsistent depths into the average.

Co-authored-by: matlabbe <[email protected]>

.dockerignore

@@ -1,2 +1,10 @@
 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

@@ -0,0 +1,93 @@
+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,9 +4,16 @@ 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,9 +4,16 @@ 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

@@ -3,10 +3,17 @@ name: CMake-ROS
 on:
   push:
     branches: 
-      - master
+      - kilted-devel
+    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:
@@ -14,31 +21,25 @@ env:
 
 concurrency:
   group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }}
-  cancel-in-progress: ${{ github.event_name == 'pull_request' }}
+  cancel-in-progress: true
 
 jobs:
   build:
-    name: ${{ matrix.ros_distribution }}
+    name: ${{ matrix.ros_distribution }}${{ matrix.use_ros2_testing && '-testing' || '' }}
     runs-on: ubuntu-latest
     concurrency:
-      group: ${{ github.workflow }}-${{ github.ref }}-${{ matrix.ros_distribution }}
+      group: ${{ github.workflow }}-${{ github.ref }}-${{ matrix.ros_distribution }}-${{ matrix.use_ros2_testing }}
       cancel-in-progress: true
     strategy:
       fail-fast: false
       matrix:
-        ros_distribution: [ humble, jazzy, kilted, lyrical, rolling]
+        ros_distribution: [ kilted ]
+        # Build against both main (what users install) and ros2-testing (closest to
+        # what the buildfarm builds bloom releases against).
+        use_ros2_testing: [ false, true ]
         include:
-        - ros_distribution: 'humble'
-          skip_keys: ""
-        - ros_distribution: 'jazzy'
-          skip_keys: ""
         - ros_distribution: 'kilted'
-          skip_keys: ""
-        - ros_distribution: 'lyrical'
-          skip_keys: "libpointmatcher"
-        - ros_distribution: 'rolling'
-          skip_keys: "libpointmatcher gtsam"
-          use_ros2_testing: true # Rolling is using ros2-testing (nightly)
+          skip_keys: "" # When releasing to ROS2, the skip_keys should be empty, patch these deps in package.xml instead.
     container:
       image: osrf/ros:${{ matrix.ros_distribution }}-desktop-full
     steps:
@@ -87,10 +88,23 @@ jobs:
         ls "$root"/tests/*.db >/dev/null || { echo "::error::no test databases in $root/tests"; exit 1; }
         echo "root=$root" >> "$GITHUB_OUTPUT"
 
+    # The osrf/ros image ships ros2-apt-source (main), which conflicts with the
+    # ros2-testing-apt-source package setup-ros tries to install. Remove it so
+    # setup-ros can switch the image to the testing repo.
+    - name: Remove ROS main apt source
+      if: matrix.use_ros2_testing
+      run: dpkg --purge ros2-apt-source
+
     - uses: ros-tooling/[email protected]
       with:
         required-ros-distributions: ${{ matrix.ros_distribution }}
-        use-ros2-testing: ${{ matrix.use_ros2_testing || false }}
+        use-ros2-testing: ${{ matrix.use_ros2_testing }}
+
+    # setup-ros doesn't upgrade on noble/resolute, so the packages preinstalled
+    # in the image would stay at their main versions. Upgrade them to testing.
+    - name: Upgrade ROS packages to testing
+      if: matrix.use_ros2_testing
+      run: apt-get update && apt-get dist-upgrade -y
     - uses: ros-tooling/[email protected]
       with:
         package-name: rtabmap

.github/workflows/cmake-windows.yml

@@ -4,9 +4,16 @@ 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:
@@ -53,6 +60,48 @@ 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
@@ -92,6 +141,105 @@ 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,9 +4,16 @@ 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

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

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

@@ -22,7 +22,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 23)
-SET(RTABMAP_PATCH_VERSION 11)
+SET(RTABMAP_PATCH_VERSION 13)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
 
@@ -620,14 +620,7 @@ IF(WITH_GTSAM)
     # Force config mode to ignore PCL's findGTSAM.cmake file
     FIND_PACKAGE(GTSAM CONFIG QUIET)
     IF(GTSAM_FOUND)
-      # 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)
+      INCLUDE(${CMAKE_CURRENT_SOURCE_DIR}/cmake_modules/CheckGTSAMFeatures.cmake)
     ENDIF(GTSAM_FOUND)
 ENDIF(WITH_GTSAM)
 
@@ -1064,6 +1057,7 @@ 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

@@ -42,7 +42,8 @@ This project is supported by [IntRoLab - Intelligent / Interactive / Integrated
            <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"/>
+           <a href="https://github.com/introlab/rtabmap/actions/workflows/docker-ros.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/docker-ros.yml/badge.svg" alt="Docker ROS Build Status"/> <br> 
+           <a href="https://github.com/introlab/rtabmap/actions/workflows/android.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/android.yml/badge.svg" alt="Android Build Status"/>
            </td>
         </tr>
      </tbody>

cmake_modules/CheckGTSAMFeatures.cmake

@@ -0,0 +1,27 @@
+# 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/DBDriver.h

@@ -331,6 +331,12 @@ 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;
@@ -457,6 +463,9 @@ 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,6 +156,40 @@ 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,6 +201,7 @@ 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,
@@ -209,8 +210,8 @@ public:
 	        int knn,
 			int checks = 32,
 			float eps = 0.0,
-			bool sorted = true) const;
-
+			bool sorted = true,
+			int cores = 1) 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
@@ -225,6 +226,7 @@ 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,
@@ -234,7 +236,8 @@ public:
 			int maxNeighbors = 0,
 			int checks = 32,
 			float eps = 0.0,
-			bool sorted = true) const;
+			bool sorted = true,
+			int cores = 1) const;
 
 private:
 	void * index_;               // rtflann backend

corelib/include/rtabmap/core/Graph.h

@@ -490,6 +490,29 @@ 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,6 +847,7 @@ private:
 	bool _stereoFromMotion;
 	unsigned int _imagePreDecimation;
 	unsigned int _imagePostDecimation;
+	bool _legacyDecimatedOctave;
 	bool _compressionParallelized;
 	float _laserScanDownsampleStepSize;
 	float _laserScanVoxelSize;

corelib/include/rtabmap/core/Parameters.h

@@ -256,6 +256,7 @@ 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.");
@@ -471,7 +472,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 or GPS) while optimizing. Currently only g2o and gtsam optimization supports this.");
+    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, 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()));
@@ -960,7 +961,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,             "",           "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_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(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,21 +470,33 @@ public:
 	 * @brief Checks if the sensor data is valid
 	 * 
 	 * Returns true if the sensor data contains at least one of:
-	 * - Valid ID (> 0) or non-zero stamp
+	 * - Valid ID (> 0)
 	 * - 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)
+	 * - Camera models (mono or stereo), at least one valid for projection
+	 *   (see CameraModel::isValidForProjection() and StereoCameraModel::isValidForProjection())
 	 * - 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() &&
@@ -493,8 +505,7 @@ public:
 			_depthConfidenceCompressed.empty() &&
 			_laserScanRaw.isEmpty() &&
 			_laserScanCompressed.isEmpty() &&
-			_cameraModels.empty() &&
-			_stereoCameraModels.empty() &&
+			!hasCameraModel &&
 			_userDataRaw.empty() &&
 			_userDataCompressed.empty() &&
 			_keypoints.size() == 0 &&
@@ -731,11 +742,15 @@ public:
 
 	/**
 	 * Set user data. Detect automatically if raw or compressed. If raw, the data is
-	 * compressed too. A matrix of type CV_8UC1 with 1 row is considered as compressed.
+	 * 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.
 	 * 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;}

corelib/include/rtabmap/core/StereoCameraModel.h

@@ -427,6 +427,51 @@ 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,6 +495,11 @@ 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,10 +41,6 @@ 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,7 +150,8 @@ 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);
+		bool distanceToCamPolicy = false,
+		int numThreads = 1); // number of threads used to compute the visible faces of the cameras (1=sequential)
 pcl::TextureMesh::Ptr RTABMAP_CORE_EXPORT createTextureMesh(
 		const pcl::PolygonMesh::Ptr & mesh,
 		const std::map<int, Transform> & poses,
@@ -163,7 +164,8 @@ 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);
+		bool distanceToCamPolicy = false,
+		int numThreads = 1); // number of threads used to compute the visible faces of the cameras (1=sequential)
 
 /**
  * Remove not textured polygon clusters. If minClusterSize<0, only the largest cluster is kept.

corelib/src/CMakeLists.txt

@@ -689,16 +689,42 @@ 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(INCLUDE_DIRS
-            ${INCLUDE_DIRS}
-            ${grid_map_core_INCLUDE_DIRS}
-        )
+        SET(grid_map_core_PUBLIC_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
@@ -898,6 +924,12 @@ 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/DBDriver.cpp

@@ -707,7 +707,10 @@ void DBDriver::getNodeData(
 			((!images || !s->sensorData().imageCompressed().empty()) &&
 			 (!scan || !s->sensorData().laserScanCompressed().isEmpty()) &&
 			 (!userData || !s->sensorData().userDataCompressed().empty()) &&
-			 (!occupancyGrid || s->sensorData().gridCellSize() != 0.0f))))
+			 (!occupancyGrid ||
+				!s->sensorData().gridGroundCellsCompressed().empty() ||
+				!s->sensorData().gridObstacleCellsCompressed().empty() ||
+				!s->sensorData().gridEmptyCellsCompressed().empty()))))
 		{
 			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
-		getLastWordId(id);
+		// Get Last word id (query directly: _dbSafeAccessMutex is already locked by DBDriver::load())
+		getLastIdQuery("Word", id);
 		dictionary.setLastWordId(id);
 
 		if(!idsOnly && uStrNumCmp(_version, "0.23.0") >= 0) {

corelib/src/Features2d.cpp

@@ -2282,20 +2282,6 @@ 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,9 +36,33 @@ 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),
@@ -910,7 +934,8 @@ void FlannIndex::knnSearch(
 		int knn,
 		int checks,
 		float eps,
-		bool sorted) const
+		bool sorted,
+		int cores) const
 {
 	if(nanoIndex_)
 	{
@@ -930,6 +955,7 @@ 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)
 	{
@@ -974,11 +1000,12 @@ void FlannIndex::radiusSearch(
 		int maxNeighbors,
 		int checks,
 		float eps,
-		bool sorted) const
+		bool sorted,
+		int cores) const
 {
 	if(nanoIndex_)
 	{
-		// "checks" doesn't apply
+		// "checks" and "cores" don't apply, it searches on one core
 		nanoIndex_->radiusSearch(query, indices, dists, radius, maxNeighbors, eps, sorted);
 		return;
 	}
@@ -990,6 +1017,7 @@ 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.find(from);
+	std::multimap<int, Link>::iterator iter = links.lower_bound(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.find(to);
+		iter = links.lower_bound(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.find(from);
+	std::multimap<int, std::pair<int, Link::Type> >::iterator iter = links.lower_bound(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.find(to);
+		iter = links.lower_bound(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.find(from);
+	std::multimap<int, int>::iterator iter = links.lower_bound(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.find(to);
+		iter = links.lower_bound(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.find(from);
+	std::multimap<int, Link>::const_iterator iter = links.lower_bound(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.find(to);
+		iter = links.lower_bound(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.find(from);
+	std::multimap<int, std::pair<int, Link::Type> >::const_iterator iter = links.lower_bound(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.find(to);
+		iter = links.lower_bound(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.find(from);
+	std::multimap<int, int>::const_iterator iter = links.lower_bound(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.find(to);
+		iter = links.lower_bound(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.find(id); jter!=bidirectionalLoopClosureLinks.end() && jter->first==id; ++jter)
+					for(std::multimap<int, Link>::const_iterator jter=bidirectionalLoopClosureLinks.lower_bound(id); jter!=bidirectionalLoopClosureLinks.end() && jter->first==id; ++jter)
 					{
 						if(posesToHyperNodes.find(jter->second.to()) == posesToHyperNodes.end() &&
 						   loopClosuresAdded.find(jter->second.to()) == loopClosuresAdded.end())
@@ -1904,6 +1904,39 @@ 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,6 +101,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	_stereoFromMotion(Parameters::defaultMemStereoFromMotion()),
     _imagePreDecimation(Parameters::defaultMemImagePreDecimation()),
 	_imagePostDecimation(Parameters::defaultMemImagePostDecimation()),
+	_legacyDecimatedOctave(false),
 	_compressionParallelized(Parameters::defaultMemCompressionParallelized()),
 	_laserScanDownsampleStepSize(Parameters::defaultMemLaserScanDownsampleStepSize()),
 	_laserScanVoxelSize(Parameters::defaultMemLaserScanVoxelSize()),
@@ -220,6 +221,23 @@ 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;
+			// 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
@@ -4777,7 +4795,10 @@ 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().gridCellSize() != 0.0f))))
+			 (!occupancyGrid ||
+				!s->sensorData().gridGroundCellsCompressed().empty() ||
+				!s->sensorData().gridObstacleCellsCompressed().empty() ||
+				!s->sensorData().gridEmptyCellsCompressed().empty()))))
 	{
 		r = s->sensorData();
 		if(!images)
@@ -5660,7 +5681,13 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
                 if(_imagePreDecimation > 1 || useProvided3dPoints)
                 {
                     float decimationRatio = 1.0f / float(_imagePreDecimation);
-                    double log2value = log(double(_imagePreDecimation))/log(2.0);
+                    // 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);
                     for(unsigned int i=0; i < keypoints.size(); ++i)
                     {
                         cv::KeyPoint & kpt = keypoints[i];
@@ -5669,7 +5696,10 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
                             kpt.pt.x *= decimationRatio;
                             kpt.pt.y *= decimationRatio;
                             kpt.size *= decimationRatio;
-                            kpt.octave += log2value;
+                            // 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));
                         }
                         if(useProvided3dPoints)
                         {
@@ -6246,7 +6276,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(preDecimation))/log(2.0);
+		double log2value = log(double(decimationRatio))/log(2.0);
 		for(std::list<int>::iterator iter=wordIds.begin(); iter!=wordIds.end() && i < keypoints.size(); ++iter, ++i)
 		{
 			cv::KeyPoint kpt = keypoints[i];
@@ -6256,7 +6286,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 				kpt.pt.x *= decimationRatio;
 				kpt.pt.y *= decimationRatio;
 				kpt.size *= decimationRatio;
-				kpt.octave += log2value;
+				kpt.octave = std::max(0, int(kpt.octave + log2value));
 			}
 			words.insert(std::make_pair(*iter, words.size()));
 			wordsKpts.push_back(kpt);
@@ -6633,6 +6663,20 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			}
 		}
 
+		bool reuseCompressedImage =
+				image.data == data.imageRaw().data &&
+				!data.imageCompressed().empty();
+		bool reuseCompressedDepth =
+				depthOrRightImage.data == data.depthOrRightRaw().data &&
+				!data.depthOrRightCompressed().empty();
+		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;
@@ -6645,23 +6689,23 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			rtabmap::CompressionThread ctDepthConfidence(depthConfidence);
 			rtabmap::CompressionThread ctLaserScan(laserScan.data());
 			rtabmap::CompressionThread ctUserData(data.userDataRaw());
-			if(!image.empty())
+			if(!image.empty() && !reuseCompressedImage)
 			{
 				ctImage.start();
 			}
-			if(!depthOrRightImage.empty())
+			if(!depthOrRightImage.empty() && !reuseCompressedDepth)
 			{
 				ctDepth.start();
 			}
-			if(!depthConfidence.empty())
+			if(!depthConfidence.empty() && !reuseCompressedDepthConfidence)
 			{
 				ctDepthConfidence.start();
 			}
-			if(!laserScan.isEmpty())
+			if(!laserScan.isEmpty() && !reuseCompressedScan)
 			{
 				ctLaserScan.start();
 			}
-			if(!data.userDataRaw().empty())
+			if(!data.userDataRaw().empty() && !reuseCompressedUserData)
 			{
 				ctUserData.start();
 			}
@@ -6674,16 +6718,16 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			compressedImage = ctImage.getCompressedData();
 			compressedDepth = ctDepth.getCompressedData();
 			compressedDepthConfidence = ctDepthConfidence.getCompressedData();
-			compressedScan = ctLaserScan.getCompressedData();
-			compressedUserData = ctUserData.getCompressedData();
+			compressedScan = reuseCompressedScan?data.laserScanCompressed().data():ctLaserScan.getCompressedData();
+			compressedUserData = reuseCompressedUserData?data.userDataCompressed():ctUserData.getCompressedData();
 		}
 		else
 		{
-			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());
+			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());
 		}
 
 		s = new Signature(id,
@@ -6747,28 +6791,32 @@ 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)
+			if(!data.userDataRaw().empty() && !isIntermediateNode && !reuseCompressedUserData)
 			{
 				ctUserData.start();
 			}
-			if(!laserScan.isEmpty() && !isIntermediateNode)
+			if(!laserScan.isEmpty() && !isIntermediateNode && !reuseCompressedScan)
 			{
 				ctLaserScan.start();
 			}
 			ctUserData.join();
 			ctLaserScan.join();
 
-			compressedScan = ctLaserScan.getCompressedData();
-			compressedUserData = ctUserData.getCompressedData();
+			compressedScan = reuseCompressedScan?data.laserScanCompressed().data():ctLaserScan.getCompressedData();
+			compressedUserData = reuseCompressedUserData && !isIntermediateNode?data.userDataCompressed():ctUserData.getCompressedData();
 		}
 		else
 		{
-			compressedScan = compressData2(laserScan.data());
-			compressedUserData = compressData2(data.userDataRaw());
+			compressedScan = reuseCompressedScan?data.laserScanCompressed().data():compressData2(laserScan.data());
+			compressedUserData = reuseCompressedUserData?data.userDataCompressed():compressData2(data.userDataRaw());
 		}
 
 		s = new Signature(id,

corelib/src/Odometry.cpp

@@ -779,6 +779,26 @@ 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);
 
@@ -817,7 +837,14 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 			}
 		}
 	}
-	else if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty() || (this->canProcessAsyncIMU() && !data.imu().empty()))
+	// 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()))
 	{
 		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.find(currentId); pter!=linksIn.end() && pter->first==currentId; ++pter)
+		for(std::multimap<int, Link>::const_iterator pter=linksIn.lower_bound(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.find(currentId); iter!=biLinks.end() && iter->first==currentId; ++iter)
+		for(std::multimap<int, std::pair<int, Link::Type> >::const_iterator iter=biLinks.lower_bound(currentId); iter!=biLinks.end() && iter->first==currentId; ++iter)
 		{
 			int toId = iter->second.first;
 			Link::Type type = iter->second.second;

corelib/src/Rtabmap.cpp

@@ -2746,7 +2746,6 @@ 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
@@ -2765,7 +2764,6 @@ 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()) && 
@@ -2778,18 +2776,23 @@ 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::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)
+							std::map<int, int>::const_iterator depthIter = proximityPathDepths.find(iter->first);
+							if(depthIter == proximityPathDepths.end())
 							{
-								nearestPoses.insert(std::make_pair(iter->first, _optimizedPoses.at(iter->first)));
+								continue;
 							}
+							nearestPoses.insert(std::make_pair(iter->first, _optimizedPoses.at(iter->first)));
 						}
 						else
 						{
@@ -4661,7 +4664,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.find(lastId); iter!=_constraints.end() && iter->first==lastId;++iter)
+				for(std::multimap<int, Link>::iterator iter=_constraints.lower_bound(lastId); iter!=_constraints.end() && iter->first==lastId;++iter)
 				{
 					if(iter->second.to() != iter->second.from())
 					{
@@ -5917,6 +5920,11 @@ 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 -= log2value;
+				kpts[i].octave = std::max(0, int(kpts[i].octave - log2value));
 			}
 			data.setFeatures(kpts, data.keypoints3D(), data.descriptors());
 		}

corelib/src/SensorData.cpp

@@ -568,7 +568,7 @@ void SensorData::setUserData(const cv::Mat & userData, bool clearPreviousData)
 	else
 	{
 		_userDataRaw = userData;
-		if(!userData.empty())
+		if(!userData.empty() && _userDataCompressed.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.find(idTo); iter!=_links.end() && iter->first == idTo; ++iter)
+		for(std::multimap<int, Link>::const_iterator iter=_links.lower_bound(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.find(idFrom);
+	std::multimap<int, Link>::iterator iter = _links.lower_bound(idFrom);
 	while(iter != _links.end() && iter->first == idFrom)
 	{
 		Link link = iter->second;

corelib/src/StereoCameraModel.cpp

@@ -598,6 +598,30 @@ 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,6 +101,7 @@ 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()),
@@ -130,6 +131,7 @@ 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);
 
@@ -1074,7 +1076,7 @@ std::list<int> VWDictionary::addNewWords(
 
 		if(isFlannStrategy(_strategy))
 		{
-			_flannIndex->knnSearch(descriptors, results, dists, k, KNN_CHECKS);
+			_flannIndex->knnSearch(descriptors, results, dists, k, KNN_CHECKS, 0.0f, true, _flannThreads);
 		}
 		else if(_strategy == kNNBruteForce)
 		{
@@ -1396,7 +1398,7 @@ std::vector<int> VWDictionary::findNN(const cv::Mat & queryIn) const
 
 			if(isFlannStrategy(_strategy))
 			{
-				_flannIndex->knnSearch(query, results, dists, k, KNN_CHECKS);
+				_flannIndex->knnSearch(query, results, dists, k, KNN_CHECKS, 0.0f, true, _flannThreads);
 			}
 			else if(_strategy == kNNBruteForce)
 			{

corelib/src/global_map/GridMap.cpp

@@ -237,6 +237,11 @@ 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))
@@ -245,8 +250,13 @@ void GridMap::assemble(const std::list<std::pair<int, Transform> > & newPoses)
 
 				if(!localGrid.is3D())
 				{
-					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());
+					if(++not3DCount == 1)
+					{
+						not3DFirstId = iter->first;
+						not3DGroundType = localGrid.groundCells.type();
+						not3DObstaclesType = localGrid.obstacleCells.type();
+						not3DEmptyType = localGrid.emptyCells.type();
+					}
 					continue;
 				}
 
@@ -339,6 +349,12 @@ 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,6 +479,11 @@ 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;
@@ -491,8 +496,13 @@ void OctoMap::assemble(const std::list<std::pair<int, Transform> > & newPoses)
 
 				if(!localGridIter->second.is3D())
 				{
-					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());
+					if(++not3DCount == 1)
+					{
+						not3DFirstId = iter->first;
+						not3DGroundType = ground.type();
+						not3DObstaclesType = obstacles.type();
+						not3DEmptyType = emptyCells.type();
+					}
 					continue;
 				}
 
@@ -762,6 +772,12 @@ 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,6 +1469,25 @@ 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,9 +504,7 @@ 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());
-#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.
+#ifndef RTABMAP_GTSAM_HAS_ATTITUDE_FACTOR_TEMPLATE
 					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,7 +100,8 @@ namespace vertigo {
         // handle derivatives
         if (H1) *H1 = *H1 * w;
         if (H2) *H2 = *H2 * w;
-        if (H3) *H3 = error /* (w*(1.0-w))*/;  // sig(x)*(1-sig(x)) is the derivative of sig(x) wrt. x
+        // error already includes w; sigmoid's derivative is w*(1-w).
+        if (H3) *H3 = error * (1.0-w);
 
         return error;
       };

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

@@ -13,6 +13,7 @@
 // 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 {
@@ -45,6 +46,7 @@ namespace vertigo {
     }
 
     // Manifold requirements
+    static constexpr int dimension = 1;
 
     /** Returns dimensionality of the tangent space */
     inline size_t dim() const { return 1; }
@@ -61,7 +63,13 @@ namespace vertigo {
     }
 
     /** @return the local coordinates of another object */
-    inline gtsam::Vector localCoordinates(const SwitchVariableLinear& t2) const { return gtsam::Vector1(t2.value() - value()); }
+    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());
+    }
 
     // Group requirements
 
@@ -108,36 +116,9 @@ namespace vertigo {
 }
 
 namespace gtsam {
-// 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);
-    }
-};
+// Use the scalar manifold's dimension, category and chart operations.
+template<> struct traits<vertigo::SwitchVariableLinear>
+    : internal::Manifold<vertigo::SwitchVariableLinear> {};
 }
 
 

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

@@ -13,6 +13,7 @@
 // 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 {
@@ -45,6 +46,7 @@ namespace vertigo {
     }
 
     // Manifold requirements
+    static constexpr int dimension = 1;
 
     /** Returns dimensionality of the tangent space */
     inline size_t dim() const { return 1; }
@@ -61,7 +63,13 @@ namespace vertigo {
     }
 
     /** @return the local coordinates of another object */
-    inline gtsam::Vector localCoordinates(const SwitchVariableSigmoid& t2) const { return gtsam::Vector1(t2.value() - value()); }
+    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());
+    }
 
     // Group requirements
 
@@ -109,36 +117,9 @@ namespace vertigo {
 
 
 namespace gtsam {
-// 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);
-    }
-};
+// Use the scalar manifold's dimension, category and chart operations.
+template<> struct traits<vertigo::SwitchVariableSigmoid>
+    : internal::Manifold<vertigo::SwitchVariableSigmoid> {};
 }
 
 #endif /* SWITCHVARIABLESIGMOID_H_ */

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

@@ -42,6 +42,9 @@
 #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> >
@@ -1051,7 +1054,8 @@ pcl::TextureMapping<PointInT>::textureMeshwithMultipleCameras2 (
 		const pcl::texture_mapping::CameraVector &cameras,
 		const rtabmap::ProgressState * state,
 		std::vector<std::map<int, pcl::PointXY> > * vertexToPixels,
-		bool distanceToCamPolicy)
+		bool distanceToCamPolicy,
+		int numThreads)
 {
 
 	if (mesh.tex_polygons.size () != 1)
@@ -1081,7 +1085,17 @@ pcl::TextureMapping<PointInT>::textureMeshwithMultipleCameras2 (
 		UWARN("Texturing cancelled!");
 		return false;
 	}
-	for (unsigned int current_cam = 0; current_cam < cameras.size(); ++current_cam)
+	// 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)
 	{
 		UDEBUG("Texture camera %d...", current_cam);
 
@@ -1259,7 +1273,6 @@ 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);
 			}
 		}
 
@@ -1276,14 +1289,55 @@ pcl::TextureMapping<PointInT>::textureMeshwithMultipleCameras2 (
 			}
 		}
 
-		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))
+		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)
 		{
-			//cancelled!
-			UWARN("Texturing cancelled!");
-			return false;
+			computeVisibleFaces((unsigned int)(chunkStart+i), chunkVisibility[i]);
 		}
+
+		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,7 +368,8 @@ 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);
+									  bool distanceToCamPolicy = false,
+									  int numThreads = 1); // number of threads used to compute the visible faces of the cameras (1=sequential)
 
     protected:
       /** \brief mesh scale control. */

corelib/src/util2d.cpp

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

corelib/src/util3d.cpp

@@ -2328,10 +2328,59 @@ pcl::PCLPointCloud2::Ptr laserScanToPointCloud2(const LaserScan & laserScan, con
 	{
 		pcl::toPCLPointCloud2(*laserScanToPointCloud(laserScan, transform), *cloud);
 	}
-	else if(laserScan.format() == LaserScan::kXYI || laserScan.format() == LaserScan::kXYZI || laserScan.format() == LaserScan::kXYZIT || laserScan.format() == LaserScan::kXYZIRT)
+	else if(laserScan.format() == LaserScan::kXYI || laserScan.format() == LaserScan::kXYZI)
 	{
 		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,7 +738,8 @@ pcl::TextureMesh::Ptr createTextureMesh(
 		const std::vector<float> & roiRatios,
 		const ProgressState * state,
 		std::vector<std::map<int, pcl::PointXY> > * vertexToPixels,
-		bool distanceToCamPolicy)
+		bool distanceToCamPolicy,
+		int numThreads)
 {
 	std::map<int, std::vector<CameraModel> > cameraSubModels;
 	for(std::map<int, CameraModel>::const_iterator iter=cameraModels.begin(); iter!=cameraModels.end(); ++iter)
@@ -760,7 +761,8 @@ pcl::TextureMesh::Ptr createTextureMesh(
 			roiRatios,
 			state,
 			vertexToPixels,
-			distanceToCamPolicy);
+			distanceToCamPolicy,
+			numThreads);
 }
 
 pcl::TextureMesh::Ptr createTextureMesh(
@@ -775,7 +777,8 @@ pcl::TextureMesh::Ptr createTextureMesh(
 		const std::vector<float> & roiRatios,
 		const ProgressState * state,
 		std::vector<std::map<int, pcl::PointXY> > * vertexToPixels,
-		bool distanceToCamPolicy)
+		bool distanceToCamPolicy,
+		int numThreads)
 {
 	UASSERT(mesh->polygons.size());
 	pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh);
@@ -837,7 +840,7 @@ pcl::TextureMesh::Ptr createTextureMesh(
 	tm.setMaxAngle(maxAngle);
 	tm.setMaxDepthError(maxDepthError);
 	tm.setMinClusterSize(minClusterSize);
-	if(tm.textureMeshwithMultipleCameras2(*textureMesh, cameras, state, vertexToPixels, distanceToCamPolicy))
+	if(tm.textureMeshwithMultipleCameras2(*textureMesh, cameras, state, vertexToPixels, distanceToCamPolicy, numThreads))
 	{
 		// compute normals for the mesh if not already here
 		bool hasNormals = false;

corelib/test/CMakeLists.txt

@@ -68,9 +68,20 @@ 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).
@@ -137,6 +148,20 @@ 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")
 ENDIF(BUILD_PERF_TESTS)
 
 # Rtabmap end-to-end replay of sample DBs (test data fetched by
@@ -155,6 +180,12 @@ 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,12 +28,21 @@ 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. 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.
+	// baseline every index has to beat.
 	bool bruteForce = false;
-	bool singleCore = 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;
 };
 
 // Every algorithm that indexes float features. The exhaustive search comes
@@ -41,9 +50,8 @@ struct Backend
 // found and for the time taken.
 const Backend FLOAT_BACKENDS[] = {
 	{"linear    exhaustive                ", FlannIndex::FLANN_INDEX_LINEAR},
-	// 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},
+	{"cv        BFMatcher                 ", FlannIndex::FLANN_INDEX_LINEAR, 1.0f, true, 1},
+	{"cv        BFMatcher threaded        ", FlannIndex::FLANN_INDEX_LINEAR, 1.0f, true, 0},
 	{"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},
@@ -64,8 +72,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},
-	{"cv        BFMatcher (hamming,1 core)", FlannIndex::FLANN_INDEX_LINEAR, 1.0f, true, true},
+	{"cv        BFMatcher hamming         ", FlannIndex::FLANN_INDEX_LINEAR, 1.0f, true, 1},
+	{"cv        BFMatcher hamming threaded", FlannIndex::FLANN_INDEX_LINEAR, 1.0f, true, 0},
 	{"rtflann   LSH                       ", FlannIndex::FLANN_INDEX_LSH},
 };
 
@@ -188,11 +196,12 @@ inline Result run(
 
 	if(backend.bruteForce)
 	{
-		// cv::setNumThreads() is global, put it back before leaving.
+		// cv::setNumThreads() is global, put it back before leaving. Left alone
+		// for cores=0: OpenCV's own default is already one thread per core.
 		const int threads = cv::getNumThreads();
-		if(backend.singleCore)
+		if(backend.cores > 0)
 		{
-			cv::setNumThreads(1);
+			cv::setNumThreads(backend.cores);
 		}
 
 		UTimer timer;
@@ -221,10 +230,7 @@ inline Result run(
 			result.radiusTime = timer.ticks();
 		}
 		result.memory = 0; // it indexes nothing
-		if(backend.singleCore)
-		{
-			cv::setNumThreads(threads);
-		}
+		cv::setNumThreads(threads);
 		return result;
 	}
 
@@ -233,7 +239,7 @@ inline Result run(
 	index.buildIndex(backend.algorithm, data, false, rebalancingFactor);
 	result.buildTime = timer.ticks();
 
-	index.knnSearch(queries, result.indices, dists, knn);
+	index.knnSearch(queries, result.indices, dists, knn, 32, 0.0f, true, backend.cores);
 	result.knnTime = timer.ticks();
 
 	if(radius > 0.0f)

corelib/test/perf_flann_index.cpp

@@ -11,6 +11,10 @@
 // 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.
@@ -517,16 +521,31 @@ 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.
-	const Backend backends[] = {
-		{"cv        BFMatcher                 ", FlannIndex::FLANN_INDEX_LINEAR, 1.0f, true},
+	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},
 		{"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)
 	{
@@ -538,7 +557,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);
+			index.radiusSearch(projected, indices, dists, radius, 0, 32, 0.0f, false, backend.cores);
 		}
 		const double perFrame = timer.ticks()/double(frames);
 
@@ -571,15 +590,31 @@ 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.
-	const Backend backends[] = {
+	std::vector<Backend> backends = {
 		{"linear    exhaustive                ", FlannIndex::FLANN_INDEX_LINEAR, 1.0f},
-		{"cv        BFMatcher                 ", FlannIndex::FLANN_INDEX_LINEAR, 1.0f, true},
+		{"cv        BFMatcher                 ", FlannIndex::FLANN_INDEX_LINEAR, 1.0f, true, 1},
+		{"cv        BFMatcher threaded        ", FlannIndex::FLANN_INDEX_LINEAR, 1.0f, true, 0},
 		{"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);
@@ -599,7 +634,7 @@ void compareDictionaryMatching(int indexedCount, int queriedCount)
 			{
 				FlannIndex index;
 				index.buildIndex(backend.algorithm, from, false, backend.rebalancingFactor);
-				index.knnSearch(to, indices, dists, KNN);
+				index.knnSearch(to, indices, dists, KNN, 32, 0.0f, true, backend.cores);
 			}
 			const double perFrame = timer.ticks()/double(frames);
 

corelib/test/perf_graph.cpp

@@ -0,0 +1,315 @@
+// 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,6 +298,45 @@ 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_dbdriver.cpp

@@ -956,6 +956,53 @@ 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,9 +50,12 @@ protected:
 		}
 	}
 
+	// Trash-checking methods are hidden in DBDriverSqlite3, call them through the base class
+	DBDriver * db() const { return driver_; }
+
 	void saveSignature(Signature * s)
 	{
-		driver_->asyncSave(s);
+		db()->asyncSave(s);
 		driver_->emptyTrashes(false);
 	}
 
@@ -103,7 +106,7 @@ TEST(DBDriverSqlite3Test, ParseParametersEnablesInMemory)
 	EXPECT_TRUE(driver.isInMemory());
 	EXPECT_TRUE(driver.isConnected());
 
-	driver.asyncSave(new Signature(1));
+	static_cast<DBDriver &>(driver).asyncSave(new Signature(1));
 	driver.emptyTrashes(false);
 	EXPECT_EQ(driver.getTotalNodesSize(), 1);
 
@@ -121,7 +124,7 @@ TEST(DBDriverSqlite3Test, InMemorySaveToFileOnClose)
 	ASSERT_TRUE(driver.openConnection(path, true));
 	EXPECT_TRUE(driver.isInMemory());
 
-	driver.asyncSave(new Signature(1, 5, 1, 50.0, "sqlite_mem", Transform(1.f, 0.f, 0.f, 0.f, 0.f, 0.f)));
+	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.emptyTrashes(false);
 	driver.closeConnection(true, path);
 
@@ -233,7 +236,7 @@ TEST_F(DBDriverSqlite3Fixture, SavesAndLoadsRichSensorData)
 	saveSignature(s);
 
 	std::list<Signature *> loaded;
-	driver_->loadSignatures(std::list<int>(1, 10), loaded);
+	db()->loadSignatures(std::list<int>(1, 10), loaded);
 	ASSERT_EQ(1u, loaded.size());
 	Signature * back = loaded.front();
 	EXPECT_EQ(10, back->id());
@@ -244,7 +247,7 @@ TEST_F(DBDriverSqlite3Fixture, SavesAndLoadsRichSensorData)
 
 	// Payloads come back compressed; ask the driver to fill them in.
 	std::list<Signature *> toFill(1, back);
-	driver_->loadNodeData(toFill);
+	db()->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";
@@ -320,9 +323,9 @@ TEST_F(DBDriverSqlite3Fixture, RawOnlySensorDataIsNotPersisted)
 	saveSignature(new Signature(42, 0, 1, 1.0, "", Transform::getIdentity(), Transform(), raw));
 
 	std::list<Signature *> loaded;
-	driver_->loadSignatures(std::list<int>(1, 42), loaded);
+	db()->loadSignatures(std::list<int>(1, 42), loaded);
 	ASSERT_EQ(1u, loaded.size());
-	driver_->loadNodeData(loaded);
+	db()->loadNodeData(loaded);
 	loaded.front()->sensorData().uncompressData();
 	EXPECT_TRUE(loaded.front()->sensorData().imageRaw().empty())
 			<< "raw-only image unexpectedly survived a save/load round trip";
@@ -363,9 +366,12 @@ 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)
 	{
-		driver_->asyncSave(s);
+		db()->asyncSave(s);
 		driver_->emptyTrashes(false);
 	}
 
@@ -400,11 +406,11 @@ TEST_P(DBSchemaVersionTest, NodesAndLinksSurviveARoundTrip)
 	EXPECT_FALSE(driver_->getDatabaseVersion().empty());
 
 	std::list<Signature *> loaded;
-	driver_->loadSignatures(std::list<int>{1, 2}, loaded);
+	db()->loadSignatures(std::list<int>{1, 2}, loaded);
 	ASSERT_EQ(2u, loaded.size()) << "nodes did not survive the round trip";
 
 	// Payloads
-	driver_->loadNodeData(loaded);
+	db()->loadNodeData(loaded);
 	for(Signature * s : loaded)
 	{
 		s->sensorData().uncompressData();
@@ -415,7 +421,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;
-	driver_->loadLinks(2, links);
+	db()->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,6 +124,29 @@ 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,6 +10,7 @@
 #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>
@@ -2843,6 +2844,144 @@ 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
@@ -2992,6 +3131,142 @@ 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
@@ -3131,6 +3406,7 @@ 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.
@@ -4160,3 +4436,248 @@ 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";
+}

corelib/test/test_odometry.cpp

@@ -543,3 +543,44 @@ 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

@@ -0,0 +1,285 @@
+// 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,5 +1,6 @@
 #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>
@@ -1337,6 +1338,49 @@ 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

@@ -2379,10 +2379,13 @@ TEST_F(RtabmapIntegrationFixture, Multisession3ItMemoryThr)
 	// and the furthest is the one with none: what comes back into the working memory is what
 	// the hypotheses are drawn over.
 	//
-	// The loop counts move with the environment, the feature extraction not seeing quite the
-	// same thing: local-retrieval-only has since been seen at 259 and at 271, over the 265-287
-	// of both-retrieval, which is why the two are no longer ordered on the count. The value
-	// diff keeps its order everywhere it has been run.
+	// The numbers move with the environment, the feature extraction not seeing quite the same
+	// thing. The loop counts: local-retrieval-only has since been seen at 259 and at 271, over
+	// the 265-287 of both-retrieval, which is why the two are no longer ordered on the count.
+	// The value diff: a CI runner has since put no-retrieval at 0.0946 and
+	// local-retrieval-only at 0.0956, both out of the bands above and on either side of the
+	// gap between them, which is why the two are ordered with a margin below. Only
+	// both-retrieval against the other two has kept its order everywhere it has been run.
 	//
 	// The posterior time each run prints is left unasserted: it is what these variants are
 	// measured for, but also what a loaded runner moves most.
@@ -2520,12 +2523,17 @@ TEST_F(RtabmapIntegrationFixture, Multisession3ItMemoryThr)
 	// against 269) while both stay inside their own bands. Only a collapse is caught here.
 	EXPECT_GE(loopsPerVariant.at("both-retrieval"), loopsPerVariant.at("local-retrieval-only") - 15);
 
-	// What the retrieval buys is asserted on the hypotheses, where the three are ordered with
-	// room to spare -- the gaps are twice the spread of a variant: what comes back into the
-	// working memory is what the hypotheses are drawn over, so bringing back what the
-	// likelihood points at lands closest to the recorded session.
+	// What the retrieval buys is asserted on the hypotheses: what comes back into the working
+	// memory is what they are drawn over, so bringing back what the likelihood points at lands
+	// closest to the recorded session. Retrieving both against retrieving nothing is the wide
+	// one -- half the value diff -- and is ordered outright.
 	EXPECT_LT(valueDiffPerVariant.at("both-retrieval"), valueDiffPerVariant.at("local-retrieval-only"));
-	EXPECT_LT(valueDiffPerVariant.at("local-retrieval-only"), valueDiffPerVariant.at("no-retrieval"));
+	EXPECT_LT(valueDiffPerVariant.at("both-retrieval"), valueDiffPerVariant.at("no-retrieval"));
+
+	// The two adjacent ones are not ordered outright: their bands overlap on another machine,
+	// where the pair came out at 0.0956 against 0.0946 while each stayed under its own cap
+	// above. Only a reversal worth the name is caught here.
+	EXPECT_LT(valueDiffPerVariant.at("local-retrieval-only"), valueDiffPerVariant.at("no-retrieval") + 0.015f);
 }
 
 TEST_F(RtabmapIntegrationFixture, AppearanceOnly_PrecisionRecall)
@@ -2949,8 +2957,9 @@ 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.85f : 0.9f);
+				(looseFloors || kazeDescriptor ? 0.80f : 0.9f);
 		const float kMinRecall    = xfeatures2dDescriptor ? 0.5f :
 				(looseFloors ? 0.7f : 0.85f);
 		EXPECT_GE(acceptedPrec, kMinPrecision)

corelib/test/test_sensordata.cpp

@@ -1,5 +1,6 @@
 #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>
@@ -186,10 +187,43 @@ 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_FALSE(data.cameraModels().empty());
+    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());
 }
 
@@ -595,6 +629,43 @@ 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)

corelib/test/test_stereocameramodel.cpp

@@ -7,6 +7,7 @@
 #include "rtabmap/utilite/UDirectory.h"
 #include "rtabmap/utilite/UFile.h"
 #include <cmath>
+#include <limits>
 
 using namespace rtabmap;
 
@@ -236,6 +237,94 @@ 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,6 +508,31 @@ 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,6 +1024,47 @@ 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,6 +2,7 @@
 #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>
@@ -82,44 +83,93 @@ 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;
 
-	// 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
+	// 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});
 	}
 
 	pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
 	util3d::extractXYZCorrespondencesRANSAC(words1, words2, cloud1, cloud2);
 
 	EXPECT_EQ(cloud1.size(), cloud2.size());
-	EXPECT_GE(cloud1.size(), 8); // At least 8 inliers from 10 consistent matches
+	EXPECT_EQ(cloud1.size(), 12); // every match is on its epipolar line
 }
 
 TEST(Util3dCorrespondencesTest, ExtractXYZCorrespondencesRANSACRejectsOutliers) {
 	std::multimap<int, pcl::PointXYZ> words1;
 	std::multimap<int, pcl::PointXYZ> words2;
 
-	// 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
+	// 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});
 	}
 
-	// 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)});
+	// 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});
+	}
 
 	pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
 	util3d::extractXYZCorrespondencesRANSAC(words1, words2, cloud1, cloud2);
 
 	EXPECT_EQ(cloud1.size(), cloud2.size());
-	EXPECT_EQ(cloud1.size(), 8);  // RANSAC should reject 2 outliers
+	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]);
+		}
+	}
 }
 
 TEST(Util3dCorrespondencesTest, ExtractXYZCorrespondencesRANSACFailsGracefullyOnTooFewMatches) {

docker/README.md

@@ -1,3 +1,36 @@
 ### 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,6 +165,10 @@ 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,11 +8,15 @@ 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 .. && \
+    cmake -DWITH_ALICE_VISION=ON -DWITH_OPENGV=ON -DBUILD_TESTING=$RUN_TESTS .. && \
     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,6 +190,10 @@ 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,6 +74,10 @@ 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,11 +8,15 @@ 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 .. && \
+    cmake -DWITH_OPENGV=ON -DBUILD_TESTING=$RUN_TESTS .. && \
     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,9 +68,12 @@ 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 .. && \
@@ -95,6 +98,10 @@ 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,11 +8,15 @@ 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 .. && \
+    cmake -DWITH_OPENGV=ON -DBUILD_TESTING=$RUN_TESTS .. && \
     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,6 +118,10 @@ 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,11 +8,15 @@ 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 .. && \
+    cmake -DWITH_OPENGV=ON -DBUILD_TESTING=$RUN_TESTS .. && \
     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,6 +117,10 @@ 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,11 +8,15 @@ 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 .. && \
+    cmake -DWITH_OPENGV=ON -DBUILD_TESTING=$RUN_TESTS .. && \
     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,6 +107,10 @@ 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,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <QDialog>
 #include <QMap>
+#include <QColor>
 #include <QtCore/QSettings>
 
 #include <rtabmap/core/Signature.h>
@@ -46,6 +47,10 @@ 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;
@@ -132,7 +137,30 @@ 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.find(ids_[i]); iter!=links.end() && iter->first==ids_[i]; ++iter)
+			for(std::multimap<int, Link>::iterator iter=links.lower_bound(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.find(ids_[i]); jter!=links.end() && jter->first == ids_[i]; ++jter)
+		for(std::multimap<int, Link>::iterator jter=links.lower_bound(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_.find(currentLink.from());
+			std::multimap<int, Link>::iterator iter = linksRefined_.lower_bound(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_.find(link.from());
+				std::multimap<int, Link>::iterator iter = linksRefined_.lower_bound(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_.find(currentLink.from());
+		std::multimap<int, Link>::iterator iter = linksRefined_.lower_bound(currentLink.from());
 		while(iter != linksRefined_.end() && iter->first == currentLink.from())
 		{
 			if(iter->second.to() == currentLink.to() &&

guilib/src/PreferencesDialog.cpp

@@ -1141,6 +1141,7 @@ 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="17" column="0">
+         <item row="18" column="0">
           <widget class="QCheckBox" name="checkBox_cameraProjection">
            <property name="text">
             <string/>
@@ -45,7 +45,7 @@
            </property>
           </widget>
          </item>
-         <item row="14" column="0">
+         <item row="15" column="0">
           <widget class="QCheckBox" name="checkBox_filtering">
            <property name="text">
             <string/>
@@ -59,7 +59,7 @@
            </property>
           </widget>
          </item>
-         <item row="18" column="1">
+         <item row="19" column="1">
           <widget class="QLabel" name="label_binaryFile_12">
            <property name="text">
             <string>Meshing.</string>
@@ -69,7 +69,7 @@
            </property>
           </widget>
          </item>
-         <item row="14" column="1">
+         <item row="15" 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="18" column="0">
+         <item row="19" column="0">
           <widget class="QCheckBox" name="checkBox_meshing">
            <property name="text">
             <string/>
            </property>
           </widget>
          </item>
-         <item row="16" column="1">
+         <item row="17" 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="15" column="0">
+         <item row="16" column="0">
           <widget class="QCheckBox" name="checkBox_smoothing">
            <property name="text">
             <string/>
@@ -229,7 +229,7 @@
            </item>
           </widget>
          </item>
-         <item row="15" column="1">
+         <item row="16" 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="17" column="1">
+         <item row="18" 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="16" column="0">
+         <item row="17" column="0">
           <widget class="QCheckBox" name="checkBox_gainCompensation">
            <property name="text">
             <string/>
@@ -403,6 +403,32 @@
            </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,6 +12185,35 @@ 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>

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>rtabmap</name>
-  <version>0.23.11</version>
+  <version>0.23.13</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/Export/main.cpp

@@ -35,6 +35,7 @@ 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>
@@ -49,8 +50,13 @@ 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
@@ -184,6 +190,8 @@ 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());
 	;
@@ -256,6 +264,7 @@ 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;
@@ -819,6 +828,23 @@ 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;
@@ -1493,6 +1519,9 @@ 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)
@@ -1503,26 +1532,42 @@ 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 = -1;
-		double stamp = 0.0;
-		dbDriver->getNodeInfo(iter->first, p, m, weight, l, stamp, gt, v, gps, s);
+		int weight;
+		double stamp;
+		dbDriver->getNodeInfo(nodeId, p, m, weight, l, stamp, gt, v, gps, s);
 
-		SensorData data;
+		out.node = Signature(nodeId, m, weight, stamp, l, pose, gt);
+		SensorData & data = out.node.sensorData();
 		bool loadImages = ((exportCloud || exportMesh) && (!cloudFromScan || texture || camProjection)) || exportImages;
 		bool loadScan = ((exportCloud || exportMesh) && cloudFromScan) || exportPosesScan;
 		if(loadImages || loadScan || export2DMap || exportOctomap)
 		{
 			dbDriver->getNodeData(
-				iter->first, 
+				nodeId, 
 				data, 
 				loadImages, 
 				loadScan,
@@ -1530,23 +1575,29 @@ 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)
 		{
-			dbDriver->getCalibration(iter->first, models, stereoModels);
+			std::vector<CameraModel> models;
+			std::vector<StereoCameraModel> stereoModels;
+			dbDriver->getCalibration(nodeId, models, stereoModels);
+			data.setCameraModels(models);
+			data.setStereoCameraModels(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(iter->first) == densityPoses.end();
+			bool densityFiltered = !densityPoses.empty() && densityPoses.find(nodeId) == densityPoses.end();
 			cv::Mat rgb;
-			cv::Mat depth;
 			cv::Mat confidence;
 			pcl::IndicesPtr indices(new std::vector<int>);
-			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
-			pcl::PointCloud<pcl::PointXYZI>::Ptr cloudI;
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud = out.cloud;
+			pcl::PointCloud<pcl::PointXYZI>::Ptr & cloudI = out.cloudI;
 			if(weight != -1)
 			{
 				if(!densityFiltered && cloudFromScan && (exportCloud || exportMesh))
@@ -1555,7 +1606,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", iter->first);
+						printf("Node %d doesn't have scan data, empty cloud is created.\n", nodeId);
 					}
 					if(decimation>1 || minRange>0.0f || maxRange)
 					{
@@ -1586,7 +1637,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", iter->first);
+									"created (if you want to create point cloud from scan, use --scan option).\n", nodeId);
 						}
 						else if(!data.depthRaw().empty() && depthEdgeBleedingFilterError>0.0f)
 						{
@@ -1617,8 +1668,9 @@ int main(int argc, char * argv[])
 				if(!UDirectory::exists(dir)) {
 					UDirectory::makeDir(dir);
 				}
-				std::string outputPath=dir+"/"+(exportImagesId?uNumber2Str(iter->first):uFormat("%f", stamp))+".jpg";
+				std::string outputPath=dir+"/"+(exportImagesId?uNumber2Str(nodeId):uFormat("%f", stamp))+".jpg";
 				cv::imwrite(outputPath, rgb);
+				#pragma omp atomic
 				++imagesExported;
 				if(!depth.empty())
 				{
@@ -1642,7 +1694,7 @@ int main(int argc, char * argv[])
 						UDirectory::makeDir(dir);
 					}
 
-					outputPath=dir+"/"+(exportImagesId?uNumber2Str(iter->first):uFormat("%f", stamp))+ext;
+					outputPath=dir+"/"+(exportImagesId?uNumber2Str(nodeId):uFormat("%f", stamp))+ext;
 					cv::imwrite(outputPath, depthExported);
 				}
 				if(!confidence.empty())
@@ -1652,7 +1704,7 @@ int main(int argc, char * argv[])
 						UDirectory::makeDir(dir);
 					}
 
-					outputPath=dir+"/"+(exportImagesId?uNumber2Str(iter->first):uFormat("%f", stamp))+".png";
+					outputPath=dir+"/"+(exportImagesId?uNumber2Str(nodeId):uFormat("%f", stamp))+".png";
 					cv::imwrite(outputPath, confidence);
 				}
 
@@ -1660,7 +1712,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(iter->first):uFormat("%f", stamp));
+					std::string modelName = (exportImagesId?uNumber2Str(nodeId):uFormat("%f", stamp));
 					if(models.size() > 1) {
 						modelName += "_" + uNumber2Str((int)i);
 					}
@@ -1674,7 +1726,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(iter->first):uFormat("%f", stamp));
+					std::string modelName = (exportImagesId?uNumber2Str(nodeId):uFormat("%f", stamp));
 					if(stereoModels.size() > 1) {
 						modelName += "_" + uNumber2Str((int)i);
 					}
@@ -1694,20 +1746,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, iter->second);
+							cloud = rtabmap::util3d::transformPointCloud(cloud, pose);
 					}
 					else if(cloudI.get() && !cloudI->empty()) {
 						cloudI = rtabmap::util3d::voxelize(cloudI, indices, voxelSize);
 						if(!cloudI->empty())
-							cloudI = rtabmap::util3d::transformPointCloud(cloudI, iter->second);
+							cloudI = rtabmap::util3d::transformPointCloud(cloudI, pose);
 					}
 				}
 				else
 				{
 					if(cloud.get() && !cloud->empty())
-						cloud = rtabmap::util3d::transformPointCloud(cloud, indices, iter->second);
+						cloud = rtabmap::util3d::transformPointCloud(cloud, indices, pose);
 					else if(cloudI.get() && !cloudI->empty())
-						cloudI = rtabmap::util3d::transformPointCloud(cloudI, indices, iter->second);
+						cloudI = rtabmap::util3d::transformPointCloud(cloudI, indices, pose);
 				}
 
 				if(filter_ceiling != 0.0 || filter_floor != 0.0f)
@@ -1722,54 +1774,90 @@ int main(int argc, char * argv[])
 					}
 				}
 
-				if(cloudFromScan)
+			}
+		}
+
+		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())
 				{
-					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));
+					*assembledCloud = *cloud;
 				}
 				else
 				{
-					rawViewpoints.insert(*iter);
+					*assembledCloud += *cloud;
 				}
-
-				if(cloud.get() && !cloud->empty())
+				rawViewpointIndices.resize(assembledCloud->size(), nodeId);
+			}
+			else if(cloudI.get() && !cloudI->empty())
+			{
+				if(assembledCloudI->empty())
 				{
-					if(assembledCloud->empty())
-					{
-						*assembledCloud = *cloud;
-					}
-					else
-					{
-						*assembledCloud += *cloud;
-					}
-					rawViewpointIndices.resize(assembledCloud->size(), iter->first);
+					*assembledCloudI = *cloudI;
 				}
-				else if(cloudI.get() && !cloudI->empty())
+				else
 				{
-					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));
+					*assembledCloudI += *cloudI;
 				}
+				rawViewpointIndices.resize(assembledCloudI->size(), nodeId);
+			}
+			if(!depth.empty()) // depth is set only when texturing (see loadNode)
+			{
+				cameraDepths.insert(std::make_pair(nodeId, depth));
 			}
 		}
 
@@ -1781,8 +1869,8 @@ int main(int argc, char * argv[])
 			}
 		}
 
-		robotPoses.insert(std::make_pair(iter->first, iter->second));
-		robotStamps.insert(std::make_pair(iter->first, stamp));
+		robotPoses.insert(std::make_pair(nodeId, pose));
+		robotStamps.insert(std::make_pair(nodeId, stamp));
 		if(models.empty() && weight == -1 && !cameraModels.empty())
 		{
 			// For intermediate nodes, use latest models
@@ -1792,7 +1880,7 @@ int main(int argc, char * argv[])
 		{
 			if(!data.imageCompressed().empty())
 			{
-				cameraModels.insert(std::make_pair(iter->first, models));
+				cameraModels.insert(std::make_pair(nodeId, models));
 			}
 			if(exportPosesCamera)
 			{
@@ -1804,15 +1892,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(iter->first, iter->second*models[i].localTransform()));
-					cameraStamps[i].insert(std::make_pair(iter->first, stamp));
+					cameraPoses[i].insert(std::make_pair(nodeId, pose*models[i].localTransform()));
+					cameraStamps[i].insert(std::make_pair(nodeId, stamp));
 				}
 			}
 		}
 		if(exportPosesScan && !data.laserScanCompressed().empty())
 		{
-			scanPoses.insert(std::make_pair(iter->first, iter->second*data.laserScanCompressed().localTransform()));
-			scanStamps.insert(std::make_pair(iter->first, stamp));
+			scanPoses.insert(std::make_pair(nodeId, pose*data.laserScanCompressed().localTransform()));
+			scanStamps.insert(std::make_pair(nodeId, stamp));
 		}
 
 		if(exportPosesGps || exportGps>=0)
@@ -1832,56 +1920,85 @@ 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(iter->first, pose));
+					gpsPoses.insert(std::make_pair(nodeId, pose));
 				}
 				if(exportGps>=0)
 				{
-					gpsValues.insert(std::make_pair(iter->first, gps));
+					gpsValues.insert(std::make_pair(nodeId, gps));
 				}
-				gpsStamps.insert(std::make_pair(iter->first, gps.stamp()));
+				gpsStamps.insert(std::make_pair(nodeId, gps.stamp()));
 			}
 		}
 
 		if(exportPosesGt && !gt.isNull())
 		{
-			gtPoses.insert(std::make_pair(iter->first, gt));
-			gtStamps.insert(std::make_pair(iter->first, stamp));
+			gtPoses.insert(std::make_pair(nodeId, gt));
+			gtStamps.insert(std::make_pair(nodeId, stamp));
 		}
 
 		if(weight != -1 && (export2DMap || exportOctomap)) {
-			cv::Mat ground;
-			cv::Mat obstacles;
-			cv::Mat empty;
-			data.uncompressDataConst(0, 0, 0, 0, &ground, &obstacles, &empty);
+			const cv::Mat & ground = data.gridGroundCellsRaw();
+			const cv::Mat & obstacles = data.gridObstacleCellsRaw();
+			const cv::Mat & empty = data.gridEmptyCellsRaw();
 			if(ground.empty() && obstacles.empty() && empty.empty()) {
-				printf("Node %d doesn't have local occupancy grid, ignored!\n", iter->first);
+				printf("Node %d doesn't have local occupancy grid, ignored!\n", nodeId);
 			}
 			else {
-				addedPosesToMap.insert(*iter);
-				localGridCache.add(iter->first, ground, obstacles, empty, data.gridCellSize(), data.gridViewPoint());
+				addedPosesToMap.insert(std::make_pair(nodeId, pose));
+				localGridCache.add(nodeId, ground, obstacles, empty, data.gridCellSize(), data.gridViewPoint());
 				if(export2DMap && !grid.update(addedPosesToMap)) {
-					printf("Failed to assemble local grid %d to global occupancy grid!\n", iter->first);
+					printf("Failed to assemble local grid %d to global occupancy grid!\n", nodeId);
 				}
 #ifdef RTABMAP_OCTOMAP
 				if(exportOctomap && !octomap.update(addedPosesToMap)) {
-					printf("Failed to assemble local grid %d to OctoMap!\n", iter->first);
+					printf("Failed to assemble local grid %d to OctoMap!\n", nodeId);
 				}
 #endif
 				localGridCache.clear();
 			}
 		}
 
-		if(optimizedPoses.size() >= 500)
+	};
+
+#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)
 		{
-			++processedNodes;
-			int percent = processedNodes*100/(int)optimizedPoses.size();
-			if(percent != lastPercent)
+			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)
 			{
-				printf("Processed %d/%d (%d%%) nodes...\n",
-					processedNodes,
-					(int)optimizedPoses.size(),
-					percent);
-				lastPercent = percent;
+				++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;
+				}
 			}
 		}
 	}
@@ -2658,7 +2775,8 @@ int main(int argc, char * argv[])
 							textureRoiRatios,
 							&progressState,
 							&vertexToPixels,
-							distanceToCamPolicy);
+							distanceToCamPolicy,
+							usedThreads);
 					printf("Texturing... done (%fs).\n", timer.ticks());
 
 					// Remove occluded polygons (polygons with no texture)

utilite/include/rtabmap/utilite/UMutex.h

@@ -159,28 +159,131 @@ public:
  *
  * @endcode
  *
+ * The lock can also be deferred, for example to only try locking it. The destructor
+ * then unlocks the mutex only if this object locked it:
+ * @code
+ * void callback()
+ * {
+ *    UScopeMutex sm(m, false); // not locked yet
+ *    if(sm.lockTry() == 0)
+ *    {
+ *       if(cond1)
+ *       {
+ *          return; // automatically unlock the mutex m
+ *       }
+ *       ...
+ *    }
+ *    // the mutex m is unlocked only if lockTry() succeeded
+ * }
+ * @endcode
+ *
+ * The object must be named: a temporary would be destroyed, and the mutex unlocked,
+ * at the end of the expression. lock(), lockTry() and unlock() can only be called on
+ * a named object, so that `if(UScopeMutex(m, false).lockTry() == 0)` doesn't compile.
+ * In C++17, the object can be scoped to an if statement instead:
+ * @code
+ * if(UScopeMutex sm(m, false); sm.lockTry() == 0)
+ * {
+ *    // locked here
+ * } // unlocked here, only if lockTry() succeeded
+ * @endcode
+ *
  * @see UMutex
  */
 class UScopeMutex
 {
 public:
-	UScopeMutex(const UMutex & mutex) :
-		mutex_(mutex)
+	/**
+	 * @param mutex the mutex to lock.
+	 * @param lockNow if true (default), the mutex is locked here. If false, it is not
+	 *        locked until lock() or lockTry() is called.
+	 */
+	UScopeMutex(const UMutex & mutex, bool lockNow = true) :
+		mutex_(mutex),
+		locked_(false)
 	{
-		mutex_.lock();
+		if(lockNow)
+		{
+			lock();
+		}
 	}
 	// backward compatibility
 	UScopeMutex(UMutex * mutex) :
-		mutex_(*mutex)
+		mutex_(*mutex),
+		locked_(false)
 	{
-		mutex_.lock();
+		lock();
 	}
+	/**
+	 * Unlock the mutex, only if this object locked it.
+	 */
 	~UScopeMutex()
 	{
-		mutex_.unlock();
+		unlock();
 	}
+
+	/**
+	 * Lock the mutex, if this object doesn't hold it already.
+	 * @return 0 on success, an error code otherwise.
+	 */
+	int lock() &
+	{
+		if(locked_)
+		{
+			return 0;
+		}
+		int r = mutex_.lock();
+		locked_ = r == 0;
+		return r;
+	}
+
+#if !defined(_WIN32) || (_WIN32_WINNT >= 0x0400)
+	/**
+	 * Try locking the mutex, if this object doesn't hold it already.
+	 * @return 0 if the mutex is held by this object, EBUSY (or another
+	 *         error code) otherwise.
+	 */
+	int lockTry() &
+	{
+		if(locked_)
+		{
+			return 0;
+		}
+		int r = mutex_.lockTry();
+		locked_ = r == 0;
+		return r;
+	}
+#endif
+
+	/**
+	 * Unlock the mutex before this object goes out of scope, only if this object locked it.
+	 * @return 0 on success (or if this object didn't hold the mutex), an error code otherwise.
+	 */
+	int unlock() &
+	{
+		if(!locked_)
+		{
+			return 0;
+		}
+		locked_ = false;
+		return mutex_.unlock();
+	}
+
+	/**
+	 * @return true if this object currently holds the mutex.
+	 */
+	bool isLocked() const
+	{
+		return locked_;
+	}
+
+private:
+	UScopeMutex(const UScopeMutex &);
+	void operator=(const UScopeMutex &);
+
 private:
 	const UMutex & mutex_;
+	bool locked_;
 };
 
 #endif // UMUTEX_H

utilite/src/UFile.cpp

@@ -96,8 +96,10 @@ std::string UFile::getExtension(const std::string &filePath)
 
 void UFile::copy(const std::string & from, const std::string & to)
 {
-	std::ifstream src(from.c_str());
-	std::ofstream dst(to.c_str());
+	// Binary, or Windows translates line endings and stops at the first 0x1A, which
+	// silently truncates or corrupts anything that is not text -- a database, an image.
+	std::ifstream src(from.c_str(), std::ios::binary);
+	std::ofstream dst(to.c_str(), std::ios::binary);
 
 	dst << src.rdbuf();
 }

utilite/test/test_ufile.cpp

@@ -3,6 +3,8 @@
 #include "rtabmap/utilite/UDirectory.h"
 #include <fstream>
 #include <cstdio>
+#include <iterator>
+#include <string>
 
 TEST(UFileTest, Exists)
 {
@@ -108,6 +110,33 @@ TEST(UFileTest, Copy)
     std::remove(destFile.c_str());
 }
 
+TEST(UFileTest, CopyKeepsBinaryContentByteForByte)
+{
+    // The bytes a text-mode copy does not survive on Windows: a lone \n, which it turns
+    // into \r\n, and 0x1A, which it reads as end of file and truncates at. A database or
+    // an image copied that way comes out corrupted.
+    const std::string sourceFile = "test_file_binary_source.bin";
+    const std::string destFile = "test_file_binary_dest.bin";
+    const std::string content("a\nb\r\nc\x1a" "d", 8);
+
+    std::ofstream file(sourceFile.c_str(), std::ios::binary);
+    file.write(content.data(), content.size());
+    file.close();
+
+    UFile::copy(sourceFile, destFile);
+
+    std::ifstream copied(destFile.c_str(), std::ios::binary);
+    const std::string copiedContent(
+            (std::istreambuf_iterator<char>(copied)), std::istreambuf_iterator<char>());
+    copied.close();
+
+    EXPECT_EQ(copiedContent, content);
+
+    // Cleanup
+    std::remove(sourceFile.c_str());
+    std::remove(destFile.c_str());
+}
+
 TEST(UFileTest, InstanceMethods)
 {
     std::string testFile = "test_file_instance.txt";

utilite/test/test_umutex.cpp

@@ -3,6 +3,8 @@
 #include <thread>
 #include <chrono>
 #include <atomic>
+#include <type_traits>
+#include <utility>
 #include <vector>
 
 TEST(UMutexTest, Constructor)
@@ -149,6 +151,144 @@ TEST(UMutexTest, UScopeMutexWithPointer)
     t.join();
 }
 
+// lock(), lockTry() and unlock() can only be called on a named UScopeMutex: a temporary
+// would unlock the mutex at the end of the expression, before the code it should protect.
+template<typename T, typename = void>
+struct CanLockTry : std::false_type {};
+template<typename T>
+struct CanLockTry<T, decltype(void(std::declval<T>().lockTry()))> : std::true_type {};
+static_assert(CanLockTry<UScopeMutex &>::value, "lockTry() must be callable on a named UScopeMutex");
+static_assert(!CanLockTry<UScopeMutex>::value, "lockTry() must not be callable on a temporary UScopeMutex");
+
+TEST(UMutexTest, UScopeMutexDeferredIsNotLocked)
+{
+    UMutex mutex;
+    {
+        UScopeMutex scopeMutex(mutex, false);
+        EXPECT_FALSE(scopeMutex.isLocked());
+
+        std::thread t([&mutex]() {
+            EXPECT_EQ(mutex.lockTry(), 0); // Not locked by the scope mutex
+            mutex.unlock();
+        });
+        t.join();
+    }
+    // The destructor must not unlock a mutex the scope mutex didn't lock
+    mutex.lock();
+    std::thread t([&mutex]() {
+        EXPECT_NE(mutex.lockTry(), 0); // Still locked by this thread
+    });
+    t.join();
+    mutex.unlock();
+}
+
+TEST(UMutexTest, UScopeMutexDeferredLock)
+{
+    UMutex mutex;
+    {
+        UScopeMutex scopeMutex(mutex, false);
+        EXPECT_EQ(scopeMutex.lock(), 0);
+        EXPECT_TRUE(scopeMutex.isLocked());
+
+        std::thread t([&mutex]() {
+            EXPECT_NE(mutex.lockTry(), 0); // Should fail
+        });
+        t.join();
+    }
+    std::thread t([&mutex]() {
+        EXPECT_EQ(mutex.lockTry(), 0); // Unlocked by the destructor
+        mutex.unlock();
+    });
+    t.join();
+}
+
+TEST(UMutexTest, UScopeMutexLockWhenHeld)
+{
+    UMutex mutex;
+    {
+        UScopeMutex scopeMutex(mutex); // locked by the constructor
+        EXPECT_EQ(scopeMutex.lock(), 0); // Already held: not locked a second time
+        EXPECT_TRUE(scopeMutex.isLocked());
+    }
+    std::thread t([&mutex]() {
+        EXPECT_EQ(mutex.lockTry(), 0); // Unlocked once by the destructor, and free
+        mutex.unlock();
+    });
+    t.join();
+}
+
+TEST(UMutexTest, UScopeMutexLockTrySucceeds)
+{
+    UMutex mutex;
+    {
+        UScopeMutex scopeMutex(mutex, false);
+        EXPECT_EQ(scopeMutex.lockTry(), 0);
+        EXPECT_TRUE(scopeMutex.isLocked());
+        EXPECT_EQ(scopeMutex.lockTry(), 0); // Already held: not locked a second time
+    }
+    std::thread t([&mutex]() {
+        EXPECT_EQ(mutex.lockTry(), 0); // Unlocked once by the destructor, and free
+        mutex.unlock();
+    });
+    t.join();
+}
+
+TEST(UMutexTest, UScopeMutexLockTryFails)
+{
+    UMutex mutex;
+    std::atomic<bool> locked(false);
+    std::atomic<bool> release(false);
+    std::thread owner([&]() {
+        mutex.lock();
+        locked = true;
+        while(!release) { std::this_thread::yield(); }
+        mutex.unlock();
+    });
+    while(!locked) { std::this_thread::yield(); }
+
+    {
+        UScopeMutex scopeMutex(mutex, false);
+        EXPECT_NE(scopeMutex.lockTry(), 0); // Held by the other thread
+        EXPECT_FALSE(scopeMutex.isLocked());
+    }
+    // The destructor didn't unlock the other thread's lock
+    std::thread t([&mutex]() {
+        EXPECT_NE(mutex.lockTry(), 0);
+    });
+    t.join();
+
+    release = true;
+    owner.join();
+    EXPECT_EQ(mutex.lockTry(), 0);
+    mutex.unlock();
+}
+
+TEST(UMutexTest, UScopeMutexEarlyUnlock)
+{
+    UMutex mutex;
+    {
+        UScopeMutex scopeMutex(mutex);
+        EXPECT_TRUE(scopeMutex.isLocked());
+        EXPECT_EQ(scopeMutex.unlock(), 0);
+        EXPECT_FALSE(scopeMutex.isLocked());
+        EXPECT_EQ(scopeMutex.unlock(), 0); // Nothing to unlock anymore
+
+        std::thread t([&mutex]() {
+            EXPECT_EQ(mutex.lockTry(), 0); // Released before the end of the scope
+            mutex.unlock();
+        });
+        t.join();
+
+        mutex.lock(); // Locked by this thread, not by the scope mutex
+    }
+    // The destructor must not unlock it
+    std::thread t([&mutex]() {
+        EXPECT_NE(mutex.lockTry(), 0);
+    });
+    t.join();
+    mutex.unlock();
+}
+
 TEST(UMutexTest, MultipleMutexes)
 {
     UMutex mutex1;