Fixing crash on windows when uFormat() is to long

Added auto gravity estimation mode
fixing M_PI not defined in tests
2026-10-12 04:49:50 +08:00 · 2026-10-11 11:45:52 -07:00 · 2026-10-11 11:36:44 -07:00 · 2026-10-11 10:49:07 -07:00 · 2026-10-11 10:45:21 -07:00 · 2026-10-10 22:12:50 -07:00
34 changed files with 2927 additions and 287 deletions
@@ -3,7 +3,7 @@ name: CMake-ROS
 on:
  push:
    branches: 
-      - rolling-devel
+      - master
    paths-ignore: &platform_only
      - '.github/workflows/android.yml'
      - '.github/workflows/ios.yml'
@@ -21,25 +21,31 @@ env:
 concurrency:
  group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }}
-  cancel-in-progress: true
+  cancel-in-progress: ${{ github.event_name == 'pull_request' }}
 jobs:
  build:
-    name: ${{ matrix.ros_distribution }}${{ matrix.use_ros2_testing && '-testing' || '' }}
+    name: ${{ matrix.ros_distribution }}
    runs-on: ubuntu-latest
    concurrency:
-      group: ${{ github.workflow }}-${{ github.ref }}-${{ matrix.ros_distribution }}-${{ matrix.use_ros2_testing }}
+      group: ${{ github.workflow }}-${{ github.ref }}-${{ matrix.ros_distribution }}
      cancel-in-progress: true
    strategy:
      fail-fast: false
      matrix:
-        ros_distribution: [ rolling ]
+        ros_distribution: [ humble, jazzy, kilted, lyrical, rolling]
        # Testing only: the osrf/ros:rolling image is already set up on ros2-testing,
        # and rolling's main repo is just a periodic snapshot of it.
        use_ros2_testing: [ 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: "" # When releasing to ROS2, the skip_keys should be empty, patch these deps in package.xml instead.
+          skip_keys: "libpointmatcher gtsam"
          use_ros2_testing: true # Rolling is using ros2-testing (nightly)
    container:
      image: osrf/ros:${{ matrix.ros_distribution }}-desktop-full
    steps:
@@ -88,23 +94,10 @@ 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 }}
+        use-ros2-testing: ${{ matrix.use_ros2_testing || false }}
    # 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
@@ -21,8 +21,8 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 # VERSION
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
-SET(RTABMAP_MINOR_VERSION 23)
+SET(RTABMAP_MINOR_VERSION 24)
-SET(RTABMAP_PATCH_VERSION 13)
+SET(RTABMAP_PATCH_VERSION 1)
 SET(RTABMAP_VERSION
  ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
@@ -8,7 +8,7 @@ rtabmap
 [![codecov](https://codecov.io/gh/introlab/rtabmap/graph/badge.svg?token=mPwvfZMOia)](https://codecov.io/gh/introlab/rtabmap)
 [![License][license-image]][license]
-[release-image]: https://img.shields.io/badge/release-0.23.1-green.svg?style=flat
+[release-image]: https://img.shields.io/github/v/release/introlab/rtabmap?color=green&style=flat
 [releases]: https://github.com/introlab/rtabmap/releases
 [downloads-image]: https://img.shields.io/github/downloads/introlab/rtabmap/total?label=downloads
@@ -34,59 +34,26 @@ This project is supported by [IntRoLab - Intelligent / Interactive / Integrated
 #### CI Latest
-  <table>
+| | Build |
-    <tbody>
+|---|---|
-        <tr>
+| Desktop | [![Linux](https://github.com/introlab/rtabmap/actions/workflows/cmake-linux.yml/badge.svg)](https://github.com/introlab/rtabmap/actions/workflows/cmake-linux.yml) [![Windows](https://github.com/introlab/rtabmap/actions/workflows/cmake-windows.yml/badge.svg)](https://github.com/introlab/rtabmap/actions/workflows/cmake-windows.yml) [![macOS](https://github.com/introlab/rtabmap/actions/workflows/cmake-macos.yml/badge.svg)](https://github.com/introlab/rtabmap/actions/workflows/cmake-macos.yml) |
-           <td>
+| ROS | [![CMake ROS](https://github.com/introlab/rtabmap/actions/workflows/cmake-ros.yml/badge.svg)](https://github.com/introlab/rtabmap/actions/workflows/cmake-ros.yml) [![Docker ROS](https://github.com/introlab/rtabmap/actions/workflows/docker-ros.yml/badge.svg)](https://github.com/introlab/rtabmap/actions/workflows/docker-ros.yml) |
-           <a href="https://github.com/introlab/rtabmap/actions/workflows/cmake-linux.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/cmake-linux.yml/badge.svg" alt="CMake Linux Build Status"/> <br> 
+| Mobile | [![Android](https://github.com/introlab/rtabmap/actions/workflows/android.yml/badge.svg)](https://github.com/introlab/rtabmap/actions/workflows/android.yml) [![iOS](https://github.com/introlab/rtabmap/actions/workflows/ios.yml/badge.svg)](https://github.com/introlab/rtabmap/actions/workflows/ios.yml) |
-           <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> 
+| Quality | [![Coverage](https://github.com/introlab/rtabmap/actions/workflows/coverage.yml/badge.svg)](https://github.com/introlab/rtabmap/actions/workflows/coverage.yml) [![Documentation](https://github.com/introlab/rtabmap/actions/workflows/docs.yml/badge.svg)](https://github.com/introlab/rtabmap/actions/workflows/docs.yml) |
           <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-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>
  </table>
 #### ROS Binaries
 `ros-$ROS_DISTRO-rtabmap`
- <table>
+| | Distro | Ubuntu | Released | In apt | Build |
-    <tbody>
+|---|---|---|---|---|---|
-        <tr>
+| ROS 1 | Noetic (EOL) | 20.04 | [![released](https://img.shields.io/badge/dynamic/yaml?url=https%3A%2F%2Fraw.githubusercontent.com%2Fros%2Frosdistro%2Fmaster%2Fnoetic%2Fdistribution.yaml&query=%24.repositories.rtabmap.release.version&label=%20)](https://github.com/ros/rosdistro/blob/master/noetic/distribution.yaml) | [![apt](https://img.shields.io/ros/v/noetic/rtabmap?label=%20)](https://index.ros.org/p/rtabmap/#noetic) |  |
-           <td rowspan="1">ROS 1</td>
+| ROS 2 | Humble | 22.04 | [![released](https://img.shields.io/badge/dynamic/yaml?url=https%3A%2F%2Fraw.githubusercontent.com%2Fros%2Frosdistro%2Fmaster%2Fhumble%2Fdistribution.yaml&query=%24.repositories.rtabmap.release.version&label=%20)](https://github.com/ros/rosdistro/blob/master/humble/distribution.yaml) | [![apt](https://img.shields.io/ros/v/humble/rtabmap?label=%20)](https://index.ros.org/p/rtabmap/#humble) | [![build](http://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary)](http://build.ros2.org/job/Hbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary/) |
-            <td>Noetic</td>
+| ROS 2 | Iron (EOL) | 22.04 | [![released](https://img.shields.io/badge/dynamic/yaml?url=https%3A%2F%2Fraw.githubusercontent.com%2Fros%2Frosdistro%2Fmaster%2Firon%2Fdistribution.yaml&query=%24.repositories.rtabmap.release.version&label=%20)](https://github.com/ros/rosdistro/blob/master/iron/distribution.yaml) | [![apt](https://img.shields.io/ros/v/iron/rtabmap?label=%20)](https://index.ros.org/p/rtabmap/#iron) |  |
-            <td><a href="http://build.ros.org/job/Nbin_ufv8_uFv8__rtabmap__ubuntu_focal_arm64__binary/"><img src="http://build.ros.org/buildStatus/icon?job=Nbin_ufv8_uFv8__rtabmap__ubuntu_focal_arm64__binary" alt="Build Status"/></td>
+| ROS 2 | Jazzy | 24.04 | [![released](https://img.shields.io/badge/dynamic/yaml?url=https%3A%2F%2Fraw.githubusercontent.com%2Fros%2Frosdistro%2Fmaster%2Fjazzy%2Fdistribution.yaml&query=%24.repositories.rtabmap.release.version&label=%20)](https://github.com/ros/rosdistro/blob/master/jazzy/distribution.yaml) | [![apt](https://img.shields.io/ros/v/jazzy/rtabmap?label=%20)](https://index.ros.org/p/rtabmap/#jazzy) | [![build](http://build.ros2.org/buildStatus/icon?job=Jbin_uN64__rtabmap__ubuntu_noble_amd64__binary)](http://build.ros2.org/job/Jbin_uN64__rtabmap__ubuntu_noble_amd64__binary/) |
-        </tr>
+| ROS 2 | Kilted | 24.04 | [![released](https://img.shields.io/badge/dynamic/yaml?url=https%3A%2F%2Fraw.githubusercontent.com%2Fros%2Frosdistro%2Fmaster%2Fkilted%2Fdistribution.yaml&query=%24.repositories.rtabmap.release.version&label=%20)](https://github.com/ros/rosdistro/blob/master/kilted/distribution.yaml) | [![apt](https://img.shields.io/ros/v/kilted/rtabmap?label=%20)](https://index.ros.org/p/rtabmap/#kilted) | [![build](http://build.ros2.org/buildStatus/icon?job=Kbin_uN64__rtabmap__ubuntu_noble_amd64__binary)](http://build.ros2.org/job/Kbin_uN64__rtabmap__ubuntu_noble_amd64__binary/) |
-        <tr>
+| ROS 2 | Lyrical | 26.04 | [![released](https://img.shields.io/badge/dynamic/yaml?url=https%3A%2F%2Fraw.githubusercontent.com%2Fros%2Frosdistro%2Fmaster%2Flyrical%2Fdistribution.yaml&query=%24.repositories.rtabmap.release.version&label=%20)](https://github.com/ros/rosdistro/blob/master/lyrical/distribution.yaml) | [![apt](https://img.shields.io/ros/v/lyrical/rtabmap?label=%20)](https://index.ros.org/p/rtabmap/#lyrical) | [![build](http://build.ros2.org/buildStatus/icon?job=Lbin_uR64__rtabmap__ubuntu_resolute_amd64__binary)](http://build.ros2.org/job/Lbin_uR64__rtabmap__ubuntu_resolute_amd64__binary/) |
-            <td rowspan="5">ROS 2</td>
+| ROS 2 | Rolling | 26.04 | [![released](https://img.shields.io/badge/dynamic/yaml?url=https%3A%2F%2Fraw.githubusercontent.com%2Fros%2Frosdistro%2Fmaster%2Frolling%2Fdistribution.yaml&query=%24.repositories.rtabmap.release.version&label=%20)](https://github.com/ros/rosdistro/blob/master/rolling/distribution.yaml) | [![apt](https://img.shields.io/ros/v/rolling/rtabmap?label=%20)](https://index.ros.org/p/rtabmap/#rolling) | [![build](http://build.ros2.org/buildStatus/icon?job=Rbin_uR64__rtabmap__ubuntu_resolute_amd64__binary)](http://build.ros2.org/job/Rbin_uR64__rtabmap__ubuntu_resolute_amd64__binary/) |
-            <td>Humble</td>
+| Docker | [rtabmap](https://hub.docker.com/r/introlab3it/rtabmap) | | | ![Docker Pulls](https://img.shields.io/docker/pulls/introlab3it/rtabmap.svg?label=pulls) | |
-            <td><a href="http://build.ros2.org/job/Hbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary" alt="Build Status"/></td>
+
-        </tr>
+*Released* is the version bloomed into [rosdistro](https://github.com/ros/rosdistro); *In apt* is what `apt install` actually gives you today. They differ while a release is waiting on a buildfarm sync.
        <tr>
            <td>Jazzy</td>
            <td><a href="http://build.ros2.org/job/Jbin_uN64__rtabmap__ubuntu_noble_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Jbin_uN64__rtabmap__ubuntu_noble_amd64__binary" alt="Build Status"/></td>
        </tr>
        <tr>
            <td>Kilted</td>
            <td><a href="http://build.ros2.org/job/Lbin_uR64__rtabmap__ubuntu_resolute_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Lbin_uR64__rtabmap__ubuntu_resolute_amd64__binary" alt="Build Status"/></td>
        </tr>
        <tr>
            <td>Lyrical</td>
            <td><a href="http://build.ros2.org/job/Kbin_uN64__rtabmap__ubuntu_noble_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Kbin_uN64__rtabmap__ubuntu_noble_amd64__binary" alt="Build Status"/></td>
        </tr>
        <tr>
            <td>Rolling</td>
            <td><a href="http://build.ros2.org/job/Rbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary" alt="Build Status"/></td>
        </tr>
        <tr>
           <td>Docker</td>
           <td>
             <a href="https://hub.docker.com/r/introlab3it/rtabmap">rtabmap</a>
           </td>
           <td><img src="https://img.shields.io/docker/pulls/introlab3it/rtabmap" alt="Docker Pulls"/></td>
        </tr>
    </tbody>
 </table>
@@ -1078,7 +1078,7 @@
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include/eigen3\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include/pcl-1.15\"",
-					"\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.23\"",
+					"\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.24\"",
 					"\"$(SRCROOT)/../android/jni/tango-gl/include\"",
 					"\"$(SRCROOT)/../android/jni/third-party/include\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/lib/vtk.framework/Headers\"",
@@ -1139,7 +1139,7 @@
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include/eigen3\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include/pcl-1.15\"",
-					"\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.23\"",
+					"\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.24\"",
 					"\"$(SRCROOT)/../android/jni/tango-gl/include\"",
 					"\"$(SRCROOT)/../android/jni/third-party/include\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/lib/vtk.framework/Headers\"",
@@ -41,7 +41,8 @@ namespace rtabmap {
 * @brief Background thread to compress or uncompress images and generic matrices.
 *
 * In compress mode, pass a source matrix to the constructor with an optional image
- * format (".png", ".jpg", ".rvl", or empty for zlib data). In uncompress mode, pass
+ * format (".png", ".jpg", ".rvl", or empty for zlib data, see @ref compressImage() for
 * depth options). In uncompress mode, pass
 * compressed bytes and set @c isImage accordingly. Call @ref UThread::start() then
 * @ref UThread::join() to obtain the result from @ref getCompressedData() or
 * @ref getUncompressedData().
@@ -70,7 +71,7 @@ public:
 	/**
 	 * @brief Constructs a thread in compress mode.
 	 * @param mat Source image or data matrix to compress.
-	 * @param format Image format: @c ".png", @c ".jpg", @c ".rvl", or empty for zlib (@ref compressData2).
+	 * @param format Image format: @c ".png", @c ".jpg", @c ".rvl" (see @ref compressImage()), or empty for zlib (@ref compressData2).
 	 */
 	CompressionThread(const cv::Mat & mat, const std::string & format = "");
 	/**
@@ -93,7 +94,63 @@ private:
 	bool compressMode_;
 };
-/** @brief Encodes @p image to a byte buffer (OpenCV @c imencode or RVL for depth). */
+/*
 * Compressed depth image layouts (all values little-endian). They are stable: they are
 * saved in databases, and rtabmap_ros converts them to and from ROS's
 * compressed_depth_image_transport messages without decompressing the images.
 *
 *  - ".png": a standard PNG file. 16UC1 depth images are 16 bits grayscale PNGs,
 *    32FC1 depth images (legacy) are 4-channel 8 bits PNGs holding the float bytes.
 *  - ".rvl" (16UC1):
 *      [0..7]   "DEPTHRVL"
 *      [8..11]  uint32 cols
 *      [12..15] uint32 rows
 *      [16..]   RVL data (see RvlCodec)
 *  - ".png:<maxDepth>:<quantization>" or ".rvl:<maxDepth>:<quantization>" (32FC1):
 *      [0..7]   "DEPTHINV"
 *      [8..11]  float depthQuantA = quantization*(quantization+1)
 *      [12..15] float depthQuantB = 1 - depthQuantA/maxDepth
 *      [16..]   the 16UC1 inverse depth image in ".png" or ".rvl" layout above, where
 *               0 is invalid and v>0 is the depth depthQuantA/(v-depthQuantB).
 */
 /** @brief Signature of the ".rvl" layout (8 bytes, not null-terminated). */
 const char kCompressedDepthRvlSignature[8] = {'D', 'E', 'P', 'T', 'H', 'R', 'V', 'L'};
 /** @brief Size of the ".rvl" header: signature, uint32 cols, uint32 rows. */
 const size_t kCompressedDepthRvlHeaderSize = 16;
 /** @brief Signature of the inverse depth layout (8 bytes, not null-terminated). */
 const char kCompressedDepthInvSignature[8] = {'D', 'E', 'P', 'T', 'H', 'I', 'N', 'V'};
 /** @brief Size of the inverse depth header: signature, float depthQuantA, float depthQuantB. */
 const size_t kCompressedDepthInvHeaderSize = 16;
 /**
 * @brief Parses an image compression format "<codec>[:<maxDepth>[:<quantization>]]".
 *
 * @param format Format, e.g., @c ".jpg", @c ".png", @c ".rvl", @c ".png:10" or @c ".rvl:20:100".
 *        The empty format is valid (general zlib compression, see @ref CompressionThread).
 * @param codec Output codec (e.g., @c ".png").
 * @param maxDepth Output maximum depth (m) of the inverse depth format, 0 if not set.
 * @param quantization Output depth quantization of the inverse depth format
 *        (100 if not set but @p maxDepth is), 0 if @p maxDepth is not set.
 * @return false if the format is invalid. The inverse depth parameters are only
 *        valid with @c ".png" and @c ".rvl", and should be positive.
 */
 bool RTABMAP_CORE_EXPORT parseImageCompressionFormat(const std::string & format, std::string & codec, float & maxDepth, float & quantization);
 /**
 * @brief Encodes @p image to a byte buffer (OpenCV @c imencode or RVL for depth).
 *
 * @param format @c ".png", @c ".jpg" or @c ".rvl" (16UC1 only), optionally followed by
 *        @c ":<maxDepth>[:<quantization>]" (see @ref parseImageCompressionFormat()).
 *        For 32FC1 depth images, if @c maxDepth is set, depth is quantized on 16 bits
 *        as inverse depth (as ROS's @c compressed_depth_image_transport) and compressed
 *        with the codec. With A=quantization*(quantization+1) and B=1-A/maxDepth, the
 *        precision is ~d^2/(2A), and depth values over @c maxDepth or under A/(65535-B)
 *        are lost (set to 0). For example, ".png:10:100" keeps depth between 0.15 and
 *        10 m with errors of 0.05 mm at 1 m and 5 mm at 10 m. Otherwise, 32FC1 depth
 *        images are compressed losslessly as 4-channel 8 bits PNG (legacy format). Other
 *        image types ignore the depth parameters.
 */
 std::vector<unsigned char> RTABMAP_CORE_EXPORT compressImage(const cv::Mat & image, const std::string & format = ".png");
 /** @brief Same as @ref compressImage() but returns a @c CV_8UC1 row matrix. */
 cv::Mat RTABMAP_CORE_EXPORT compressImage2(const cv::Mat & image, const std::string & format = ".png");
@@ -102,6 +159,8 @@ cv::Mat RTABMAP_CORE_EXPORT compressImage2(const cv::Mat & image, const std::str
 cv::Mat RTABMAP_CORE_EXPORT uncompressImage(const cv::Mat & bytes);
 /** @brief Decodes compressed image bytes to a @cv::Mat. */
 cv::Mat RTABMAP_CORE_EXPORT uncompressImage(const std::vector<unsigned char> & bytes);
 /** @brief Decodes compressed image bytes to a @cv::Mat. */
 cv::Mat RTABMAP_CORE_EXPORT uncompressImage(const unsigned char * bytes, size_t size);
 /** @brief Compresses a matrix with zlib; appends rows, cols and type at the end. */
 std::vector<unsigned char> RTABMAP_CORE_EXPORT compressData(const cv::Mat & data);
@@ -122,7 +181,10 @@ std::string RTABMAP_CORE_EXPORT uncompressString(const cv::Mat & bytes);
 /**
 * @brief Detects the compression format of depth image bytes.
- * @return @c ".rvl" if the buffer has an RVL signature, otherwise @c ".png".
+ * @return @c ".rvl" if the buffer has an RVL signature, @c ".png:<maxDepth>:<quantization>"
 *         or @c ".rvl:<maxDepth>:<quantization>" for inverse depth images (see
 *         @ref compressImage()), otherwise @c ".png". The returned format can be passed
 *         back to @ref compressImage() to compress in the same format.
 */
 std::string RTABMAP_CORE_EXPORT compressedDepthFormat(const cv::Mat & bytes);
 /** @overload */
@@ -130,5 +192,6 @@ std::string RTABMAP_CORE_EXPORT compressedDepthFormat(const std::vector<unsigned
 /** @overload */
 std::string RTABMAP_CORE_EXPORT compressedDepthFormat(const unsigned char * bytes, size_t size);
 } /* namespace rtabmap */
 #endif /* COMPRESSION_H_ */
@@ -0,0 +1,233 @@
 /*
 Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 Redistribution and use in source and binary forms, with or without
 modification, are permitted provided that the following conditions are met:
    * Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in the
      documentation and/or other materials provided with the distribution.
    * Neither the name of the Universite de Sherbrooke nor the
      names of its contributors may be used to endorse or promote products
      derived from this software without specific prior written permission.
 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 #ifndef RTABMAP_CORE_IMUMOTIONPREDICTOR_H_
 #define RTABMAP_CORE_IMUMOTIONPREDICTOR_H_
 #include <rtabmap/core/rtabmap_core_export.h>
 #include <rtabmap/core/Transform.h>
 #include <rtabmap/core/IMU.h>
 #include <Eigen/Geometry>
 #include <map>
 #include <vector>
 namespace rtabmap {
 /**
 * @brief Predicts the pose of the base frame from the last odometry pose and the IMU.
 *
 * Between two odometry updates, the pose is propagated with the IMU: the orientation is
 * the IMU's own, re-expressed in the odometry frame, and the position is integrated from
 * the velocity at the last odometry update and the gravity-compensated acceleration. That
 * velocity is re-estimated at every odometry update, from the displacement since an
 * odometry pose a short window back (see velocityWindow) corrected by the acceleration
 * measured in between, so the position never drifts for long: only the motion since the
 * last update is predicted.
 *
 * This is what lidar deskewing needs: the pose at every point's time, during a sweep that
 * started after the last pose odometry estimated.
 *
 * The IMU samples are given as they are measured (see addImu()):
 * the orientation of the IMU in its world frame and its specific force, from which gravity
 * is removed here. That world frame must be gravity aligned with +z up, as in ROS (REP-103,
 * e.g. ENU); its yaw doesn't matter. An orientation given in a frame with z down (NED)
 * must be converted first, otherwise gravity is added instead of removed. The lever arm between the IMU and the base origin is
 * ignored: its centripetal and tangential accelerations are small over the fraction of a
 * second this predicts.
 *
 * The gravity removed can be estimated from the IMU itself (see the constructor): an
 * accelerometer can read a few percent off at rest (scale or bias), and that error,
 * integrated as an acceleration, would bias the predicted velocity along gravity. The
 * accelerometer is averaged over the windows where it is quiet: with a gyro and an
 * accelerometer that barely change, the IMU is not accelerating (at rest or at constant
 * velocity), so it only measures gravity. The IMU must then be held still (or at constant
 * velocity) for a moment, e.g., before moving: until then, standard gravity is used, and
 * a steady acceleration without rotation would be taken for gravity. Only the error along
 * gravity is corrected, which is all of it for an IMU that stays about level; one changing
 * attitude would need its axes calibrated.
 *
 * Not thread-safe: a caller sharing it between threads must lock around every call.
 */
 class RTABMAP_CORE_EXPORT ImuMotionPredictor
 {
 public:
 	/**
 	 * The IMU acceleration is used only with a velocity window (> 0): over a single frame,
 	 * the velocity is too noisy to be carried forward with it. Without it (window of 0, or
 	 * no acceleration in the IMU samples), the position follows the last velocity
 	 * (constant velocity model).
 	 *
 	 * @param maxPoseInterval odometry poses older (s) than this are not used to estimate
 	 *                        the velocity, which is null without one
 	 * @param velocityWindow  the velocity is estimated from the displacement since the
 	 *                        newest pose at least this old (s), corrected by the
 	 *                        acceleration measured since, so that it is the velocity at the
 	 *                        last pose, not an average. Over a single frame interval, the
 	 *                        noise of the odometry poses would be of the order of the
 	 *                        velocity itself. 0: the displacement since the previous pose,
 	 *                        without acceleration.
 	 * @param gravity         magnitude (m/s^2) of the gravity removed from the specific force
 	 *                        given to addImu(), standard gravity by default. <= 0: estimated
 	 *                        from the IMU when it is still (see the class description), with
 	 *                        standard gravity until then.
 	 *
 	 * These are fixed for the life of the predictor (there is no setter): changing them
 	 * while it estimates would mix poses and samples taken under different settings.
 	 */
 	explicit ImuMotionPredictor(
 			double maxPoseInterval = 1.0,
 			double velocityWindow = 0.5,
 			double gravity = 9.80665);
 	double maxPoseInterval() const {return maxPoseInterval_;}
 	double velocityWindow() const {return velocityWindow_;}
 	/// Gravity (m/s^2) removed from the specific force: the one given, or the estimate
 	double gravity() const {return gravity_;}
 	/// Whether the gravity is estimated from the IMU (see the constructor)
 	bool isGravityEstimated() const {return gravityEstimated_;}
 	/// Number of quiet IMU windows the gravity estimate is from (0: not estimated yet)
 	size_t gravityWindows() const {return gravityWindowsCount_;}
 	/**
 	 * @brief Adds an IMU measurement.
 	 * @param stamp time of the measurement (s)
 	 * @param imu   orientation of the IMU in its world frame (gravity aligned, +z up), linear
 	 *              acceleration as measured (the specific force, which includes the
 	 *              reaction to gravity) and the transform from the base frame to the IMU.
 	 *              Without orientation, the measurement is ignored; without linear
 	 *              acceleration (all zeros, or a covariance of -1), only its orientation
 	 *              is used.
 	 */
 	void addImu(double stamp, const IMU & imu);
 	/**
 	 * @brief Adds an odometry pose, from which the next poses are predicted.
 	 * @param stamp time of the pose (s)
 	 * @param pose  pose of the base frame in the odometry frame; a null pose (odometry
 	 *              lost) resets the prediction until the next valid pose
 	 */
 	void addPose(double stamp, const rtabmap::Transform & pose);
 	/// Forgets the poses and the IMU samples. The gravity estimate is kept: it is the
 	/// sensor's.
 	void reset();
 	/**
 	 * @brief Predicts the pose of the base frame in the odometry frame.
 	 * @param stamp time (s) of the prediction, normally after the last pose
 	 * @return the predicted pose, null if there is no IMU sample yet. Without a pose (none
 	 *         yet, or the last one was null), the orientation alone is predicted, in the
 	 *         IMU's world frame and at the origin: still the relative rotation between two
 	 *         stamps, which is what deskewing needs most.
 	 */
 	rtabmap::Transform predict(double stamp) const;
 	/// Whether there is a pose to predict from (none yet, or the last one was null).
 	bool hasPose() const;
 	/// Stamp of the last pose added, 0 if there is none.
 	double lastPoseStamp() const;
 	/// Velocity (m/s) of the base in the odometry frame at the last pose.
 	Eigen::Vector3d velocity() const;
 	/// Number of IMU samples kept.
 	size_t samples() const;
 private:
 	// orientation: of the base frame in the IMU's world frame; acceleration: of the base in
 	// that frame, gravity removed
 	// specificForce: in that frame too, including the reaction to gravity (see
 	// hasAcceleration)
 	void addSample(double stamp, const Eigen::Quaterniond & orientation,
 			const Eigen::Vector3d & specificForce, bool hasAcceleration);
 	// Gravity estimation, from the IMU's own specific force and angular velocity norm
 	void estimateGravity(double stamp, const Eigen::Vector3d & specificForce, double angularVelocity);
 	struct Sample
 	{
 		Eigen::Quaterniond orientation;
 		// Kept with gravity, which is removed when used: the estimate can change
 		Eigen::Vector3d specificForce;
 		bool hasAcceleration;
 	};
 	// Acceleration of a sample, gravity removed (null without acceleration)
 	Eigen::Vector3d accelerationOf(const Sample & sample) const;
 	Eigen::Quaterniond orientationAt(double stamp) const;
 	Eigen::Vector3d accelerationAt(double stamp) const;
 	void integrate(double from, double to, const Eigen::Quaterniond & rotation,
 			Eigen::Vector3d & velocity, Eigen::Vector3d & position) const;
 	void updateIntegration() const;
 	// The acceleration integrated from the last pose up to a sample's stamp, in the
 	// odometry frame: predicting a stamp then only integrates from the sample before it.
 	struct Integrated
 	{
 		Eigen::Vector3d acceleration; // at that stamp
 		Eigen::Vector3d velocity;     // change since the last pose
 		Eigen::Vector3d position;     // change since the last pose (without its velocity)
 	};
 private:
 	double maxPoseInterval_;
 	double velocityWindow_;
 	double gravity_;
 	bool gravityEstimated_;
 	std::map<double, Sample> samples_;
 	// Gravity estimation: the samples of the current window, and the gravity measured over
 	// the last quiet windows
 	struct StillSample
 	{
 		double stamp;
 		Eigen::Vector3d specificForce; // in the IMU frame
 		double angularVelocity;        // norm
 	};
 	std::vector<StillSample> stillWindow_;
 	std::vector<double> gravityWindows_;
 	size_t gravityWindowsCount_;
 	// Recent odometry poses, to estimate the velocity from (see velocityWindow)
 	std::map<double, rtabmap::Transform> poses_;
 	// The last odometry pose and the state predictions start from.
 	double poseStamp_;
 	rtabmap::Transform pose_;
 	Eigen::Vector3d velocity_;
 	// Rotation from the IMU's world frame to the odometry frame, at the last pose: the two
 	// are both gravity aligned, but their yaw differ.
 	Eigen::Quaterniond worldToOdom_;
 	// Built lazily by predict(), from the last pose to the newest sample; cleared when the
 	// pose or the samples it was built from change.
 	mutable std::map<double, Integrated> integrated_;
 	// Whether the acceleration at the last pose's stamp (the first entry) is final: it is
 	// held constant from the newest sample until a sample after that stamp is received.
 	mutable bool integratedStartIsFinal_;
 };
 }
 #endif /* RTABMAP_CORE_IMUMOTIONPREDICTOR_H_ */
@@ -848,6 +848,7 @@ private:
 	unsigned int _imagePreDecimation;
 	unsigned int _imagePostDecimation;
 	bool _legacyDecimatedOctave;
 	bool _inverseDepthCompressionAllowed; // database version >= 0.24
 	bool _compressionParallelized;
 	float _laserScanDownsampleStepSize;
 	float _laserScanVoxelSize;
@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Transform.h>
 #include <rtabmap/core/SensorData.h>
 #include <rtabmap/core/Parameters.h>
 #include <rtabmap/core/ImuMotionPredictor.h>
 namespace rtabmap {
@@ -189,6 +190,7 @@ private:
 	std::vector<StereoCameraModel> stereoModels_;
 	std::vector<CameraModel> models_;
 	std::map<double, Transform> imus_;
 	ImuMotionPredictor imuMotionPredictor_; // fed when IMU is received (motion guess and deskewing)
 };
 } /* namespace rtabmap */
@@ -222,7 +222,7 @@ class RTABMAP_CORE_EXPORT Parameters
    RTABMAP_PARAM(Mem, NotLinkedNodesKept,          bool, true,     "Keep not linked nodes in db (rehearsed nodes and deleted nodes).");
    RTABMAP_PARAM(Mem, IntermediateNodeDataKept,    bool, false,    "Keep intermediate node data in db.");
    RTABMAP_PARAM_STR(Mem, ImageCompressionFormat,   ".jpg",        "RGB image compression format. It should be \".jpg\" or \".png\".");
-    RTABMAP_PARAM_STR(Mem, DepthCompressionFormat,   ".rvl",        "Depth image compression format for 16UC1 depth type. It should be \".png\" or \".rvl\". If depth type is 32FC1, \".png\" is used.");
+    RTABMAP_PARAM_STR(Mem, DepthCompressionFormat,   ".rvl",        "Depth image compression format. It should be \".png\" or \".rvl\", optionally followed by \":maxDepth[:quantization]\" (e.g., \".rvl:10:100\", quantization is 100 by default) to compress 32FC1 depth images as 16 bits inverse depth with that codec (same quantization than ROS's compressed_depth_image_transport). 16UC1 depth images are always compressed losslessly with the codec. Warning: the inverse depth format is lossy, the precision is ~d^2/(2*q*(q+1)) (q=quantization, e.g., 0.05 mm at 1 m and 5 mm at 10 m with q=100) and depth values over maxDepth or under ~q*(q+1)/65535 meters (0.15 m with q=100) are lost. Without depth parameters, 32FC1 depth images are compressed losslessly in \".png\" format (4 channels 8 bits). Databases with depth images compressed in inverse depth format cannot be opened by rtabmap versions under 0.24.");
    RTABMAP_PARAM(Mem, STMSize,                   unsigned int, 10, "Short-term memory size.");
    RTABMAP_PARAM(Mem, IncrementalMemory,           bool, true,     "SLAM mode, otherwise it is Localization mode.");
    RTABMAP_PARAM(Mem, LocalizationReadOnly,        bool, false,    uFormat("In localization mode, open the database in read-only mode (ignored if %s=true). Currrenty incompatible with memory management (%s and %s cannot be used) and if there are disjoint sessions in working memory. Last localization pose won't be saved back in the database at the end of the session, so the robot will always restart to original last localization pose, unless %s is used or an external initial pose is provided on initialization.", kMemIncrementalMemory().c_str(), kRtabmapLoopThr().c_str(), kRtabmapMemoryThr().c_str(), kRGBDStartAtOrigin().c_str()).c_str());
@@ -456,7 +456,7 @@ class RTABMAP_CORE_EXPORT Parameters
 #else
 #ifdef RTABMAP_G2O
    RTABMAP_PARAM(Optimizer, Strategy,        int, 1,          "Graph optimization strategy: 0=TORO, 1=g2o, 2=GTSAM and 3=Ceres.");
-    RTABMAP_PARAM(Optimizer, Iterations,      int, 20,         "Optimization iterations.");
+    RTABMAP_PARAM(Optimizer, Iterations,      int, 30,         "Optimization iterations.");
    RTABMAP_PARAM(Optimizer, Epsilon,         double, 0.0,     "Stop optimizing when the error improvement is less than this value.");
 #else
 #ifdef RTABMAP_CERES
@@ -489,7 +489,7 @@ class RTABMAP_CORE_EXPORT Parameters
    RTABMAP_PARAM(Optimizer, Baseline,           double, 0.075,   "When doing bundle adjustment with RGB-D data (mono camera + depth), set a fake baseline (m) so the BA backend treats depth as stereo disparity. Applies to all BA-capable backends (g2o, GTSAM, Ceres). Set to 0 to keep the problem mono (depth observations are ignored). For real stereo data the baseline in the calibration (Tx) is used directly.");
    RTABMAP_PARAM(Optimizer, PixelVariance,     double, 1.0,     "Pixel variance used on the u/v axes of every bundle adjustment reprojection edge. Applies to all BA-capable backends (g2o, GTSAM, Ceres). Should approximate the squared 1-sigma keypoint localization error in pixels. Set higher (e.g. 4-9) if features are noisy (low texture, motion blur, low light, or large detector scale). Set lower (e.g. 0.01-0.1) if features are sub-pixel refined (Lucas-Kanade tracking, parabolic peak interpolation). Intuition: the lower the pixel variance, the more the optimizer trusts the keypoint positions.");
-    RTABMAP_PARAM(Optimizer, DisparityVariance, double, 1.0,     "Disparity variance used on the disparity axis (u - u_right) of stereo / RGB-D bundle adjustment edges. Applies to all BA-capable backends (g2o, GTSAM, Ceres). Defaults to the same value as PixelVariance for backward compatibility. Set higher (e.g. 2-4) if your depth source is noisier than your feature detector's u/v precision (typical for stereo block matchers / SGM at long range). Set lower (e.g. 0.01-0.1) if your depth source is more accurate than the u/v detector (typical for ToF / LiDAR-fused depth where range is measured directly rather than triangulated). Intuition: the lower the disparity variance, the more the optimizer trusts the depth measurements. Geometric note: wider baseline and/or higher image resolution improve a block matcher's effective disparity precision (larger disparity magnitudes and finer sub-pixel refinement), so wide-baseline high-resolution stereo pairs can usually afford a lower disparity variance (e.g. 0.1-0.5); narrow-baseline low-resolution pairs should keep it higher (e.g. 1-4).");
+    RTABMAP_PARAM(Optimizer, DisparityVariance, double, 1.0,     "Disparity variance used on the disparity axis (u - u_right) of stereo / RGB-D bundle adjustment edges (g2o, GTSAM, Ceres). Defaults to the same value as PixelVariance for backward compatibility. The lower it is, the more the optimizer trusts the depth. Set higher (e.g. 2-4) if the depth is noisier than the features' u/v precision (stereo block matchers / SGM at long range, narrow-baseline low-resolution stereo). Set lower (e.g. 0.01-0.1) if it is more accurate (ToF / LiDAR-fused depth, measured rather than triangulated), or around 0.1-0.5 for wide-baseline high-resolution stereo.");
    RTABMAP_PARAM(Optimizer, RobustKernelDelta, double, 8,       "Robust kernel delta used for bundle adjustment (0 means don't use robust kernel). Applies to all BA-capable backends (g2o, GTSAM, Ceres). Observations with chi2 over this threshold will be ignored in the second optimization pass.");
    RTABMAP_PARAM(GTSAM, Optimizer,       int, 1,          "0=Levenberg 1=GaussNewton 2=Dogleg");
@@ -512,13 +512,14 @@ class RTABMAP_CORE_EXPORT Parameters
    RTABMAP_PARAM(Odom, KalmanProcessNoise,     float, 0.001, "Process noise covariance value.");
    RTABMAP_PARAM(Odom, KalmanMeasurementNoise, float, 0.01,  "Process measurement covariance value.");
    RTABMAP_PARAM(Odom, GuessMotion,            bool, true,   "Guess next transformation from the last motion computed.");
-    RTABMAP_PARAM(Odom, GuessSmoothingDelay,    float, 0,     uFormat("Guess smoothing delay (s). Estimated velocity is averaged based on last transforms up to this maximum delay. This can help to get smoother velocity prediction. Last velocity computed is used directly if \"%s\" is set or the delay is below the odometry rate.", kOdomFilteringStrategy().c_str()));
+    RTABMAP_PARAM(Odom, ImuGravity,             float, 9.80665, uFormat("Gravity magnitude (m/s^2) removed from the IMU linear acceleration (used with \"%s\" > 0). Standard gravity by default. Set it to what the accelerometer reads at rest to compensate for its scale error, or for another gravity than Earth's. 0: estimated from the IMU while it is still (gyro and accelerometer quiet over 0.5 s), with standard gravity until then: the robot should then be perfectly still for a moment (e.g., at start), and a steady acceleration without rotation would be taken for gravity.", kOdomGuessSmoothingDelay().c_str()));
    RTABMAP_PARAM(Odom, GuessSmoothingDelay,    float, 0,     uFormat("Guess smoothing delay (s). The velocity is averaged over the last transforms up to this delay, for a smoother velocity prediction. The last velocity is used directly if \"%s\" is set or the delay is below the odometry rate. With an IMU giving orientation and linear acceleration, a delay > 0 also enables the IMU acceleration: the velocity is the displacement over this delay corrected by the acceleration measured since, so it is not delayed, and the motion guess and lidar deskewing (\"%s\") integrate the acceleration from it (~0.5 s recommended). With 0, the IMU only gives the orientation. Without IMU, the average is delayed by half this delay: useful for platforms with inertia, high frame rates, or lidar deskewing, where the velocity noise would feed back into the next poses.", kOdomFilteringStrategy().c_str(), kOdomDeskewing().c_str()));
    RTABMAP_PARAM(Odom, KeyFrameThr,            float, 0.3,   "[Visual] Create a new keyframe when the number of inliers drops under this ratio of features in last frame. Setting the value to 0 means that a keyframe is created for each processed frame.");
    RTABMAP_PARAM(Odom, VisKeyFrameThr,         int, 150,     "[Visual] Create a new keyframe when the number of inliers drops under this threshold. Setting the value to 0 means that a keyframe is created for each processed frame.");
    RTABMAP_PARAM(Odom, ScanKeyFrameThr,        float, 0.9,   "[Geometry] Create a new keyframe when the number of ICP inliers drops under this ratio of points in last frame's scan. Setting the value to 0 means that a keyframe is created for each processed frame.");
    RTABMAP_PARAM(Odom, ImageDecimation,     unsigned int, 1, uFormat("Decimation of the RGB image before registration. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. If %s is true and if depth is smaller than decimated RGB, depth may be interpolated to match RGB size for feature detection.", kVisDepthAsMask().c_str()));
    RTABMAP_PARAM(Odom, AlignWithGround,        bool, false,  "Align odometry with the ground on initialization.");
-    RTABMAP_PARAM(Odom, Deskewing,              bool, true,   "Lidar deskewing. If input lidar has time channel, it will be deskewed with a constant motion model (with IMU orientation and/or guess if provided).");
+    RTABMAP_PARAM(Odom, Deskewing,              bool, true,   uFormat("Lidar deskewing. If input lidar has time channel, it will be deskewed. With an IMU, the pose of every point is predicted from the previous frame: orientation from the IMU, translation from the velocity (with the IMU acceleration if \"%s\" > 0). Without IMU, with a constant motion model (or the guess if provided).", kOdomGuessSmoothingDelay().c_str()));
    // Odometry Frame-to-Map
    RTABMAP_PARAM(OdomF2M, MaxSize,             int, 2000,    "[Visual] Local map size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words.");
@@ -598,6 +598,8 @@ public:
 	/**
 	 * Set image data. Detect automatically if raw or compressed.
 	 * A matrix of type CV_8UC1 with 1 row is considered as compressed.
 	 * An invalid @p model (not CameraModel::isValidForProjection()) without any image is
 	 * a placeholder (e.g., scan-only data): it is not added, so cameraModels() is empty.
 	 * @param clearPreviousData, clear previous raw and compressed images before setting the new ones.
 	 */
 	void setRGBDImage(const cv::Mat & rgb, const cv::Mat & depth, const CameraModel & model, bool clearPreviousData = true);
@@ -1025,6 +1027,9 @@ public:
 #endif
 private:
 	/// Whether setRGBDImage() keeps @p model: not an invalid model without any image.
 	bool keepCameraModel(const CameraModel & model, const cv::Mat & rgb, const cv::Mat & depth, bool clearPreviousData) const;
 	int _id; ///< Unique sensor data ID (0 if invalid)
 	double _stamp; ///< Timestamp in seconds
@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef UTIL3D_H_
 #define UTIL3D_H_
 #include <functional>
 #include "rtabmap/core/rtabmap_core_export.h"
 #include <pcl/point_cloud.h>
@@ -1295,6 +1296,27 @@ LaserScan RTABMAP_CORE_EXPORT deskew(
 		double inputStamp,
 		const rtabmap::Transform & velocity);
 /**
 * @brief Deskews a lidar scan with a motion given by the caller.
 *
 * Same as the velocity overload, but the motion during the sweep comes from @p motion,
 * for instance a pose predicted from an IMU.
 *
 * @param input      scan with a time channel (`kXYZIT` or `kXYZIRT`)
 * @param inputStamp stamp of the scan, which the time channel is relative to
 * @param motion     for a stamp (s) in the sweep, the pose of the scan's base frame at that
 *                   stamp relative to the base frame at @p inputStamp; a null transform
 *                   aborts deskewing
 * @param slerp      call @p motion only for the first and last points and interpolate in
 *                   between, instead of calling it for every time of the sweep
 * @return the deskewed scan, empty on error
 */
 LaserScan RTABMAP_CORE_EXPORT deskew(
 		const LaserScan & input,
 		double inputStamp,
 		const std::function<rtabmap::Transform(double stamp)> & motion,
 		bool slerp = false);
 } // namespace util3d
 } // namespace rtabmap
@@ -88,6 +88,7 @@ SET(SRC_FILES
    RegistrationVis.cpp
    Odometry.cpp
    ImuMotionPredictor.cpp
    OdometryThread.cpp
 	OdometryInfo.cpp
    odometry/OdometryF2M.cpp
@@ -28,9 +28,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Compression.h"
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/utilite/UStl.h>
 #include <opencv2/opencv.hpp>
 #include <zlib.h>
 #include <cmath>
 #include <cstring>
 namespace rtabmap {
@@ -67,16 +70,117 @@ int deserializeMatType(int serializedType)
 			((serializedType >> kSerializedCnShift) & 511) + 1);
 }
 // Default quantization when only the maximum depth is set in the format.
 const float kDefaultDepthQuantization = 100.0f;
 bool hasSignature(const unsigned char * bytes, size_t size, const void * signature)
 {
 	return bytes && size >= 8 && memcmp(bytes, signature, 8) == 0;
 }
 // Values over maxDepth, NaN, inf, 0 and negative values are set to 0 (invalid),
 // as well as values too close to be represented on 16 bits (under
 // depthQuantA / (65535 - depthQuantB) meters).
 cv::Mat depthToInvDepth(const cv::Mat & depth, float maxDepth, float quantization, float & depthQuantA, float & depthQuantB)
 {
 	UASSERT(depth.type() == CV_32FC1);
 	depthQuantA = quantization * (quantization + 1.0f);
 	depthQuantB = 1.0f - depthQuantA / maxDepth;
 	cv::Mat invDepth(depth.size(), CV_16UC1);
 	for(int i=0; i<depth.rows; ++i)
 	{
 		const float * in = depth.ptr<float>(i);
 		uint16_t * out = invDepth.ptr<uint16_t>(i);
 		for(int j=0; j<depth.cols; ++j)
 		{
 			const float d = in[j];
 			if(d > 0.0f && d < maxDepth) // false for NaN
 			{
 				// Rounded (ROS truncates), the decoding is the same.
 				const float v = depthQuantA / d + depthQuantB + 0.5f;
 				out[j] = v < 65536.0f ? (uint16_t)v : 0;
 			}
 			else
 			{
 				out[j] = 0;
 			}
 		}
 	}
 	return invDepth;
 }
 cv::Mat invDepthToDepth(const cv::Mat & invDepth, float depthQuantA, float depthQuantB)
 {
 	UASSERT(invDepth.type() == CV_16UC1);
 	cv::Mat depth(invDepth.size(), CV_32FC1);
 	for(int i=0; i<invDepth.rows; ++i)
 	{
 		const uint16_t * in = invDepth.ptr<uint16_t>(i);
 		float * out = depth.ptr<float>(i);
 		for(int j=0; j<invDepth.cols; ++j)
 		{
 			out[j] = in[j] ? depthQuantA / (float(in[j]) - depthQuantB) : 0.0f;
 		}
 	}
 	return depth;
 }
 void invDepthParameters(float depthQuantA, float depthQuantB, float & maxDepth, float & quantization)
 {
 	// inverse of depthQuantA = q*(q+1) and depthQuantB = 1 - depthQuantA/maxDepth
 	quantization = (std::sqrt(1.0f + 4.0f*depthQuantA) - 1.0f) / 2.0f;
 	maxDepth = depthQuantA / (1.0f - depthQuantB);
 }
 }  // namespace
-// format : ".jpg" ".png" ".rvl" "" (empty is general)
+bool parseImageCompressionFormat(const std::string & format, std::string & codec, float & maxDepth, float & quantization)
 {
 	codec.clear();
 	maxDepth = 0.0f;
 	quantization = 0.0f;
 	std::vector<std::string> fields = uListToVector(uSplit(format, ':'));
 	if(fields.empty())
 	{
 		return format.empty(); // empty is general (zlib)
 	}
 	if(fields[0].size() < 2 || fields[0][0] != '.')
 	{
 		return false;
 	}
 	if(fields.size() > 1)
 	{
 		// Inverse depth parameters only for formats supporting 16UC1
 		if((fields[0] != ".png" && fields[0] != ".rvl") || fields.size() > 3)
 		{
 			return false;
 		}
 		for(size_t i=1; i<fields.size(); ++i)
 		{
 			if(!uIsNumber(fields[i]) || uStr2Float(fields[i]) <= 0.0f)
 			{
 				return false;
 			}
 		}
 		maxDepth = uStr2Float(fields[1]);
 		quantization = fields.size() == 3 ? uStr2Float(fields[2]) : kDefaultDepthQuantization;
 	}
 	codec = fields[0];
 	return true;
 }
 // format : ".jpg" ".png" ".rvl" "" (empty is general), see parseImageCompressionFormat()
 CompressionThread::CompressionThread(const cv::Mat & mat, const std::string & format) :
 	uncompressedData_(mat),
 	format_(format),
 	image_(!format.empty()),
 	compressMode_(true)
 {
-	UASSERT(format.empty() || format.compare(".jpg") == 0 || format.compare(".png") == 0 || format.compare(".rvl") == 0);
+	std::string codec;
 	float maxDepth, quantization;
 	UASSERT_MSG(parseImageCompressionFormat(format, codec, maxDepth, quantization) &&
 			(codec.empty() || codec == ".jpg" || codec == ".png" || codec == ".rvl"),
 			uFormat("Invalid compression format \"%s\"", format.c_str()).c_str());
 }
 // assume image
 CompressionThread::CompressionThread(const cv::Mat & bytes, bool isImage) :
@@ -131,21 +235,43 @@ void CompressionThread::mainLoop()
 	this->kill();
 }
-// ".jpg" or ".png" or ".rvl"
+// ".jpg" or ".png" or ".rvl", with optional ":maxDepth[:quantization]" for 32FC1 depth images
 std::vector<unsigned char> compressImage(const cv::Mat & image, const std::string & format)
 {
 	std::vector<unsigned char> bytes;
 	if(!image.empty())
 	{
-		if(image.type() == CV_32FC1)
+		std::string codec;
 		float maxDepth, quantization;
 		if(!parseImageCompressionFormat(format, codec, maxDepth, quantization) || codec.empty())
 		{
 			UERROR("Invalid image compression format \"%s\"", format.c_str());
 			return bytes;
 		}
 		if(image.type() == CV_32FC1 && maxDepth > 0.0f)
 		{
 			float depthQuantA, depthQuantB;
 			cv::Mat invDepth = depthToInvDepth(image, maxDepth, quantization, depthQuantA, depthQuantB);
 			std::vector<unsigned char> invDepthBytes = compressImage(invDepth, codec);
 			if(!invDepthBytes.empty())
 			{
 				bytes.resize(kCompressedDepthInvHeaderSize + invDepthBytes.size());
 				memcpy(&bytes[0], kCompressedDepthInvSignature, 8);
 				memcpy(&bytes[8], &depthQuantA, 4);
 				memcpy(&bytes[12], &depthQuantB, 4);
 				memcpy(&bytes[kCompressedDepthInvHeaderSize], invDepthBytes.data(), invDepthBytes.size());
 			}
 		}
 		else if(image.type() == CV_32FC1)
 		{
 			//save in 8bits-4channel
 			cv::Mat bgra(image.size(), CV_8UC4, image.data);
 			cv::imencode(".png", bgra, bytes);
 		}
-		else if(format == ".rvl")
+		else if(codec == ".rvl")
 		{
-			bytes = {'D', 'E', 'P', 'T', 'H', 'R', 'V', 'L'};
+			bytes.assign(kCompressedDepthRvlSignature, kCompressedDepthRvlSignature+8);
 			int numPixels = image.rows * image.cols;
        	// In the worst case, RVL compression results in ~1.5x larger data.
        	bytes.resize(3 * numPixels + 20);
@@ -154,18 +280,18 @@ std::vector<unsigned char> compressImage(const cv::Mat & image, const std::strin
        	memcpy(&bytes[8], &cols, 4);
        	memcpy(&bytes[12], &rows, 4);
        	RvlCodec rvl;
-        	int compressedSize = rvl.CompressRVL(image.ptr<uint16_t>(), &bytes[16], numPixels);
+        	int compressedSize = rvl.CompressRVL(image.ptr<uint16_t>(), &bytes[kCompressedDepthRvlHeaderSize], numPixels);
-        	bytes.resize(16 + compressedSize);
+        	bytes.resize(kCompressedDepthRvlHeaderSize + compressedSize);
 		}
 		else
 		{
-			cv::imencode(format, image, bytes);
+			cv::imencode(codec, image, bytes);
 		}
 	}
 	return bytes;
 }
-// ".jpg" or ".png" or ".rvl"
+// ".jpg" or ".png" or ".rvl", with optional ":maxDepth[:quantization]" for 32FC1 depth images
 cv::Mat compressImage2(const cv::Mat & image, const std::string & format)
 {
 	std::vector<unsigned char> bytes = compressImage(image, format);
@@ -177,25 +303,65 @@ cv::Mat compressImage2(const cv::Mat & image, const std::string & format)
 }
 cv::Mat uncompressImage(const cv::Mat & bytes)
 {
 	if(bytes.empty())
 	{
 		return cv::Mat();
 	}
 	return uncompressImage(bytes.data, bytes.total()*bytes.elemSize());
 }
 cv::Mat uncompressImage(const std::vector<unsigned char> & bytes)
 {
 	return uncompressImage(bytes.data(), bytes.size());
 }
 cv::Mat uncompressImage(const unsigned char * bytes, size_t size)
 {
 	cv::Mat image;
-	if(!bytes.empty())
+	if(bytes && size)
 	{
-		if (compressedDepthFormat(bytes) == ".rvl")
+		if(hasSignature(bytes, size, kCompressedDepthInvSignature))
 		{
 			if(size <= kCompressedDepthInvHeaderSize)
 			{
 				UERROR("Inverse depth image is truncated (%d bytes).", (int)size);
 				return image;
 			}
 			float depthQuantA, depthQuantB;
 			memcpy(&depthQuantA, &bytes[8], 4);
 			memcpy(&depthQuantB, &bytes[12], 4);
 			cv::Mat invDepth = uncompressImage(&bytes[kCompressedDepthInvHeaderSize], size - kCompressedDepthInvHeaderSize);
 			if(invDepth.type() == CV_16UC1)
 			{
 				image = invDepthToDepth(invDepth, depthQuantA, depthQuantB);
 			}
 			else if(!invDepth.empty())
 			{
 				UERROR("Inverse depth image should be 16UC1 (type=%d).", invDepth.type());
 			}
 		}
 		else if(hasSignature(bytes, size, kCompressedDepthRvlSignature))
 		{
 			if(size < kCompressedDepthRvlHeaderSize)
 			{
 				UERROR("RVL depth image is truncated (%d bytes).", (int)size);
 				return image;
 			}
 			uint32_t cols, rows;
-        	memcpy(&cols, &bytes.data[8], 4);
+        	memcpy(&cols, &bytes[8], 4);
-        	memcpy(&rows, &bytes.data[12], 4);
+        	memcpy(&rows, &bytes[12], 4);
 			image = cv::Mat(rows, cols, CV_16UC1);
 			RvlCodec rvl;
-        	rvl.DecompressRVL(&bytes.data[16], image.ptr<uint16_t>(), cols * rows);
+        	rvl.DecompressRVL(&bytes[kCompressedDepthRvlHeaderSize], image.ptr<uint16_t>(), cols * rows);
 		}
 		else
 		{
 			const cv::Mat buf(1, (int)size, CV_8UC1, (void *)bytes);
 #if CV_MAJOR_VERSION>2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4)
-			image = cv::imdecode(bytes, cv::IMREAD_UNCHANGED);
+			image = cv::imdecode(buf, cv::IMREAD_UNCHANGED);
 #else
-			image = cv::imdecode(bytes, -1);
+			image = cv::imdecode(buf, -1);
 #endif
 			if(image.type() == CV_8UC4)
 			{
@@ -210,36 +376,6 @@ cv::Mat uncompressImage(const cv::Mat & bytes)
 	return image;
 }
 cv::Mat uncompressImage(const std::vector<unsigned char> & bytes)
 {
 	cv::Mat image;
 	if(bytes.size())
 	{
 		if (compressedDepthFormat(bytes) == ".rvl")
 		{
 			uint32_t cols, rows;
        	memcpy(&cols, &bytes[8], 4);
        	memcpy(&rows, &bytes[12], 4);
 			image = cv::Mat(rows, cols, CV_16UC1);
 			RvlCodec rvl;
        	rvl.DecompressRVL(&bytes[16], image.ptr<uint16_t>(), cols * rows);
 		}
 		else
 		{
 #if CV_MAJOR_VERSION>2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4)
 			image = cv::imdecode(bytes, cv::IMREAD_UNCHANGED);
 #else
 			image = cv::imdecode(bytes, -1);
 #endif
 			if(image.type() == CV_8UC4)
 			{
 				image = cv::Mat(image.size(), CV_32FC1, image.data).clone();
 			}
 		}
 	}
 	return image;
 }
 std::vector<unsigned char> compressData(const cv::Mat & data)
 {
 	std::vector<unsigned char> bytes;
@@ -381,10 +517,17 @@ std::string compressedDepthFormat(const unsigned char * bytes, size_t size)
 	std::string format;
 	if(bytes && size)
 	{
-		size_t maxlen = std::min(size, size_t(8));
+		if(hasSignature(bytes, size, kCompressedDepthInvSignature) && size > kCompressedDepthInvHeaderSize)
-		std::vector<unsigned char> signature(maxlen);
+		{
-		memcpy(&signature[0], bytes, maxlen);
+			float depthQuantA, depthQuantB, maxDepth, quantization;
-		if (std::string(signature.begin(), signature.end()) == "DEPTHRVL")
+			memcpy(&depthQuantA, &bytes[8], 4);
 			memcpy(&depthQuantB, &bytes[12], 4);
 			invDepthParameters(depthQuantA, depthQuantB, maxDepth, quantization);
 			format = uFormat("%s:%g:%g",
 					compressedDepthFormat(&bytes[kCompressedDepthInvHeaderSize], size - kCompressedDepthInvHeaderSize).c_str(),
 					maxDepth, quantization);
 		}
 		else if(hasSignature(bytes, size, kCompressedDepthRvlSignature))
 		{
 			format = ".rvl";
 		}
@@ -0,0 +1,494 @@
 /*
 Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 Redistribution and use in source and binary forms, with or without
 modification, are permitted provided that the following conditions are met:
    * Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in the
      documentation and/or other materials provided with the distribution.
    * Neither the name of the Universite de Sherbrooke nor the
      names of its contributors may be used to endorse or promote products
      derived from this software without specific prior written permission.
 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 #include <rtabmap/core/ImuMotionPredictor.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <algorithm>
 #include <vector>
 namespace rtabmap {
 // Samples older than this (s) behind the newest one are dropped, so the buffer stays
 // bounded while no odometry pose comes to trim it.
 static const double kMaxBufferDuration = 10.0;
 // Gravity estimation (see the class description). The IMU is still over a window if its
 // angular velocity stays under kStillMaxAngularVelocity and the standard deviation of its
 // specific force, on every axis, under kStillMaxAccelerationStd. Those are above the noise
 // and vibrations of an IMU on a robot at rest (e.g., ~0.1 m/s^2 on a drone on the ground,
 // against ~1 m/s^2 in flight), and well under what any acceleration that matters would
 // cause. The gravity is the median over the last kGravityWindows quiet windows.
 static const double kStillWindow = 0.5;              // s
 static const double kStillMaxAngularVelocity = 0.05; // rad/s
 static const double kStillMaxAccelerationStd = 0.15; // m/s^2
 static const size_t kStillMinSamples = 5;
 static const size_t kGravityWindows = 100;
 static const double kStandardGravity = 9.80665;
 ImuMotionPredictor::ImuMotionPredictor(double maxPoseInterval, double velocityWindow, double gravity) :
 	maxPoseInterval_(maxPoseInterval),
 	velocityWindow_(velocityWindow),
 	gravity_(gravity > 0.0 ? gravity : kStandardGravity),
 	gravityEstimated_(gravity <= 0.0),
 	gravityWindowsCount_(0),
 	poseStamp_(0.0),
 	velocity_(Eigen::Vector3d::Zero()),
 	worldToOdom_(Eigen::Quaterniond::Identity()),
 	integratedStartIsFinal_(false)
 {
 }
 void ImuMotionPredictor::addImu(double stamp, const IMU & imu)
 {
 	const cv::Vec4d & o = imu.orientation();
 	const Eigen::Quaterniond imuOrientation(o[3], o[0], o[1], o[2]);
 	if(imu.empty() ||
 	   imuOrientation.norm() < 0.5 ||
 	   (!imu.orientationCovariance().empty() && imu.orientationCovariance().at<double>(0,0) == -1.0))
 	{
 		// No orientation
 		return;
 	}
 	const Eigen::Quaterniond worldToImu = imuOrientation.normalized();
 	const Eigen::Quaterniond baseToImu = imu.localTransform().isNull()?
 			Eigen::Quaterniond::Identity():
 			imu.localTransform().getQuaterniond();
 	const cv::Vec3d & f = imu.linearAcceleration();
 	const bool hasAcceleration = (f[0] != 0.0 || f[1] != 0.0 || f[2] != 0.0) &&
 			(imu.linearAccelerationCovariance().empty() || imu.linearAccelerationCovariance().at<double>(0,0) != -1.0);
 	if(hasAcceleration && gravityEstimated_)
 	{
 		const cv::Vec3d & w = imu.angularVelocity();
 		estimateGravity(stamp, Eigen::Vector3d(f[0], f[1], f[2]), Eigen::Vector3d(w[0], w[1], w[2]).norm());
 	}
 	// The specific force includes the reaction to gravity, up in the world frame: it is
 	// removed when used (accelerationOf())
 	addSample(stamp, worldToImu * baseToImu.inverse(),
 			hasAcceleration ? Eigen::Vector3d(worldToImu * Eigen::Vector3d(f[0], f[1], f[2])) : Eigen::Vector3d::Zero(),
 			hasAcceleration);
 }
 void ImuMotionPredictor::estimateGravity(double stamp, const Eigen::Vector3d & specificForce, double angularVelocity)
 {
 	if(!stillWindow_.empty() && stamp < stillWindow_.back().stamp)
 	{
 		// Out of order
 		stillWindow_.clear();
 	}
 	if(!stillWindow_.empty() && stamp - stillWindow_.front().stamp >= kStillWindow)
 	{
 		// The window is complete (windows don't overlap: each sample counts once)
 		if(stillWindow_.size() >= kStillMinSamples)
 		{
 			Eigen::Vector3d mean = Eigen::Vector3d::Zero();
 			double maxAngularVelocity = 0.0;
 			for(size_t i=0; i<stillWindow_.size(); ++i)
 			{
 				mean += stillWindow_[i].specificForce;
 				maxAngularVelocity = std::max(maxAngularVelocity, stillWindow_[i].angularVelocity);
 			}
 			mean /= double(stillWindow_.size());
 			Eigen::Vector3d variance = Eigen::Vector3d::Zero();
 			for(size_t i=0; i<stillWindow_.size(); ++i)
 			{
 				variance += (stillWindow_[i].specificForce - mean).cwiseAbs2();
 			}
 			variance /= double(stillWindow_.size());
 			if(maxAngularVelocity < kStillMaxAngularVelocity &&
 			   variance.maxCoeff() < kStillMaxAccelerationStd*kStillMaxAccelerationStd)
 			{
 				// Not accelerating: the mean specific force is gravity, whatever the
 				// attitude (the norm of the mean, not the mean of the norms, which the
 				// noise would bias up)
 				gravityWindows_.push_back(mean.norm());
 				if(gravityWindows_.size() > kGravityWindows)
 				{
 					gravityWindows_.erase(gravityWindows_.begin());
 				}
 				++gravityWindowsCount_;
 				std::vector<double> sorted = gravityWindows_;
 				std::nth_element(sorted.begin(), sorted.begin() + sorted.size()/2, sorted.end());
 				const double gravity = sorted[sorted.size()/2];
 				if(gravityWindowsCount_ == 1)
 				{
 					UINFO("IMU gravity estimated at %f m/s^2 (the IMU is still)", gravity);
 				}
 				else
 				{
 					UDEBUG("IMU gravity estimated at %f m/s^2 (%d still windows)", gravity, (int)gravityWindowsCount_);
 				}
 				if(gravity != gravity_)
 				{
 					gravity_ = gravity;
 					// The accelerations integrated changed
 					integrated_.clear();
 				}
 			}
 		}
 		stillWindow_.clear();
 	}
 	StillSample sample;
 	sample.stamp = stamp;
 	sample.specificForce = specificForce;
 	sample.angularVelocity = angularVelocity;
 	stillWindow_.push_back(sample);
 }
 void ImuMotionPredictor::addSample(double stamp, const Eigen::Quaterniond & orientation,
 		const Eigen::Vector3d & specificForce, bool hasAcceleration)
 {
 	Sample sample;
 	sample.orientation = orientation.normalized();
 	sample.specificForce = specificForce;
 	sample.hasAcceleration = hasAcceleration;
 	samples_[stamp] = sample;
 	if(!integrated_.empty() && stamp <= integrated_.rbegin()->first)
 	{
 		// Out of order: what was integrated after it changes
 		integrated_.clear();
 	}
 	while(samples_.size() > 2 && samples_.begin()->first < stamp - kMaxBufferDuration)
 	{
 		samples_.erase(samples_.begin());
 		integrated_.clear();
 	}
 }
 void ImuMotionPredictor::addPose(double stamp, const rtabmap::Transform & pose)
 {
 	integrated_.clear();
 	if(pose.isNull())
 	{
 		pose_.setNull();
 		poseStamp_ = 0.0;
 		velocity_.setZero();
 		poses_.clear();
 		return;
 	}
 	Eigen::Vector3d velocity = Eigen::Vector3d::Zero();
 	Eigen::Quaterniond worldToOdom = Eigen::Quaterniond::Identity();
 	if(!samples_.empty())
 	{
 		// The odometry and the IMU agree on the orientation of the base at that time,
 		// whatever the yaw of their frames.
 		worldToOdom = (pose.getQuaterniond() * orientationAt(stamp).inverse()).normalized();
 	}
 	// The velocity is estimated from the displacement since a previous pose. Not the
 	// last one: odometry's own noise, divided by a frame interval, would be of the order of
 	// the velocity itself, and since the prediction deskews the next scans, that noise would
 	// feed back into the next poses. The newest pose at least velocityWindow_ old is used
 	// instead (or the oldest kept if there is none yet), not older than maxPoseInterval_.
 	poses_.erase(poses_.lower_bound(stamp), poses_.end());
 	poses_.erase(poses_.begin(), poses_.lower_bound(stamp - maxPoseInterval_));
 	std::map<double, rtabmap::Transform>::const_iterator reference = poses_.begin();
 	for(std::map<double, rtabmap::Transform>::const_iterator iter = poses_.begin();
 		iter != poses_.end() && iter->first <= stamp - velocityWindow_; ++iter)
 	{
 		reference = iter;
 	}
 	if(reference != poses_.end())
 	{
 		const double interval = stamp - reference->first;
 		const Eigen::Vector3d displacement(
 				pose.x() - reference->second.x(),
 				pose.y() - reference->second.y(),
 				pose.z() - reference->second.z());
 		if(velocityWindow_ > 0.0 &&
 		   !samples_.empty() &&
 		   samples_.begin()->first <= reference->first &&
 		   samples_.rbegin()->first >= stamp)
 		{
 			// displacement = v0*T + D, with D the double integral of the acceleration
 			// over the interval: solve for v0, the velocity at the reference pose, then
 			// carry it to this pose with the single integral V.
 			Eigen::Vector3d deltaVelocity;
 			Eigen::Vector3d deltaPosition;
 			integrate(reference->first, stamp, worldToOdom, deltaVelocity, deltaPosition);
 			velocity = (displacement - deltaPosition) / interval + deltaVelocity;
 		}
 		else
 		{
 			// Average velocity over the interval (no window, or the samples don't cover it)
 			velocity = displacement / interval;
 		}
 	}
 	pose_ = pose;
 	poseStamp_ = stamp;
 	velocity_ = velocity;
 	worldToOdom_ = worldToOdom;
 	poses_[stamp] = pose;
 	// The next velocities are estimated from the poses kept: keep the samples from the
 	// oldest one, including the last sample before it to interpolate at its stamp.
 	std::map<double, Sample>::iterator iter = samples_.upper_bound(poses_.begin()->first);
 	if(iter != samples_.begin())
 	{
 		--iter;
 		samples_.erase(samples_.begin(), iter);
 	}
 }
 void ImuMotionPredictor::reset()
 {
 	samples_.clear();
 	stillWindow_.clear();
 	poses_.clear();
 	integrated_.clear();
 	pose_.setNull();
 	poseStamp_ = 0.0;
 	velocity_.setZero();
 	worldToOdom_.setIdentity();
 }
 rtabmap::Transform ImuMotionPredictor::predict(double stamp) const
 {
 	if(samples_.empty())
 	{
 		return rtabmap::Transform();
 	}
 	if(pose_.isNull())
 	{
 		// No pose yet (or lost): only the orientation is known, in the IMU's world frame.
 		const Eigen::Quaterniond orientation = orientationAt(stamp);
 		return rtabmap::Transform(0, 0, 0, orientation.x(), orientation.y(), orientation.z(), orientation.w());
 	}
 	const Eigen::Quaterniond orientation = (worldToOdom_ * orientationAt(stamp)).normalized();
 	// With t0 the stamp of the last pose, p0 its position and v0 the velocity there, and
 	// a(u) the acceleration (gravity removed, in the odometry frame), the position at t is:
 	//
 	//   p(t) = p0 + v0*(t-t0) + D(t),  with D(t) = integral_t0^t integral_t0^s a(u) du ds
 	//
 	// D(t) is the displacement due to the change of velocity since t0, V(s) = integral_t0^s a(u) du.
 	Eigen::Vector3d position(pose_.x(), pose_.y(), pose_.z()); // p0
 	position += velocity_ * (stamp - poseStamp_);               // + v0*(t-t0)
 	if(velocityWindow_ <= 0.0)
 	{
 		// No acceleration without a velocity window: constant velocity
 	}
 	else if(stamp >= poseStamp_)
 	{
 		// + D(t). D and V are kept at every sample stamp since t0 (integrated_), so only
 		// the part from the last sample ti <= t is integrated here, with dt = t-ti:
 		//
 		//   D(t) = D(ti) + V(ti)*dt + integral_ti^t integral_ti^s a(u) du ds
 		//
 		// Between samples the acceleration is linear, from a(ti) to a(t), for which that
 		// last double integral is exactly dt^2*(2*a(ti) + a(t))/6. Those are the same
 		// segments integrate() would go through, without redoing all those before ti.
 		updateIntegration();
 		std::map<double, Integrated>::const_iterator from = integrated_.upper_bound(stamp);
 		--from; // ti: the first entry is at t0, so there is one
 		const double dt = stamp - from->first;
 		const Eigen::Vector3d accelerationB = worldToOdom_ * accelerationAt(stamp); // a(t)
 		position += from->second.position +                       // D(ti)
 				from->second.velocity * dt +                          // V(ti)*dt
 				dt * dt * (2.0 * from->second.acceleration + accelerationB) / 6.0; // a(ti) to a(t)
 	}
 	else
 	{
 		// + D(t), backward from t0: only for points stamped before the last pose, rare
 		Eigen::Vector3d deltaVelocity;
 		Eigen::Vector3d deltaPosition;
 		integrate(poseStamp_, stamp, worldToOdom_, deltaVelocity, deltaPosition);
 		position += deltaPosition;
 	}
 	return rtabmap::Transform(position.x(), position.y(), position.z(),
 			orientation.x(), orientation.y(), orientation.z(), orientation.w());
 }
 bool ImuMotionPredictor::hasPose() const
 {
 	return !pose_.isNull();
 }
 double ImuMotionPredictor::lastPoseStamp() const
 {
 	return poseStamp_;
 }
 Eigen::Vector3d ImuMotionPredictor::velocity() const
 {
 	return velocity_;
 }
 size_t ImuMotionPredictor::samples() const
 {
 	return samples_.size();
 }
 // samples_ must not be empty for the four below.
 void ImuMotionPredictor::updateIntegration() const
 {
 	if(!integrated_.empty() && !integratedStartIsFinal_ && samples_.rbegin()->first >= poseStamp_)
 	{
 		// The acceleration at the pose was held from the newest sample, which is no
 		// longer the newest
 		integrated_.clear();
 	}
 	if(integrated_.empty())
 	{
 		Integrated start;
 		start.acceleration = worldToOdom_ * accelerationAt(poseStamp_);
 		start.velocity.setZero();
 		start.position.setZero();
 		integrated_[poseStamp_] = start;
 		integratedStartIsFinal_ = samples_.rbegin()->first >= poseStamp_;
 	}
 	// Extend to the samples received since
 	for(std::map<double, Sample>::const_iterator iter = samples_.upper_bound(integrated_.rbegin()->first);
 		iter != samples_.end(); ++iter)
 	{
 		const std::pair<const double, Integrated> & previous = *integrated_.rbegin();
 		const double dt = iter->first - previous.first;
 		Integrated next;
 		// Same segment as in integrate(), from the previous sample ti to this one:
 		// D(ti+1) = D(ti) + V(ti)*dt + dt^2*(2*a(ti) + a(ti+1))/6, V(ti+1) = V(ti) + dt*(a(ti) + a(ti+1))/2
 		next.acceleration = worldToOdom_ * accelerationOf(iter->second);
 		next.position = previous.second.position + previous.second.velocity * dt +
 				dt * dt * (2.0 * previous.second.acceleration + next.acceleration) / 6.0;
 		next.velocity = previous.second.velocity + dt * (previous.second.acceleration + next.acceleration) / 2.0;
 		integrated_.insert(integrated_.end(), std::make_pair(iter->first, next));
 	}
 }
 Eigen::Quaterniond ImuMotionPredictor::orientationAt(double stamp) const
 {
 	std::map<double, Sample>::const_iterator after = samples_.lower_bound(stamp);
 	if(after == samples_.end())
 	{
 		return samples_.rbegin()->second.orientation;
 	}
 	if(after == samples_.begin() || after->first == stamp)
 	{
 		return after->second.orientation;
 	}
 	std::map<double, Sample>::const_iterator before = std::prev(after);
 	const double ratio = (stamp - before->first) / (after->first - before->first);
 	return before->second.orientation.slerp(ratio, after->second.orientation);
 }
 Eigen::Vector3d ImuMotionPredictor::accelerationOf(const Sample & sample) const
 {
 	if(!sample.hasAcceleration)
 	{
 		return Eigen::Vector3d::Zero();
 	}
 	return sample.specificForce - Eigen::Vector3d(0, 0, gravity_);
 }
 Eigen::Vector3d ImuMotionPredictor::accelerationAt(double stamp) const
 {
 	std::map<double, Sample>::const_iterator after = samples_.lower_bound(stamp);
 	if(after == samples_.end())
 	{
 		return accelerationOf(samples_.rbegin()->second);
 	}
 	if(after == samples_.begin() || after->first == stamp)
 	{
 		return accelerationOf(after->second);
 	}
 	std::map<double, Sample>::const_iterator before = std::prev(after);
 	const double ratio = (stamp - before->first) / (after->first - before->first);
 	const Eigen::Vector3d accelerationBefore = accelerationOf(before->second);
 	return accelerationBefore + ratio * (accelerationOf(after->second) - accelerationBefore);
 }
 void ImuMotionPredictor::integrate(double from, double to, const Eigen::Quaterniond & rotation,
 		Eigen::Vector3d & velocity, Eigen::Vector3d & position) const
 {
 	velocity.setZero();
 	position.setZero();
 	if(from == to)
 	{
 		return;
 	}
 	// Breakpoints: the bounds and every sample in between, in the direction of
 	// integration (backward if "to" is before "from"). Between two of them the
 	// acceleration is linear, which the segment update below integrates exactly; before
 	// the first sample and after the last one, it is held constant.
 	std::vector<double> stamps;
 	stamps.push_back(from);
 	if(from < to)
 	{
 		for(std::map<double, Sample>::const_iterator iter = samples_.upper_bound(from);
 			iter != samples_.end() && iter->first < to; ++iter)
 		{
 			stamps.push_back(iter->first);
 		}
 	}
 	else
 	{
 		std::map<double, Sample>::const_iterator iter = samples_.lower_bound(from);
 		while(iter != samples_.begin())
 		{
 			--iter;
 			if(iter->first <= to)
 			{
 				break;
 			}
 			stamps.push_back(iter->first);
 		}
 	}
 	stamps.push_back(to);
 	// Returned: with a(u) the acceleration rotated by "rotation", from "from" (t0) to "to" (t),
 	//
 	//   velocity = V(t) = integral_t0^t a(u) du
 	//   position = D(t) = integral_t0^t integral_t0^s a(u) du ds
 	//
 	// that is, the change of velocity and the displacement it causes (from a velocity null
 	// at t0: the caller adds v0*(t-t0)). They are accumulated breakpoint by breakpoint: from
 	// ti to the next one ti+1, with dt = ti+1 - ti and a(u) linear from a(ti) to a(ti+1),
 	//
 	//   D(ti+1) = D(ti) + V(ti)*dt + dt^2*(2*a(ti) + a(ti+1))/6
 	//   V(ti+1) = V(ti) + dt*(a(ti) + a(ti+1))/2
 	//
 	// both exact for a linear acceleration (dt is negative backward, the same formulas hold).
 	Eigen::Vector3d accelerationA = rotation * accelerationAt(stamps[0]); // a(t0)
 	for(size_t i=1; i<stamps.size(); ++i)
 	{
 		const double dt = stamps[i] - stamps[i-1];
 		const Eigen::Vector3d accelerationB = rotation * accelerationAt(stamps[i]); // a(ti+1)
 		position += velocity * dt +                                     // V(ti)*dt
 				dt * dt * (2.0 * accelerationA + accelerationB) / 6.0;  // a(ti) to a(ti+1)
 		velocity += dt * (accelerationA + accelerationB) / 2.0;         // trapezoid of a
 		accelerationA = accelerationB;
 	}
 }
 }
@@ -102,6 +102,7 @@ Memory::Memory(const ParametersMap & parameters) :
    _imagePreDecimation(Parameters::defaultMemImagePreDecimation()),
 	_imagePostDecimation(Parameters::defaultMemImagePostDecimation()),
 	_legacyDecimatedOctave(false),
 	_inverseDepthCompressionAllowed(true),
 	_compressionParallelized(Parameters::defaultMemCompressionParallelized()),
 	_laserScanDownsampleStepSize(Parameters::defaultMemLaserScanDownsampleStepSize()),
 	_laserScanVoxelSize(Parameters::defaultMemLaserScanVoxelSize()),
@@ -228,6 +229,11 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
 			// filling it that way; a new one gets the corrected scaling.
 			_legacyDecimatedOctave =
 					uStrNumCmp(_dbDriver->getDatabaseVersion(), "0.23.12") < 0;
 			// Depth images compressed as inverse depth cannot be read before 0.24, which
 			// would still open databases created with Db/TargetVersion < 0.24 or by an
 			// older version.
 			_inverseDepthCompressionAllowed =
 					uStrNumCmp(_dbDriver->getDatabaseVersion(), "0.24.0") >= 0;
 			// Only where the descriptors stored in the map end up different: keypoints
 			// from odometry, scaled into the pre-decimated image before being described.
 			if(_legacyDecimatedOctave && _useOdometryFeatures && _imagePreDecimation > 1)
@@ -826,6 +832,19 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(params, Parameters::kMemIntermediateNodeDataKept(), _saveIntermediateNodeData);
 	Parameters::parse(params, Parameters::kMemImageCompressionFormat(), _rgbCompressionFormat);
 	Parameters::parse(params, Parameters::kMemDepthCompressionFormat(), _depthCompressionFormat);
 	{
 		std::string codec;
 		float maxDepth, quantization;
 		if(!parseImageCompressionFormat(_depthCompressionFormat, codec, maxDepth, quantization) ||
 		   (codec != ".png" && codec != ".rvl"))
 		{
 			UWARN("Invalid %s=\"%s\", using default \"%s\".",
 					Parameters::kMemDepthCompressionFormat().c_str(),
 					_depthCompressionFormat.c_str(),
 					Parameters::defaultMemDepthCompressionFormat().c_str());
 			_depthCompressionFormat = Parameters::defaultMemDepthCompressionFormat();
 		}
 	}
 	Parameters::parse(params, Parameters::kMemRehearsalIdUpdatedToNewOne(), _idUpdatedToNewOneRehearsal);
 	Parameters::parse(params, Parameters::kMemGenerateIds(), _generateIds);
 	Parameters::parse(params, Parameters::kMemBadSignaturesIgnored(), _badSignaturesIgnored);
@@ -5227,7 +5246,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 	if(!isIntermediateNode)
 	{
 		// We need raw images if we need to extract features and/or do tag detection
-		bool needRawImages = _feature2D->getMaxFeatures() >= 0 && 
+		bool needRawImages = (_feature2D->getMaxFeatures() >= 0 &&
 			(!_useOdometryFeatures ||
 			 data.keypoints().empty() ||
 			 (int)data.keypoints().size() != data.descriptors().rows ||
@@ -5235,7 +5254,9 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			 _detectMarkers ||
 			 _rotateImagesUpsideUp ||
 			 _imagePostDecimation > 1 ||
-			 (_createOccupancyGrid && _localMapMaker->isGridFromDepth()));
+			 (_createOccupancyGrid && _localMapMaker->isGridFromDepth()))) ||
 			// Images rectified below: stereo always, RGB-D unless only its features are
 			(!_imagesAlreadyRectified && !(_rectifyOnlyFeatures && data.stereoCameraModels().empty()));
 		// Note: we could avoid uncompressing scan if we don't do any filtering
 		// and if we don't use it for local occupancy grid
@@ -6625,6 +6646,44 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		std::vector<unsigned char> imageBytes;
 		std::vector<unsigned char> depthBytes;
 		std::string depthCompressionFormat = _depthCompressionFormat;
 		bool reuseCompressedDepth =
 				depthOrRightImage.data == data.depthOrRightRaw().data &&
 				!data.depthOrRightCompressed().empty();
 		if(!_inverseDepthCompressionAllowed)
 		{
 			std::string codec;
 			float maxDepth, quantization;
 			if(parseImageCompressionFormat(depthCompressionFormat, codec, maxDepth, quantization) && maxDepth > 0.0f)
 			{
 				static bool warned = false;
 				if(!warned)
 				{
 					UWARN("%s=\"%s\": inverse depth compression format is not compatible with database "
 						  "version %s (requires >= 0.24, see %s), \"%s\" format is used instead. This "
 						  "warning is only printed once.",
 						Parameters::kMemDepthCompressionFormat().c_str(),
 						depthCompressionFormat.c_str(),
 						_dbDriver?_dbDriver->getDatabaseVersion().c_str():"",
 						Parameters::kDbTargetVersion().c_str(),
 						codec.c_str());
 					warned = true;
 				}
 				depthCompressionFormat = codec;
 			}
 			if(reuseCompressedDepth &&
 			   compressedDepthFormat(data.depthOrRightCompressed()).find(':') != std::string::npos)
 			{
 				// Already compressed as inverse depth (e.g., received from ROS's
 				// compressed_depth_image_transport), re-compress it.
 				reuseCompressedDepth = false;
 				if(depthOrRightImage.empty())
 				{
 					depthOrRightImage = uncompressImage(data.depthOrRightCompressed());
 				}
 			}
 		}
 		if(!depthOrRightImage.empty() && depthOrRightImage.type() == CV_32FC1)
 		{
 			if(_saveDepth16Format)
@@ -6640,7 +6699,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 				}
 				depthOrRightImage = util2d::cvtDepthFromFloat(depthOrRightImage);
 			}
-			else if(_depthCompressionFormat == ".rvl")
+			else if(depthCompressionFormat == ".rvl")
 			{
 				static bool warned = false;
 				if(!warned)
@@ -6650,13 +6709,16 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 						  "images will be compressed in \".png\" format instead. Explicitly "
 						  "set %s to true to keep using \"%s\" format and images will be "
 						  "converted to 16bits for convenience (warning: that would "
-						  "remove all depth values over 65 meters). Explicitly set %s=\".png\" "
+						  "remove all depth values over 65 meters). Set %s=\".rvl:<maxDepth>:<quantization>\" "
 						  "(e.g., \".rvl:10:100\") to compress them in RVL as 16 bits inverse depth "
 						  "(lossy, see parameter's description). Explicitly set %s=\".png\" "
 						  "to suppress this warning. This warning is only printed once.",
 						Parameters::kMemSaveDepth16Format().c_str(),
 						Parameters::kMemDepthCompressionFormat().c_str(),
-						_depthCompressionFormat.c_str(),
+						depthCompressionFormat.c_str(),
 						Parameters::kMemSaveDepth16Format().c_str(),
-						_depthCompressionFormat.c_str(),
+						depthCompressionFormat.c_str(),
 						Parameters::kMemDepthCompressionFormat().c_str(),
 						Parameters::kMemDepthCompressionFormat().c_str());
 					warned = true;
 				}
@@ -6666,9 +6728,8 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		bool reuseCompressedImage =
 				image.data == data.imageRaw().data &&
 				!data.imageCompressed().empty();
-		bool reuseCompressedDepth =
+		reuseCompressedDepth = reuseCompressedDepth &&
-				depthOrRightImage.data == data.depthOrRightRaw().data &&
+				depthOrRightImage.data == data.depthOrRightRaw().data;
 				!data.depthOrRightCompressed().empty();
 		bool reuseCompressedDepthConfidence =
 				depthConfidence.data == data.depthConfidenceRaw().data &&
 				!data.depthConfidenceCompressed().empty();
@@ -6685,7 +6746,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		if(_compressionParallelized)
 		{
 			rtabmap::CompressionThread ctImage(image, _rgbCompressionFormat);
-			rtabmap::CompressionThread ctDepth(depthOrRightImage, depthOrRightImage.type() == CV_32FC1 || depthOrRightImage.type() == CV_16UC1?_depthCompressionFormat:_rgbCompressionFormat);
+			rtabmap::CompressionThread ctDepth(depthOrRightImage, depthOrRightImage.type() == CV_32FC1 || depthOrRightImage.type() == CV_16UC1?depthCompressionFormat:_rgbCompressionFormat);
 			rtabmap::CompressionThread ctDepthConfidence(depthConfidence);
 			rtabmap::CompressionThread ctLaserScan(laserScan.data());
 			rtabmap::CompressionThread ctUserData(data.userDataRaw());
@@ -6724,7 +6785,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		else
 		{
 			compressedImage = reuseCompressedImage?cv::Mat():compressImage2(image, _rgbCompressionFormat);
-			compressedDepth = reuseCompressedDepth?cv::Mat():compressImage2(depthOrRightImage, depthOrRightImage.type() == CV_32FC1 || depthOrRightImage.type() == CV_16UC1?_depthCompressionFormat:_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());
@@ -161,6 +161,13 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 	Parameters::parse(parameters, Parameters::kOdomHolonomic(), _holonomic);
 	Parameters::parse(parameters, Parameters::kOdomGuessMotion(), guessFromMotion_);
 	Parameters::parse(parameters, Parameters::kOdomGuessSmoothingDelay(), guessSmoothingDelay_);
 	{
 		float imuGravity = Parameters::defaultOdomImuGravity();
 		Parameters::parse(parameters, Parameters::kOdomImuGravity(), imuGravity);
 		// The velocity is estimated over the smoothing delay, and the IMU acceleration used
 		// only with one (> 0)
 		imuMotionPredictor_ = ImuMotionPredictor(1.0, guessSmoothingDelay_, imuGravity);
 	}
 	Parameters::parse(parameters, Parameters::kOdomFillInfoData(), _fillInfoData);
 	Parameters::parse(parameters, Parameters::kOdomFilteringStrategy(), _filteringStrategy);
 	Parameters::parse(parameters, Parameters::kOdomParticleSize(), _particleSize);
@@ -228,6 +235,7 @@ void Odometry::reset(const Transform & initialPose)
 	framesProcessed_ = 0;
 	imuLastTransform_.setNull();
 	imus_.clear();
 	imuMotionPredictor_.reset();
 	if(_force3DoF || particleFilters_.size())
 	{
 		float x,y,z, roll,pitch,yaw;
@@ -312,9 +320,11 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 {
 	UASSERT_MSG(data.id() >= 0, uFormat("Input data should have ID greater or equal than 0 (id=%d)!", data.id()).c_str());
-	// cache imu data
+	if(!data.imu().empty())
 	if(!data.imu().empty() && !this->canProcessAsyncIMU())
 	{
 		if(!this->canProcessAsyncIMU())
 		{
 			// cache imu data
 			if(!(data.imu().orientation()[0] == 0.0 && data.imu().orientation()[1] == 0.0 && data.imu().orientation()[2] == 0.0))
 			{
 				Transform orientation(0,0,0, data.imu().orientation()[0], data.imu().orientation()[1], data.imu().orientation()[2], data.imu().orientation()[3]);
@@ -323,23 +333,13 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 						orientation*
 						data.imu().localTransform().rotation().inverse();
 			if(	this->getPose().r11() == 1.0f && this->getPose().r22() == 1.0f && this->getPose().r33() == 1.0f &&
 				this->framesProcessed() == 0)
 			{
 				Eigen::Quaterniond imuQuat = imuT.getQuaterniond();
 				Transform previous = this->getPose();
 				Transform newFramePose = Transform(previous.x(), previous.y(), previous.z(), imuQuat.x(), imuQuat.y(), imuQuat.z(), imuQuat.w());
 				UWARN("Updated initial pose from %s to %s with IMU orientation", previous.prettyPrint().c_str(), newFramePose.prettyPrint().c_str());
 				std::map<double, rtabmap::Transform> imus = imus_;
 				this->reset(newFramePose);
 				imus_ = imus;
 			}
 				imus_.insert(std::make_pair(data.stamp(), imuT));
 				if(imus_.size() > 1000)
 				{
 					imus_.erase(imus_.begin());
 				}
 				imuMotionPredictor_.addImu(data.stamp(), data.imu());
 			}
 			else
 			{
@@ -347,6 +347,23 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 			}
 		}
 		// IMU-only update: nothing more to do once the IMU is cached, except for approaches
 		// processing it themselves. 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.
 		if(data.imageRaw().empty() && data.imageCompressed().empty() &&
 		   data.laserScanRaw().isEmpty() && data.laserScanCompressed().isEmpty() &&
 		   data.cameraModels().empty() && data.stereoCameraModels().empty())
 		{
 			if(this->canProcessAsyncIMU())
 			{
 				this->computeTransform(data, Transform(), info);
 			}
 			return Transform(); // Return null on IMU-only updates
 		}
 	}
 	if((data.imageRaw().empty() && !data.imageCompressed().empty()) ||
 	   (data.depthOrRightRaw().empty() && !data.depthOrRightCompressed().empty()) ||
 	   (data.laserScanRaw().empty() && !data.laserScanCompressed().empty()))
@@ -588,12 +605,40 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 		}
 	}
 	// Initial orientation from the IMU at the first frame's own stamp (unless an initial pose
 	// with a rotation was given). Not from the first IMU sample received, which can be much
 	// older (e.g., IMU buffered while waiting for the first frame), nor from the newest one,
 	// which can be after the stamp (lidar deskewing needs IMU up to the end of the sweep).
 	if(this->framesProcessed() == 0 && !imus_.empty() &&
 	   this->getPose().r11() == 1.0f && this->getPose().r22() == 1.0f && this->getPose().r33() == 1.0f)
 	{
 		// Interpolated at the stamp, or the closest sample if the IMU doesn't cover it (e.g.,
 		// the only sample received is just after the frame)
 		Transform imuAtStamp = Transform::getTransform(imus_, data.stamp());
 		if(imuAtStamp.isNull())
 		{
 			imuAtStamp = data.stamp() < imus_.begin()->first?imus_.begin()->second:imus_.rbegin()->second;
 		}
 		if(!imuAtStamp.isNull())
 		{
 			const Eigen::Quaterniond q = imuAtStamp.getQuaterniond();
 			const Transform previous = this->getPose();
 			const Transform initialPose(previous.x(), previous.y(), previous.z(), q.x(), q.y(), q.z(), q.w());
 			UWARN("Updated initial pose from %s to %s with IMU orientation", previous.prettyPrint().c_str(), initialPose.prettyPrint().c_str());
 			std::map<double, rtabmap::Transform> imus = imus_;
 			ImuMotionPredictor imuMotionPredictor = imuMotionPredictor_;
 			this->reset(initialPose);
 			imus_ = imus;
 			imuMotionPredictor_ = imuMotionPredictor;
 		}
 	}
 	// KITTI datasets start with stamp=0
 	double dt = previousStamp_>0.0f || (previousStamp_==0.0f && framesProcessed()==1)?data.stamp() - previousStamp_:0.0;
 	Transform guess = dt>0.0 && guessFromMotion_ && !velocityGuess_.isNull()?Transform::getIdentity():Transform();
 	if(!(dt>0.0 || (dt == 0.0 && velocityGuess_.isNull())))
 	{
-		if(guessFromMotion_ && (!data.imageRaw().empty() || !data.laserScanRaw().isEmpty()))
+		if(guessFromMotion_)
 		{
 			UERROR("Guess from motion is set but dt is invalid! Odometry is then computed without guess. (dt=%f previous transform=%s)", dt, velocityGuess_.prettyPrint().c_str());
 		}
@@ -646,6 +691,17 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 					orientation.r11(), orientation.r12(), orientation.r13(), guess.x(),
 					orientation.r21(), orientation.r22(), orientation.r23(), guess.y(),
 					orientation.r31(), orientation.r32(), orientation.r33(), guess.z());
 			if(guessFromMotion_ && guessSmoothingDelay_ > 0.0f && imuMotionPredictor_.hasPose())
 			{
 				// Translation (and orientation) predicted from the previous pose with the
 				// IMU acceleration, instead of a constant velocity: the velocity is the one
 				// over the smoothing delay, carried to the previous frame with the IMU.
 				Transform predicted = imuMotionPredictor_.predict(data.stamp());
 				if(!predicted.isNull())
 				{
 					guess = _pose.inverse() * predicted;
 				}
 			}
 			if(_force3DoF)
 			{
 				guess = guess.to3DoF();
@@ -657,21 +713,54 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 			UWARN("Could not find imu transform at %f", data.stamp());
 		}
 	}
-	else if(!guess.isNull() && (!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())) {
+	else if(!guess.isNull()) {
 		UDEBUG("Using guess from motion %s", guess.prettyPrint().c_str());
 	}
 	UTimer time;
-	// Deskewing lidar
+	// Deskewing lidar, if the scan has a time spread (not already deskewed: deskewing zeroes
-	if( _deskewing &&
+	// the time channel)
 	const bool scanHasTimeSpread =
 			!data.laserScanRaw().empty() &&
 			data.laserScanRaw().hasTime() &&
 			data.laserScanRaw().data().ptr<float>(0, data.laserScanRaw().size()-1)[data.laserScanRaw().getTimeOffset()] !=
 			data.laserScanRaw().data().ptr<float>(0, 0)[data.laserScanRaw().getTimeOffset()];
 	if( _deskewing &&
 		scanHasTimeSpread &&
 		!imus_.empty() &&
 		!imuMotionPredictor_.predict(data.stamp()).isNull())
 	{
 		UDEBUG("Deskewing with IMU begin");
 		// Every point's pose predicted with the IMU since the previous frame: orientation
 		// from the IMU, translation from the velocity (carried with the IMU acceleration
 		// with a smoothing delay). Before the first pose, only the orientation.
 		const Transform referenceInverse = imuMotionPredictor_.predict(data.stamp()).inverse();
 		auto motion = [&](double stamp)
 		{
 			Transform pose = imuMotionPredictor_.predict(stamp);
 			if(pose.isNull())
 			{
 				return pose;
 			}
 			pose = referenceInverse * pose;
 			return _force3DoF?pose.to3DoF():pose;
 		};
 		LaserScan scanDeskewed = util3d::deskew(data.laserScanRaw(), data.stamp(), motion);
 		if(!scanDeskewed.isEmpty())
 		{
 			data.setLaserScan(scanDeskewed);
 		}
 		info->timeDeskewing = time.ticks();
 		UDEBUG("Deskewing end");
 	}
 	else if( _deskewing &&
 		scanHasTimeSpread &&
 		dt > 0 &&
 		!guess.isNull())
 	{
 		UDEBUG("Deskewing begin");
-		// Recompute velocity
+		// Constant velocity
 		float vx,vy,vz, vroll,vpitch,vyaw;
 		guess.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);
@@ -683,38 +772,6 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 		vpitch /= dt;
 		vyaw /= dt;
 		if(!imus_.empty())
 		{
 			float scanTime =
 				data.laserScanRaw().data().ptr<float>(0, data.laserScanRaw().size()-1)[data.laserScanRaw().getTimeOffset()] -
 				data.laserScanRaw().data().ptr<float>(0, 0)[data.laserScanRaw().getTimeOffset()];
 			// replace orientation velocity based on IMU (if available)
 			Transform imuFirstScan = Transform::getTransform(imus_,
 					data.stamp() +
 					data.laserScanRaw().data().ptr<float>(0, 0)[data.laserScanRaw().getTimeOffset()]);
 			Transform imuLastScan = Transform::getTransform(imus_,
 					data.stamp() +
 					data.laserScanRaw().data().ptr<float>(0, data.laserScanRaw().size()-1)[data.laserScanRaw().getTimeOffset()]);
 			if(!imuFirstScan.isNull() && !imuLastScan.isNull())
 			{
 				Transform orientation = imuFirstScan.inverse() * imuLastScan;
 				orientation.getEulerAngles(vroll, vpitch, vyaw);
 				if(_force3DoF)
 				{
 					vroll=0;
 					vpitch=0;
 					vyaw /= scanTime;
 				}
 				else
 				{
 					vroll /= scanTime;
 					vpitch /= scanTime;
 					vyaw /= scanTime;
 				}
 			}
 		}
 		Transform velocity(vx,vy,vz,vroll,vpitch,vyaw);
 		LaserScan scanDeskewed = util3d::deskew(data.laserScanRaw(), data.stamp(), velocity);
 		if(!scanDeskewed.isEmpty())
@@ -837,23 +894,11 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 			}
 		}
 	}
-	// A frame that brings its own features carries no image, and a frame whose scene was
+	else
 	// 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);
 	}
 	if(data.imageRaw().empty() && data.laserScanRaw().isEmpty() && !data.imu().empty())
 	{
 		return Transform(); // Return null on IMU-only updates
 	}
 	if(info)
 	{
 		info->timeEstimation = time.ticks();
@@ -877,6 +922,12 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 		}
 	}
 	if(t.isNull())
 	{
 		// Lost: no velocity can be estimated across the reset that follows
 		imuMotionPredictor_.addPose(data.stamp(), Transform());
 	}
 	if(!t.isNull())
 	{
 		_resetCurrentCount = _resetCountdown;
@@ -1032,6 +1083,21 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 			velocityGuess_.setNull();
 		}
 		{
 			const Transform newPose = _pose * t;
 			imuMotionPredictor_.addPose(data.stamp(), newPose);
 			if(guessSmoothingDelay_ > 0.0f && !imus_.empty() &&
 			   !velocityGuess_.isNull() && _filteringStrategy != 1 && particleFilters_.empty())
 			{
 				// The translational velocity over the smoothing delay, carried to this
 				// frame with the IMU acceleration (see ImuMotionPredictor), in this frame.
 				const Eigen::Vector3d v = newPose.getQuaterniond().inverse() * imuMotionPredictor_.velocity();
 				float vx,vy,vz, vroll,vpitch,vyaw;
 				velocityGuess_.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);
 				velocityGuess_ = Transform(v.x(), v.y(), v.z(), vroll, vpitch, vyaw);
 			}
 		}
 		if(info)
 		{
 			distanceTravelled_ += t.getNorm();
@@ -34,6 +34,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/utilite/UTimer.h"
 #include <algorithm>
 namespace rtabmap {
 OdometryThread::OdometryThread(Odometry * odometry, unsigned int dataBufferMaxSize) :
@@ -236,13 +238,24 @@ bool OdometryThread::getData(SensorEvent & event)
 	{
 		if(!_dataBuffer.empty())
 		{
-			// Send IMU up to stamp greater than image (OpenVINS needs this).
+			// Send IMU up to stamp greater than image (OpenVINS needs this). For a lidar
 			// scan with a time channel, up to the end of its sweep: deskewing predicts the
 			// pose of every point with the IMU (approaches processing the IMU themselves
 			// get it as before).
 			double imuUntil = _dataBuffer.front().data().stamp();
 			const LaserScan & scan = _dataBuffer.front().data().laserScanRaw();
 			if(!_odometry->canProcessAsyncIMU() && !scan.isEmpty() && scan.hasTime())
 			{
 				imuUntil += std::max(0.0f, std::max(
 						scan.data().ptr<float>(0, 0)[scan.getTimeOffset()],
 						scan.data().ptr<float>(0, scan.size()-1)[scan.getTimeOffset()]));
 			}
 			while(!_imuBuffer.empty())
 			{
 				_odometry->process(_imuBuffer.front());
 				double stamp =_imuBuffer.front().stamp();
 				_imuBuffer.pop_front();
-				if(stamp > _dataBuffer.front().data().stamp()) {
+				if(stamp > imuUntil) {
 					break;
 				}
 			}
@@ -313,6 +313,24 @@ SensorData::~SensorData()
 {
 }
 bool SensorData::keepCameraModel(
 		const CameraModel & model,
 		const cv::Mat & rgb,
 		const cv::Mat & depth,
 		bool clearPreviousData) const
 {
 	// An invalid model without any image is only a placeholder (e.g., scan-only data
 	// created with CameraModel()): it is not kept, so that cameraModels() is empty when
 	// there is no camera. An invalid model with an image is kept: images can be used
 	// without calibration, and they are split per camera model.
 	return model.isValidForProjection() ||
 			!rgb.empty() ||
 			!depth.empty() ||
 			(!clearPreviousData && (
 					!_imageRaw.empty() || !_imageCompressed.empty() ||
 					!_depthOrRightRaw.empty() || !_depthOrRightCompressed.empty()));
 }
 void SensorData::setRGBDImage(
 		const cv::Mat & rgb,
 		const cv::Mat & depth,
@@ -320,7 +338,10 @@ void SensorData::setRGBDImage(
 		bool clearPreviousData)
 {
 	std::vector<CameraModel> models;
 	if(keepCameraModel(model, rgb, depth, clearPreviousData))
 	{
 		models.push_back(model);
 	}
 	setRGBDImage(rgb, depth, models, clearPreviousData);
 }
 void SensorData::setRGBDImage(
@@ -331,7 +352,10 @@ void SensorData::setRGBDImage(
 	bool clearPreviousData)
 {
 	std::vector<CameraModel> models;
 	if(keepCameraModel(model, rgb, depth, clearPreviousData))
 	{
 		models.push_back(model);
 	}
 	setRGBDImage(rgb, depth, depthConfidence, models, clearPreviousData);
 }
 void SensorData::setRGBDImage(
@@ -73,7 +73,7 @@ unsigned long VisualWord::getMemoryUsed() const
 {
 	unsigned long memoryUsage = sizeof(VisualWord);
 	memoryUsage += _references.size() * (sizeof(int)*2+sizeof(std::map<int ,int>::iterator)) + sizeof(std::map<int ,int>);
-	memoryUsage += _descriptor.total() * _descriptor.elemSize();
+	memoryUsage += _descriptor.empty()?0:_descriptor.total() * _descriptor.elemSize();
 	return memoryUsage;
 }
@@ -3822,17 +3822,12 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr loadCloud(
 	return util3d::transformPointCloud(cloud, transform);
 }
-LaserScan deskew(
+static LaserScan deskewImpl(
 		const LaserScan & input,
 		double inputStamp,
-		const rtabmap::Transform & velocity)
+		const std::function<rtabmap::Transform(double)> & motion,
 		bool slerp)
 {
 	if(velocity.isNull())
 	{
 		UERROR("velocity should be valid!");
 		return LaserScan();
 	}
 	if(!input.hasTime())
 	{
 		UERROR("input scan doesn't have a \"time\" channel! Supported formats: \"%s\", \"%s\".",
@@ -3861,20 +3856,14 @@ LaserScan deskew(
 		return LaserScan();
 	}
 	// With slerp, the poses of the base frame at the first and last stamps (relative to
 	// the base frame at inputStamp), interpolated in between
 	rtabmap::Transform firstPose;
 	rtabmap::Transform lastPose;
-
+	if(slerp)
-	float vx,vy,vz, vroll,vpitch,vyaw;
+	{
-	velocity.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);
+		firstPose = motion(firstStamp);
-
+		lastPose = motion(lastStamp);
 	//  1- The pose of base frame in odom frame at first stamp
 	//  2- The pose of base frame in odom frame at last stamp
 	double dt1 = firstStamp - inputStamp;
 	double dt2 = lastStamp - inputStamp;
 	firstPose = rtabmap::Transform(vx*dt1, vy*dt1, vz*dt1, vroll*dt1, vpitch*dt1, vyaw*dt1);
 	lastPose = rtabmap::Transform(vx*dt2, vy*dt2, vz*dt2, vroll*dt2, vpitch*dt2, vyaw*dt2);
 		if(firstPose.isNull())
 		{
 			UERROR("Could not get transform between stamps %f and %f!",
@@ -3889,6 +3878,7 @@ LaserScan deskew(
 					inputStamp);
 			return LaserScan();
 		}
 	}
 	double stamp;
 	UTimer processingTime;
@@ -3918,7 +3908,12 @@ LaserScan deskew(
 		{
 			const float * inputPtr = input.data().ptr<float>(0, u);
 			stamp = inputStamp + inputPtr[offsetTime];
-			rtabmap::Transform transform = firstPose.interpolate((stamp-firstStamp) / scanTime, lastPose);
+			rtabmap::Transform transform = slerp?firstPose.interpolate((stamp-firstStamp) / scanTime, lastPose):motion(stamp);
 			if(transform.isNull())
 			{
 				UERROR("Could not get transform between stamps %f and %f!", stamp, inputStamp);
 				return LaserScan();
 			}
 			for(int v=0; v<input.data().rows; ++v)
 			{
@@ -3960,7 +3955,12 @@ LaserScan deskew(
 		{
 			const float * inputPtr = input.data().ptr<float>(v, 0);
 			stamp = inputStamp + inputPtr[offsetTime];
-			rtabmap::Transform transform = firstPose.interpolate((stamp-firstStamp) / scanTime, lastPose);
+			rtabmap::Transform transform = slerp?firstPose.interpolate((stamp-firstStamp) / scanTime, lastPose):motion(stamp);
 			if(transform.isNull())
 			{
 				UERROR("Could not get transform between stamps %f and %f!", stamp, inputStamp);
 				return LaserScan();
 			}
 			for(int u=0; u<input.data().cols; ++u)
 			{
@@ -3996,6 +3996,42 @@ LaserScan deskew(
 	return LaserScan(output, input.maxPoints(), input.rangeMax(), outputFormat, input.localTransform());
 }
 LaserScan deskew(
 		const LaserScan & input,
 		double inputStamp,
 		const rtabmap::Transform & velocity)
 {
 	if(velocity.isNull())
 	{
 		UERROR("velocity should be valid!");
 		return LaserScan();
 	}
 	float vx,vy,vz, vroll,vpitch,vyaw;
 	velocity.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);
 	// The pose of the base frame at a stamp relative to the one at inputStamp, with a
 	// constant velocity: computed at the first and last stamps, interpolated in between
 	auto motion = [&](double stamp)
 	{
 		const double dt = stamp - inputStamp;
 		return rtabmap::Transform(vx*dt, vy*dt, vz*dt, vroll*dt, vpitch*dt, vyaw*dt);
 	};
 	return deskewImpl(input, inputStamp, motion, true);
 }
 LaserScan deskew(
 		const LaserScan & input,
 		double inputStamp,
 		const std::function<rtabmap::Transform(double stamp)> & motion,
 		bool slerp)
 {
 	if(!motion)
 	{
 		UERROR("motion should be set!");
 		return LaserScan();
 	}
 	return deskewImpl(input, inputStamp, motion, slerp);
 }
 }
@@ -42,8 +42,10 @@ set(corelib_test_sources
 	test_link.cpp                       #Link.h
 	test_optimizer.cpp                  #Optimizer.h
 	test_gps.cpp                        #GPS.h
 	test_parameters.cpp                 #Parameters.h
 	test_imu.cpp                        #IMU.h
 	test_imufilter.cpp                  #IMUFilter.h
 	test_imumotionpredictor.cpp         #ImuMotionPredictor.h
 	test_imuthread.cpp                  #IMUThread.h
 	test_landmark.cpp                   #Landmark.h
 	test_localgrid.cpp                  #LocalGrid.h
@@ -162,6 +164,20 @@ IF(BUILD_PERF_TESTS)
 	set_tests_properties(test_graph_perf PROPERTIES
 			TIMEOUT ${_perf_timeout}
 			LABELS  "performance")
 	# Comparison of the depth image compression approaches (sizes, times, errors) for
 	# 16UC1 and 32FC1 depth images: PNG, RVL, zlib, the legacy 4-channel PNG of 32FC1
 	# images, their conversion to 16UC1 millimeters, and their quantization as 16 bits
 	# inverse depth (Mem/DepthCompressionFormat=".png:max:q" or ".rvl:max:q"):
 	#   bin/test_compression_perf
 	#   bin/test_compression_perf --gtest_filter=*Synthetic*
 	add_executable(test_compression_perf perf_compression.cpp)
 	target_link_libraries(test_compression_perf gtest_main rtabmap_core)
 	add_test(NAME test_compression_perf COMMAND test_compression_perf)
 	set_tests_properties(test_compression_perf PROPERTIES
 			TIMEOUT ${_perf_timeout}
 			LABELS  "performance")
 ENDIF(BUILD_PERF_TESTS)
 # Rtabmap end-to-end replay of sample DBs (test data fetched by
@@ -0,0 +1,342 @@
 // Comparison of the depth image compression approaches of Compression.h, for each
 // depth type rtabmap receives:
 //
 //   16UC1 (millimeters):
 //     - ".png"                  lossless, 16 bits grayscale PNG
 //     - ".rvl"                  lossless, RVL (Mem/DepthCompressionFormat default)
 //     - zlib                    lossless, compressData2(), as a reference
 //   32FC1 (meters):
 //     - ".png"                  lossless, float bytes as a 4-channel 8 bits PNG (legacy)
 //     - zlib                    lossless, compressData2(), as a reference
 //     - 16UC1 mm + ".png/.rvl"  lossy, util2d::cvtDepthFromFloat() then 16 bits codec,
 //                               what Mem/SaveDepth16Format=true does
 //     - ".png:max:q/.rvl:max:q" lossy, 16 bits quantized inverse depth (same
 //                               quantization than ROS's compressed_depth_image_transport)
 //
 // over the depth images of data/rgbd/depth (a structured light camera, millimeters),
 // the same images converted to meters in 32FC1 (as many drivers publish them), and a
 // synthetic 32FC1 image with continuous values, like stereo or lidar projected depth.
 //
 // Its own executable, run by ctest under the "performance" label, so that its seconds
 // of benchmarking stay out of the unit test shards:
 //   ctest -L performance    to run them
 //   ctest -LE performance   to skip them
 //   bin/test_compression_perf --gtest_filter=*Synthetic*
 //
 // The times are reported rather than asserted on, as they depend on the machine. What
 // is asserted is that the lossless approaches give back the same image, and that the
 // lossy ones stay within their error bounds for the depth range they keep.
 #include <gtest/gtest.h>
 #include <rtabmap/core/Compression.h>
 #include <rtabmap/core/util2d.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/utilite/UTimer.h>
 #include <opencv2/imgcodecs.hpp>
 #include <algorithm>
 #include <cmath>
 #include <cstdio>
 #include <functional>
 #include <string>
 #include <vector>
 using namespace rtabmap;
 namespace {
 static const int ITERATIONS = 15;
 struct Approach
 {
 	std::string name;
 	std::function<std::vector<unsigned char>(const cv::Mat &)> encode;
 	std::function<cv::Mat(const std::vector<unsigned char> &)> decode;
 	bool lossless;
 	float maxDepth;     // meters, lossy approaches only: depth kept under it
 	float minDepth;     // meters, lossy approaches only: depth kept over it
 	std::function<float(float)> tolerance; // meters, lossy approaches only, for a depth in meters
 };
 struct Result
 {
 	size_t bytes = 0;
 	double encodeMs = 0.0;
 	double decodeMs = 0.0;
 	double maxError = 0.0;   // mm, over the depth range kept
 	double rmse = 0.0;       // mm, over the depth range kept
 	double lost = 0.0;       // % of the valid pixels set to 0
 	int outOfTolerance = 0;  // pixels with an error over the tolerance
 };
 double median(std::vector<double> v)
 {
 	std::sort(v.begin(), v.end());
 	return v[v.size()/2];
 }
 float toMeters(const cv::Mat & depth, int r, int c)
 {
 	return depth.type() == CV_16UC1 ? float(depth.at<uint16_t>(r, c)) * 0.001f : depth.at<float>(r, c);
 }
 Result run(const cv::Mat & depth, const Approach & approach)
 {
 	Result result;
 	std::vector<unsigned char> bytes;
 	cv::Mat restored;
 	std::vector<double> encodeTimes, decodeTimes;
 	for(int i=0; i<ITERATIONS; ++i)
 	{
 		UTimer timer;
 		bytes = approach.encode(depth);
 		encodeTimes.push_back(timer.restart() * 1000.0);
 		restored = approach.decode(bytes);
 		decodeTimes.push_back(timer.ticks() * 1000.0);
 	}
 	result.bytes = bytes.size();
 	result.encodeMs = median(encodeTimes);
 	result.decodeMs = median(decodeTimes);
 	EXPECT_EQ(restored.size(), depth.size());
 	EXPECT_EQ(restored.type(), approach.lossless ? depth.type() : restored.type());
 	if(restored.size() != depth.size())
 	{
 		return result;
 	}
 	if(approach.lossless)
 	{
 		EXPECT_EQ(memcmp(restored.data, depth.data, depth.total()*depth.elemSize()), 0);
 		return result;
 	}
 	int valid = 0, lost = 0, kept = 0;
 	double sumSq = 0.0;
 	for(int r=0; r<depth.rows; ++r)
 	{
 		for(int c=0; c<depth.cols; ++c)
 		{
 			const float d = toMeters(depth, r, c);
 			if(!(std::isfinite(d) && d > 0.0f))
 			{
 				continue;
 			}
 			++valid;
 			const float out = toMeters(restored, r, c);
 			if(out == 0.0f)
 			{
 				++lost;
 				// Only allowed outside the kept range
 				if(d >= approach.minDepth && d < approach.maxDepth)
 				{
 					++result.outOfTolerance;
 				}
 				continue;
 			}
 			const double err = std::fabs(out - d);
 			result.maxError = std::max(result.maxError, err*1000.0);
 			sumSq += err*err*1e6;
 			++kept;
 			if(err > approach.tolerance(d))
 			{
 				++result.outOfTolerance;
 			}
 		}
 	}
 	result.rmse = kept ? std::sqrt(sumSq / kept) : 0.0;
 	result.lost = valid ? 100.0 * lost / valid : 0.0;
 	EXPECT_EQ(result.outOfTolerance, 0) << approach.name;
 	return result;
 }
 void report(const std::string & title, const cv::Mat & depth, const std::vector<Approach> & approaches)
 {
 	const size_t raw = depth.total() * depth.elemSize();
 	std::printf("\n%s: %dx%d %s, %zu bytes raw\n", title.c_str(), depth.cols, depth.rows,
 			depth.type() == CV_16UC1 ? "16UC1" : "32FC1", raw);
 	std::printf("  %-22s %10s %7s %10s %10s %11s %10s %8s\n",
 			"approach", "bytes", "ratio", "encode ms", "decode ms", "max err mm", "rmse mm", "lost %");
 	for(const Approach & approach : approaches)
 	{
 		SCOPED_TRACE(title + " " + approach.name);
 		const Result r = run(depth, approach);
 		if(approach.lossless)
 		{
 			std::printf("  %-22s %10zu %6.1fx %10.2f %10.2f %11s %10s %8s\n",
 					approach.name.c_str(), r.bytes, double(raw)/double(r.bytes), r.encodeMs, r.decodeMs,
 					"lossless", "-", "-");
 		}
 		else
 		{
 			std::printf("  %-22s %10zu %6.1fx %10.2f %10.2f %11.3f %10.3f %8.2f\n",
 					approach.name.c_str(), r.bytes, double(raw)/double(r.bytes), r.encodeMs, r.decodeMs,
 					r.maxError, r.rmse, r.lost);
 		}
 	}
 	std::fflush(stdout);
 }
 std::vector<unsigned char> encode(const cv::Mat & depth, const std::string & format)
 {
 	return compressImage(depth, format);
 }
 cv::Mat decode(const std::vector<unsigned char> & bytes)
 {
 	return uncompressImage(bytes);
 }
 std::vector<unsigned char> encodeZlib(const cv::Mat & depth)
 {
 	return compressData(depth);
 }
 cv::Mat decodeZlib(const std::vector<unsigned char> & bytes)
 {
 	return uncompressData(bytes);
 }
 std::vector<Approach> approaches16U()
 {
 	using namespace std::placeholders;
 	return {
 		{".png", std::bind(encode, _1, ".png"), decode, true, 0, 0, nullptr},
 		{".rvl", std::bind(encode, _1, ".rvl"), decode, true, 0, 0, nullptr},
 		{"zlib", encodeZlib, decodeZlib, true, 0, 0, nullptr}};
 }
 Approach invDepth(const std::string & codec, float maxDepth, float quantization)
 {
 	using namespace std::placeholders;
 	const float A = quantization * (quantization + 1.0f);
 	const float B = 1.0f - A / maxDepth;
 	const std::string format = uFormat("%s:%g:%g", codec.c_str(), maxDepth, quantization);
 	return {format, std::bind(encode, _1, format), decode, false,
 		maxDepth,
 		A / (65535.0f - B) * 1.001f,
 		[A](float d) { return 0.51f * d * d / A + 1e-6f; }};
 }
 Approach depth16(const std::string & codec)
 {
 	return {"16UC1 mm + " + codec,
 		[codec](const cv::Mat & depth) { return compressImage(util2d::cvtDepthFromFloat(depth), codec); },
 		[](const std::vector<unsigned char> & bytes) { return util2d::cvtDepthToFloat(uncompressImage(bytes)); },
 		false,
 		65.535f,
 		0.0f,
 		[](float) { return 0.001f + 1e-6f; }}; // truncated to millimeters
 }
 std::vector<Approach> approaches32F()
 {
 	using namespace std::placeholders;
 	return {
 		{".png (legacy RGBA)", std::bind(encode, _1, ".png"), decode, true, 0, 0, nullptr},
 		{"zlib", encodeZlib, decodeZlib, true, 0, 0, nullptr},
 		depth16(".png"),
 		depth16(".rvl"),
 		invDepth(".png", 10.0f, 100.0f),
 		invDepth(".rvl", 10.0f, 100.0f),
 		invDepth(".png", 40.0f, 100.0f),
 		invDepth(".rvl", 40.0f, 100.0f),
 		invDepth(".rvl", 40.0f, 200.0f)};
 }
 std::vector<cv::Mat> loadSampleDepths()
 {
 	std::vector<cv::Mat> depths;
 	for(const std::string & name : {"17.png", "154.png"})
 	{
 		const std::string path = std::string(RTABMAP_TEST_DATA_ROOT) + "/rgbd/depth/" + name;
 		cv::Mat depth = cv::imread(path, cv::IMREAD_UNCHANGED);
 		if(depth.type() == CV_16UC1)
 		{
 			depths.push_back(depth);
 		}
 		else
 		{
 			std::printf("Cannot load 16UC1 depth image \"%s\", skipped.\n", path.c_str());
 		}
 	}
 	return depths;
 }
 // Ground plane, walls and boxes seen by a 640x480 camera, with continuous
 // values up to ~35 m, noise growing with depth (as stereo) and holes.
 cv::Mat makeSyntheticDepth(int cols = 640, int rows = 480)
 {
 	cv::RNG rng(42);
 	const float fx = 0.75f * cols, cx = cols / 2.0f, cy = rows / 2.0f;
 	const float cameraHeight = 1.0f;
 	cv::Mat depth(rows, cols, CV_32FC1);
 	for(int v=0; v<rows; ++v)
 	{
 		for(int u=0; u<cols; ++u)
 		{
 			const float x = (u - cx) / fx; // ray direction, z = 1
 			const float y = (v - cy) / fx;
 			float d = 35.0f; // far wall
 			if(y > 0.0f)
 			{
 				d = std::min(d, cameraHeight / y); // ground
 			}
 			if(x < 0.0f)
 			{
 				d = std::min(d, 3.0f / -x); // left wall, 3 m away
 			}
 			// boxes
 			if(x > 0.05f && x < 0.25f && y > -0.1f && y < cameraHeight / 2.5f)
 			{
 				d = std::min(d, 2.5f - 1.5f * x);
 			}
 			if(x > -0.35f && x < -0.15f && y > -0.2f && y < cameraHeight / 12.0f)
 			{
 				d = std::min(d, 12.0f);
 			}
 			d += (float)rng.gaussian(0.002 * d * d); // stereo-like noise
 			depth.at<float>(v, u) = d;
 		}
 	}
 	// Holes
 	for(int i=0; i<40; ++i)
 	{
 		const int u = rng.uniform(0, cols - 20), v = rng.uniform(0, rows - 20);
 		depth(cv::Rect(u, v, rng.uniform(2, 20), rng.uniform(2, 20))).setTo(0.0f);
 	}
 	return depth;
 }
 } // namespace
 TEST(CompressionPerf, SampleDepth16UC1)
 {
 	const std::vector<cv::Mat> depths = loadSampleDepths();
 	if(depths.empty())
 	{
 		GTEST_SKIP() << "No sample depth images in " << RTABMAP_TEST_DATA_ROOT << "/rgbd/depth";
 	}
 	for(size_t i=0; i<depths.size(); ++i)
 	{
 		report(uFormat("Sample depth %d", (int)i), depths[i], approaches16U());
 	}
 }
 TEST(CompressionPerf, SampleDepth32FC1)
 {
 	const std::vector<cv::Mat> depths = loadSampleDepths();
 	if(depths.empty())
 	{
 		GTEST_SKIP() << "No sample depth images in " << RTABMAP_TEST_DATA_ROOT << "/rgbd/depth";
 	}
 	for(size_t i=0; i<depths.size(); ++i)
 	{
 		report(uFormat("Sample depth %d in meters", (int)i), util2d::cvtDepthToFloat(depths[i]), approaches32F());
 	}
 }
 TEST(CompressionPerf, Synthetic32FC1)
 {
 	report("Synthetic continuous depth", makeSyntheticDepth(), approaches32F());
 	report("Synthetic continuous depth HD", makeSyntheticDepth(1280, 720), approaches32F());
 }
@@ -1,6 +1,9 @@
 #include <gtest/gtest.h>
 #include <rtabmap/core/Compression.h>
 #include <rtabmap/utilite/UException.h>
 #include <opencv2/core.hpp>
 #include <cstring>
 #include <limits>
 using namespace rtabmap;
@@ -183,3 +186,228 @@ TEST(CompressionTest, CompressionThreadDataRoundTrip)
 	expectMatEqual(uncompressThread.getUncompressedData(), data);
 }
 namespace {
 // 32FC1 depth image covering [minDepth, maxDepth[ with sub-millimeter values,
 // and the invalid values of the inverse depth format on the first row.
 cv::Mat makeFloatDepth(int rows, int cols, float minDepth, float maxDepth)
 {
 	cv::Mat depth(rows, cols, CV_32FC1);
 	for(int r = 0; r < rows; ++r)
 	{
 		for(int c = 0; c < cols; ++c)
 		{
 			depth.at<float>(r, c) = minDepth + (maxDepth - minDepth) * float(r * cols + c) / float(rows * cols);
 		}
 	}
 	return depth;
 }
 // Error bound of the inverse depth format: half a quantization step.
 float invDepthTolerance(float d, float quantization)
 {
 	// (with some margin for the float rounding of A/d + B, up to ~66000)
 	return 0.51f * d * d / (quantization * (quantization + 1.0f)) + 1e-6f;
 }
 } // namespace
 TEST(CompressionTest, ParseImageCompressionFormat)
 {
 	std::string codec;
 	float maxDepth, quantization;
 	EXPECT_TRUE(parseImageCompressionFormat("", codec, maxDepth, quantization));
 	EXPECT_TRUE(codec.empty());
 	EXPECT_EQ(maxDepth, 0.0f);
 	EXPECT_TRUE(parseImageCompressionFormat(".jpg", codec, maxDepth, quantization));
 	EXPECT_EQ(codec, ".jpg");
 	EXPECT_EQ(maxDepth, 0.0f);
 	EXPECT_EQ(quantization, 0.0f);
 	EXPECT_TRUE(parseImageCompressionFormat(".rvl", codec, maxDepth, quantization));
 	EXPECT_EQ(codec, ".rvl");
 	EXPECT_EQ(maxDepth, 0.0f);
 	EXPECT_TRUE(parseImageCompressionFormat(".png:20", codec, maxDepth, quantization));
 	EXPECT_EQ(codec, ".png");
 	EXPECT_FLOAT_EQ(maxDepth, 20.0f);
 	EXPECT_FLOAT_EQ(quantization, 100.0f);
 	EXPECT_TRUE(parseImageCompressionFormat(".rvl:10.5:50", codec, maxDepth, quantization));
 	EXPECT_EQ(codec, ".rvl");
 	EXPECT_FLOAT_EQ(maxDepth, 10.5f);
 	EXPECT_FLOAT_EQ(quantization, 50.0f);
 	EXPECT_FALSE(parseImageCompressionFormat("png", codec, maxDepth, quantization));
 	EXPECT_FALSE(parseImageCompressionFormat(".jpg:10:100", codec, maxDepth, quantization));
 	EXPECT_FALSE(parseImageCompressionFormat(".png:abc", codec, maxDepth, quantization));
 	EXPECT_FALSE(parseImageCompressionFormat(".png:0:100", codec, maxDepth, quantization));
 	EXPECT_FALSE(parseImageCompressionFormat(".png:-10:100", codec, maxDepth, quantization));
 	EXPECT_FALSE(parseImageCompressionFormat(".png:10:0", codec, maxDepth, quantization));
 	EXPECT_FALSE(parseImageCompressionFormat(".png:10:100:1", codec, maxDepth, quantization));
 }
 TEST(CompressionTest, InvalidFormatReturnsEmpty)
 {
 	const cv::Mat depth = makeFloatDepth(4, 4, 1.0f, 2.0f);
 	EXPECT_TRUE(compressImage(depth, ".jpg:10").empty());
 	EXPECT_TRUE(compressImage(depth, ".png:x").empty());
 }
 TEST(CompressionTest, InverseDepthRoundTrip)
 {
 	const float maxDepth = 10.0f;
 	const float quantization = 100.0f;
 	const float minDepth = quantization * (quantization + 1.0f) / (65535.0f + quantization * (quantization + 1.0f) / maxDepth);
 	cv::Mat depth = makeFloatDepth(48, 64, minDepth * 1.001f, maxDepth * 0.999f);
 	const float invalid[] = {
 			0.0f, -1.0f, maxDepth, maxDepth * 2.0f, minDepth * 0.9f,
 			std::numeric_limits<float>::quiet_NaN(),
 			std::numeric_limits<float>::infinity(),
 			-std::numeric_limits<float>::infinity()};
 	const int nInvalid = sizeof(invalid) / sizeof(float);
 	for(int i = 0; i < nInvalid; ++i)
 	{
 		depth.at<float>(0, i) = invalid[i];
 	}
 	for(const std::string codec : {".png", ".rvl"})
 	{
 		SCOPED_TRACE(codec);
 		const std::string format = codec + ":10:100";
 		const std::vector<unsigned char> bytes = compressImage(depth, format);
 		ASSERT_FALSE(bytes.empty());
 		EXPECT_LT(bytes.size(), depth.total() * depth.elemSize() / 2);
 		EXPECT_EQ(compressedDepthFormat(bytes), format);
 		const cv::Mat restored = uncompressImage(bytes);
 		ASSERT_EQ(restored.type(), CV_32FC1);
 		ASSERT_EQ(restored.size(), depth.size());
 		for(int r = 0; r < depth.rows; ++r)
 		{
 			for(int c = 0; c < depth.cols; ++c)
 			{
 				const float d = depth.at<float>(r, c);
 				if(r == 0 && c < nInvalid)
 				{
 					EXPECT_EQ(restored.at<float>(r, c), 0.0f) << "input=" << d;
 				}
 				else
 				{
 					ASSERT_NEAR(restored.at<float>(r, c), d, invDepthTolerance(d, quantization)) << "r=" << r << " c=" << c;
 				}
 			}
 		}
 		// Re-compressing with the detected format gives back the same bytes
 		// (e.g., DatabaseViewer saving an edited depth image).
 		EXPECT_EQ(compressImage(restored, compressedDepthFormat(bytes)), compressImage(restored, format));
 		// Same through cv::Mat and thread overloads
 		CompressionThread compressThread(depth, format);
 		compressThread.start();
 		compressThread.join();
 		const cv::Mat bytesMat = compressThread.getCompressedData();
 		ASSERT_EQ(bytesMat.total(), bytes.size());
 		EXPECT_EQ(memcmp(bytesMat.data, bytes.data(), bytes.size()), 0);
 		CompressionThread uncompressThread(bytesMat, true);
 		uncompressThread.start();
 		uncompressThread.join();
 		expectMatEqual(uncompressThread.getUncompressedData(), restored);
 	}
 }
 TEST(CompressionTest, InverseDepthQuantizationParameters)
 {
 	const cv::Mat depth = makeFloatDepth(32, 32, 1.0f, 39.0f);
 	const std::vector<unsigned char> bytes = compressImage(depth, ".png:40:50");
 	EXPECT_EQ(compressedDepthFormat(bytes), ".png:40:50");
 	const cv::Mat restored = uncompressImage(bytes);
 	ASSERT_EQ(restored.type(), CV_32FC1);
 	for(int r = 0; r < depth.rows; ++r)
 	{
 		for(int c = 0; c < depth.cols; ++c)
 		{
 			const float d = depth.at<float>(r, c);
 			ASSERT_NEAR(restored.at<float>(r, c), d, invDepthTolerance(d, 50.0f));
 		}
 	}
 }
 TEST(CompressionTest, InverseDepthNonContinuousImage)
 {
 	const cv::Mat depth = makeFloatDepth(20, 30, 1.0f, 5.0f);
 	const cv::Mat roi = depth(cv::Rect(3, 2, 10, 8));
 	ASSERT_FALSE(roi.isContinuous());
 	const cv::Mat restored = uncompressImage(compressImage(roi, ".rvl:10:100"));
 	ASSERT_EQ(restored.size(), roi.size());
 	for(int r = 0; r < roi.rows; ++r)
 	{
 		for(int c = 0; c < roi.cols; ++c)
 		{
 			const float d = roi.at<float>(r, c);
 			ASSERT_NEAR(restored.at<float>(r, c), d, invDepthTolerance(d, 100.0f));
 		}
 	}
 }
 TEST(CompressionTest, DepthParametersIgnoredFor16UC1)
 {
 	cv::Mat depth(24, 32, CV_16UC1);
 	cv::randu(depth, 0, 20000); // includes values over the max depth below
 	for(const std::string codec : {".png", ".rvl"})
 	{
 		SCOPED_TRACE(codec);
 		const std::vector<unsigned char> bytes = compressImage(depth, codec + ":10:100");
 		EXPECT_EQ(bytes, compressImage(depth, codec));
 		EXPECT_EQ(compressedDepthFormat(bytes), codec);
 		expectMatEqual(uncompressImage(bytes), depth);
 	}
 }
 TEST(CompressionTest, LegacyFloatDepthIsLossless)
 {
 	const cv::Mat depth = makeFloatDepth(16, 16, 0.01f, 100.0f);
 	for(const std::string format : {".png", ".rvl"})
 	{
 		SCOPED_TRACE(format);
 		const std::vector<unsigned char> bytes = compressImage(depth, format);
 		EXPECT_EQ(compressedDepthFormat(bytes), ".png");
 		const cv::Mat restored = uncompressImage(bytes);
 		ASSERT_EQ(restored.type(), CV_32FC1);
 		EXPECT_EQ(memcmp(restored.data, depth.data, depth.total() * depth.elemSize()), 0);
 	}
 }
 TEST(CompressionTest, MalformedDepthFormatsDecodeToEmpty)
 {
 	// Signature and header only, no payload
 	std::vector<unsigned char> invDepth = {'D', 'E', 'P', 'T', 'H', 'I', 'N', 'V'};
 	invDepth.resize(16, 0);
 	EXPECT_TRUE(uncompressImage(invDepth).empty());
 	EXPECT_EQ(compressedDepthFormat(invDepth), ".png") << "too short to be inverse depth";
 	// Inverse depth header followed by an 8 bits image instead of a 16 bits one
 	const std::vector<unsigned char> png8 = compressImage(cv::Mat(4, 4, CV_8UC1, cv::Scalar(1)), ".png");
 	invDepth.insert(invDepth.end(), png8.begin(), png8.end());
 	EXPECT_TRUE(uncompressImage(invDepth).empty());
 	// RVL signature without its size
 	const std::vector<unsigned char> rvl = {'D', 'E', 'P', 'T', 'H', 'R', 'V', 'L', 4, 0};
 	EXPECT_TRUE(uncompressImage(rvl).empty());
 	EXPECT_EQ(compressedDepthFormat(rvl), ".rvl");
 	EXPECT_TRUE(uncompressImage(nullptr, 0).empty());
 }
 TEST(CompressionTest, CompressionThreadRejectsInvalidFormat)
 {
 	// std::string: a string literal would select the (bytes, isImage) constructor
 	const cv::Mat depth(4, 4, CV_32FC1, cv::Scalar(1.0f));
 	EXPECT_THROW(CompressionThread(depth, std::string(".jpg:10")), UException);
 	EXPECT_THROW(CompressionThread(depth, std::string(".bmp")), UException);
 	EXPECT_NO_THROW(CompressionThread(depth, std::string(".rvl:10:100")));
 }
@@ -0,0 +1,460 @@
 // M_PI on MSVC (must come before any header including <cmath>)
 #ifndef _USE_MATH_DEFINES
 #define _USE_MATH_DEFINES
 #endif
 #include <gtest/gtest.h>
 #include <cmath>
 #include <algorithm>
 #include <functional>
 #include <rtabmap/core/ImuMotionPredictor.h>
 #include <rtabmap/core/IMU.h>
 using rtabmap::ImuMotionPredictor;
 namespace {
 Eigen::Quaterniond yaw(double angle)
 {
 	return Eigen::Quaterniond(Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitZ()));
 }
 rtabmap::Transform pose(const Eigen::Vector3d & position, double angle)
 {
 	return rtabmap::Transform(position.x(), position.y(), position.z(), 0, 0, angle);
 }
 /// An IMU at rest or accelerating, as measured: orientation of the IMU in its world
 /// frame, and the specific force (acceleration minus gravity) in the IMU frame.
 rtabmap::IMU measuredImu(const Eigen::Quaterniond & worldToImu,
 		const Eigen::Vector3d & accelerationInWorld, double gravity,
 		const rtabmap::Transform & baseToImu)
 {
 	const Eigen::Vector3d f = worldToImu.inverse() * (accelerationInWorld + Eigen::Vector3d(0, 0, gravity));
 	const Eigen::Quaterniond q = worldToImu.normalized();
 	return rtabmap::IMU(cv::Vec4d(q.x(), q.y(), q.z(), q.w()), cv::Mat::eye(3,3,CV_64FC1),
 			cv::Vec3d(0,0,0), cv::Mat::eye(3,3,CV_64FC1),
 			cv::Vec3d(f.x(), f.y(), f.z()), cv::Mat::eye(3,3,CV_64FC1),
 			baseToImu);
 }
 /// IMU measurements at 200 Hz over [from, to] of an IMU at the base origin, oriented and
 /// accelerating (in its world frame) as given. Without accelerometer, it measures no
 /// linear acceleration at all. The stamps are on the 200 Hz grid (from must be too).
 void addSamples(ImuMotionPredictor & predictor, double from, double to,
 		const std::function<Eigen::Quaterniond(double)> & orientation,
 		const std::function<Eigen::Vector3d(double)> & acceleration,
 		bool withAccelerometer = true)
 {
 	// An integer over 200 rather than from + i*0.005: with FMA (e.g., -march=x86-64-v3),
 	// -0.05 + 10*0.005 gives -1.7e-18, not 0, putting that sample on the wrong side of
 	// a step at t=0.
 	const double first = std::round(from * 200.0);
 	for(int i=0; (first + i) / 200.0 <= to + 1e-9; ++i)
 	{
 		const double t = (first + i) / 200.0;
 		rtabmap::IMU imu = measuredImu(orientation(t), acceleration(t), predictor.gravity(), rtabmap::Transform::getIdentity());
 		if(!withAccelerometer)
 		{
 			imu = rtabmap::IMU(imu.orientation(), imu.orientationCovariance(),
 					imu.angularVelocity(), imu.angularVelocityCovariance(),
 					cv::Vec3d(0,0,0), imu.linearAccelerationCovariance(),
 					imu.localTransform());
 		}
 		predictor.addImu(t, imu);
 	}
 }
 }  // namespace
 TEST(ImuMotionPredictor, predicts_only_the_orientation_without_a_pose)
 {
 	ImuMotionPredictor predictor;
 	EXPECT_TRUE(predictor.predict(1.0).isNull());
 	predictor.addImu(1.0, measuredImu(yaw(0.3), Eigen::Vector3d(1, 0, 0), 9.80665, rtabmap::Transform::getIdentity()));
 	predictor.addImu(1.1, measuredImu(yaw(0.5), Eigen::Vector3d(1, 0, 0), 9.80665, rtabmap::Transform::getIdentity()));
 	const rtabmap::Transform predicted = predictor.predict(1.05);
 	ASSERT_FALSE(predicted.isNull()) << "the orientation is known without a pose";
 	EXPECT_NEAR(predicted.theta(), 0.4, 1e-5);
 	EXPECT_NEAR(predicted.x(), 0.0, 1e-9) << "no position without a pose";
 	ImuMotionPredictor withoutImu;
 	withoutImu.addPose(1.0, rtabmap::Transform::getIdentity());
 	EXPECT_TRUE(withoutImu.predict(1.0).isNull()) << "no imu yet";
 }
 TEST(ImuMotionPredictor, follows_a_constant_velocity)
 {
 	ImuMotionPredictor predictor;
 	const Eigen::Vector3d velocity(1.0, -0.5, 0.2);
 	addSamples(predictor, 0.0, 0.3,
 			[](double) { return Eigen::Quaterniond::Identity(); },
 			[](double) { return Eigen::Vector3d::Zero(); });
 	predictor.addPose(0.0, pose(Eigen::Vector3d::Zero(), 0));
 	EXPECT_TRUE(predictor.velocity().isZero()) << "a single pose has no velocity";
 	predictor.addPose(0.1, pose(velocity * 0.1, 0));
 	EXPECT_TRUE(predictor.velocity().isApprox(velocity, 1e-6));
 	const rtabmap::Transform predicted = predictor.predict(0.25);
 	ASSERT_FALSE(predicted.isNull());
 	EXPECT_NEAR(predicted.x(), velocity.x() * 0.25, 1e-6);
 	EXPECT_NEAR(predicted.y(), velocity.y() * 0.25, 1e-6);
 	EXPECT_NEAR(predicted.z(), velocity.z() * 0.25, 1e-6);
 }
 TEST(ImuMotionPredictor, integrates_the_acceleration)
 {
 	// From rest at t=0 with a constant 2 m/s^2: p = t^2, v = 2t. The velocity at the
 	// second pose is the instantaneous one, not the average over the interval, and the
 	// prediction keeps accelerating. An IMU without accelerometer gives a constant
 	// velocity instead.
 	const double a = 2.0;
 	for(bool withAccelerometer : {true, false})
 	{
 		ImuMotionPredictor predictor;
 		addSamples(predictor, -0.05, 0.3,
 				[](double) { return Eigen::Quaterniond::Identity(); },
 				[&](double t) { return Eigen::Vector3d(t < 0.0 ? 0.0 : a, 0, 0); },
 				withAccelerometer);
 		predictor.addPose(0.0, pose(Eigen::Vector3d::Zero(), 0));
 		predictor.addPose(0.1, pose(Eigen::Vector3d(0.5*a*0.01, 0, 0), 0));
 		const double t = 0.2;
 		const rtabmap::Transform predicted = predictor.predict(t);
 		ASSERT_FALSE(predicted.isNull());
 		if(withAccelerometer)
 		{
 			EXPECT_NEAR(predictor.velocity().x(), a*0.1, 1e-6);
 			EXPECT_NEAR(predicted.x(), 0.5*a*t*t, 1e-6);
 		}
 		else
 		{
 			// Constant velocity model: the average velocity over the last interval.
 			EXPECT_NEAR(predictor.velocity().x(), 0.5*a*0.1, 1e-6);
 			EXPECT_NEAR(predicted.x(), 0.5*a*0.01 + 0.5*a*0.1*(t-0.1), 1e-6);
 		}
 	}
 }
 TEST(ImuMotionPredictor, expresses_the_imu_in_the_odometry_frame)
 {
 	// The IMU's world frame and the odometry frame differ by 90 degrees of yaw. The base
 	// turns at 1 rad/s and accelerates along the IMU world's x, which is the odometry's y.
 	const double rate = 1.0;
 	const double offset = M_PI/2.0;
 	const double a = 3.0;
 	ImuMotionPredictor predictor;
 	addSamples(predictor, 0.0, 0.3,
 			[&](double t) { return yaw(rate*t); },
 			[&](double) { return Eigen::Vector3d(a, 0, 0); });
 	predictor.addPose(0.0, pose(Eigen::Vector3d::Zero(), offset));
 	predictor.addPose(0.1, pose(Eigen::Vector3d(0, 0.5*a*0.01, 0), offset + rate*0.1));
 	const double t = 0.25;
 	const rtabmap::Transform predicted = predictor.predict(t);
 	ASSERT_FALSE(predicted.isNull());
 	EXPECT_NEAR(predicted.x(), 0.0, 1e-6);
 	EXPECT_NEAR(predicted.y(), 0.5*a*t*t, 1e-6);
 	EXPECT_NEAR(predicted.theta(), offset + rate*t, 1e-5);
 }
 TEST(ImuMotionPredictor, a_lost_pose_resets_the_prediction)
 {
 	ImuMotionPredictor predictor;
 	addSamples(predictor, 0.0, 0.5,
 			[](double) { return Eigen::Quaterniond::Identity(); },
 			[](double) { return Eigen::Vector3d::Zero(); });
 	predictor.addPose(0.0, pose(Eigen::Vector3d::Zero(), 0));
 	predictor.addPose(0.1, pose(Eigen::Vector3d(0.1, 0, 0), 0));
 	ASSERT_FALSE(predictor.velocity().isZero());
 	predictor.addPose(0.2, rtabmap::Transform());
 	EXPECT_TRUE(predictor.predict(0.25).isIdentity()) << "orientation only, which is constant here";
 	// After a reset of the odometry, the pose jumps: no velocity across it.
 	predictor.addPose(0.3, pose(Eigen::Vector3d(10, 0, 0), 0));
 	EXPECT_TRUE(predictor.velocity().isZero());
 	EXPECT_NEAR(predictor.predict(0.4).x(), 10.0, 1e-6);
 }
 TEST(ImuMotionPredictor, poses_too_far_apart_give_no_velocity)
 {
 	ImuMotionPredictor predictor(0.5);
 	addSamples(predictor, 0.0, 1.5,
 			[](double) { return Eigen::Quaterniond::Identity(); },
 			[](double) { return Eigen::Vector3d::Zero(); });
 	predictor.addPose(0.0, pose(Eigen::Vector3d::Zero(), 0));
 	predictor.addPose(1.0, pose(Eigen::Vector3d(1, 0, 0), 0));
 	EXPECT_TRUE(predictor.velocity().isZero());
 }
 TEST(ImuMotionPredictor, a_longer_window_averages_out_the_pose_noise)
 {
 	// 1 m/s along x, with odometry poses alternating 1 cm on each side of the truth: the
 	// worst case for a velocity differenced over one frame, which sees 0.2 m/s of noise.
 	for(double window : {0.0, 0.5})
 	{
 		ImuMotionPredictor predictor(1.0, window);
 		addSamples(predictor, 0.0, 2.0,
 				[](double) { return Eigen::Quaterniond::Identity(); },
 				[](double) { return Eigen::Vector3d::Zero(); });
 		double maxError = 0.0;
 		for(int i=0; i<=15; ++i)
 		{
 			const double t = i*0.1;
 			predictor.addPose(t, pose(Eigen::Vector3d(t + (i%2?0.01:-0.01), 0, 0), 0));
 			if(i >= 10)
 			{
 				maxError = std::max(maxError, std::fabs(predictor.velocity().x() - 1.0));
 			}
 		}
 		if(window == 0.0)
 		{
 			EXPECT_NEAR(maxError, 0.2, 1e-6) << "differenced over one frame";
 		}
 		else
 		{
 			EXPECT_LT(maxError, 0.05) << "differenced over half a second";
 		}
 	}
 }
 TEST(ImuMotionPredictor, keeps_only_the_samples_since_the_last_pose)
 {
 	ImuMotionPredictor predictor;
 	addSamples(predictor, 0.0, 0.2,
 			[](double) { return Eigen::Quaterniond::Identity(); },
 			[](double) { return Eigen::Vector3d::Zero(); });
 	ASSERT_EQ(predictor.samples(), 41u);
 	predictor.addPose(0.1025, pose(Eigen::Vector3d::Zero(), 0));
 	// 0.100 (the last one before the pose, to interpolate at its stamp) .. 0.200
 	EXPECT_EQ(predictor.samples(), 21u);
 	predictor.reset();
 	EXPECT_EQ(predictor.samples(), 0u);
 	EXPECT_TRUE(predictor.predict(0.2).isNull());
 }
 TEST(ImuMotionPredictor, removes_gravity_from_what_the_imu_measures)
 {
 	// The IMU is mounted rolled by 90 degrees on a level base at rest: it measures gravity
 	// along its own y. Once removed, nothing moves, and the base stays level.
 	const rtabmap::Transform baseToImu(0, 0, 0, M_PI/2.0, 0, 0);
 	const Eigen::Quaterniond worldToImu = baseToImu.getQuaterniond();
 	ImuMotionPredictor predictor;
 	for(int i=0; i<=60; ++i)
 	{
 		predictor.addImu(i*0.005, measuredImu(worldToImu, Eigen::Vector3d::Zero(), 9.80665, baseToImu));
 	}
 	predictor.addPose(0.0, rtabmap::Transform::getIdentity());
 	const rtabmap::Transform predicted = predictor.predict(0.3);
 	ASSERT_FALSE(predicted.isNull());
 	EXPECT_NEAR(predicted.getNorm(), 0.0, 1e-6) << "gravity was not removed";
 	EXPECT_TRUE(predicted.getQuaterniond().isApprox(Eigen::Quaterniond::Identity(), 1e-6)) << "the base orientation is the imu's, unmounted";
 }
 TEST(ImuMotionPredictor, removes_the_gravity_it_is_given)
 {
 	// On the Moon, at rest: the IMU measures 1.62 m/s^2 up.
 	ImuMotionPredictor predictor(1.0, 0.5, 1.62);
 	for(int i=0; i<=60; ++i)
 	{
 		predictor.addImu(i*0.005, measuredImu(Eigen::Quaterniond::Identity(), Eigen::Vector3d::Zero(), 1.62,
 				rtabmap::Transform::getIdentity()));
 	}
 	predictor.addPose(0.0, rtabmap::Transform::getIdentity());
 	EXPECT_NEAR(predictor.predict(0.3).z(), 0.0, 1e-6);
 	// Earth's gravity removed from the same measurement: it looks like falling.
 	ImuMotionPredictor earth;
 	for(int i=0; i<=60; ++i)
 	{
 		earth.addImu(i*0.005, measuredImu(Eigen::Quaterniond::Identity(), Eigen::Vector3d::Zero(), 1.62,
 				rtabmap::Transform::getIdentity()));
 	}
 	earth.addPose(0.0, rtabmap::Transform::getIdentity());
 	EXPECT_NEAR(earth.predict(0.2).z(), 0.5*(1.62-9.80665)*0.04, 1e-6);
 }
 TEST(ImuMotionPredictor, an_imu_without_acceleration_is_not_a_free_fall)
 {
 	ImuMotionPredictor predictor;
 	const Eigen::Quaterniond q(Eigen::AngleAxisd(0.3, Eigen::Vector3d::UnitZ()));
 	for(int i=0; i<=60; ++i)
 	{
 		predictor.addImu(i*0.005, rtabmap::IMU(cv::Vec4d(q.x(), q.y(), q.z(), q.w()), cv::Mat::eye(3,3,CV_64FC1),
 				cv::Vec3d(0,0,0), cv::Mat::eye(3,3,CV_64FC1),
 				cv::Vec3d(0,0,0), cv::Mat::eye(3,3,CV_64FC1),
 				rtabmap::Transform::getIdentity()));
 	}
 	predictor.addPose(0.0, rtabmap::Transform::getIdentity());
 	EXPECT_NEAR(predictor.predict(0.3).getNorm(), 0.0, 1e-6);
 	// And one without orientation is ignored altogether.
 	ImuMotionPredictor noOrientation;
 	noOrientation.addImu(0.0, rtabmap::IMU(cv::Vec4d(0,0,0,0), cv::Mat(),
 			cv::Vec3d(0,0,0), cv::Mat(), cv::Vec3d(0,0,9.8), cv::Mat(), rtabmap::Transform::getIdentity()));
 	EXPECT_EQ(noOrientation.samples(), 0u);
 }
 TEST(ImuMotionPredictor, predicts_the_same_whatever_the_order_samples_and_predictions_come_in)
 {
 	// The integration since the last pose is kept between predictions: it must not go
 	// stale when samples arrive after a prediction, or out of order. Compared with a
 	// predictor given every sample before predicting anything.
 	auto acceleration = [](double t) { return Eigen::Vector3d(std::sin(20*t), std::cos(15*t), 0.3*t); };
 	auto orientation = [](double t) { return yaw(0.5*t); };
 	auto imu = [&](double t) { return measuredImu(orientation(t), acceleration(t), 9.80665, rtabmap::Transform::getIdentity()); };
 	ImuMotionPredictor reference;
 	for(int i=0; i<=80; ++i) reference.addImu(i*0.005, imu(i*0.005));
 	reference.addPose(0.0, rtabmap::Transform::getIdentity());
 	reference.addPose(0.1, pose(Eigen::Vector3d(0.05, 0, 0), 0.05));
 	ImuMotionPredictor incremental;
 	for(int i=0; i<=40; ++i) if(i != 30) incremental.addImu(i*0.005, imu(i*0.005));
 	incremental.addPose(0.0, rtabmap::Transform::getIdentity());
 	incremental.addPose(0.1, pose(Eigen::Vector3d(0.05, 0, 0), 0.05)); // velocity needs up to 0.1: covered
 	incremental.predict(0.12);   // integrates without the sample at 0.15
 	incremental.predict(0.3);    // beyond the newest sample (0.2)
 	incremental.addImu(0.15, imu(0.15));   // out of order
 	for(int i=41; i<=80; ++i)
 	{
 		incremental.addImu(i*0.005, imu(i*0.005));
 		if(i % 7 == 0) incremental.predict(i*0.005 - 0.0012);
 	}
 	for(double t : {0.1, 0.1013, 0.15, 0.2337, 0.4, 0.45})
 	{
 		const rtabmap::Transform a = reference.predict(t);
 		const rtabmap::Transform b = incremental.predict(t);
 		ASSERT_FALSE(a.isNull());
 		ASSERT_FALSE(b.isNull());
 		EXPECT_NEAR(a.x(), b.x(), 1e-6) << "t=" << t;
 		EXPECT_NEAR(a.y(), b.y(), 1e-6) << "t=" << t;
 		EXPECT_NEAR(a.z(), b.z(), 1e-6) << "t=" << t;
 	}
 }
 TEST(ImuMotionPredictor, holds_the_acceleration_at_the_pose_until_a_newer_sample)
 {
 	// The pose comes after the newest sample: the acceleration there is held from that
 	// sample, until a newer one says otherwise.
 	ImuMotionPredictor reference;
 	ImuMotionPredictor incremental;
 	auto imu = [](double a) { return measuredImu(Eigen::Quaterniond::Identity(), Eigen::Vector3d(a, 0, 0), 9.80665, rtabmap::Transform::getIdentity()); };
 	for(ImuMotionPredictor * p : {&reference, &incremental})
 	{
 		p->addImu(0.0, imu(0.0));
 		p->addImu(0.1, imu(0.0));
 		p->addPose(0.15, rtabmap::Transform::getIdentity());
 	}
 	incremental.predict(0.2);
 	for(ImuMotionPredictor * p : {&reference, &incremental})
 	{
 		p->addImu(0.2, imu(4.0));
 		p->addImu(0.3, imu(4.0));
 	}
 	EXPECT_NEAR(reference.predict(0.3).x(), incremental.predict(0.3).x(), 1e-9);
 }
 TEST(ImuMotionPredictor, ignores_the_acceleration_without_a_velocity_window)
 {
 	// From rest with a constant 2 m/s^2: with a window of 0, the velocity is the one of the
 	// last interval and is kept constant, as without IMU.
 	const double a = 2.0;
 	ImuMotionPredictor predictor(1.0, 0.0);
 	addSamples(predictor, -0.05, 0.3,
 			[](double) { return Eigen::Quaterniond::Identity(); },
 			[&](double t) { return Eigen::Vector3d(t < 0.0 ? 0.0 : a, 0, 0); });
 	predictor.addPose(0.0, pose(Eigen::Vector3d::Zero(), 0));
 	predictor.addPose(0.1, pose(Eigen::Vector3d(0.5*a*0.01, 0, 0), 0));
 	EXPECT_NEAR(predictor.velocity().x(), 0.5*a*0.1, 1e-6);
 	EXPECT_NEAR(predictor.predict(0.2).x(), 0.5*a*0.01 + 0.5*a*0.1*0.1, 1e-6);
 }
 namespace {
 /// An IMU tilted by 0.1 rad in pitch whose accelerometer reads `measuredGravity` at rest,
 /// with a vibration of the given amplitude (m/s^2) on every axis and the given angular
 /// velocity (rad/s, about z).
 rtabmap::IMU tiltedImu(double t, double measuredGravity, double vibration = 0.05, double angularVelocity = 0.0)
 {
 	const Eigen::Quaterniond worldToImu(Eigen::AngleAxisd(0.1, Eigen::Vector3d::UnitY()));
 	const Eigen::Vector3d f = worldToImu.inverse() * Eigen::Vector3d(0, 0, measuredGravity) +
 			vibration * Eigen::Vector3d(std::sin(t*331.0), std::sin(t*457.0), std::sin(t*563.0));
 	return rtabmap::IMU(cv::Vec4d(worldToImu.x(), worldToImu.y(), worldToImu.z(), worldToImu.w()), cv::Mat::eye(3,3,CV_64FC1),
 			cv::Vec3d(0, 0, angularVelocity), cv::Mat::eye(3,3,CV_64FC1),
 			cv::Vec3d(f.x(), f.y(), f.z()), cv::Mat::eye(3,3,CV_64FC1),
 			rtabmap::Transform::getIdentity());
 }
 }  // namespace
 TEST(ImuMotionPredictor, estimates_the_gravity_while_the_imu_is_still)
 {
 	// An accelerometer reading 9.55 m/s^2 at rest, as the netherdrone's does.
 	ImuMotionPredictor predictor(1.0, 0.5, 0.0);
 	EXPECT_TRUE(predictor.isGravityEstimated());
 	EXPECT_DOUBLE_EQ(predictor.gravity(), 9.80665) << "standard gravity until estimated";
 	for(int i=0; i<100; ++i) // 0.5 s at 200 Hz: the first window is not complete yet
 	{
 		predictor.addImu(i*0.005, tiltedImu(i*0.005, 9.55));
 	}
 	EXPECT_EQ(predictor.gravityWindows(), 0u);
 	EXPECT_DOUBLE_EQ(predictor.gravity(), 9.80665);
 	for(int i=100; i<=400; ++i)
 	{
 		predictor.addImu(i*0.005, tiltedImu(i*0.005, 9.55));
 	}
 	EXPECT_GE(predictor.gravityWindows(), 3u);
 	EXPECT_NEAR(predictor.gravity(), 9.55, 0.005) << "whatever the attitude";
 	// At rest, it doesn't look like falling anymore.
 	predictor.addPose(1.5, rtabmap::Transform::getIdentity());
 	predictor.addPose(2.0, rtabmap::Transform::getIdentity());
 	EXPECT_NEAR(predictor.predict(2.0).z(), 0.0, 1e-3);
 	for(int i=401; i<=460; ++i)
 	{
 		predictor.addImu(i*0.005, tiltedImu(i*0.005, 9.55));
 	}
 	EXPECT_NEAR(predictor.predict(2.3).z(), 0.0, 2e-3);
 	// The estimate is the sensor's: a reset keeps it.
 	predictor.reset();
 	EXPECT_NEAR(predictor.gravity(), 9.55, 0.005);
 }
 TEST(ImuMotionPredictor, does_not_take_a_moving_imu_for_a_still_one)
 {
 	// Turning (centripetal acceleration, gravity moving between the axes) or vibrating
 	// as in flight: not still, standard gravity is kept.
 	ImuMotionPredictor turning(1.0, 0.5, 0.0);
 	ImuMotionPredictor vibrating(1.0, 0.5, 0.0);
 	for(int i=0; i<=400; ++i)
 	{
 		turning.addImu(i*0.005, tiltedImu(i*0.005, 9.55, 0.05, 0.5));
 		vibrating.addImu(i*0.005, tiltedImu(i*0.005, 9.55, 1.0));
 	}
 	EXPECT_EQ(turning.gravityWindows(), 0u);
 	EXPECT_DOUBLE_EQ(turning.gravity(), 9.80665);
 	EXPECT_EQ(vibrating.gravityWindows(), 0u);
 	EXPECT_DOUBLE_EQ(vibrating.gravity(), 9.80665);
 }
 TEST(ImuMotionPredictor, keeps_the_gravity_it_is_given)
 {
 	ImuMotionPredictor predictor;
 	for(int i=0; i<=400; ++i)
 	{
 		predictor.addImu(i*0.005, tiltedImu(i*0.005, 9.55));
 	}
 	EXPECT_FALSE(predictor.isGravityEstimated());
 	EXPECT_EQ(predictor.gravityWindows(), 0u);
 	EXPECT_DOUBLE_EQ(predictor.gravity(), 9.80665);
 }
@@ -4681,3 +4681,128 @@ TEST(MemoryTest, CreateSignatureRecompressesStereoPairAfterRectification)
 	EXPECT_GT(cv::countNonZero(uncompressImage(stored.depthOrRightCompressed()) != right), 0)
 			<< "stored right image still holds the unrectified pixels";
 }
 // ---------------------------------------------------------------------------
 // Mem/DepthCompressionFormat with inverse depth (".rvl:max:q"), which databases
 // older than 0.24 cannot hold: rtabmap 0.23 would still open them (e.g., created
 // with Db/TargetVersion=0.23.0) but could not decode their depth images.
 // ---------------------------------------------------------------------------
 namespace {
 enum DepthInput
 {
 	kRawDepth,
 	kCompressedDepthWithRaw,   // e.g., received from ROS and decoded
 	kCompressedDepthOnly       // raw depth not needed (no features extracted here)
 };
 struct InverseDepthCase
 {
 	const char * targetVersion;
 	DepthInput input;
 	const char * depthCompressionFormat;
 	bool parallelCompression;
 	const char * expectedFormat;
 };
 class MemoryInverseDepthTest : public ::testing::TestWithParam<InverseDepthCase> {};
 } // namespace
 TEST_P(MemoryInverseDepthTest, StoredDepthFormatFollowsDatabaseVersion)
 {
 	const InverseDepthCase & cs = GetParam();
 	ParametersMap params = defaultMemoryParams();
 	params[Parameters::kMemBinDataKept()] = "true";
 	params[Parameters::kMemDepthCompressionFormat()] = cs.depthCompressionFormat;
 	params[Parameters::kMemCompressionParallelized()] = cs.parallelCompression ? "true" : "false";
 	params[Parameters::kDbTargetVersion()] = cs.targetVersion;
 	Memory memory(params);
 	const std::string dbPath = uniqueDbPath();
 	ASSERT_TRUE(memory.init(dbPath, true, params));
 	const cv::Mat rgb(16, 16, CV_8UC3, cv::Scalar(10, 20, 30));
 	cv::Mat depth(16, 16, CV_32FC1);
 	cv::randu(depth, 0.5f, 8.0f);
 	const CameraModel model(10.0, 10.0, 8.0, 8.0, CameraModel::opticalRotation());
 	SensorData data;
 	if(cs.input == kRawDepth)
 	{
 		data = SensorData(rgb, depth, model);
 	}
 	else
 	{
 		data = SensorData(compressImage2(rgb, ".png"), compressImage2(depth, ".png:10:100"), model);
 		if(cs.input == kCompressedDepthWithRaw)
 		{
 			data.uncompressData();
 			ASSERT_FALSE(data.depthRaw().empty());
 		}
 	}
 	ASSERT_TRUE(memory.update(data, Transform(0, 0, 0, 0, 0, 0), cv::Mat::eye(6, 6, CV_64FC1) * 0.01));
 	const Signature * s = memory.getSignature(memory.getLastSignatureId());
 	ASSERT_NE(s, nullptr);
 	const cv::Mat & stored = s->sensorData().depthOrRightCompressed();
 	ASSERT_FALSE(stored.empty());
 	EXPECT_EQ(compressedDepthFormat(stored), cs.expectedFormat);
 	const cv::Mat restored = uncompressImage(stored);
 	ASSERT_EQ(restored.type(), CV_32FC1);
 	ASSERT_EQ(restored.size(), depth.size());
 	EXPECT_LT(cv::norm(restored, depth, cv::NORM_INF), 0.01);
 	memory.close(false);
 	UFile::erase(dbPath);
 }
 INSTANTIATE_TEST_SUITE_P(
 		DatabaseVersions,
 		MemoryInverseDepthTest,
 		::testing::Values(
 				InverseDepthCase{"", kRawDepth, ".rvl:10:100", true, ".rvl:10:100"},
 				InverseDepthCase{"", kRawDepth, ".rvl:10:100", false, ".rvl:10:100"},
 				InverseDepthCase{"", kCompressedDepthWithRaw, ".rvl:10:100", true, ".png:10:100"},  // reused as is
 				InverseDepthCase{"", kCompressedDepthOnly, ".rvl:10:100", true, ".png:10:100"},     // reused as is
 				InverseDepthCase{"0.23.0", kRawDepth, ".rvl:10:100", true, ".png"},                // legacy 32FC1 format
 				InverseDepthCase{"0.23.0", kCompressedDepthWithRaw, ".rvl:10:100", true, ".png"},  // re-compressed
 				InverseDepthCase{"0.23.0", kCompressedDepthOnly, ".rvl:10:100", true, ".png"},     // decompressed, re-compressed
 				InverseDepthCase{"", kRawDepth, ".rvl", true, ".png"},                             // RVL is 16UC1 only: legacy
 				InverseDepthCase{"", kRawDepth, ".jpg", true, ".png"}));                           // invalid: default ".rvl"
 // Compressed images that Memory rectifies (Rtabmap/ImagesAlreadyRectified=false) are
 // decoded for it, even when nothing else needs them (no feature extraction here): they
 // are stored rectified, not as received.
 TEST(MemoryTest, DecodesCompressedImagesToRectifyThem)
 {
 	for(bool alreadyRectified : {true, false})
 	{
 		SCOPED_TRACE(alreadyRectified ? "already rectified" : "rectified by Memory");
 		ParametersMap params = defaultMemoryParams();
 		params[Parameters::kMemBinDataKept()] = "true";
 		params[Parameters::kRtabmapImagesAlreadyRectified()] = alreadyRectified ? "true" : "false";
 		Memory memory(params);
 		ASSERT_TRUE(memory.init(""));
 		cv::Mat rgb(48, 64, CV_8UC3);
 		cv::randu(rgb, 0, 255);
 		const cv::Mat K = (cv::Mat_<double>(3, 3) << 50, 0, 32, 0, 50, 24, 0, 0, 1);
 		const cv::Mat D = (cv::Mat_<double>(1, 5) << -0.3, 0.1, 0, 0, 0);
 		const cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
 		const cv::Mat P = (cv::Mat_<double>(3, 4) << 50, 0, 32, 0, 0, 50, 24, 0, 0, 0, 1, 0);
 		const CameraModel model("cam", cv::Size(64, 48), K, D, R, P, CameraModel::opticalRotation());
 		ASSERT_TRUE(model.isValidForRectification());
 		const cv::Mat compressed = compressImage2(rgb, ".png");
 		SensorData data(compressed, cv::Mat(), model);
 		ASSERT_TRUE(memory.update(data, Transform(0, 0, 0, 0, 0, 0), cv::Mat::eye(6, 6, CV_64FC1) * 0.01));
 		const Signature * s = memory.getSignature(memory.getLastSignatureId());
 		ASSERT_NE(s, nullptr);
 		const cv::Mat & stored = s->sensorData().imageCompressed();
 		ASSERT_FALSE(stored.empty());
 		const bool sameBytes = stored.total() == compressed.total() &&
 				memcmp(stored.data, compressed.data, compressed.total()) == 0;
 		EXPECT_EQ(sameBytes, alreadyRectified);
 	}
 }
@@ -11,6 +11,10 @@
 #include <opencv2/core.hpp>
 #include <opencv2/imgcodecs.hpp>
 #include <memory>
 #include <rtabmap/core/IMU.h>
 #include <rtabmap/utilite/UStl.h>
 #include <cmath>
 #include <limits>
 #include <string>
 using namespace rtabmap;
@@ -584,3 +588,208 @@ TEST(OdometryTest, RefusesAFirstScanTooSmallForTheCorrespondenceRatio)
 	EXPECT_FALSE(unchecked->process(uncheckedData).isNull())
 			<< "a scan of unknown sweep size was refused";
 }
 // ---------------------------------------------------------------------------
 // Lidar deskewing with an IMU (Odom/Deskewing): a sensor moving in a box-shaped room,
 // whose scans are generated point by point from where the sensor was at each point's
 // time, as a spinning lidar measures them.
 // ---------------------------------------------------------------------------
 namespace {
 const Eigen::Vector3d kRoomMin(-6.0, -4.0, -1.5);
 const Eigen::Vector3d kRoomMax(7.0, 5.0, 2.5);
 const double kSweep = 0.1;     // s, first column to last
 const int kRings = 16;
 const int kColumns = 512;
 const double kGravity = 9.80665;
 /// Pose of the sensor at time t, and its acceleration (world frame).
 struct Trajectory
 {
 	double yaw = 0.0;              // rad, heading at t=0
 	double yawRate = 0.0;          // rad/s
 	double acceleration = 0.0;     // m/s^2 along x, from rest at t=0
 	Transform pose(double t) const
 	{
 		return Transform(float(0.5*acceleration*t*t), 0, 0, 0, 0, float(yaw + yawRate*t));
 	}
 	Eigen::Vector3d linearAcceleration() const { return Eigen::Vector3d(acceleration, 0, 0); }
 };
 /// The scan of a sweep starting at @p stamp: organized (rings x columns), time on columns.
 LaserScan makeSweep(const Trajectory & trajectory, double stamp)
 {
 	cv::Mat data(kRings, kColumns, CV_32FC(5));
 	for(int u=0; u<kColumns; ++u)
 	{
 		const double dt = kSweep * double(u) / double(kColumns-1);
 		const Transform pose = trajectory.pose(stamp + dt);
 		const Eigen::Matrix3d rotation = pose.toEigen3d().linear();
 		const Eigen::Vector3d origin(pose.x(), pose.y(), pose.z());
 		const double azimuth = 2.0*M_PI*double(u)/double(kColumns);
 		for(int v=0; v<kRings; ++v)
 		{
 			const double elevation = (-15.0 + 30.0*double(v)/double(kRings-1)) * M_PI / 180.0;
 			const Eigen::Vector3d direction(std::cos(elevation)*std::cos(azimuth), std::cos(elevation)*std::sin(azimuth), std::sin(elevation));
 			const Eigen::Vector3d world = rotation * direction;
 			// Distance to the wall the ray hits
 			double range = std::numeric_limits<double>::max();
 			for(int i=0; i<3; ++i)
 			{
 				if(world[i] > 1e-9) range = std::min(range, (kRoomMax[i] - origin[i]) / world[i]);
 				else if(world[i] < -1e-9) range = std::min(range, (kRoomMin[i] - origin[i]) / world[i]);
 			}
 			float * p = data.ptr<float>(v, u);
 			p[0] = float(range*direction.x());
 			p[1] = float(range*direction.y());
 			p[2] = float(range*direction.z());
 			p[3] = 1.0f;
 			p[4] = float(dt);
 		}
 	}
 	return LaserScan(data, kRings*kColumns, 0.0f, LaserScan::kXYZIT, Transform::getIdentity());
 }
 /// What the IMU, at the sensor's origin, measures at time t.
 IMU makeImu(const Trajectory & trajectory, double t)
 {
 	const Eigen::Quaterniond q = trajectory.pose(t).getQuaterniond();
 	const Eigen::Vector3d f = q.inverse() * (trajectory.linearAcceleration() + Eigen::Vector3d(0, 0, kGravity));
 	return IMU(cv::Vec4d(q.x(), q.y(), q.z(), q.w()), cv::Mat::eye(3,3,CV_64FC1),
 			cv::Vec3d(0, 0, trajectory.yawRate), cv::Mat::eye(3,3,CV_64FC1),
 			cv::Vec3d(f.x(), f.y(), f.z()), cv::Mat::eye(3,3,CV_64FC1),
 			Transform::getIdentity());
 }
 /// RMS distance (m) of the scan's points to the nearest wall, with the sensor at @p pose.
 double wallDistance(const LaserScan & scan, const Transform & pose)
 {
 	double sum = 0.0;
 	int n = 0;
 	for(int i=0; i<scan.size(); ++i)
 	{
 		const float * p = scan.data().ptr<float>(0, i);
 		const Transform world = pose * Transform(p[0], p[1], p[2], 0, 0, 0);
 		const Eigen::Vector3d w(world.x(), world.y(), world.z());
 		double d = std::numeric_limits<double>::max();
 		for(int k=0; k<3; ++k)
 		{
 			d = std::min(d, std::fabs(w[k] - kRoomMin[k]));
 			d = std::min(d, std::fabs(w[k] - kRoomMax[k]));
 		}
 		sum += d*d;
 		++n;
 	}
 	return n?std::sqrt(sum/n):0.0;
 }
 /**
 * Feeds @p frames sweeps, one every 0.1 s from t=0.1, with the IMU at 200 Hz up to the end
 * of each sweep (as OdometryThread does), and returns the RMS wall distance of the last
 * sweep as odometry left it in the frame (deskewed or not).
 */
 double deskewedWallDistance(const Trajectory & trajectory, int frames, const ParametersMap & extra)
 {
 	ParametersMap parameters;
 	parameters.insert(ParametersPair(Parameters::kRegStrategy(), "1"));
 	parameters.insert(ParametersPair(Parameters::kIcpVoxelSize(), "0"));
 	parameters.insert(ParametersPair(Parameters::kIcpPointToPlane(), "true"));
 	parameters.insert(ParametersPair(Parameters::kIcpPointToPlaneK(), "10"));
 	parameters.insert(ParametersPair(Parameters::kIcpMaxTranslation(), "0"));
 	parameters.insert(ParametersPair(Parameters::kIcpMaxCorrespondenceDistance(), "0.5"));
 	for(const ParametersPair & p : extra) uInsert(parameters, p);
 	std::unique_ptr<Odometry> odometry(Odometry::create(parameters));
 	double imuStamp = 0.0;
 	double distance = -1.0;
 	for(int i=1; i<=frames; ++i)
 	{
 		const double stamp = 0.1*i;
 		for(; imuStamp <= stamp + kSweep + 0.005; imuStamp += 0.005)
 		{
 			SensorData imu(makeImu(trajectory, imuStamp), 0, imuStamp);
 			odometry->process(imu);
 		}
 		SensorData data(makeSweep(trajectory, stamp), cv::Mat(), cv::Mat(), CameraModel(), i, stamp);
 		OdometryInfo info;
 		const Transform pose = odometry->process(data, &info);
 		EXPECT_FALSE(pose.isNull()) << "frame " << i << " not registered";
 		distance = wallDistance(data.laserScanRaw(), trajectory.pose(stamp));
 	}
 	return distance;
 }
 }  // namespace
 TEST(OdometryTest, DeskewsWithTheImuOrientation)
 {
 	// Turning at 2 rad/s: 11 degrees over a sweep, so the far walls are smeared by about a
 	// metre. On the first frame, before any pose, only the IMU orientation is used.
 	Trajectory turning;
 	turning.yawRate = 2.0;
 	ParametersMap off;
 	off.insert(ParametersPair(Parameters::kOdomDeskewing(), "false"));
 	const double skewed = deskewedWallDistance(turning, 1, off);
 	const double deskewed = deskewedWallDistance(turning, 1, ParametersMap());
 	EXPECT_GT(skewed, 0.1) << "the scan should be smeared without deskewing";
 	EXPECT_LT(deskewed, 0.01) << "the points should be back on the walls";
 }
 TEST(OdometryTest, DeskewsWithTheImuAccelerationOverTheSmoothingDelay)
 {
 	// Accelerating at 2 m/s^2 from rest. With Odom/GuessSmoothingDelay, the velocity is
 	// carried to each frame with the IMU acceleration, and the prediction integrates it
 	// over the sweep. At 0, the acceleration is not used: the velocity of the last
 	// interval lags behind, and the sweep is still skewed.
 	Trajectory accelerating;
 	accelerating.acceleration = 2.0;
 	// Not facing exactly along x: an IMU orientation whose x, y and z are all zero (the
 	// identity) is read by RTAB-Map's odometry as "not set", and the IMU ignored.
 	accelerating.yaw = 0.3;
 	ParametersMap noDelay;
 	noDelay.insert(ParametersPair(Parameters::kOdomGuessSmoothingDelay(), "0"));
 	ParametersMap delay;
 	delay.insert(ParametersPair(Parameters::kOdomGuessSmoothingDelay(), "0.5"));
 	const double withoutAcceleration = deskewedWallDistance(accelerating, 12, noDelay);
 	const double withAcceleration = deskewedWallDistance(accelerating, 12, delay);
 	EXPECT_LT(withAcceleration, 0.005) << "the points should be back on the walls";
 	EXPECT_GT(withoutAcceleration, withAcceleration * 3.0);
 }
 TEST(OdometryTest, InitialOrientationIsTheImuOrientationAtTheFirstFrame)
 {
 	// IMU received for 2 s before the first frame, while the sensor turns at 1 rad/s: the
 	// first frame must start from the orientation at its own stamp, not at the first IMU
 	// sample (2 rad earlier).
 	Trajectory turning;
 	turning.yaw = 0.3;
 	turning.yawRate = 1.0;
 	ParametersMap parameters;
 	parameters.insert(ParametersPair(Parameters::kRegStrategy(), "1"));
 	parameters.insert(ParametersPair(Parameters::kIcpVoxelSize(), "0"));
 	std::unique_ptr<Odometry> odometry(Odometry::create(parameters));
 	const double stamp = 2.0;
 	for(double t = 0.0; t <= stamp + kSweep + 0.005; t += 0.005)
 	{
 		SensorData imu(makeImu(turning, t), 0, t);
 		odometry->process(imu);
 	}
 	SensorData data(makeSweep(turning, stamp), cv::Mat(), cv::Mat(), CameraModel(), 1, stamp);
 	OdometryInfo info;
 	const Transform pose = odometry->process(data, &info);
 	ASSERT_FALSE(pose.isNull());
 	// The IMU was fed up to the end of the sweep (as OdometryThread does), but the orientation
 	// is interpolated at the frame's stamp
 	EXPECT_NEAR(pose.theta(), turning.yaw + turning.yawRate*stamp, 0.002);
 	// An initial pose given with a rotation is kept
 	std::unique_ptr<Odometry> given(Odometry::create(parameters));
 	given->reset(Transform(0, 0, 0, 0, 0, 1.0f));
 	for(double t = 0.0; t <= stamp; t += 0.005)
 	{
 		SensorData imu(makeImu(turning, t), 0, t);
 		given->process(imu);
 	}
 	EXPECT_NEAR(given->getPose().theta(), 1.0, 1e-6);
 }
@@ -0,0 +1,20 @@
 #include <gtest/gtest.h>
 #include <rtabmap/core/Parameters.h>
 using rtabmap::Parameters;
 using rtabmap::ParametersMap;
 TEST(Parameters, descriptions_are_shorter_than_1024_characters)
 {
 	// Long descriptions don't fit in the GUI's tooltips nor in a ROS parameter listing:
 	// keep them short, the details belong in the documentation. 1024 was also the size
 	// beyond which uFormat(), which formats most of them, aborted on Windows.
 	const ParametersMap & parameters = Parameters::getDefaultParameters();
 	ASSERT_FALSE(parameters.empty());
 	for(ParametersMap::const_iterator iter = parameters.begin(); iter != parameters.end(); ++iter)
 	{
 		const std::string description = Parameters::getDescription(iter->first);
 		EXPECT_LT(description.size(), 1024u) << iter->first << " has a description of "
 				<< description.size() << " characters: shorten it";
 	}
 }
@@ -199,7 +199,7 @@ TEST(SensorDataTest, IsValidWithCameraModel)
 {
    SensorData data;
    data.setRGBDImage(cv::Mat(), cv::Mat(), CameraModel());
-    EXPECT_FALSE(data.cameraModels().empty());
+    EXPECT_TRUE(data.cameraModels().empty()); // invalid without image: placeholder not kept
    EXPECT_FALSE(data.isValid()); // not valid for projection
    data.setRGBDImage(cv::Mat(), cv::Mat(), CameraModel(525.0, 525.0, 320.0, 240.0));
@@ -986,3 +986,32 @@ TEST(SensorDataTest, DifferentDepthTypes)
    EXPECT_EQ(data.depthOrRightRaw().type(), CV_32FC1);
 }
 // An invalid CameraModel without any image is a placeholder (e.g., lidar odometry
 // creating scan-only data with CameraModel()): it is not kept. With an image, it is kept,
 // as images can be used without calibration.
 TEST(SensorDataTest, InvalidCameraModelIsKeptOnlyWithImages)
 {
 	const LaserScan scan(cv::Mat(1, 3, CV_32FC2, cv::Scalar(1.0f, 0.0f)), 0, 10.0f, LaserScan::kXY);
 	const SensorData scanOnly(scan, cv::Mat(), cv::Mat(), CameraModel(), 1, 1.0);
 	EXPECT_TRUE(scanOnly.cameraModels().empty());
 	EXPECT_TRUE(scanOnly.isValid());
 	const cv::Mat image(4, 6, CV_8UC1, cv::Scalar(1));
 	const SensorData uncalibrated(image, CameraModel(), 1, 1.0);
 	EXPECT_EQ(uncalibrated.cameraModels().size(), 1u);
 	const SensorData compressedOnly(compressImage2(image, ".png"), CameraModel(), 1, 1.0);
 	EXPECT_EQ(compressedOnly.cameraModels().size(), 1u);
 	const CameraModel valid(10.0, 10.0, 3.0, 2.0);
 	const SensorData calibratedNoImage(scan, cv::Mat(), cv::Mat(), valid, 1, 1.0);
 	EXPECT_EQ(calibratedNoImage.cameraModels().size(), 1u) << "valid models are always kept";
 	// Keeping the images already there: they still need their model
 	SensorData data(image, CameraModel(), 1, 1.0);
 	data.setRGBDImage(cv::Mat(), cv::Mat(), CameraModel(), false);
 	EXPECT_EQ(data.cameraModels().size(), 1u);
 	data.setRGBDImage(cv::Mat(), cv::Mat(), CameraModel(), true);
 	EXPECT_TRUE(data.cameraModels().empty()) << "images cleared, nothing left to describe";
 }
@@ -2114,3 +2114,41 @@ TEST(Util3dTest, DeskewValidScan) {
    // empty scan
    EXPECT_TRUE(util3d::deskew(LaserScan(), inputStamp, velocity).empty());
 }
 TEST(Util3dTest, DeskewWithMotion) {
    // Three points measured at y=0, 1 s before, at and 1 s after the scan stamp, while the
    // base moves along y as (t-stamp)^2: a motion no constant velocity describes.
    cv::Mat data = cv::Mat::zeros(1, 3, CV_32FC(5));
    float * dataPtr = (float*)data.data;
    dataPtr[0] = 1;  dataPtr[4] = -1;
    dataPtr[5] = 1;  dataPtr[9] = 0;
    dataPtr[10] = 1; dataPtr[14] = 1;
    LaserScan scan(data, 3, 10.0f, LaserScan::kXYZIT);
    const double inputStamp = 1000.0;
    std::vector<double> stamps;
    auto motion = [&](double stamp) {
        stamps.push_back(stamp);
        const double dt = stamp - inputStamp;
        return Transform(0.0, dt*dt, 0.0, 0.0, 0.0, 0.0);
    };
    // Asked for every point time, each point is moved by its own pose
    LaserScan result = util3d::deskew(scan, inputStamp, motion);
    ASSERT_EQ(result.size(), 3);
    EXPECT_EQ(stamps.size(), 3u);
    EXPECT_FLOAT_EQ(result.field(0, 1), 1.0f);
    EXPECT_FLOAT_EQ(result.field(1, 1), 0.0f);
    EXPECT_FLOAT_EQ(result.field(2, 1), 1.0f);
    // With slerp, only the ends are asked for, the middle point is interpolated between them
    stamps.clear();
    result = util3d::deskew(scan, inputStamp, motion, true);
    ASSERT_EQ(result.size(), 3);
    EXPECT_EQ(stamps.size(), 2u);
    EXPECT_FLOAT_EQ(result.field(1, 1), 1.0f);
    // A failing motion, or none
    EXPECT_TRUE(util3d::deskew(scan, inputStamp, [](double) { return Transform(); }).empty());
    EXPECT_TRUE(util3d::deskew(scan, inputStamp, std::function<Transform(double)>()).empty());
 }
@@ -1504,6 +1504,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->odom_flow_scanKeyframeThr->setObjectName(Parameters::kOdomScanKeyFrameThr().c_str());
 	_ui->odom_flow_guessMotion->setObjectName(Parameters::kOdomGuessMotion().c_str());
 	_ui->odom_guess_smoothing_delay->setObjectName(Parameters::kOdomGuessSmoothingDelay().c_str());
 	_ui->odom_imu_gravity->setObjectName(Parameters::kOdomImuGravity().c_str());
 	_ui->odom_imageDecimation->setObjectName(Parameters::kOdomImageDecimation().c_str());
 	_ui->odom_alignWithGround->setObjectName(Parameters::kOdomAlignWithGround().c_str());
 	_ui->odom_lidar_deskewing->setObjectName(Parameters::kOdomDeskewing().c_str());
@@ -12391,7 +12391,7 @@ see Sqlite3 doc 'PRAGMA synchronous'.</string>
                    <item row="5" column="1">
                     <widget class="QLabel" name="label_760">
                      <property name="text">
-                       <string>Depth image compression format (should be &quot;.png&quot; or &quot;.rvl&quot;).</string>
+                       <string>Depth image compression format (should be &quot;.png&quot; or &quot;.rvl&quot;). Add &quot;:maxDepth[:quantization]&quot; (e.g., &quot;.rvl:10:100&quot;) to compress 32FC1 depth images as 16 bits inverse depth (lossy: precision ~d²/(2q(q+1)), depth over maxDepth is lost, databases cannot be opened by versions &lt; 0.24). 16UC1 depth images are always compressed losslessly.</string>
                      </property>
                      <property name="wordWrap">
                       <bool>true</bool>
@@ -16427,7 +16427,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                        </property>
                       </widget>
                      </item>
-                      <item row="14" column="1">
+                      <item row="15" column="1">
                       <widget class="QLabel" name="label_232">
                        <property name="text">
                         <string>Data buffer size (0 means inf).</string>
@@ -16570,7 +16570,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                        </property>
                       </widget>
                      </item>
-                      <item row="12" column="0">
+                      <item row="13" column="0">
                       <widget class="QDoubleSpinBox" name="odom_flow_keyframeThr">
                        <property name="maximum">
                         <double>1.000000000000000</double>
@@ -16590,7 +16590,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                        </property>
                       </widget>
                      </item>
-                      <item row="11" column="0">
+                      <item row="12" column="0">
                       <widget class="QSpinBox" name="odom_VisKeyFrameThr">
                        <property name="maximum">
                         <number>9999</number>
@@ -16600,7 +16600,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                        </property>
                       </widget>
                      </item>
-                      <item row="13" column="1">
+                      <item row="14" column="1">
                       <widget class="QLabel" name="label_246">
                        <property name="text">
                         <string>[Geometry] Create a new keyframe when the number of inliers drops under this threshold. Setting value to 0 means that a keyframe is created for each processed frame.</string>
@@ -16613,7 +16613,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                        </property>
                       </widget>
                      </item>
-                      <item row="10" column="1">
+                      <item row="11" column="1">
                       <widget class="QLabel" name="label_248">
                        <property name="text">
                         <string>Decimation of the RGB image before registration. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. If Visual Registration -&gt; Visual Feature -&gt; Depth as Mask is true and if depth is smaller than decimated RGB, depth may be interpolated to match RGB size for feature detection.</string>
@@ -16709,7 +16709,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                        </property>
                       </widget>
                      </item>
-                      <item row="15" column="0">
+                      <item row="16" column="0">
                       <widget class="QPushButton" name="pushButton_testOdometry">
                        <property name="text">
                         <string>Test odometry</string>
@@ -16729,14 +16729,14 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                        </property>
                       </widget>
                      </item>
-                      <item row="14" column="0">
+                      <item row="15" column="0">
                       <widget class="QSpinBox" name="odom_dataBufferSize">
                        <property name="maximum">
                         <number>999999</number>
                        </property>
                       </widget>
                      </item>
-                      <item row="12" column="1">
+                      <item row="13" column="1">
                       <widget class="QLabel" name="label_196">
                        <property name="text">
                         <string>[Visual] Create a new keyframe when the number of inliers drops under this ratio of features in last frame. Setting the value to 0 means that a keyframe is created for each processed frame.</string>
@@ -16749,7 +16749,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                        </property>
                       </widget>
                      </item>
-                      <item row="10" column="0">
+                      <item row="11" column="0">
                       <widget class="QSpinBox" name="odom_imageDecimation">
                        <property name="minimum">
                         <number>1</number>
@@ -16778,7 +16778,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                        </property>
                       </widget>
                      </item>
-                      <item row="13" column="0">
+                      <item row="14" column="0">
                       <widget class="QDoubleSpinBox" name="odom_flow_scanKeyframeThr">
                        <property name="maximum">
                         <double>1.000000000000000</double>
@@ -16794,7 +16794,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                      <item row="8" column="1">
                       <widget class="QLabel" name="label_520">
                        <property name="text">
-                         <string>Guess smoothing delay (s). Estimated velocity is averaged based on last transforms up to this maximum delay. This can help to get smoother velocity prediction. Last velocity computed is used directly if a filtering strategy is set or the delay is below the odometry rate.</string>
+                         <string>Guess smoothing delay (s). The velocity is averaged over the last transforms up to this delay, for a smoother velocity prediction. The last velocity is used directly if a filtering strategy is set or the delay is below the odometry rate. With an IMU giving orientation and linear acceleration, a delay &gt; 0 also enables the IMU acceleration: the velocity is the displacement over this delay corrected by the acceleration measured since, so it is not delayed, and the motion guess and lidar deskewing integrate the acceleration from it (~0.5 s recommended). With 0, the IMU only gives the orientation. Without IMU, the average is delayed by half this delay: useful for platforms with inertia, high frame rates, or lidar deskewing, where the velocity noise would feed back into the next poses.</string>
                        </property>
                        <property name="wordWrap">
                         <bool>true</bool>
@@ -16831,7 +16831,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                        </property>
                       </widget>
                      </item>
-                      <item row="11" column="1">
+                      <item row="12" column="1">
                       <widget class="QLabel" name="label_354">
                        <property name="text">
                         <string>[Visual] Create a new keyframe when the number of inliers drops under this threshold. Setting the value to 0 means that a keyframe is created for each processed frame.</string>
@@ -16844,10 +16844,10 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                        </property>
                       </widget>
                      </item>
-                      <item row="9" column="1">
+                      <item row="10" column="1">
                       <widget class="QLabel" name="label_7461">
                        <property name="text">
-                         <string>Lidar deskewing. If input lidar has time channel, it will be deskewed with a constant motion model (with IMU orientation and/or guess if provided).</string>
+                         <string>Lidar deskewing. If input lidar has time channel, it will be deskewed. With an IMU, the pose of every point is predicted from the previous frame: orientation from the IMU, translation from the velocity (with the IMU acceleration if the guess smoothing delay is &gt; 0). Without IMU, with a constant motion model (or the guess if provided).</string>
                        </property>
                        <property name="wordWrap">
                         <bool>true</bool>
@@ -16857,13 +16857,48 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                        </property>
                       </widget>
                      </item>
-                      <item row="9" column="0">
+                      <item row="10" column="0">
                       <widget class="QCheckBox" name="odom_lidar_deskewing">
                        <property name="text">
                         <string/>
                        </property>
                       </widget>
                      </item>
                      <item row="9" column="0">
                       <widget class="QDoubleSpinBox" name="odom_imu_gravity">
                        <property name="specialValueText">
                         <string>Auto</string>
                        </property>
                        <property name="suffix">
                         <string> m/s²</string>
                        </property>
                        <property name="decimals">
                         <number>5</number>
                        </property>
                        <property name="maximum">
                         <double>100.000000000000000</double>
                        </property>
                        <property name="singleStep">
                         <double>0.010000000000000</double>
                        </property>
                        <property name="value">
                         <double>9.806650000000000</double>
                        </property>
                       </widget>
                      </item>
                      <item row="9" column="1">
                       <widget class="QLabel" name="label_odom_imu_gravity">
                        <property name="text">
                         <string>Gravity magnitude removed from the IMU linear acceleration (used with a guess smoothing delay &gt; 0 above). Standard gravity by default. Set it to what the accelerometer reads at rest to compensate for its scale error, or for another gravity than Earth's. Auto (0): estimated from the IMU while it is still, with standard gravity until then: the robot should then be perfectly still for a moment (e.g., at start).</string>
                        </property>
                        <property name="wordWrap">
                         <bool>true</bool>
                        </property>
                        <property name="textInteractionFlags">
                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
                        </property>
                       </widget>
                      </item>
                     </layout>
                    </item>
                    <item>
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
  <name>rtabmap</name>
-  <version>0.23.13</version>
+  <version>0.24.1</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>
@@ -18,12 +18,12 @@
  <depend>gtsam</depend>
  <!-- <depend>libopenni2-dev</depend> --> <!-- not available on Jessie -->
  <depend>libpcl-all-dev</depend> <!--  include libvtk-qt -->
-  <!-- <depend>libpointmatcher</depend> Not available on rolling--> <!-- optional but recommended if lidar is used, also not available on 32 bits system, but rtabmap can be built without it -->
+  <depend>libpointmatcher</depend> <!-- optional but recommended if lidar is used, also not available on 32 bits system, but rtabmap can be built without it -->
  <!-- <depend>libproj-dev</depend> needed due to error in vtk6 (kinetic)-->
  <depend>libsqlite3-dev</depend>
  <depend>liboctomap-dev</depend>
-<!--  <depend>grid_map_core</depend> # not available on Rolling -->
+  <!-- <depend>grid_map_core</depend> --> <!-- till this PR is released https://github.com/ANYbotics/grid_map/pull/499 -->
-  <depend>qtbase5-dev</depend>
+  <depend>qt_gui_cpp</depend> <!-- libqt4-dev or libqt5-dev -->
  <depend>zlib</depend>
  <export>
@@ -338,7 +338,8 @@ std::string uFormatv (const char *fmt, va_list args)
        // Try to vsnprintf into our buffer.
 #ifdef _MSC_VER
-    	int needed = vsnprintf_s(buf, size, size, fmt, argsTmp);
+    	// _TRUNCATE: return -1 if it doesn't fit (count=size would call the invalid parameter handler and abort)
    	int needed = vsnprintf_s(buf, size, _TRUNCATE, fmt, argsTmp);
 #else
    	int needed = vsnprintf (buf, size, fmt, argsTmp);
 #endif
@@ -201,3 +201,14 @@ TEST(UConversionTest, UFormat)
    EXPECT_NE(result.find("42"), std::string::npos);
 }
 TEST(UConversionTest, UFormatLongerThanItsFirstBuffer)
 {
    // Over the 1024 bytes uFormat first tries: formatted whole, not truncated (MSVC's
    // checked vsnprintf_s used to abort the process instead).
    const std::string longText(3000, 'a');
    std::string result = uFormat("%s %d", longText.c_str(), 42);
    EXPECT_EQ(result, longText + " 42");
    result = uFormat("%s", std::string(1023, 'b').c_str());
    EXPECT_EQ(result, std::string(1023, 'b'));
 }
Author	SHA1	Message	Date
matlabbe	05b5661e6b	Fixing crash on windows when uFormat() is to long	2026-10-11 11:45:52 -07:00
matlabbe	526c422643	Added auto gravity estimation mode	2026-10-11 11:36:44 -07:00
matlabbe	5d447d9bb0	fixing M_PI not defined in tests	2026-10-11 10:49:07 -07:00
matlabbe	3357599379	fixing sign issue with x86-64-v3	2026-10-11 10:45:21 -07:00
matlabbe	9e27446236	refactored odom's imu init orientation	2026-10-10 22:12:50 -07:00
matlabbe	de1761bd6d	simplified parameters	2026-10-10 18:26:20 -07:00
matlabbe	874bc35d22	Imu motion predictor	2026-10-10 17:02:13 -07:00
matlabbe	8d93f807c8	Updating g2o default max iterations (#1790 ) Stereo outdoor demo with g2o ( epsilon=0 and optimizer=0) requires at least 25 iterations to converge correctly.	2026-10-10 12:31:45 -07:00
matlabbe	aa95581cd2	Support Inverted Depth compression (with png or rvl) (#1785 ) * Support Inverted Depth compression (with png or rvl) * Updated parameter's ui description * ios: fixing minor version bump * Improved test coverage for this branch * fixing opencv debug assert * SensorData: ignore invalid CameraModel * added lazy decoding check	2026-10-03 19:24:14 -07:00
matlabbe	a4e7f13522	Updated README's badges	2026-10-01 20:57:02 -07:00