Sparse Bayes (#1748)

* Sparse Bayes

* updated perf test

* improved tests with real data

* Making sparse works in incremental mapping

* bookkeeping optimization

* small opt

* refactoring

* splitting dense and sparse in different classes to make the code more lisible

* cleanup comments

* fixing CI

* Making all Bayes tests testing both dense and sparse

* Added multisession_3it integration test (test memory management, multisession and dense/sparse bayes in that settings)

* optimized sparse when transfer/retrieval happens (was slower than dense for that case)

* Testing retrieval param variants

* Updated multisession_3it integration tests to compare loop closure hypotheses

* bump version

* Fixed ui sum of prediction

* adding g2o gtsam to linux ci

* cleanup

* added debug crash log for ci

* Simplified Bayes/SparsePrediction description

* Dont show too dense for sparse on small maps (e.g., when we just started a new map)

* fixing amd64v3 issue with gtsam on ci ubuntu 26

* Dot not auto switch to dense based on map size.

* updating test range

* added coverage tests

* Adressing coverage

* ignore one line in coverage for purpose

.github/scripts/ros-deps-env.sh

@@ -0,0 +1,19 @@
+# Puts the graph optimizers taken from the ROS 2 repo (see cmake-linux.yml) on
+# the loader path, for a shell that is about to run something built against
+# them. Sourced, not executed: it exports into the caller.
+#
+#   source .github/scripts/ros-deps-env.sh /opt/ros/humble
+#
+# A missing or empty prefix is a build that took every dependency from the
+# Ubuntu archive: nothing to add.
+#
+# Both directories are needed. GTSAM and g2o install their libraries in the
+# multiarch subdirectory, while the prefix's own lib/ holds the rest, and
+# libgtsam.so carries no RUNPATH -- so the loader finds neither it nor the
+# libmetis-gtsam.so it pulls in without being told where to look.
+ros_prefix="${1:-}"
+if [ -n "$ros_prefix" ] && [ -d "$ros_prefix" ]; then
+  ros_lib="$ros_prefix/lib"
+  export LD_LIBRARY_PATH="${ros_lib}:${ros_lib}/$(gcc -dumpmachine)${LD_LIBRARY_PATH:+:$LD_LIBRARY_PATH}"
+  echo "LD_LIBRARY_PATH=$LD_LIBRARY_PATH"
+fi

.github/workflows/cmake-linux.yml

@@ -28,22 +28,43 @@ jobs:
       matrix:
         build_name: [ubuntu-22.04, ubuntu-24.04, ubuntu-24.04-with-opengv, ubuntu-26.04]
         include:
+        # GTSAM is in no Ubuntu release, and libg2o-dev only appears in the
+        # archive from noble on (jammy has no g2o at all).
+        #
+        # 26.04 builds GTSAM itself (gtsam_ref) rather than taking it from the
+        # ROS repo: that runner is on Ubuntu's amd64v3 variant, so rtabmap and
+        # the archive libraries are AVX builds where Eigen aligns to 32, while
+        # the ROS binary is plain x86-64 where it aligns to 16 and uses plain
+        # malloc/free. Allocating on one side and freeing on the other gives
+        # "double free or corruption (out)". Forcing EIGEN_MAX_ALIGN_BYTES=16
+        # on rtabmap would only move the mismatch onto PCL, g2o and Ceres.
         - build_name: ubuntu-22.04
           os: ubuntu-22.04
           extra_deps: "libunwind-dev libceres-dev"
-          extra_cmake_def: "-DWITH_CERES=ON -DWITH_PYTHON=ON"
+          ros_deps: "ros-humble-libg2o ros-humble-gtsam"
+          ros_prefix: "/opt/ros/humble"
+          extra_cmake_def: "-DWITH_CERES=ON -DWITH_PYTHON=ON -DWITH_G2O=ON -DWITH_GTSAM=ON"
         - build_name: ubuntu-24.04
           os: ubuntu-24.04
           extra_deps: "libg2o-dev libceres-dev"
-          extra_cmake_def: "-DWITH_CERES=ON -DWITH_PYTHON=ON"
+          ros_deps: "ros-jazzy-gtsam"
+          ros_prefix: "/opt/ros/jazzy"
+          extra_cmake_def: "-DWITH_CERES=ON -DWITH_PYTHON=ON -DWITH_G2O=ON -DWITH_GTSAM=ON"
         - build_name: ubuntu-24.04-with-opengv
           os: ubuntu-24.04
           extra_deps: "libg2o-dev libceres-dev"
-          extra_cmake_def: "-DWITH_CERES=ON -DWITH_PYTHON=ON -DBUILD_OPENGV=ON"
+          ros_deps: "ros-jazzy-gtsam"
+          ros_prefix: "/opt/ros/jazzy"
+          extra_cmake_def: "-DWITH_CERES=ON -DWITH_PYTHON=ON -DWITH_G2O=ON -DWITH_GTSAM=ON -DBUILD_OPENGV=ON"
         - build_name: ubuntu-26.04
           os: ubuntu-26.04
-          extra_deps: "libg2o-dev libceres-dev"
+          # libboost-all-dev for the GTSAM build
-          extra_cmake_def: "-DWITH_CERES=ON -DWITH_PYTHON=ON -DBUILD_OPENGV=ON"
+          extra_deps: "libg2o-dev libceres-dev libboost-all-dev"
+          ros_deps: ""
+          ros_prefix: ""
+          # The tag ros-lyrical-gtsam 4.3.0 is built from
+          gtsam_ref: "4.3a0-ros"
+          extra_cmake_def: "-DWITH_CERES=ON -DWITH_PYTHON=ON -DWITH_G2O=ON -DWITH_GTSAM=ON -DBUILD_OPENGV=ON"
 
     steps:
     - uses: actions/checkout@v4
@@ -67,7 +88,61 @@ jobs:
       run: |
         DEBIAN_FRONTEND=noninteractive
         sudo apt-get update
-        sudo apt-get -y install libopencv-dev libpcl-dev git cmake software-properties-common libyaml-cpp-dev ${{ matrix.extra_deps }}
+        sudo apt-get -y install libopencv-dev libpcl-dev git cmake software-properties-common libyaml-cpp-dev curl gnupg lsb-release ${{ matrix.extra_deps }}
+
+    - name: Install Graph Optimizers From The ROS Repo
+      if: matrix.ros_deps != ''
+      env:
+        DEBIAN_FRONTEND: noninteractive
+      run: |
+        sudo curl -fsSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \
+          -o /usr/share/keyrings/ros-archive-keyring.gpg
+        echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" \
+          | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null
+        sudo apt-get update
+        sudo apt-get -y install ${{ matrix.ros_deps }}
+
+    # The compiler's x86-64 baseline, for the cache key: it decides Eigen's
+    # alignment, so a GTSAM built under one must not be restored under the
+    # other. v3 packages still declare Architecture: amd64, so dpkg cannot tell.
+    - name: Toolchain baseline
+      if: matrix.gtsam_ref != ''
+      id: toolchain
+      run: |
+        avx=$(echo | gcc -dM -E -x c++ - | grep -c __AVX__ || true)
+        echo "gcc $(gcc -dumpversion), __AVX__=$avx"
+        echo "id=gcc$(gcc -dumpversion)-avx${avx}" >> "$GITHUB_OUTPUT"
+
+    - name: Restore GTSAM
+      if: matrix.gtsam_ref != ''
+      id: cache-gtsam
+      uses: actions/cache@v4
+      with:
+        path: ${{github.workspace}}/gtsam-install
+        key: gtsam-${{ matrix.gtsam_ref }}-${{ matrix.os }}-${{ steps.toolchain.outputs.id }}
+
+    - name: Build GTSAM
+      if: matrix.gtsam_ref != '' && steps.cache-gtsam.outputs.cache-hit != 'true'
+      run: |
+        git clone --depth 1 --branch ${{ matrix.gtsam_ref }} https://github.com/borglab/gtsam.git ${{runner.temp}}/gtsam-src
+        # Tag-specific fixes, named after the tag.
+        patch_file="${{github.workspace}}/patches/gtsam_$(echo '${{ matrix.gtsam_ref }}' | tr '.' '_').patch"
+        if [ -f "$patch_file" ]; then
+          echo "Applying $patch_file"
+          git -C ${{runner.temp}}/gtsam-src apply "$patch_file"
+        fi
+        cmake -S ${{runner.temp}}/gtsam-src -B ${{runner.temp}}/gtsam-build \
+          -DCMAKE_BUILD_TYPE=Release \
+          -DCMAKE_INSTALL_PREFIX=${{github.workspace}}/gtsam-install \
+          -DGTSAM_USE_SYSTEM_EIGEN=ON \
+          -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \
+          -DGTSAM_BUILD_WITH_WERROR=OFF \
+          -DGTSAM_BUILD_TESTS=OFF \
+          -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \
+          -DGTSAM_BUILD_UNSTABLE=OFF \
+          -DGTSAM_BUILD_PYTHON=OFF \
+          -DBUILD_SHARED_LIBS=ON
+        cmake --build ${{runner.temp}}/gtsam-build --target install -j $(nproc)
 
     - name: Set up Python
       uses: actions/setup-python@v5
@@ -81,7 +156,12 @@ jobs:
 
     - name: Configure CMake
       run: |
-        cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} -DPython3_EXECUTABLE=$(which python3) -Dpybind11_DIR=$(python3 -m pybind11 --cmakedir) ${{ matrix.extra_cmake_def }}
+        # Both prefixes when a job has both.
+        prefix_path="${{ matrix.ros_prefix }}"
+        if [ -d "${{github.workspace}}/gtsam-install" ]; then
+          prefix_path="${prefix_path:+$prefix_path;}${{github.workspace}}/gtsam-install"
+        fi
+        cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} -DCMAKE_PREFIX_PATH="$prefix_path" -DPython3_EXECUTABLE=$(which python3) -Dpybind11_DIR=$(python3 -m pybind11 --cmakedir) ${{ matrix.extra_cmake_def }}
 
     - name: Build
       run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}}
@@ -89,6 +169,8 @@ jobs:
     - name: Info
       working-directory: ${{github.workspace}}/build/bin
       run: |
+        source ${{github.workspace}}/.github/scripts/ros-deps-env.sh "${{ matrix.ros_prefix }}"
+        source ${{github.workspace}}/.github/scripts/ros-deps-env.sh "${{github.workspace}}/gtsam-install"
         ./rtabmap-console --version
 
     - name: Test
@@ -101,4 +183,32 @@ jobs:
       env:
         PYTHONNOUSERSITE: 1
       run: |
-        ctest -C ${{env.BUILD_TYPE}} -V -LE performance
+        source ${{github.workspace}}/.github/scripts/ros-deps-env.sh "${{ matrix.ros_prefix }}"
+        source ${{github.workspace}}/.github/scripts/ros-deps-env.sh "${{github.workspace}}/gtsam-install"
+        # Keep a core file per crashing test, for the backtrace step below: an
+        # abort otherwise leaves only its message, and may not reproduce
+        # elsewhere.
+        ulimit -c unlimited
+        sudo sysctl -w kernel.core_pattern="${{github.workspace}}/build/core.%e.%p" > /dev/null
+        ctest -C ${{env.BUILD_TYPE}} -V -LE performance
+
+    - name: Backtrace of crashed tests
+      if: failure()
+      working-directory: ${{github.workspace}}/build
+      run: |
+        shopt -s nullglob
+        cores=(core.*)
+        if [ ${#cores[@]} -eq 0 ]; then
+          echo "No core file: the failure was an assertion or a wrong value, not a crash."
+          exit 0
+        fi
+        sudo apt-get -y install gdb
+        source ${{github.workspace}}/.github/scripts/ros-deps-env.sh "${{ matrix.ros_prefix }}"
+        source ${{github.workspace}}/.github/scripts/ros-deps-env.sh "${{github.workspace}}/gtsam-install"
+        for core in "${cores[@]}"; do
+          exe="${core#core.}"
+          exe="bin/${exe%.*}"
+          echo "::group::$core ($exe)"
+          gdb -batch -ex "thread apply all bt" "$exe" "$core" || true
+          echo "::endgroup::"
+        done

.github/workflows/cmake-ros.yml

@@ -55,8 +55,13 @@ jobs:
       if: steps.cache-testdata.outputs.cache-hit != 'true'
       shell: bash
       run: |
-        # The ROS images are slim; fetch_test_data.sh needs curl.
+        # The ROS images are slim; fetch_test_data.sh needs curl, and 7-Zip for
+        # the assets that come as an archive. The package holding it is
+        # p7zip-full up to Ubuntu 24.04 and 7zip on the newer ones.
         command -v curl >/dev/null || (apt-get update && apt-get install -y --no-install-recommends curl)
+        command -v 7z >/dev/null || command -v 7zz >/dev/null || (apt-get update && \
+          (apt-get install -y --no-install-recommends p7zip-full || \
+           apt-get install -y --no-install-recommends 7zip))
         bash scripts/fetch_test_data.sh
 
     # `${{ github.workspace }}` is expanded by the runner on the *host*

.github/workflows/coverage.yml

@@ -140,10 +140,16 @@ jobs:
           fi
           LCOV_IGNORE=(--ignore-errors gcov,source,graph,mismatch,unused)
 
+          # The whole of corelib/utilite, test objects included, not just their src/.
+          # An inline function defined in a header is emitted in whichever
+          # translation unit wins comdat folding, which is often a test one, and
+          # its counters then live in that unit's .gcda: capturing only src/
+          # reports such a function as uncovered however often it is called. The
+          # test sources themselves are dropped below, after the counters are read.
           lcov --gcov-tool "$GCOV_TOOL" "${LCOV_IGNORE[@]}" \
             --capture \
-            --directory ${{ github.workspace }}/build/corelib/src \
+            --directory ${{ github.workspace }}/build/corelib \
-            --directory ${{ github.workspace }}/build/utilite/src \
+            --directory ${{ github.workspace }}/build/utilite \
             --output-file lcov.info
 
           lcov "${LCOV_IGNORE[@]}" --extract lcov.info \
@@ -161,6 +167,7 @@ jobs:
             '*/rtflann/*' \
             '*/corelib/test/*' \
             '*/utilite/test/*' \
+            '*/_deps/*' \
             --output-file lcov.info
 
           lcov --summary lcov.info

CMakeLists.txt

@@ -22,7 +22,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 23)
-SET(RTABMAP_PATCH_VERSION 10)
+SET(RTABMAP_PATCH_VERSION 11)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
 

corelib/include/rtabmap/core/BayesFilter.h

@@ -32,7 +32,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <opencv2/core/core.hpp>
 #include <list>
+#include <map>
 #include <set>
+#include <utility>
+#include <vector>
 #include "rtabmap/utilite/UEventsHandler.h"
 #include "rtabmap/core/Parameters.h"
 
@@ -41,6 +44,12 @@ namespace rtabmap {
 class Memory;
 class Signature;
 
+namespace bayes {
+class PredictionModel;
+class DensePrediction;
+class SparsePrediction;
+}
+
 /**
  * @class BayesFilter
  * @brief Recursive Bayesian filter for loop-closure hypothesis estimation in RTAB-Map.
@@ -64,6 +73,7 @@ class Signature;
  * - @ref Parameters::kBayesPredictionLC() — transition probabilities per graph depth level.
  * - @ref Parameters::kBayesVirtualPlacePriorThr() — prior for the virtual place.
  * - @ref Parameters::kBayesFullPredictionUpdate() — regenerate the full prediction matrix each iteration.
+ * - @ref Parameters::kBayesSparsePrediction() — keep the prediction sparse and multiply it sparsely.
  *
  * @see Memory::getNeighborsId()
  * @see Rtabmap
@@ -91,12 +101,14 @@ public:
 	 * The prediction matrix is generated or updated from @ref Memory using the ids present
 	 * in @p likelihood.
 	 *
+	 * Read the result with @ref getPosteriorIds() and @ref getPosteriorValues().
+	 *
 	 * @param memory Working memory instance (must not be null).
 	 * @param likelihood Observation likelihood per signature id (must not be empty).
-	 * @return Reference to the internal posterior map (id → probability). On error (null
+	 * @return False on error (null memory, empty likelihood, or invalid prediction model),
-	 *         memory, empty likelihood, or invalid prediction model), returns the unchanged posterior.
+	 *         the posterior being left unchanged.
 	 */
-	const std::map<int, float> & computePosterior(const Memory * memory, const std::map<int, float> & likelihood);
+	bool computePosterior(const Memory * memory, const std::map<int, float> & likelihood);
 
 	/**
 	 * @brief Clears posterior, prediction matrix and cached neighbor indices.
@@ -119,16 +131,30 @@ public:
 	void setPredictionLC(const std::string & prediction);
 
 	/**
-	 * @brief Returns the current posterior probability map.
+	 * @brief The locations the posterior is over, ascending by id.
-	 * @return Map of signature id to normalized posterior probability. This is the probability to be at the given location.
+	 *
+	 * The virtual place (@ref Memory::kIdVirtual) is the first of them when it is one.
 	 */
-	const std::map<int, float> & getPosterior() const {return _posterior;}
+	const std::vector<int> & getPosteriorIds() const {return _posteriorIds;}
+
+	/**
+	 * @brief The probability of each location of @ref getPosteriorIds(), in the same order.
+	 */
+	const std::vector<float> & getPosteriorValues() const {return _posteriorValues;}
 
 	/**
 	 * @brief Returns the virtual place prior threshold.
 	 * @return Value in [0, 1] used when building the virtual place row of the prediction matrix.
 	 */
-	float getVirtualPlacePrior() const {return _virtualPlacePrior;}
+	float getVirtualPlacePrior() const;
+
+	/**
+	 * @brief Whether the prediction is being kept in its sparse form rather than as a matrix.
+	 *
+	 * False when @ref Parameters::kBayesSparsePrediction() is disabled, and over a model whose
+	 * values sum to less than 1, which leaves no zero in a column to keep out of the values.
+	 */
+	bool isPredictionSparse() const;
 
 	/**
 	 * @brief Returns the loop-closure prediction model as a vector of values.
@@ -149,6 +175,10 @@ public:
 	 * transition probabilities according to @ref getPredictionLC(). When @p ids match the
 	 * current posterior keys, the cached matrix may be returned without recomputation.
 	 *
+	 * When the prediction is being kept sparse, the matrix is expanded from it rather than
+	 * kept: it costs the memory that keeping the prediction sparse is saving, so ask for it to
+	 * read, dump or compare the prediction, not on every iteration.
+	 *
 	 * @param memory Working memory instance (must not be null).
 	 * @param ids Ordered list of signature ids (often includes @ref Memory::kIdVirtual as first element).
 	 * @return Square CV_32FC1 matrix of size ids.size() × ids.size().
@@ -163,32 +193,36 @@ public:
 
 private:
 	/**
-	 * @brief Incrementally updates the prediction matrix when ids are added or removed.
+	 * @brief Realigns the posterior with the ids of the likelihood.
+	 *
+	 * Keeps the probability of the locations that are in both. Called only when the ids differ.
 	 */
-	cv::Mat updatePrediction(const cv::Mat & oldPrediction,
+	void updatePosterior(const Memory * memory, const std::map<int, float> & likelihood);
-			const Memory * memory,
-			const std::vector<int> & oldIds,
-			const std::vector<int> & newIds);
 
 	/**
-	 * @brief Realigns the posterior map with the current set of likelihood ids.
+	 * @brief Settles whether the prediction is kept sparse, from the parameter and the model.
+	 *
+	 * Called when either of the two changes rather than on every iteration, and releases the
+	 * sparse form when the answer is no.
 	 */
-	void updatePosterior(const Memory * memory, const std::vector<int> & likelihoodIds);
+	void updateKeepSparse();
-
-	/**
-	 * @brief Normalizes one row of the prediction matrix and applies the virtual place probability.
-	 */
-	void normalize(cv::Mat & prediction, unsigned int index, float addedProbabilitiesSum, bool virtualPlaceUsed) const;
 
 private:
-	std::map<int, float> _posterior;              ///< Current posterior (signature id → probability).
+	std::vector<int> _posteriorIds;               ///< The locations the posterior is over, ascending by id.
-	cv::Mat _prediction;                          ///< Cached prediction/transition matrix.
+	std::vector<float> _posteriorValues;          ///< The probability of each of them, in the same order.
-	float _virtualPlacePrior;                     ///< Prior for virtual place transitions.
+	std::vector<int> _likelihoodIds;              ///< The ids of the likelihood of an iteration, in its order.
-	std::vector<double> _predictionLC;            ///< Model `{Vp, Lc, l1, l2, ...}`.
+	std::vector<float> _likelihoodValues;         ///< The likelihood of an iteration, in the same order.
-	bool _fullPredictionUpdate;                   ///< If true, rebuild the full prediction matrix each time.
+	std::vector<float> _priorValues;              ///< The prior of an iteration, in the same order.
-	float _totalPredictionLCValues;               ///< Sum of all values in _predictionLC.
+
-	float _predictionEpsilon;                     ///< Minimum non-zero probability in the model.
+	bayes::PredictionModel * _model;              ///< The `{Vp, Lc, l1, ...}` model and the column arithmetic of it.
-	std::map<int, std::map<int, int> > _neighborsIndex; ///< Cached neighbor margins per signature id.
+	bayes::DensePrediction * _dense;              ///< The prediction as a matrix, used when it is not kept sparse.
+	bayes::SparsePrediction * _sparse;            ///< The prediction as its values only, one column at a time.
+	std::map<int, std::map<int, int> > _neighborsIndex; ///< Cached neighbor margins per signature id, for the incremental updates.
+
+	bool _fullPredictionUpdate;                   ///< If true, rebuild the whole prediction each time.
+	bool _sparsePrediction;                       ///< Keep the prediction sparse (Bayes/SparsePrediction).
+	bool _keepSparse;                             ///< Whether it is being kept sparse: the parameter, over a model that leaves nothing sparse to keep.
+	bool _predictionChanged;                      ///< True when the prediction has to be built again.
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/Parameters.h

@@ -388,8 +388,9 @@ class RTABMAP_CORE_EXPORT Parameters
 
     // BayesFilter
     RTABMAP_PARAM(Bayes, VirtualPlacePriorThr, float, 0.9,  "Virtual place prior. Considering that we are at a new place, this is the prior probability to move again to a new place (unvisited location). The prior probability to move to a previously visited location is 1 - VirtualPlacePriorThr (split equally against all previously visited locations).");
-    RTABMAP_PARAM_STR(Bayes, PredictionLC, "0.1 0.36 0.30 0.16 0.062 0.0151 0.00255 0.000324 2.5e-05 1.3e-06 4.8e-08 1.2e-09 1.9e-11 2.2e-13 1.7e-15 8.5e-18 2.9e-20 6.9e-23", "Prediction of loop closures (Gaussian-like, here with sigma=1.6) - Format: {VirtualPlaceProb, LoopClosureProb, NeighborLvl1, NeighborLvl2, ...}. Considering we are at a previously visited location, the first value is the probability to move to a new place (unvisited location), the second value is the probability to stay at the same location, the third value is the probability to move to a neighbor or loop closure at the first depth level, the fourth value is the probability to move to a neighbor or loop closure at the second depth level, etc. If the sum of the values is not 1, the difference is normalized against all remaining visited locations. Normally, the sum of these values should be 1.");
+    RTABMAP_PARAM_STR(Bayes, PredictionLC, "0.1 0.36 0.30 0.16 0.062 0.0151 0.00255 0.000324 2.5e-05 1e-06 4.8e-08 1.2e-09 1.9e-11 2.2e-13 1.7e-15 8.5e-18 2.9e-20 6.9e-23", "Prediction of loop closures (Gaussian-like, here with sigma=1.6) - Format: {VirtualPlaceProb, LoopClosureProb, NeighborLvl1, NeighborLvl2, ...}. Considering we are at a previously visited location, the first value is the probability to move to a new place (unvisited location), the second value is the probability to stay at the same location, the third value is the probability to move to a neighbor or loop closure at the first depth level, the fourth value is the probability to move to a neighbor or loop closure at the second depth level, etc. If the sum of the values is not 1, the difference is normalized against all remaining visited locations. Normally, the sum of these values should be 1.");
     RTABMAP_PARAM(Bayes, FullPredictionUpdate, bool, false, "Regenerate all the prediction matrix on each iteration (otherwise only removed/added ids are updated).");
+    RTABMAP_PARAM(Bayes, SparsePrediction, bool, true, uFormat("Use a sparse representation of the prediction instead of a dense matrix, which significantly reduces memory usage and processing time on large maps. Ignored when the values of %s sum to less than 1, as the prediction is then not sparse.", kBayesPredictionLC().c_str()).c_str());
 
     // Verify hypotheses
     RTABMAP_PARAM(VhEp, Enabled, bool, false,       uFormat("Verify visual loop closure hypothesis by computing a fundamental matrix. This is done prior to transformation computation when %s is enabled.", kRGBDEnabled().c_str()));

corelib/src/BayesFilter.cpp

@@ -24,33 +24,40 @@ 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/BayesFilter.h"
 #include "rtabmap/core/Memory.h"
 #include "rtabmap/core/Signature.h"
 #include "rtabmap/core/Parameters.h"
-#include <iostream>
+
+#include "bayes/DensePrediction.h"
+#include "bayes/PredictionModel.h"
+#include "bayes/SparsePrediction.h"
+
 #include <set>
-#if __cplusplus >= 201103L
-#include <unordered_map>
-#include <unordered_set>
-#endif
 
 #include "rtabmap/utilite/UtiLite.h"
 
 namespace rtabmap {
 
 BayesFilter::BayesFilter(const ParametersMap & parameters) :
-	_virtualPlacePrior(Parameters::defaultBayesVirtualPlacePriorThr()),
+	_model(new bayes::PredictionModel()),
+	_dense(new bayes::DensePrediction()),
+	_sparse(new bayes::SparsePrediction()),
 	_fullPredictionUpdate(Parameters::defaultBayesFullPredictionUpdate()),
-	_totalPredictionLCValues(0.0f),
+	_sparsePrediction(Parameters::defaultBayesSparsePrediction()),
-	_predictionEpsilon(0.0f)
+	_keepSparse(false),
+	_predictionChanged(true)
 {
+	_model->setVirtualPlacePrior(Parameters::defaultBayesVirtualPlacePriorThr());
 	this->setPredictionLC(Parameters::defaultBayesPredictionLC());
 	this->parseParameters(parameters);
 }
 
-BayesFilter::~BayesFilter() {
+BayesFilter::~BayesFilter()
+{
+	delete _model;
+	delete _dense;
+	delete _sparse;
 }
 
 void BayesFilter::parseParameters(const ParametersMap & parameters)
@@ -60,371 +67,258 @@ void BayesFilter::parseParameters(const ParametersMap & parameters)
 	{
 		this->setPredictionLC((*iter).second);
 	}
-	Parameters::parse(parameters, Parameters::kBayesVirtualPlacePriorThr(), _virtualPlacePrior);
+	float virtualPlacePrior = _model->virtualPlacePrior();
+	if(Parameters::parse(parameters, Parameters::kBayesVirtualPlacePriorThr(), virtualPlacePrior))
+	{
+		UASSERT(virtualPlacePrior >= 0 && virtualPlacePrior <= 1.0f);
+		_model->setVirtualPlacePrior(virtualPlacePrior);
+	}
 	Parameters::parse(parameters, Parameters::kBayesFullPredictionUpdate(), _fullPredictionUpdate);
-
+	if(Parameters::parse(parameters, Parameters::kBayesSparsePrediction(), _sparsePrediction))
-	UASSERT(_virtualPlacePrior >= 0 && _virtualPlacePrior <= 1.0f);
+	{
+		// The sparse view is rebuilt on the next posterior if it was just enabled, and
+		// released if it was just disabled.
+		_predictionChanged = true;
+		this->updateKeepSparse();
+	}
 }
 
 // format = {Virtual place, Loop closure, level1, level2, l3, l4...}
 void BayesFilter::setPredictionLC(const std::string & prediction)
 {
-	std::list<std::string> strValues = uSplit(prediction, ' ');
+	if(_model->set(prediction))
-	if(strValues.size() < 2)
 	{
-		UERROR("The number of values < 2 (prediction=\"%s\")", prediction.c_str());
+		// A new model changes the values of the prediction, and whether any of it is worth
+		// keeping sparse.
+		_predictionChanged = true;
+		this->updateKeepSparse();
 	}
-	else
+}
-	{
-		std::vector<double> tmpValues(strValues.size());
-		int i=0;
-		bool valid = true;
-		for(std::list<std::string>::iterator iter = strValues.begin(); iter!=strValues.end(); ++iter)
-		{
-			tmpValues[i] = uStr2Float((*iter).c_str());
-			//UINFO("%d=%e", i, tmpValues[i]);
-			if(tmpValues[i] < 0.0 || tmpValues[i]>1.0)
-			{
-				valid = false;
-				break;
-			}
-			++i;
-		}
 
-		if(!valid)
+// Asked for by the parameter, and possible only over a model whose values sum to 1: below that,
-		{
+// normalize() spreads the difference over every zero of a column and there is nothing sparse
-			UERROR("The prediction is not valid (values must be between >0 && <=1) prediction=\"%s\"", prediction.c_str());
+// left to keep. Nothing else gives the sparse form up, however densely the graph is linked, so
-		}
+// that what the parameter measures is the sparse form and not a fallback to the matrix.
-		else
+void BayesFilter::updateKeepSparse()
-		{
+{
-			_predictionLC = tmpValues;
+	_keepSparse = _sparsePrediction && !_model->spreadsOverAllLocations();
-		}
+	if(_sparsePrediction && !_keepSparse)
-	}
-	_totalPredictionLCValues = 0.0f;
-	for(unsigned int j=0; j<_predictionLC.size(); ++j)
 	{
-		_totalPredictionLCValues += _predictionLC[j];
+		UWARN("%s is enabled but the values of %s sum to %f, less than 1: the difference is "
-		if(j==0 || _predictionLC[j] < _predictionEpsilon)
+			  "spread over every location, which leaves no zero in a column for the sparse form "
-		{
+			  "to keep out, so the prediction is held as a matrix instead.",
-			_predictionEpsilon = _predictionLC[j];
+			  Parameters::kBayesSparsePrediction().c_str(),
-		}
+			  Parameters::kBayesPredictionLC().c_str(), _model->total());
 	}
-	if(!_predictionLC.empty())
+	if(!_keepSparse)
 	{
-		UDEBUG("predictionEpsilon = %f", _predictionEpsilon);
+		_sparse->clear();
 	}
 }
 
 const std::vector<double> & BayesFilter::getPredictionLC() const
 {
 	// {Vp, Lc, l1, l2, l3, l4...}
-	return _predictionLC;
+	return _model->values();
 }
 
 std::string BayesFilter::getPredictionLCStr() const
 {
-	std::string values;
+	return _model->str();
-	for(unsigned int i=0; i<_predictionLC.size(); ++i)
+}
-	{
+
-		values.append(uNumber2Str(_predictionLC[i]));
+float BayesFilter::getVirtualPlacePrior() const
-		if(i+1 < _predictionLC.size())
+{
-		{
+	return _model->virtualPlacePrior();
-			values.append(" ");
+}
-		}
+
-	}
+bool BayesFilter::isPredictionSparse() const
-	return values;
+{
+	return !_sparse->empty();
 }
 
 void BayesFilter::reset()
 {
-	_posterior.clear();
+	_posteriorIds.clear();
-	_prediction = cv::Mat();
+	_posteriorValues.clear();
+	_dense->clear();
+	_sparse->clear();
+	_predictionChanged = true;
 	_neighborsIndex.clear();
 }
 
-const std::map<int, float> & BayesFilter::computePosterior(const Memory * memory, const std::map<int, float> & likelihood)
+bool BayesFilter::computePosterior(const Memory * memory, const std::map<int, float> & likelihood)
 {
 	ULOGGER_DEBUG("");
 
 	if(!memory)
 	{
 		ULOGGER_ERROR("Memory is Null!");
-		return _posterior;
+		return false;
 	}
 
 	if(!likelihood.size())
 	{
 		ULOGGER_ERROR("likelihood is empty!");
-		return _posterior;
+		return false;
 	}
 
-	if(_predictionLC.size() < 2)
+	UASSERT(_model->valid());
-	{
-		ULOGGER_ERROR("Prediction is not valid!");
-		return _posterior;
-	}
 
 	UTimer timer;
 	timer.start();
 
-	cv::Mat prior;
+	// One walk of the likelihood: its values into a vector, and its ids against the ones the
-	cv::Mat posterior;
+	// posterior is indexed by. Everything below then works on vectors.
+	_likelihoodIds.resize(likelihood.size());
+	_likelihoodValues.resize(likelihood.size());
+	bool sameIds = _posteriorIds.size() == likelihood.size();
+	{
+		size_t k = 0;
+		for(std::map<int, float>::const_iterator iter=likelihood.begin(); iter!=likelihood.end(); ++iter, ++k)
+		{
+			_likelihoodIds[k] = iter->first;
+			_likelihoodValues[k] = iter->second;
+			if(sameIds && _posteriorIds[k] != iter->first)
+			{
+				sameIds = false;
+			}
+		}
+	}
+	const std::vector<int> & ids = _likelihoodIds;
 
-	float sum = 0;
-	int j=0;
 	// Recursive Bayes estimation...
 	// STEP 1 - Prediction : Prior*lastPosterior
-	_prediction = this->generatePrediction(memory, uKeys(likelihood));
+	//
+	// The prediction is kept in its sparse form only, the matrix never being allocated:
+	// built once, then carried over to the locations of the next iteration. Over a fixed
+	// graph nothing changes and there is nothing to do; while mapping, the appended
+	// locations reach only a few of the columns and only those are built again. A location
+	// leaving the working memory shifts the index of every one after it, and is answered by
+	// building the prediction again, which is what the dense update does then as well.
+	if(!sameIds)
+	{
+		_predictionChanged = true;
+	}
+	if(_keepSparse)
+	{
+		// Nothing to do at all when neither the prediction nor the locations changed.
+		if(_predictionChanged || _sparse->ids() != ids)
+		{
+			// The neighborhoods are kept only when locations can be added, which is what the
+			// update needs them for: over a fixed graph one per location is as much memory
+			// again as the values of the prediction.
+			if(_fullPredictionUpdate || !_sparse->update(*_model, memory, ids, _neighborsIndex))
+			{
+				_sparse->generate(*_model, memory, ids,
+						memory->isIncremental() ? &_neighborsIndex : 0);
+			}
+		}
+		UDEBUG("STEP1-generate prior=%fs, values=%d", timer.ticks(), (int)_sparse->values());
 
-	UDEBUG("STEP1-generate prior=%fs, rows=%d, cols=%d", timer.ticks(), _prediction.rows, _prediction.cols);
+		// The matrix is released as soon as the sparse form takes over. It is built for the
-	//std::cout << "Prediction=" << _prediction << std::endl;
+		// locations of the iteration it was built on, and the locations move on while the
+		// sparse form is the one being used, so it can neither be multiplied nor carried over
+		// once the sparse form gives the prediction back. A fallback to the matrix builds it
+		// again.
+		_dense->clear();
+	}
+	else
+	{
+		_sparse->clear();
+		if(_predictionChanged || _dense->empty())
+		{
+			// Only when it has to be: over a fixed graph the matrix of the last iteration is
+			// the one this iteration wants.
+			_dense->generate(*_model, memory, ids, _fullPredictionUpdate, &_neighborsIndex);
+		}
+		UDEBUG("STEP1-generate prior=%fs, rows=%d, cols=%d", timer.ticks(),
+				_dense->matrix().rows, _dense->matrix().cols);
+		//std::cout << "Prediction=" << _dense->matrix() << std::endl;
+	}
+	// Cleared once, after whichever of the two built it: the sparse form, or the matrix when
+	// the prediction is not kept sparse.
+	_predictionChanged = false;
 
 	// Adjust the last posterior if some images were
-	// reactivated or removed from the working memory
+	// reactivated or removed from the working memory. After the prediction, which is built
-	posterior = cv::Mat(likelihood.size(), 1, CV_32FC1);
+	// against the ids the posterior still holds from the last iteration.
-	this->updatePosterior(memory, uKeys(likelihood));
+	if(!sameIds)
-	j=0;
-	for(std::map<int, float>::const_iterator i=_posterior.begin(); i!= _posterior.end(); ++i)
 	{
-		((float*)posterior.data)[j++] = (*i).second;
+		this->updatePosterior(memory, likelihood);
 	}
-	ULOGGER_DEBUG("STEP1-update posterior=%fs, posterior rows=%d, _posterior size=%d", timer.ticks(), posterior.rows, (int)_posterior.size());
+	UASSERT(_posteriorValues.size() == likelihood.size());
-	//std::cout << "LastPosterior=" << posterior << std::endl;
+	ULOGGER_DEBUG("STEP1-update posterior=%fs, posterior size=%d", timer.ticks(), (int)_posteriorValues.size());
 
 	// Multiply prediction matrix with the last posterior
 	// (m,m) X (m,1) = (m,1)
-	prior = _prediction * posterior;
+	// Held sparse, or as the matrix when updateKeepSparse() gave the sparse form up.
-	ULOGGER_DEBUG("STEP1-matrix mult time=%fs", timer.ticks());
+	const bool sparse = !_sparse->empty();
+	if(sparse)
+	{
+		_sparse->multiply(_posteriorValues, _priorValues);
+	}
+	else
+	{
+		_dense->multiply(_posteriorValues, _priorValues);
+	}
+	const float * priorPtr = &_priorValues[0];
+	ULOGGER_DEBUG("STEP1-matrix mult time=%fs (sparse=%d)", timer.ticks(), sparse?1:0);
 	//std::cout << "ResultingPrior=" << prior << std::endl;
 
-	ULOGGER_DEBUG("STEP1-matrix mult time=%fs", timer.ticks());
-	std::vector<float> likelihoodValues = uValues(likelihood);
-	//std::cout << "Likelihood=" << cv::Mat(likelihoodValues) << std::endl;
-
 	// STEP 2 - Update : Multiply with observations (likelihood)
-	j=0;
+	// The likelihood, the posterior and the prior are all indexed the same way, so the three
-	for(std::map<int, float>::const_iterator i=likelihood.begin(); i!= likelihood.end(); ++i)
+	// are walked side by side.
+	float sum = 0;
+	for(size_t k=0; k<_posteriorValues.size(); ++k)
 	{
-		std::map<int, float>::iterator p =_posterior.find((*i).first);
+		_posteriorValues[k] = _likelihoodValues[k] * priorPtr[k];
-		if(p!= _posterior.end())
+		sum += _posteriorValues[k];
-		{
-			(*p).second = (*i).second * ((float*)prior.data)[j++];
-			sum+=(*p).second;
-		}
-		else
-		{
-			ULOGGER_ERROR("Problem1! can't find id=%d", (*i).first);
-		}
 	}
 	ULOGGER_DEBUG("STEP2-likelihood time=%fs", timer.ticks());
-	//std::cout << "Posterior (before normalization)=" << _posterior << std::endl;
 
 	// Normalize
 	ULOGGER_DEBUG("sum=%f", sum);
 	if(sum != 0)
 	{
-		for(std::map<int, float>::iterator i=_posterior.begin(); i!= _posterior.end(); ++i)
+		for(size_t k=0; k<_posteriorValues.size(); ++k)
 		{
-			(*i).second /= sum;
+			_posteriorValues[k] /= sum;
 		}
 	}
+
 	ULOGGER_DEBUG("normalize time=%fs", timer.ticks());
-	//std::cout << "Posterior=" << _posterior << std::endl;
+	return true;
-
-	return _posterior;
-}
-
-float addNeighborProb(cv::Mat & prediction,
-			unsigned int col,
-			const std::map<int, int> & neighbors,
-			const std::vector<double> & predictionLC,
-#if __cplusplus >= 201103L
-			const std::unordered_map<int, int> & idToIndex
-#else
-			const std::map<int, int> & idToIndex
-#endif
-			)
-{
-	UASSERT(col < (unsigned int)prediction.cols &&
-			col < (unsigned int)prediction.rows);
-
-	float sum=0.0f;
-	float * dataPtr = (float*)prediction.data;
-	for(std::map<int, int>::const_iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
-	{
-		if(iter->first>=0)
-		{
-#if __cplusplus >= 201103L
-			std::unordered_map<int, int>::const_iterator jter = idToIndex.find(iter->first);
-#else
-			std::map<int, int>::const_iterator jter = idToIndex.find(iter->first);
-#endif
-			if(jter != idToIndex.end())
-			{
-				UASSERT((iter->second+1) < (int)predictionLC.size());
-				sum += dataPtr[col + jter->second*prediction.cols] = predictionLC[iter->second+1];
-			}
-		}
-	}
-	return sum;
 }
 
 cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector<int> & ids)
 {
-	std::vector<int> oldIds = uKeys(_posterior);
+	if(!_sparse->empty() && _sparse->ids() == ids)
-	if(oldIds.size() == ids.size() &&
-		memcmp(oldIds.data(), ids.data(), oldIds.size()*sizeof(int)) == 0)
 	{
-		return _prediction;
+		// Expanded from the sparse form, which holds the same prediction. The matrix costs
+		// what keeping it sparse is saving, so it is built to be read and not kept.
+		return _sparse->toMatrix();
 	}
-
+	if(!_dense->empty() && _dense->ids() == ids)
-	if(!_fullPredictionUpdate && !_prediction.empty())
 	{
-		return updatePrediction(_prediction, memory, oldIds, ids);
+		return _dense->matrix();
 	}
-	UDEBUG("");
+	UASSERT(memory && _model->valid() && ids.size());
-
+	return _dense->generate(*_model, memory, ids, _fullPredictionUpdate, &_neighborsIndex);
-	UASSERT(memory &&
-		   _predictionLC.size() >= 2 &&
-		   ids.size());
-
-	UTimer timer;
-	timer.start();
-	UTimer timerGlobal;
-	timerGlobal.start();
-
-#if __cplusplus >= 201103L
-	std::unordered_map<int,int> idToIndexMap;
-	idToIndexMap.reserve(ids.size());
-#else
-	std::map<int,int> idToIndexMap;
-#endif
-	for(unsigned int i=0; i<ids.size(); ++i)
-	{
-		if(ids[i]>0)
-		{
-			idToIndexMap[ids[i]] = i;
-		}
-	}
-
-
-	//int rows = prediction.rows;
-	cv::Mat prediction = cv::Mat::zeros(ids.size(), ids.size(), CV_32FC1);
-	int cols = prediction.cols;
-
-	// Each prior is a column vector
-	UDEBUG("_predictionLC.size()=%d",(int)_predictionLC.size());
-	std::set<int> idsDone;
-
-	for(unsigned int i=0; i<ids.size(); ++i)
-	{
-		if(idsDone.find(ids[i]) == idsDone.end())
-		{
-			if(ids[i] > 0)
-			{
-				// Set high values (gaussians curves) to loop closure neighbors
-
-				// ADD prob for each neighbors
-				std::map<int, int> neighbors = memory->getNeighborsId(ids[i], _predictionLC.size()-1, 0, false, false, true, true);
-
-				if(!_fullPredictionUpdate)
-				{
-					uInsert(_neighborsIndex, std::make_pair(ids[i], neighbors));
-				}
-
-				std::list<int> idsLoopMargin;
-				//filter neighbors in STM
-				for(std::map<int, int>::iterator iter=neighbors.begin(); iter!=neighbors.end();)
-				{
-					if(memory->isInSTM(iter->first))
-					{
-						neighbors.erase(iter++);
-					}
-					else
-					{
-						if(iter->second == 0 && idToIndexMap.find(iter->first)!=idToIndexMap.end())
-						{
-							idsLoopMargin.push_back(iter->first);
-						}
-						++iter;
-					}
-				}
-
-				// should at least have 1 id in idsMarginLoop
-				if(idsLoopMargin.size() == 0)
-				{
-					UFATAL("No 0 margin neighbor for signature %d !?!?", ids[i]);
-				}
-
-				// same neighbor tree for loop signatures (margin = 0)
-				for(std::list<int>::iterator iter = idsLoopMargin.begin(); iter!=idsLoopMargin.end(); ++iter)
-				{
-					if(!_fullPredictionUpdate)
-					{
-						uInsert(_neighborsIndex, std::make_pair(*iter, neighbors));
-					}
-
-					float sum = 0.0f; // sum values added
-					int index = idToIndexMap.at(*iter);
-					sum += addNeighborProb(prediction, index, neighbors, _predictionLC, idToIndexMap);
-					idsDone.insert(*iter);
-					this->normalize(prediction, index, sum, ids[0]<0);
-				}
-			}
-			else
-			{
-				// Set the virtual place prior
-				if(_virtualPlacePrior > 0)
-				{
-					if(cols>1) // The first must be the virtual place
-					{
-						((float*)prediction.data)[i] = _virtualPlacePrior;
-						float val = (1.0-_virtualPlacePrior)/(cols-1);
-						for(int j=1; j<cols; j++)
-						{
-							((float*)prediction.data)[i + j*cols] = val;
-						}
-					}
-					else if(cols>0)
-					{
-						((float*)prediction.data)[i] = 1;
-					}
-				}
-				else
-				{
-					// Only for some tests...
-					// when _virtualPlacePrior=0, set all priors to the same value
-					if(cols>1)
-					{
-						float val = 1.0/cols;
-						for(int j=0; j<cols; j++)
-						{
-							((float*)prediction.data)[i + j*cols] = val;
-						}
-					}
-					else if(cols>0)
-					{
-						((float*)prediction.data)[i] = 1;
-					}
-				}
-			}
-		}
-	}
-
-	ULOGGER_DEBUG("time = %fs", timerGlobal.ticks());
-
-	return prediction;
 }
 
 unsigned long BayesFilter::getMemoryUsed() const
 {
 	long memoryUsage = sizeof(BayesFilter);
-	memoryUsage += _posterior.size() * (sizeof(float)+sizeof(int)+sizeof(std::map<int, float>::iterator)) + sizeof(std::map<int, float>);
+	memoryUsage += _dense->memoryUsed();
-	if(!_prediction.empty())
+	memoryUsage += _sparse->memoryUsed();
-	{
+	memoryUsage += _model->memoryUsed();
-		memoryUsage += _prediction.total() * _prediction.elemSize();
+	// The vectors an iteration works on, indexed the same way as the posterior.
-	}
+	memoryUsage += _posteriorIds.capacity() * sizeof(int);
-	memoryUsage += _predictionLC.size() * sizeof(double);
+	memoryUsage += _posteriorValues.capacity() * sizeof(float);
+	memoryUsage += _likelihoodIds.capacity() * sizeof(int);
+	memoryUsage += _likelihoodValues.capacity() * sizeof(float);
+	memoryUsage += _priorValues.capacity() * sizeof(float);
 	memoryUsage += _neighborsIndex.size() * (sizeof(int)+sizeof(std::map<int, int>)+sizeof(std::map<int, std::map<int, int> >::iterator)) + sizeof(std::map<int, std::map<int, int> >);
 	for(std::map<int, std::map<int, int> >::const_iterator iter=_neighborsIndex.begin(); iter!=_neighborsIndex.end(); ++iter)
 	{
@@ -433,308 +327,36 @@ unsigned long BayesFilter::getMemoryUsed() const
 	return memoryUsage;
 }
 
-void BayesFilter::normalize(cv::Mat & prediction, unsigned int index, float addedProbabilitiesSum, bool virtualPlaceUsed) const
+void BayesFilter::updatePosterior(const Memory * memory, const std::map<int, float> & likelihood)
-{
-	UASSERT(index < (unsigned int)prediction.rows && index < (unsigned int)prediction.cols);
-
-	int cols = prediction.cols;
-	// ADD values of not found neighbors to loop closure
-	if(addedProbabilitiesSum < _totalPredictionLCValues-_predictionLC[0])
-	{
-		float delta = _totalPredictionLCValues-_predictionLC[0]-addedProbabilitiesSum;
-		((float*)prediction.data)[index + index*cols] += delta;
-		addedProbabilitiesSum+=delta;
-	}
-
-	float allOtherPlacesValue = 0;
-	if(_totalPredictionLCValues < 1)
-	{
-		allOtherPlacesValue = 1.0f - _totalPredictionLCValues;
-	}
-
-	// Set all loop events to small values according to the model
-	if(allOtherPlacesValue > 0 && cols>1)
-	{
-		float value = allOtherPlacesValue / float(cols - 1);
-		for(int j=virtualPlaceUsed?1:0; j<cols; ++j)
-		{
-			if(((float*)prediction.data)[index + j*cols] == 0)
-			{
-				((float*)prediction.data)[index + j*cols] = value;
-				addedProbabilitiesSum += ((float*)prediction.data)[index + j*cols];
-			}
-		}
-	}
-
-	//normalize this row
-	float maxNorm = 1 - (virtualPlaceUsed?_predictionLC[0]:0); // 1 - virtual place probability
-	if(addedProbabilitiesSum<maxNorm-0.0001 || addedProbabilitiesSum>maxNorm+0.0001)
-	{
-		for(int j=virtualPlaceUsed?1:0; j<cols; ++j)
-		{
-			((float*)prediction.data)[index + j*cols] *= maxNorm / addedProbabilitiesSum;
-			if(((float*)prediction.data)[index + j*cols] < _predictionEpsilon)
-			{
-				((float*)prediction.data)[index + j*cols] = 0.0f;
-			}
-		}
-		addedProbabilitiesSum = maxNorm;
-	}
-
-	// ADD virtual place prob
-	if(virtualPlaceUsed)
-	{
-		((float*)prediction.data)[index] = _predictionLC[0];
-		addedProbabilitiesSum += ((float*)prediction.data)[index];
-	}
-
-	//debug
-	//for(int j=0; j<cols; ++j)
-	//{
-	//	ULOGGER_DEBUG("test col=%d = %f", i, prediction.data.fl[i + j*cols]);
-	//}
-
-	if(addedProbabilitiesSum<0.99 || addedProbabilitiesSum > 1.01)
-	{
-		UWARN("Prediction is not normalized sum=%f", addedProbabilitiesSum);
-	}
-}
-
-cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
-		const Memory * memory,
-		const std::vector<int> & oldIds,
-		const std::vector<int> & newIds)
-{
-	UTimer timer;
-	UDEBUG("");
-
-	UASSERT(memory &&
-		oldIds.size() &&
-		newIds.size() &&
-		oldIds.size() == (unsigned int)oldPrediction.cols &&
-		oldIds.size() == (unsigned int)oldPrediction.rows);
-
-	cv::Mat prediction = cv::Mat::zeros(newIds.size(), newIds.size(), CV_32FC1);
-	UDEBUG("time creating prediction = %fs", timer.restart());
-
-	// Create id to index maps
-#if __cplusplus >= 201103L
-	std::unordered_set<int> oldIdsSet(oldIds.begin(), oldIds.end());
-#else
-	std::set<int> oldIdsSet(oldIds.begin(), oldIds.end());
-#endif
-	UDEBUG("time creating old ids set = %fs", timer.restart());
-
-#if __cplusplus >= 201103L
-	std::unordered_map<int,int> newIdToIndexMap;
-	newIdToIndexMap.reserve(newIds.size());
-#else
-	std::map<int,int> newIdToIndexMap;
-#endif
-	for(unsigned int i=0; i<newIds.size(); ++i)
-	{
-		if(newIds[i]>0)
-		{
-			newIdToIndexMap[newIds[i]] = i;
-		}
-	}
-
-	UDEBUG("time creating id-index vector (size=%d oldIds.back()=%d newIds.back()=%d) = %fs", (int)newIdToIndexMap.size(), oldIds.back(), newIds.back(), timer.restart());
-
-	//Get removed ids
-	std::set<int> removedIds;
-	for(unsigned int i=0; i<oldIds.size(); ++i)
-	{
-		if(oldIds[i] > 0 && newIdToIndexMap.find(oldIds[i]) == newIdToIndexMap.end())
-		{
-			removedIds.insert(removedIds.end(), oldIds[i]);
-			_neighborsIndex.erase(oldIds[i]);
-			UDEBUG("removed id=%d at oldIndex=%d", oldIds[i], i);
-		}
-	}
-	UDEBUG("time getting removed ids = %fs", timer.restart());
-
-	bool oldAllCopied = false;
-	if(removedIds.empty() &&
-		newIds.size() > oldIds.size() &&
-		memcmp(oldIds.data(), newIds.data(), oldIds.size()*sizeof(int)) == 0)
-	{
-		oldPrediction.copyTo(cv::Mat(prediction, cv::Range(0, oldPrediction.rows), cv::Range(0, oldPrediction.cols)));
-		oldAllCopied = true;
-		UDEBUG("Copied all old prediction: = %fs", timer.ticks());
-	}
-
-	int added = 0;
-	// get ids to update
-	std::set<int> idsToUpdate;
-	for(unsigned int i=0; i<oldIds.size() || i<newIds.size(); ++i)
-	{
-		if(i<oldIds.size())
-		{
-			if(removedIds.find(oldIds[i]) != removedIds.end())
-			{
-				unsigned int cols = oldPrediction.cols;
-				int count = 0;
-				for(unsigned int j=0; j<cols; ++j)
-				{
-					if(j!=i && removedIds.find(oldIds[j]) == removedIds.end())
-					{
-						//UDEBUG("to update id=%d from id=%d removed (value=%f)", oldIds[j], oldIds[i], ((const float *)oldPrediction.data)[i + j*cols]);
-						idsToUpdate.insert(oldIds[j]);
-						++count;
-					}
-				}
-				UDEBUG("From removed id %d, %d neighbors to update.", oldIds[i], count);
-			}
-		}
-		if(i<newIds.size() && oldIdsSet.find(newIds[i]) == oldIdsSet.end())
-		{
-			if(_neighborsIndex.find(newIds[i]) == _neighborsIndex.end())
-			{
-				std::map<int, int> neighbors = memory->getNeighborsId(newIds[i], _predictionLC.size()-1, 0, false, false, true, true);
-
-				for(std::map<int, int>::iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
-				{
-					std::map<int, std::map<int, int> >::iterator jter = _neighborsIndex.find(iter->first);
-					if(jter != _neighborsIndex.end())
-					{
-						uInsert(jter->second, std::make_pair(newIds[i], iter->second));
-					}
-				}
-				_neighborsIndex.insert(std::make_pair(newIds[i], neighbors));
-			}
-			const std::map<int, int> & neighbors = _neighborsIndex.at(newIds[i]);
-			//std::map<int, int> neighbors = memory->getNeighborsId(newIds[i], _predictionLC.size()-1, 0, false, false, true, true);
-
-			float sum = addNeighborProb(prediction, i, neighbors, _predictionLC, newIdToIndexMap);
-			this->normalize(prediction, i, sum, newIds[0]<0);
-
-			++added;
-			int count = 0;
-			for(std::map<int,int>::const_iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
-			{
-				if(oldIdsSet.find(iter->first)!=oldIdsSet.end() &&
-				   removedIds.find(iter->first) == removedIds.end())
-				{
-					idsToUpdate.insert(iter->first);
-					++count;
-				}
-			}
-			UDEBUG("From added id %d, %d neighbors to update.", newIds[i], count);
-		}
-	}
-	UDEBUG("time getting %d ids to update = %fs", (int)idsToUpdate.size(), timer.restart());
-
-	UTimer t1;
-	double e0=0,e1=0, e2=0, e3=0, e4=0;
-	// update modified/added ids
-	int modified = 0;
-	for(std::set<int>::iterator iter = idsToUpdate.begin(); iter!=idsToUpdate.end(); ++iter)
-	{
-		int id = *iter;
-		if(id > 0)
-		{
-			int index = newIdToIndexMap.at(id);
-
-			e0 = t1.ticks();
-			std::map<int, std::map<int, int> >::iterator kter = _neighborsIndex.find(id);
-			UASSERT_MSG(kter != _neighborsIndex.end(), uFormat("Did not find %d (current index size=%d)", id, (int)_neighborsIndex.size()).c_str());
-			const std::map<int, int> & neighbors = kter->second;
-			//std::map<int, int> neighbors = memory->getNeighborsId(id, _predictionLC.size()-1, 0, false, false, true, true);
-			e1+=t1.ticks();
-
-			float sum = addNeighborProb(prediction, index, neighbors, _predictionLC, newIdToIndexMap);
-			e3+=t1.ticks();
-
-			this->normalize(prediction, index, sum, newIds[0]<0);
-			++modified;
-			e4+=t1.ticks();
-		}
-	}
-	UDEBUG("time updating modified/added %d ids = %fs (e0=%f e1=%f e2=%f e3=%f e4=%f)", (int)idsToUpdate.size(), timer.restart(), e0, e1, e2, e3, e4);
-
-	int copied = 0;
-	if(!oldAllCopied)
-	{
-		//UDEBUG("oldIds.size()=%d, oldPrediction.cols=%d, oldPrediction.rows=%d", oldIds.size(), oldPrediction.cols, oldPrediction.rows);
-		//UDEBUG("newIdToIndexMap.size()=%d, prediction.cols=%d, prediction.rows=%d", newIdToIndexMap.size(), prediction.cols, prediction.rows);
-		// copy not changed probabilities
-		for(unsigned int i=0; i<oldIds.size(); ++i)
-		{
-			if(oldIds[i]>0 && removedIds.find(oldIds[i]) == removedIds.end() && idsToUpdate.find(oldIds[i]) == idsToUpdate.end())
-			{
-				for(int j=0; j<oldPrediction.cols; ++j)
-				{
-					if(oldIds[j]>0 && removedIds.find(oldIds[j]) == removedIds.end())
-					{
-						//UDEBUG("i=%d, j=%d", i, j);
-						//UDEBUG("oldIds[i]=%d, oldIds[j]=%d", oldIds[i], oldIds[j]);
-						//UDEBUG("newIdToIndexMap.at(oldIds[i])=%d", newIdToIndexMap.at(oldIds[i]));
-						//UDEBUG("newIdToIndexMap.at(oldIds[j])=%d", newIdToIndexMap.at(oldIds[j]));
-						float v = ((const float *)oldPrediction.data)[i + j*oldPrediction.cols];
-						int ii = newIdToIndexMap.at(oldIds[i]);
-						int jj = newIdToIndexMap.at(oldIds[j]);
-						((float *)prediction.data)[ii + jj*prediction.cols] = v;
-						//if(ii != jj)
-						//{
-						//	((float *)prediction.data)[jj + ii*prediction.cols] = v;
-						//}
-					}
-				}
-				++copied;
-			}
-		}
-		UDEBUG("time copying = %fs", timer.restart());
-	}
-
-	//update virtual place
-	if(newIds[0] < 0)
-	{
-		if(prediction.cols>1) // The first must be the virtual place
-		{
-			((float*)prediction.data)[0] = _virtualPlacePrior;
-			float val = (1.0-_virtualPlacePrior)/(prediction.cols-1);
-			for(int j=1; j<prediction.cols; j++)
-			{
-				((float*)prediction.data)[j*prediction.cols] = val;
-				((float*)prediction.data)[j] = _predictionLC[0];
-			}
-		}
-		else if(prediction.cols>0)
-		{
-			((float*)prediction.data)[0] = 1;
-		}
-	}
-	UDEBUG("time updating virtual place = %fs", timer.restart());
-
-	UDEBUG("Modified=%d, Added=%d, Copied=%d", modified, added, copied);
-	return prediction;
-}
-
-void BayesFilter::updatePosterior(const Memory * memory, const std::vector<int> & likelihoodIds)
 {
 	ULOGGER_DEBUG("");
-	std::map<int, float> newPosterior;
+	const bool wasEmpty = _posteriorIds.empty();
-	for(std::vector<int>::const_iterator i=likelihoodIds.begin(); i != likelihoodIds.end(); ++i)
+	std::vector<int> ids;
+	std::vector<float> values;
+	ids.reserve(likelihood.size());
+	values.reserve(likelihood.size());
+	// Both the likelihood and the posterior are ascending by id, so the two are merged in one
+	// walk, k only ever moving forward: for each location of the likelihood, advance the
+	// posterior up to it. A location in both keeps its probability, a location removed from
+	// the working memory is left behind, and a location that came back gets 0 (1 on the very
+	// first iteration, where the posterior starts uniform).
+	size_t k = 0;
+	for(std::map<int, float>::const_iterator iter=likelihood.begin(); iter!=likelihood.end(); ++iter)
 	{
-		std::map<int, float>::iterator post = _posterior.find(*i);
+		while(k < _posteriorIds.size() && _posteriorIds[k] < iter->first)
-		if(post == _posterior.end())
 		{
-			if(_posterior.size() == 0)
+			++k;
-			{
-				newPosterior.insert(std::pair<int, float>(*i, 1));
-			}
-			else
-			{
-				newPosterior.insert(std::pair<int, float>(*i, 0));
-			}
 		}
-		else
+		float value = wasEmpty ? 1.0f : 0.0f;
+		if(k < _posteriorIds.size() && _posteriorIds[k] == iter->first)
 		{
-			newPosterior.insert(std::pair<int, float>((*post).first, (*post).second));
+			value = _posteriorValues[k];
 		}
+		ids.push_back(iter->first);
+		values.push_back(value);
 	}
-	_posterior = newPosterior;
+	_posteriorIds.swap(ids);
+	_posteriorValues.swap(values);
 }
 
 } // namespace rtabmap

corelib/src/CMakeLists.txt

@@ -47,6 +47,9 @@ SET(SRC_FILES
     VisualWord.cpp
     VWDictionary.cpp
     BayesFilter.cpp
+    bayes/PredictionModel.cpp
+    bayes/DensePrediction.cpp
+    bayes/SparsePrediction.cpp
     Parameters.cpp
     Signature.cpp
     Features2d.cpp

corelib/src/Rtabmap.cpp

@@ -1258,7 +1258,6 @@ bool Rtabmap::process(
 	std::map<int, float> adjustedLikelihood;
 	std::map<int, float> likelihood;
 	std::map<int, int> weights;
-	std::map<int, float> posterior;
 	std::list<std::pair<int, float> > reactivateHypotheses;
 
 	std::map<int, int> childCount;
@@ -2138,7 +2137,7 @@ bool Rtabmap::process(
 			ULOGGER_INFO("getting posterior...");
 
 			// Compute the posterior
-			posterior = _bayesFilter->computePosterior(_memory, likelihood);
+			_bayesFilter->computePosterior(_memory, likelihood);
 			timePosteriorCalculation = timer.ticks();
 			ULOGGER_INFO("timePosteriorCalculation=%fs",timePosteriorCalculation);
 
@@ -2152,17 +2151,20 @@ bool Rtabmap::process(
 			// Select the highest hypothesis
 			//============================================================
 			ULOGGER_INFO("creating hypotheses...");
-			if(posterior.size())
+			const std::vector<int> & posteriorIds = _bayesFilter->getPosteriorIds();
+			const std::vector<float> & posteriorValues = _bayesFilter->getPosteriorValues();
+			if(posteriorIds.size())
 			{
-				for(std::map<int, float>::const_reverse_iterator iter = posterior.rbegin(); iter != posterior.rend(); ++iter)
+				// Highest id first, so the highest id wins on equal probabilities.
+				for(size_t i=posteriorIds.size(); i-- > 0;)
 				{
-					if(iter->first > 0 && iter->second > _highestHypothesis.second)
+					if(posteriorIds[i] > 0 && posteriorValues[i] > _highestHypothesis.second)
 					{
-						_highestHypothesis = *iter;
+						_highestHypothesis = std::make_pair(posteriorIds[i], posteriorValues[i]);
 					}
 				}
 				// With the virtual place, use sum of LC probabilities (1 - virtual place hypothesis).
-				_highestHypothesis.second = 1-posterior.begin()->second;
+				_highestHypothesis.second = 1-posteriorValues[0];
 			}
 			timeHypothesesCreation = timer.ticks();
 			ULOGGER_INFO("Highest hypothesis=%d, value=%f, timeHypothesesCreation=%fs", _highestHypothesis.first, _highestHypothesis.second, timeHypothesesCreation);
@@ -2193,7 +2195,7 @@ bool Rtabmap::process(
 				if(_highestHypothesis.second >= loopThr)
 				{
 					rejectedLoopClosure = true;
-					if(posterior.size() <= 2 && loopThr>0.0f)
+					if(_bayesFilter->getPosteriorIds().size() <= 2 && loopThr>0.0f)
 					{
 						// Ignore loop closure if there is only one loop closure hypothesis
 						UDEBUG("rejected hypothesis: single hypothesis");
@@ -4194,7 +4196,9 @@ bool Rtabmap::process(
 	}
 
 	// Posterior is empty if a bad signature is detected
-	float vpHypothesis = posterior.size()?posterior.at(Memory::kIdVirtual):0.0f;
+	// The virtual place is the first location of the posterior when it is one of them.
+	const std::vector<int> & vpIds = _bayesFilter->getPosteriorIds();
+	float vpHypothesis = (vpIds.size() && vpIds[0]==Memory::kIdVirtual)?_bayesFilter->getPosteriorValues()[0]:0.0f;
 	int loopId = _loopClosureHypothesis.first>0?_loopClosureHypothesis.first:lastProximitySpaceClosureId;
 
 	// prepare statistics
@@ -4411,6 +4415,13 @@ bool Rtabmap::process(
 				statistics_.setWeights(weights);
 				if(_publishPdf)
 				{
+					const std::vector<int> & ids = _bayesFilter->getPosteriorIds();
+					const std::vector<float> & values = _bayesFilter->getPosteriorValues();
+					std::map<int, float> posterior;
+					for(size_t i=0; i<ids.size(); ++i)
+					{
+						posterior.insert(posterior.end(), std::make_pair(ids[i], values[i]));
+					}
 					statistics_.setPosterior(posterior);
 				}
 				if(_publishLikelihood)

corelib/src/bayes/DensePrediction.cpp

@@ -0,0 +1,369 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "bayes/DensePrediction.h"
+
+#include "rtabmap/core/Memory.h"
+#include "rtabmap/utilite/UtiLite.h"
+
+#include <set>
+#if __cplusplus >= 201103L
+#include <unordered_set>
+#endif
+
+namespace rtabmap {
+namespace bayes {
+
+const cv::Mat & DensePrediction::generate(const PredictionModel & model, const Memory * memory,
+		const std::vector<int> & ids, bool fullUpdate, NeighborsCache * cache)
+{
+	// The update carries the matrix already there over, so it can only be done against the
+	// locations that matrix is built for. There is none to carry over when the sparse form has
+	// been used since, or when the model changed, and every column is built again.
+	if(!fullUpdate && !matrix_.empty() && ids_.size() == (size_t)matrix_.cols)
+	{
+		matrix_ = this->update(model, memory, ids_, ids, cache);
+	}
+	else
+	{
+		matrix_ = this->generateFull(model, memory, ids, fullUpdate?0:cache);
+	}
+	ids_ = ids;
+	return matrix_;
+}
+
+void DensePrediction::multiply(const std::vector<float> & posterior, std::vector<float> & prior) const
+{
+	UASSERT(!matrix_.empty());
+	UASSERT_MSG(matrix_.cols == (int)posterior.size(),
+			uFormat("posterior=%d prediction=%d", (int)posterior.size(), matrix_.cols).c_str());
+
+	// A header over the posterior, so the multiplication reads it where it is. The product
+	// itself is left to OpenCV to allocate: asked to write into a matrix of ours it takes a
+	// path orders of magnitude slower, and copying the result back is only one value per
+	// location.
+	const cv::Mat posteriorMat((int)posterior.size(), 1, CV_32FC1, (void*)&posterior[0]);
+	const cv::Mat priorMat = matrix_ * posteriorMat;
+	prior.assign((const float *)priorMat.data, (const float *)priorMat.data + priorMat.rows);
+}
+
+unsigned long DensePrediction::memoryUsed() const
+{
+	unsigned long memory = ids_.capacity() * sizeof(int);
+	if(!matrix_.empty())
+	{
+		memory += (unsigned long)(matrix_.total() * matrix_.elemSize());
+	}
+	return memory;
+}
+
+// The matrix built column by column, every column from the neighborhood of one location.
+cv::Mat DensePrediction::generateFull(const PredictionModel & model, const Memory * memory,
+		const std::vector<int> & ids, NeighborsCache * cache) const
+{
+	UASSERT(memory &&
+		   model.values().size() >= 2 &&
+		   ids.size());
+
+	UTimer timer;
+	timer.start();
+	UTimer timerGlobal;
+	timerGlobal.start();
+
+	IdToIndexMap idToIndexMap;
+#if __cplusplus >= 201103L
+	idToIndexMap.reserve(ids.size());
+#endif
+	for(unsigned int i=0; i<ids.size(); ++i)
+	{
+		if(ids[i]>0)
+		{
+			idToIndexMap[ids[i]] = i;
+		}
+	}
+
+
+	//int rows = prediction.rows;
+	cv::Mat prediction = cv::Mat::zeros(ids.size(), ids.size(), CV_32FC1);
+	int cols = prediction.cols;
+
+	// Each prior is a column vector
+	UDEBUG("model.values().size()=%d",(int)model.values().size());
+	std::set<int> idsDone;
+
+	for(unsigned int i=0; i<ids.size(); ++i)
+	{
+		if(idsDone.find(ids[i]) == idsDone.end())
+		{
+			if(ids[i] > 0)
+			{
+				// Set high values (gaussians curves) to loop closure neighbors
+				std::list<int> idsLoopMargin;
+				std::map<int, int> neighbors = resolveNeighbors(
+						memory, ids[i], model.depth(), idToIndexMap, idsLoopMargin, cache);
+
+				// same neighbor tree for loop signatures (margin = 0)
+				for(std::list<int>::iterator iter = idsLoopMargin.begin(); iter!=idsLoopMargin.end(); ++iter)
+				{
+					if(cache)
+					{
+						uInsert(*cache, std::make_pair(*iter, neighbors));
+					}
+
+					float sum = 0.0f; // sum values added
+					int index = idToIndexMap.at(*iter);
+					float * column = (float*)prediction.data + index;
+					sum += model.addNeighborProb(column, cols, neighbors, idToIndexMap);
+					idsDone.insert(*iter);
+					model.normalize(column, cols, cols, index, sum, ids[0]<0);
+				}
+			}
+			else
+			{
+				// Set the virtual place prior
+				model.fillVirtualPlaceColumn((float*)prediction.data + i, cols, cols);
+			}
+		}
+	}
+
+	ULOGGER_DEBUG("time = %fs", timerGlobal.ticks());
+
+	return prediction;
+}
+
+cv::Mat DensePrediction::update(const PredictionModel & model, const Memory * memory,
+		const std::vector<int> & oldIds, const std::vector<int> & newIds,
+		NeighborsCache * cache) const
+{
+	UTimer timer;
+	UDEBUG("");
+
+	UASSERT(memory &&
+		oldIds.size() &&
+		newIds.size() &&
+		oldIds.size() == (unsigned int)matrix_.cols &&
+		oldIds.size() == (unsigned int)matrix_.rows);
+
+	cv::Mat prediction = cv::Mat::zeros(newIds.size(), newIds.size(), CV_32FC1);
+	UDEBUG("time creating prediction = %fs", timer.restart());
+
+	// Create id to index maps
+#if __cplusplus >= 201103L
+	std::unordered_set<int> oldIdsSet(oldIds.begin(), oldIds.end());
+#else
+	std::set<int> oldIdsSet(oldIds.begin(), oldIds.end());
+#endif
+	UDEBUG("time creating old ids set = %fs", timer.restart());
+
+	IdToIndexMap newIdToIndexMap;
+#if __cplusplus >= 201103L
+	newIdToIndexMap.reserve(newIds.size());
+#endif
+	for(unsigned int i=0; i<newIds.size(); ++i)
+	{
+		if(newIds[i]>0)
+		{
+			newIdToIndexMap[newIds[i]] = i;
+		}
+	}
+
+	UDEBUG("time creating id-index vector (size=%d oldIds.back()=%d newIds.back()=%d) = %fs", (int)newIdToIndexMap.size(), oldIds.back(), newIds.back(), timer.restart());
+
+	//Get removed ids
+	std::set<int> removedIds;
+	for(unsigned int i=0; i<oldIds.size(); ++i)
+	{
+		if(oldIds[i] > 0 && newIdToIndexMap.find(oldIds[i]) == newIdToIndexMap.end())
+		{
+			removedIds.insert(removedIds.end(), oldIds[i]);
+			(*cache).erase(oldIds[i]);
+			UDEBUG("removed id=%d at oldIndex=%d", oldIds[i], i);
+		}
+	}
+	UDEBUG("time getting removed ids = %fs", timer.restart());
+
+	bool oldAllCopied = false;
+	if(removedIds.empty() &&
+		newIds.size() > oldIds.size() &&
+		memcmp(oldIds.data(), newIds.data(), oldIds.size()*sizeof(int)) == 0)
+	{
+		matrix_.copyTo(cv::Mat(prediction, cv::Range(0, matrix_.rows), cv::Range(0, matrix_.cols)));
+		oldAllCopied = true;
+		UDEBUG("Copied all old prediction: = %fs", timer.ticks());
+	}
+
+	int added = 0;
+	// get ids to update
+	std::set<int> idsToUpdate;
+	for(unsigned int i=0; i<oldIds.size() || i<newIds.size(); ++i)
+	{
+		if(i<oldIds.size())
+		{
+			if(removedIds.find(oldIds[i]) != removedIds.end())
+			{
+				unsigned int cols = matrix_.cols;
+				int count = 0;
+				for(unsigned int j=0; j<cols; ++j)
+				{
+					if(j!=i && removedIds.find(oldIds[j]) == removedIds.end())
+					{
+						//UDEBUG("to update id=%d from id=%d removed (value=%f)", oldIds[j], oldIds[i], ((const float *)matrix_.data)[i + j*cols]);
+						idsToUpdate.insert(oldIds[j]);
+						++count;
+					}
+				}
+				UDEBUG("From removed id %d, %d neighbors to update.", oldIds[i], count);
+			}
+		}
+		if(i<newIds.size() && oldIdsSet.find(newIds[i]) == oldIdsSet.end())
+		{
+			if((*cache).find(newIds[i]) == (*cache).end())
+			{
+				std::map<int, int> neighbors = memory->getNeighborsId(newIds[i], model.depth(), 0, false, false, true, true);
+
+				for(std::map<int, int>::iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
+				{
+					std::map<int, std::map<int, int> >::iterator jter = (*cache).find(iter->first);
+					if(jter != (*cache).end())
+					{
+						uInsert(jter->second, std::make_pair(newIds[i], iter->second));
+					}
+				}
+				(*cache).insert(std::make_pair(newIds[i], neighbors));
+			}
+			const std::map<int, int> & neighbors = (*cache).at(newIds[i]);
+			//std::map<int, int> neighbors = memory->getNeighborsId(newIds[i], model.depth(), 0, false, false, true, true);
+
+			float * column = (float*)prediction.data + i;
+			float sum = model.addNeighborProb(column, prediction.cols, neighbors, newIdToIndexMap);
+			model.normalize(column, prediction.cols, prediction.cols, i, sum, newIds[0]<0);
+
+			++added;
+			int count = 0;
+			for(std::map<int,int>::const_iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
+			{
+				if(oldIdsSet.find(iter->first)!=oldIdsSet.end() &&
+				   removedIds.find(iter->first) == removedIds.end())
+				{
+					idsToUpdate.insert(iter->first);
+					++count;
+				}
+			}
+			UDEBUG("From added id %d, %d neighbors to update.", newIds[i], count);
+		}
+	}
+	UDEBUG("time getting %d ids to update = %fs", (int)idsToUpdate.size(), timer.restart());
+
+	UTimer t1;
+	double e0=0,e1=0, e2=0, e3=0, e4=0;
+	// update modified/added ids
+	int modified = 0;
+	for(std::set<int>::iterator iter = idsToUpdate.begin(); iter!=idsToUpdate.end(); ++iter)
+	{
+		int id = *iter;
+		if(id > 0)
+		{
+			int index = newIdToIndexMap.at(id);
+
+			e0 = t1.ticks();
+			std::map<int, std::map<int, int> >::iterator kter = (*cache).find(id);
+			UASSERT_MSG(kter != (*cache).end(), uFormat("Did not find %d (current index size=%d)", id, (int)(*cache).size()).c_str());
+			const std::map<int, int> & neighbors = kter->second;
+			//std::map<int, int> neighbors = memory->getNeighborsId(id, model.depth(), 0, false, false, true, true);
+			e1+=t1.ticks();
+
+			float * column = (float*)prediction.data + index;
+			float sum = model.addNeighborProb(column, prediction.cols, neighbors, newIdToIndexMap);
+			e3+=t1.ticks();
+
+			model.normalize(column, prediction.cols, prediction.cols, index, sum, newIds[0]<0);
+			++modified;
+			e4+=t1.ticks();
+		}
+	}
+	UDEBUG("time updating modified/added %d ids = %fs (e0=%f e1=%f e2=%f e3=%f e4=%f)", (int)idsToUpdate.size(), timer.restart(), e0, e1, e2, e3, e4);
+
+	int copied = 0;
+	if(!oldAllCopied)
+	{
+		//UDEBUG("oldIds.size()=%d, matrix_.cols=%d, matrix_.rows=%d", oldIds.size(), matrix_.cols, matrix_.rows);
+		//UDEBUG("newIdToIndexMap.size()=%d, prediction.cols=%d, prediction.rows=%d", newIdToIndexMap.size(), prediction.cols, prediction.rows);
+		// copy not changed probabilities
+		for(unsigned int i=0; i<oldIds.size(); ++i)
+		{
+			if(oldIds[i]>0 && removedIds.find(oldIds[i]) == removedIds.end() && idsToUpdate.find(oldIds[i]) == idsToUpdate.end())
+			{
+				for(int j=0; j<matrix_.cols; ++j)
+				{
+					if(oldIds[j]>0 && removedIds.find(oldIds[j]) == removedIds.end())
+					{
+						//UDEBUG("i=%d, j=%d", i, j);
+						//UDEBUG("oldIds[i]=%d, oldIds[j]=%d", oldIds[i], oldIds[j]);
+						//UDEBUG("newIdToIndexMap.at(oldIds[i])=%d", newIdToIndexMap.at(oldIds[i]));
+						//UDEBUG("newIdToIndexMap.at(oldIds[j])=%d", newIdToIndexMap.at(oldIds[j]));
+						float v = ((const float *)matrix_.data)[i + j*matrix_.cols];
+						int ii = newIdToIndexMap.at(oldIds[i]);
+						int jj = newIdToIndexMap.at(oldIds[j]);
+						((float *)prediction.data)[ii + jj*prediction.cols] = v;
+						//if(ii != jj)
+						//{
+						//	((float *)prediction.data)[jj + ii*prediction.cols] = v;
+						//}
+					}
+				}
+				++copied;
+			}
+		}
+		UDEBUG("time copying = %fs", timer.restart());
+	}
+
+	//update virtual place
+	if(newIds[0] < 0)
+	{
+		if(prediction.cols>1) // The first must be the virtual place
+		{
+			((float*)prediction.data)[0] = model.virtualPlacePrior();
+			float val = (1.0-model.virtualPlacePrior())/(prediction.cols-1);
+			for(int j=1; j<prediction.cols; j++)
+			{
+				((float*)prediction.data)[j*prediction.cols] = val;
+				((float*)prediction.data)[j] = model.values()[0];
+			}
+		}
+		else if(prediction.cols>0)
+		{
+			((float*)prediction.data)[0] = 1;
+		}
+	}
+	UDEBUG("time updating virtual place = %fs", timer.restart());
+
+	UDEBUG("Modified=%d, Added=%d, Copied=%d", modified, added, copied);
+	return prediction;
+}
+
+} // namespace bayes
+} // namespace rtabmap

corelib/src/bayes/DensePrediction.h

@@ -0,0 +1,91 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef RTABMAP_BAYES_DENSEPREDICTION_H_
+#define RTABMAP_BAYES_DENSEPREDICTION_H_
+
+#include "bayes/PredictionModel.h"
+
+#include <opencv2/core/core.hpp>
+#include <vector>
+
+namespace rtabmap {
+
+class Memory;
+
+namespace bayes {
+
+/**
+ * @brief The prediction as a matrix, one column per location.
+ *
+ * The matrix costs the number of locations squared, whatever the graph puts in it, which on a
+ * large map is most of what the Bayes filter holds and most of what an iteration reads. See
+ * SparsePrediction for the form that keeps only the values.
+ */
+class DensePrediction
+{
+public:
+	bool empty() const {return matrix_.empty();}
+	const cv::Mat & matrix() const {return matrix_;}
+
+	/// The locations the matrix is built for, which the incremental update carries over.
+	const std::vector<int> & ids() const {return ids_;}
+
+	void clear() {matrix_ = cv::Mat(); ids_.clear();}
+
+	/**
+	 * @brief Builds the matrix for @p ids and keeps it, along with the ids it is built for.
+	 *
+	 * The matrix already there is carried over when it is built for locations @p ids only
+	 * appends to; otherwise every column is built again.
+	 *
+	 * @param fullUpdate Rebuilds every column rather than carrying the matrix over.
+	 * @param cache Filled with the neighborhoods, for a later incremental update.
+	 */
+	const cv::Mat & generate(const PredictionModel & model, const Memory * memory,
+			const std::vector<int> & ids, bool fullUpdate, NeighborsCache * cache);
+
+	/// prior = prediction x posterior.
+	void multiply(const std::vector<float> & posterior, std::vector<float> & prior) const;
+
+	unsigned long memoryUsed() const;
+
+private:
+	cv::Mat generateFull(const PredictionModel & model, const Memory * memory,
+			const std::vector<int> & ids, NeighborsCache * cache) const;
+	cv::Mat update(const PredictionModel & model, const Memory * memory,
+			const std::vector<int> & oldIds, const std::vector<int> & newIds,
+			NeighborsCache * cache) const;
+
+	cv::Mat matrix_;
+	std::vector<int> ids_;
+};
+
+} // namespace bayes
+} // namespace rtabmap
+
+#endif /* RTABMAP_BAYES_DENSEPREDICTION_H_ */

corelib/src/bayes/PredictionModel.cpp

@@ -0,0 +1,302 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "bayes/PredictionModel.h"
+
+#include "rtabmap/core/Memory.h"
+#include "rtabmap/utilite/UtiLite.h"
+
+namespace rtabmap {
+namespace bayes {
+
+// format = {Virtual place, Loop closure, level1, level2, l3, l4...}
+bool PredictionModel::set(const std::string & prediction)
+{
+	bool set = false;
+	std::list<std::string> strValues = uSplit(prediction, ' ');
+	if(strValues.size() < 2)
+	{
+		UERROR("The number of values < 2 (prediction=\"%s\")", prediction.c_str());
+	}
+	else
+	{
+		std::vector<double> tmpValues(strValues.size());
+		int i=0;
+		bool valid = true;
+		for(std::list<std::string>::iterator iter = strValues.begin(); iter!=strValues.end(); ++iter)
+		{
+			tmpValues[i] = uStr2Float((*iter).c_str());
+			//UINFO("%d=%e", i, tmpValues[i]);
+			if(tmpValues[i] < 0.0 || tmpValues[i]>1.0)
+			{
+				valid = false;
+				break;
+			}
+			++i;
+		}
+
+		if(!valid)
+		{
+			UERROR("The prediction is not valid (values must be between >0 && <=1) prediction=\"%s\"", prediction.c_str());
+		}
+		else
+		{
+			values_ = tmpValues;
+			set = true;
+		}
+	}
+	total_ = 0.0f;
+	for(unsigned int j=0; j<values_.size(); ++j)
+	{
+		total_ += values_[j];
+		if(j==0 || values_[j] < epsilon_)
+		{
+			epsilon_ = values_[j];
+		}
+	}
+	if(!values_.empty())
+	{
+		UDEBUG("predictionEpsilon = %f", epsilon_);
+	}
+	return set;
+}
+
+std::string PredictionModel::str() const
+{
+	std::string values;
+	for(unsigned int i=0; i<values_.size(); ++i)
+	{
+		values.append(uNumber2Str(values_[i]));
+		if(i+1 < values_.size())
+		{
+			values.append(" ");
+		}
+	}
+	return values;
+}
+
+// A column of the prediction matrix, given as a pointer to its first value and the
+// step between two of them: the matrix stores a column strided by its width, while the
+// sparse build below fills one contiguous column at a time. Both go through this and
+// through BayesFilter::normalize(), so that the probabilities cannot end up differing
+// between the two.
+float PredictionModel::addNeighborProb(float * column, size_t stride,
+			const std::map<int, int> & neighbors, const IdToIndexMap & idToIndex) const
+{
+	float sum=0.0f;
+	for(std::map<int, int>::const_iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
+	{
+		if(iter->first>=0)
+		{
+			IdToIndexMap::const_iterator jter = idToIndex.find(iter->first);
+			if(jter != idToIndex.end())
+			{
+				UASSERT((iter->second+1) < (int)values_.size());
+				sum += column[jter->second*stride] = values_[iter->second+1];
+			}
+		}
+	}
+	return sum;
+}
+
+void PredictionModel::normalize(float * column, size_t stride, int size, unsigned int index, float addedProbabilitiesSum, bool virtualPlaceUsed) const
+{
+	UASSERT(index < (unsigned int)size);
+
+	int cols = size;
+	// ADD values of not found neighbors to loop closure
+	if(addedProbabilitiesSum < total_-values_[0])
+	{
+		float delta = total_-values_[0]-addedProbabilitiesSum;
+		column[index*stride] += delta;
+		addedProbabilitiesSum+=delta;
+	}
+
+	float allOtherPlacesValue = 0;
+	if(total_ < 1)
+	{
+		allOtherPlacesValue = 1.0f - total_;
+	}
+
+	// Set all loop events to small values according to the model
+	if(allOtherPlacesValue > 0 && cols>1)
+	{
+		float value = allOtherPlacesValue / float(cols - 1);
+		for(int j=virtualPlaceUsed?1:0; j<cols; ++j)
+		{
+			if(column[j*stride] == 0)
+			{
+				column[j*stride] = value;
+				addedProbabilitiesSum += column[j*stride];
+			}
+		}
+	}
+
+	//normalize this row
+	float maxNorm = 1 - (virtualPlaceUsed?values_[0]:0); // 1 - virtual place probability
+	if(addedProbabilitiesSum<maxNorm-0.0001 || addedProbabilitiesSum>maxNorm+0.0001)
+	{
+		for(int j=virtualPlaceUsed?1:0; j<cols; ++j)
+		{
+			column[j*stride] *= maxNorm / addedProbabilitiesSum;
+			if(column[j*stride] < epsilon_)
+			{
+				column[j*stride] = 0.0f;
+			}
+		}
+		addedProbabilitiesSum = maxNorm;
+	}
+
+	// ADD virtual place prob
+	if(virtualPlaceUsed)
+	{
+		column[0] = values_[0];
+		addedProbabilitiesSum += column[0];
+	}
+
+	//debug
+	//for(int j=0; j<cols; ++j)
+	//{
+	//	ULOGGER_DEBUG("test col=%d = %f", i, prediction.data.fl[i + j*cols]);
+	//}
+
+	// Left out of the coverage report: no input gets here. Whatever the column held, the
+	// scaling above leaves addedProbabilitiesSum at maxNorm, which is 1 without the virtual
+	// place and 1 minus its probability with it -- and that probability is then added back.
+	// It is kept as a canary for whoever changes the arithmetic above.
+	if(addedProbabilitiesSum<0.99 || addedProbabilitiesSum > 1.01)
+	{
+		UWARN("Prediction is not normalized sum=%f", addedProbabilitiesSum); // LCOV_EXCL_LINE
+	}
+}
+
+// The column of the virtual place, the hypothesis of being at a location that was
+// never visited: the probability of moving again to a new one, then the rest split
+// equally over the visited ones.
+void PredictionModel::fillVirtualPlaceColumn(float * column, size_t stride, int size) const
+{
+	if(virtualPlacePrior_ > 0)
+	{
+		if(size>1) // The first must be the virtual place
+		{
+			column[0] = virtualPlacePrior_;
+			float val = (1.0-virtualPlacePrior_)/(size-1);
+			for(int j=1; j<size; ++j)
+			{
+				column[j*stride] = val;
+			}
+		}
+		else if(size>0)
+		{
+			column[0] = 1;
+		}
+	}
+	else
+	{
+		// Only for some tests...
+		// when virtualPlacePrior_=0, set all priors to the same value
+		if(size>1)
+		{
+			float val = 1.0/size;
+			for(int j=0; j<size; ++j)
+			{
+				column[j*stride] = val;
+			}
+		}
+		else if(size>0)
+		{
+			column[0] = 1;
+		}
+	}
+}
+
+// The neighbors of a location within the depth of the prediction model, and the
+// locations that are at margin 0 of it, meaning the same place: their columns all hold
+// the probabilities of this same neighborhood. Shared by the dense and the sparse
+// builds, this being the part that reads the graph.
+//
+// cache is filled when not null, for updatePrediction() to reuse.
+std::map<int, int> resolveNeighbors(const Memory * memory, int id, int maxDepth,
+		const IdToIndexMap & idToIndexMap, std::list<int> & idsAtMargin0, NeighborsCache * cache)
+{
+	std::map<int, int> neighbors = memory->getNeighborsId(id, maxDepth, 0, false, false, true, true);
+
+	if(cache)
+	{
+		uInsert(*cache, std::make_pair(id, neighbors));
+	}
+
+	idsAtMargin0.clear();
+	//filter neighbors in STM
+	for(std::map<int, int>::iterator iter=neighbors.begin(); iter!=neighbors.end();)
+	{
+		if(memory->isInSTM(iter->first))
+		{
+			neighbors.erase(iter++);
+		}
+		else
+		{
+			if(iter->second == 0 && idToIndexMap.find(iter->first)!=idToIndexMap.end())
+			{
+				idsAtMargin0.push_back(iter->first);
+			}
+			++iter;
+		}
+	}
+
+	// should at least have 1 id in idsMarginLoop
+	if(idsAtMargin0.size() == 0)
+	{
+		UFATAL("No 0 margin neighbor for signature %d !?!?", id);
+	}
+	return neighbors;
+}
+
+// The neighborhood of a location, from the cache the incremental update needs, adding it
+// there and to the neighborhoods of its own neighbors when it is not there yet.
+const std::map<int, int> & cachedNeighbors(const Memory * memory, int id, int maxDepth,
+		NeighborsCache & cache)
+{
+	std::map<int, std::map<int, int> >::const_iterator iter = cache.find(id);
+	if(iter == cache.end())
+	{
+		std::map<int, int> neighbors = memory->getNeighborsId(id, maxDepth, 0, false, false, true, true);
+		for(std::map<int, int>::iterator jter=neighbors.begin(); jter!=neighbors.end(); ++jter)
+		{
+			std::map<int, std::map<int, int> >::iterator kter = cache.find(jter->first);
+			if(kter != cache.end())
+			{
+				uInsert(kter->second, std::make_pair(id, jter->second));
+			}
+		}
+		iter = cache.insert(std::make_pair(id, neighbors)).first;
+	}
+	return iter->second;
+}
+
+} // namespace bayes
+} // namespace rtabmap

corelib/src/bayes/PredictionModel.h

@@ -0,0 +1,143 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef RTABMAP_BAYES_PREDICTIONMODEL_H_
+#define RTABMAP_BAYES_PREDICTIONMODEL_H_
+
+#include <list>
+#include <map>
+#include <string>
+#include <vector>
+#if __cplusplus >= 201103L
+#include <unordered_map>
+#endif
+
+namespace rtabmap {
+
+class Memory;
+
+namespace bayes {
+
+/// Where each location sits in the prediction: its id to its row and column.
+#if __cplusplus >= 201103L
+typedef std::unordered_map<int, int> IdToIndexMap;
+#else
+typedef std::map<int, int> IdToIndexMap;
+#endif
+
+/// The neighborhood of the locations it was asked for, which the incremental updates of the
+/// prediction read instead of walking the graph again.
+typedef std::map<int, std::map<int, int> > NeighborsCache;
+
+/**
+ * @brief The loop closure prediction model, and the column arithmetic that follows from it.
+ *
+ * Format `{Vp, Lc, l1, l2, ...}`: the probability of moving to a new place, of staying at the
+ * same location, then of moving to a neighbor at each depth of the graph. See
+ * Parameters::kBayesPredictionLC().
+ *
+ * A column of the prediction is the distribution over where the robot moves to from one
+ * location. Both the dense and the sparse prediction fill their columns through this, so the
+ * probabilities cannot end up differing between them. A column is given as the pointer to its
+ * first value and the step between two of them: the matrix stores a column strided by its
+ * width, while the sparse build fills one contiguous column at a time.
+ */
+class PredictionModel
+{
+public:
+	/**
+	 * @brief Sets the model from a space separated list of probabilities.
+	 * @return False when the string does not hold at least two values in [0, 1], the previous
+	 *         model being kept.
+	 */
+	bool set(const std::string & prediction);
+
+	const std::vector<double> & values() const {return values_;}
+	std::string str() const;
+
+	/// How deep in the graph a column reaches: one less than the number of values.
+	int depth() const {return (int)values_.size()-1;}
+
+	/// Whether the values leave probability for normalize() to spread over every other
+	/// location, which fills every zero of a column and leaves nothing sparse to keep.
+	bool spreadsOverAllLocations() const {return total_ < 1;}
+
+	float total() const {return total_;}
+	bool valid() const {return values_.size() >= 2;}
+
+	float virtualPlacePrior() const {return virtualPlacePrior_;}
+	void setVirtualPlacePrior(float prior) {virtualPlacePrior_ = prior;}
+
+	/**
+	 * @brief Puts the probability of each neighbor into a column.
+	 * @return The sum of what it put there, which normalize() takes.
+	 */
+	float addNeighborProb(float * column, size_t stride, const std::map<int, int> & neighbors,
+			const IdToIndexMap & idToIndex) const;
+
+	/**
+	 * @brief Normalizes one column and applies the virtual place probability.
+	 * @param index Index of the location the column is for, so of its diagonal value.
+	 * @param addedProbabilitiesSum What addNeighborProb() put in it.
+	 * @param virtualPlaceUsed Whether the first location is the virtual place.
+	 */
+	void normalize(float * column, size_t stride, int size, unsigned int index,
+			float addedProbabilitiesSum, bool virtualPlaceUsed) const;
+
+	/// Fills the column of the virtual place, the hypothesis of a location never visited.
+	void fillVirtualPlaceColumn(float * column, size_t stride, int size) const;
+
+	unsigned long memoryUsed() const {return values_.capacity() * sizeof(double);}
+
+private:
+	std::vector<double> values_;
+	float total_ = 0.0f;
+	float epsilon_ = 0.0f;          ///< Smallest probability of the model, under which normalize() drops a value.
+	float virtualPlacePrior_ = 0.0f;
+};
+
+/**
+ * @brief The neighbors of a location within the depth of the model, and the locations at
+ *        margin 0 of it, meaning the same place: their columns hold this same neighborhood.
+ *
+ * @param cache Filled when not null, for the incremental updates to reuse.
+ */
+std::map<int, int> resolveNeighbors(const Memory * memory, int id, int maxDepth,
+		const IdToIndexMap & idToIndexMap, std::list<int> & idsAtMargin0, NeighborsCache * cache);
+
+/**
+ * @brief The neighborhood of a location from @p cache, querying and caching it when absent.
+ *
+ * Caching it also adds the location to the neighborhoods of its own neighbors.
+ */
+const std::map<int, int> & cachedNeighbors(const Memory * memory, int id, int maxDepth,
+		NeighborsCache & cache);
+
+} // namespace bayes
+} // namespace rtabmap
+
+#endif /* RTABMAP_BAYES_PREDICTIONMODEL_H_ */

corelib/src/bayes/SparsePrediction.cpp

@@ -0,0 +1,549 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "bayes/SparsePrediction.h"
+
+#include "rtabmap/core/Memory.h"
+#include "rtabmap/core/Parameters.h"
+#include "rtabmap/utilite/UtiLite.h"
+
+namespace rtabmap {
+namespace bayes {
+
+void SparsePrediction::clear()
+{
+	columns_.clear();
+	values_.clear();
+	ids_.clear();
+	used_ = 0;
+}
+
+cv::Mat SparsePrediction::toMatrix() const
+{
+	const int size = (int)columns_.size();
+	cv::Mat matrix = cv::Mat::zeros(size, size, CV_32FC1);
+	for(int col=0; col<size; ++col)
+	{
+		const Column & slot = columns_[col];
+		for(size_t i=slot.offset; i<slot.offset+slot.size; ++i)
+		{
+			matrix.at<float>(values_[i].first, col) = values_[i].second;
+		}
+	}
+	return matrix;
+}
+
+unsigned long SparsePrediction::memoryUsed() const
+{
+	return values_.capacity() * sizeof(std::pair<int, float>)
+			+ columns_.capacity() * sizeof(Column)
+			+ ids_.capacity() * sizeof(int);
+}
+
+// Takes the non zero values of a freshly built column into the prediction, and leaves the
+// buffer zeroed for the next one, which saves clearing the whole of it every time.
+//
+// The values of every column live in one array, so that the multiplication reads them the
+// way memory likes to be read. A column keeps the room it was given: rebuilt into fewer
+// values it stays where it is, rebuilt into more than it has room for it is put at the end
+// and the room it had is left behind, to be recovered by compact(). Asking
+// for a little more than is needed, when the column is one being rebuilt, buys the room for
+// it to grow a few times in place.
+void SparsePrediction::takeColumn(std::vector<float> & column, int index, bool withRoomToGrow)
+{
+	size_t count = 0;
+	for(size_t row=0; row<column.size(); ++row)
+	{
+		if(column[row] != 0.0f)
+		{
+			++count;
+		}
+	}
+
+	Column & slot = columns_[index];
+	used_ -= slot.size;
+	if(count > slot.capacity)
+	{
+		slot.offset = values_.size();
+		slot.capacity = withRoomToGrow ? count + count/8 + 4 : count;
+		values_.resize(slot.offset + slot.capacity);
+	}
+	slot.size = count;
+	used_ += count;
+
+	size_t i = slot.offset;
+	for(size_t row=0; row<column.size(); ++row)
+	{
+		if(column[row] != 0.0f)
+		{
+			values_[i++] = std::make_pair((int)row, column[row]);
+			column[row] = 0.0f;
+		}
+	}
+}
+
+// Packs the columns back into the order they are multiplied in, giving each the room it
+// needs and no more. Called when the room left behind by rebuilt columns has grown to a
+// quarter of what is in use, and at the end of a full build, whose columns are not built in
+// the order of their index.
+void SparsePrediction::compact()
+{
+	std::vector<std::pair<int, float> > packed;
+	packed.reserve(used_);
+	for(size_t i=0; i<columns_.size(); ++i)
+	{
+		Column & slot = columns_[i];
+		const size_t offset = packed.size();
+		packed.insert(packed.end(),
+				values_.begin()+slot.offset,
+				values_.begin()+slot.offset+slot.size);
+		slot.offset = offset;
+		slot.capacity = slot.size;
+	}
+	values_.swap(packed);
+}
+
+// The prediction built in its sparse form, the matrix never being allocated.
+//
+// A column of the prediction only holds the neighbors of one location within the depth of
+// the prediction model, so on a large map the matrix is mostly zeros, while holding it
+// costs the size of the working memory squared against the far smaller size of the values
+// in it. Each column is built in a buffer of its own instead, through the same
+// addNeighborProb() and normalize() as the dense build, and only its non zero values are
+// kept. Every column keeps the room it was given in values_, so that update() can rebuild
+// one of them without moving the others.
+//
+// The columns are not built in the order of their index: a column is built for every
+// location at margin 0 of the one being expanded, so several are built at once.
+//
+// Always built, whatever its columns come to hold: the caller asked for the prediction sparse
+// and gets it sparse, so that what it measures is the sparse form and not a fallback. The one
+// prediction with nothing sparse to keep, of a model whose values sum to less than 1,
+// normalize() spreading the difference over every zero of a column, never reaches here: the
+// caller answers that one with the matrix without asking.
+void SparsePrediction::generate(const PredictionModel & model, const Memory * memory,
+		const std::vector<int> & ids, NeighborsCache * cache)
+{
+	UASSERT(memory && model.valid() && ids.size());
+
+	UTimer timer;
+	this->clear();
+
+	const int size = (int)ids.size();
+
+	IdToIndexMap idToIndexMap;
+#if __cplusplus >= 201103L
+	idToIndexMap.reserve(ids.size());
+#endif
+	for(int i=0; i<size; ++i)
+	{
+		if(ids[i]>0)
+		{
+			idToIndexMap[ids[i]] = i;
+		}
+	}
+
+	columns_.assign(size, Column());
+	std::vector<float> column(size, 0.0f);
+
+	std::set<int> idsDone;
+	for(int i=0; i<size; ++i)
+	{
+		if(idsDone.find(ids[i]) != idsDone.end())
+		{
+			continue;
+		}
+
+		if(ids[i] > 0)
+		{
+			std::list<int> idsLoopMargin;
+			std::map<int, int> neighbors = resolveNeighbors(
+					memory, ids[i], model.depth(), idToIndexMap, idsLoopMargin, cache);
+
+			// same neighbor tree for loop signatures (margin = 0)
+			for(std::list<int>::iterator iter=idsLoopMargin.begin(); iter!=idsLoopMargin.end(); ++iter)
+			{
+				if(cache)
+				{
+					uInsert(*cache, std::make_pair(*iter, neighbors));
+				}
+				const int index = idToIndexMap.at(*iter);
+				const float sum = model.addNeighborProb(&column[0], 1, neighbors, idToIndexMap);
+				model.normalize(&column[0], 1, size, index, sum, ids[0]<0);
+				this->takeColumn(column, index, false);
+				idsDone.insert(*iter);
+			}
+		}
+		else
+		{
+			model.fillVirtualPlaceColumn(&column[0], 1, size);
+			this->takeColumn(column, i, false);
+		}
+	}
+	// The columns were not built in the order of their index, so they are packed into it.
+	this->compact();
+	ids_ = ids;
+
+	const size_t nnz = used_;
+	UDEBUG("Sparse prediction: %ld/%ld values (%.2f%%), %ld MB against the %ld MB of the "
+		   "matrix, built in %fs",
+			(long)nnz, (long)size*size, 100.0*double(nnz)/(double(size)*double(size)),
+			(long)(this->memoryUsed()/1048576),
+			(long)((size_t)size*(size_t)size*sizeof(float)/1048576),
+			timer.ticks());
+}
+
+// One column, from the neighborhood of the location it is for. Read from the cache, which
+// generate() filled and which the graph is only walked again for when a location came back
+// from long-term memory after its neighborhood was dropped.
+void SparsePrediction::buildColumn(const PredictionModel & model, const Memory * memory, int id,
+		int index, const std::vector<int> & ids, const IdToIndexMap & idToIndex,
+		std::vector<float> & buffer, NeighborsCache & cache)
+{
+	const std::map<int, int> & neighbors = cachedNeighbors(memory, id, model.depth(), cache);
+	const float sum = model.addNeighborProb(&buffer[0], 1, neighbors, idToIndex);
+	model.normalize(&buffer[0], 1, (int)ids.size(), index, sum, ids[0]<0);
+	this->takeColumn(buffer, index, true);
+}
+
+// Carries the prediction over to the locations of an iteration, which costs the columns whose
+// contents changed rather than a walk of the graph per column.
+//
+// Returns false when there is nothing to carry over, which the caller answers by calling
+// generate().
+bool SparsePrediction::update(const PredictionModel & model, const Memory * memory,
+		const std::vector<int> & newIds, NeighborsCache & cache)
+{
+	if(ids_.empty() || newIds.empty() || columns_.size() != ids_.size())
+	{
+		return false;
+	}
+
+	// Appended to, or changed in any other way: the first keeps every index, the second has
+	// to lay the columns out again.
+	const bool appendedTo =
+			newIds.size() > ids_.size() &&
+			memcmp(ids_.data(), newIds.data(), ids_.size()*sizeof(int)) == 0;
+	return appendedTo
+			? this->updateAppended(model, memory, newIds, cache)
+			: this->updateRemapped(model, memory, newIds, cache);
+}
+
+// The same prediction after locations were appended, without building it again.
+//
+// Every location that was already there keeps its index, so the columns already built
+// still apply: only the ones the new locations reach have to be built again, and the
+// column of the virtual place, whose values are shared out over however many locations
+// there are. What a column holds does not otherwise depend on how many there are, the
+// model summing to 1 leaving normalize() nothing to spread over the others.
+bool SparsePrediction::updateAppended(const PredictionModel & model, const Memory * memory,
+		const std::vector<int> & newIds, NeighborsCache & cache)
+{
+	UTimer timer;
+	const std::vector<int> & oldIds = ids_;
+	const int size = (int)newIds.size();
+
+	IdToIndexMap newIdToIndexMap;
+#if __cplusplus >= 201103L
+	newIdToIndexMap.reserve(newIds.size());
+#endif
+	for(int i=0; i<size; ++i)
+	{
+		if(newIds[i]>0)
+		{
+			newIdToIndexMap[newIds[i]] = i;
+		}
+	}
+
+	columns_.resize(size);   // the appended columns start out empty
+	std::vector<float> column(size, 0.0f);
+
+	// The appended locations, and the ones that were already there whose neighborhood the
+	// appended ones are now part of.
+	std::set<int> idsToUpdate;
+	for(size_t i=oldIds.size(); i<newIds.size(); ++i)
+	{
+		// Every appended location is a visited one: the virtual place is the first of them
+		// and an append keeps the index of everything that was already there.
+		UASSERT(newIds[i] > 0);
+		const std::map<int, int> & neighbors = cachedNeighbors(memory, newIds[i], model.depth(), cache);
+		const float sum = model.addNeighborProb(&column[0], 1, neighbors, newIdToIndexMap);
+		model.normalize(&column[0], 1, size, (int)i, sum, newIds[0]<0);
+		this->takeColumn(column, (int)i, true);
+		for(std::map<int, int>::const_iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
+		{
+			const IdToIndexMap::const_iterator jter = newIdToIndexMap.find(iter->first);
+			if(jter != newIdToIndexMap.end() && (size_t)jter->second < oldIds.size())
+			{
+				idsToUpdate.insert(iter->first);
+			}
+		}
+	}
+
+	for(std::set<int>::const_iterator iter=idsToUpdate.begin(); iter!=idsToUpdate.end(); ++iter)
+	{
+		this->buildColumn(model, memory, *iter, newIdToIndexMap.at(*iter),
+				newIds, newIdToIndexMap, column, cache);
+	}
+
+	// The virtual place shares what is left of its probability over the visited locations,
+	// so its column depends on how many of them there are.
+	if(newIds[0] < 0)
+	{
+		model.fillVirtualPlaceColumn(&column[0], 1, size);
+		this->takeColumn(column, 0, true);
+	}
+
+	// The room left behind by the columns that outgrew their slot, once it is a quarter of
+	// what is in use.
+	const size_t waste = values_.size() - used_;
+	const bool compacted = waste > used_/4;
+	if(compacted)
+	{
+		this->compact();
+	}
+	const size_t appended = newIds.size()-oldIds.size();
+	ids_ = newIds;
+
+	UDEBUG("Sparse prediction: %d locations appended, %d columns rebuilt of %d, %ld values, "
+		   "%ld left behind%s, updated in %fs",
+			(int)appended, (int)idsToUpdate.size(), size,
+			(long)used_, (long)waste, compacted?" (packed again)":"",
+			timer.ticks());
+	return true;
+}
+
+// The same prediction after the locations changed in any other way than being appended to:
+// locations gone from the working memory as it is capped, locations back from long-term
+// memory in the middle of the ones already there, or both at once.
+//
+// The index of a location moves, so the columns are laid out again -- but a column is only
+// built again when what goes in it changed, which is when:
+//   * the location was not there before, so it has no column yet;
+//   * one of those is now part of its neighborhood, so it gains a value;
+//   * it shared its probability with a location that is gone, which normalize() now shares
+//     out over the ones that remain.
+// Every other column is the same values at another row, which is a copy. That is what
+// separates this from generate(): the graph is walked for the columns that changed, not for
+// every one of them.
+bool SparsePrediction::updateRemapped(const PredictionModel & model, const Memory * memory,
+		const std::vector<int> & newIds, NeighborsCache & cache)
+{
+	UTimer timer;
+	const std::vector<int> & oldIds = ids_;
+	const int size = (int)newIds.size();
+
+	// The virtual place appearing or disappearing changes every column, normalize() holding
+	// back its probability on all of them, so there would be nothing to carry over.
+	if((oldIds[0] < 0) != (newIds[0] < 0))
+	{
+		return false;
+	}
+
+	IdToIndexMap newIdToIndexMap;
+	IdToIndexMap oldIdToIndexMap;
+#if __cplusplus >= 201103L
+	newIdToIndexMap.reserve(newIds.size());
+	oldIdToIndexMap.reserve(oldIds.size());
+#endif
+	for(int i=0; i<size; ++i)
+	{
+		if(newIds[i]>0)
+		{
+			newIdToIndexMap[newIds[i]] = i;
+		}
+	}
+	for(size_t i=0; i<oldIds.size(); ++i)
+	{
+		if(oldIds[i]>0)
+		{
+			oldIdToIndexMap[oldIds[i]] = (int)i;
+		}
+	}
+
+	// Where each location went, and which ones are gone. The virtual place is the first of
+	// both, so it does not move.
+	std::vector<int> oldToNew(oldIds.size(), -1);
+	size_t removed = 0;
+	for(size_t i=0; i<oldIds.size(); ++i)
+	{
+		if(oldIds[i] <= 0)
+		{
+			oldToNew[i] = 0;
+			continue;
+		}
+		const IdToIndexMap::const_iterator iter = newIdToIndexMap.find(oldIds[i]);
+		if(iter == newIdToIndexMap.end())
+		{
+			// Its neighborhood is no longer ours to keep, as the dense update does too.
+			cache.erase(oldIds[i]);
+			++removed;
+		}
+		else
+		{
+			oldToNew[i] = iter->second;
+		}
+	}
+
+	// The locations that were not there before, and the ones whose neighborhood they are
+	// part of.
+	std::set<int> idsToBuild;
+	for(int i=0; i<size; ++i)
+	{
+		if(newIds[i] <= 0 || oldIdToIndexMap.find(newIds[i]) != oldIdToIndexMap.end())
+		{
+			continue;
+		}
+		idsToBuild.insert(newIds[i]);
+		const std::map<int, int> & neighbors = cachedNeighbors(memory, newIds[i], model.depth(), cache);
+		for(std::map<int, int>::const_iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
+		{
+			if(iter->first > 0 &&
+			   newIdToIndexMap.find(iter->first) != newIdToIndexMap.end() &&
+			   oldIdToIndexMap.find(iter->first) != oldIdToIndexMap.end())
+			{
+				idsToBuild.insert(iter->first);
+			}
+		}
+	}
+
+	// And the ones holding a value on a row that is gone.
+	if(removed)
+	{
+		for(size_t i=0; i<oldIds.size(); ++i)
+		{
+			if(oldIds[i] <= 0 || oldToNew[i] < 0 ||
+			   idsToBuild.find(oldIds[i]) != idsToBuild.end())
+			{
+				continue;
+			}
+			const Column & slot = columns_[i];
+			for(size_t v=slot.offset; v<slot.offset+slot.size; ++v)
+			{
+				if(oldToNew[values_[v].first] < 0)
+				{
+					idsToBuild.insert(oldIds[i]);
+					break;
+				}
+			}
+		}
+	}
+
+	// The columns that are carried over, at their new index and packed as they go: the room
+	// left behind by the ones that are gone or built again is not carried with them.
+	std::vector<Column> keptColumns(size);
+	std::vector<std::pair<int, float> > keptValues;
+	keptValues.reserve(used_);
+	size_t keptUsed = 0;
+	size_t carried = 0;
+	for(size_t i=0; i<oldIds.size(); ++i)
+	{
+		const int index = oldToNew[i];
+		if(index < 0 || oldIds[i] <= 0 || idsToBuild.find(oldIds[i]) != idsToBuild.end())
+		{
+			continue;
+		}
+		const Column & slot = columns_[i];
+		Column & kept = keptColumns[index];
+		kept.offset = keptValues.size();
+		kept.size = slot.size;
+		kept.capacity = slot.size;
+		for(size_t v=slot.offset; v<slot.offset+slot.size; ++v)
+		{
+			// The rows of a column are ascending, and so are both id vectors, so a remapped
+			// row stays after the one before it.
+			keptValues.push_back(std::make_pair(oldToNew[values_[v].first], values_[v].second));
+		}
+		keptUsed += slot.size;
+		++carried;
+	}
+	columns_.swap(keptColumns);
+	values_.swap(keptValues);
+	used_ = keptUsed;
+
+	std::vector<float> column(size, 0.0f);
+	for(std::set<int>::const_iterator iter=idsToBuild.begin(); iter!=idsToBuild.end(); ++iter)
+	{
+		this->buildColumn(model, memory, *iter, newIdToIndexMap.at(*iter),
+				newIds, newIdToIndexMap, column, cache);
+	}
+
+	// The virtual place shares what is left of its probability over the visited locations,
+	// so its column depends on how many of them there are.
+	if(newIds[0] < 0)
+	{
+		model.fillVirtualPlaceColumn(&column[0], 1, size);
+		this->takeColumn(column, 0, true);
+	}
+
+	const size_t waste = values_.size() - used_;
+	const bool compacted = waste > used_/4;
+	if(compacted)
+	{
+		this->compact();
+	}
+	ids_ = newIds;
+
+	UDEBUG("Sparse prediction: %d locations removed, %d columns carried over and %d built "
+		   "again of %d, %ld values, %ld left behind%s, updated in %fs",
+			(int)removed, (int)carried, (int)idsToBuild.size(), size,
+			(long)used_, (long)waste, compacted?" (packed again)":"",
+			timer.ticks());
+	return true;
+}
+
+void SparsePrediction::multiply(const std::vector<float> & posterior, std::vector<float> & prior) const
+{
+	const size_t size = columns_.size();
+	UASSERT(size > 0);
+	UASSERT_MSG(posterior.size() == size,
+			uFormat("posterior=%d prediction=%d", (int)posterior.size(), (int)size).c_str());
+
+	prior.assign(size, 0.0f);
+	const float * posteriorPtr = &posterior[0];
+	float * priorPtr = &prior[0];
+
+	// The prior is the sum of the columns of the prediction weighted by the posterior.
+	// Going by column is the order the values are stored in, and lets a location the
+	// posterior has ruled out be skipped whole.
+	for(size_t col=0; col<size; ++col)
+	{
+		const float weight = posteriorPtr[col];
+		if(weight == 0.0f)
+		{
+			continue;
+		}
+		const Column & slot = columns_[col];
+		for(size_t i=slot.offset; i<slot.offset+slot.size; ++i)
+		{
+			priorPtr[values_[i].first] += values_[i].second * weight;
+		}
+	}
+}
+
+} // namespace bayes
+} // namespace rtabmap

corelib/src/bayes/SparsePrediction.h

@@ -0,0 +1,142 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef RTABMAP_BAYES_SPARSEPREDICTION_H_
+#define RTABMAP_BAYES_SPARSEPREDICTION_H_
+
+#include "bayes/PredictionModel.h"
+
+#include <opencv2/core/core.hpp>
+#include <utility>
+#include <vector>
+
+namespace rtabmap {
+
+class Memory;
+
+namespace bayes {
+
+/**
+ * @brief The prediction as its values only, one column at a time.
+ *
+ * A column holds the neighbors of one location within the depth of the model, so on a large
+ * map the matrix DensePrediction would build is mostly zeros: holding it costs the number of
+ * locations squared, against the far smaller number of values in it. Each column is built in a
+ * buffer of its own and only its non-zero values are kept, so nothing of that size is ever
+ * allocated.
+ *
+ * The values of every column live in one array, which the multiplication reads the way memory
+ * likes to be read, and a column keeps the room it was given so that update() can rebuild one
+ * without moving the others.
+ */
+class SparsePrediction
+{
+public:
+	bool empty() const {return columns_.empty();}
+	const std::vector<int> & ids() const {return ids_;}
+	size_t values() const {return used_;}
+	void clear();
+
+	/**
+	 * @brief Builds it for @p ids, whatever its columns come to hold.
+	 *
+	 * The prediction of a model that leaves probability to spread has no zero left in a column
+	 * and nothing sparse to keep, which the caller answers with the matrix rather than asking
+	 * for this. Nothing else falls back to one.
+	 *
+	 * @param cache Filled with the neighborhoods when not null, which update() needs.
+	 */
+	void generate(const PredictionModel & model, const Memory * memory,
+			const std::vector<int> & ids, NeighborsCache * cache);
+
+	/**
+	 * @brief Carries it over to @p ids without walking the graph again.
+	 *
+	 * Only the columns whose contents changed are built again, from the neighborhoods of
+	 * @p cache: the ones of the locations that were not there before, of their neighbors, and
+	 * of the locations that shared their probability with one that is gone. Every other column
+	 * is carried over, at another index when locations were removed.
+	 *
+	 * @return False when there is nothing to carry over: no prediction yet, or the virtual place
+	 *         appearing or disappearing. The caller answers by calling generate(), which is also
+	 *         what fills @p cache.
+	 */
+	bool update(const PredictionModel & model, const Memory * memory,
+			const std::vector<int> & ids, NeighborsCache & cache);
+
+	/// prior = prediction x posterior.
+	void multiply(const std::vector<float> & posterior, std::vector<float> & prior) const;
+
+	/**
+	 * @brief The same prediction as a matrix, for the one caller that wants to look at it.
+	 *
+	 * The matrix costs what keeping the prediction sparse is saving, so this builds one to be
+	 * read, dumped or compared against DensePrediction, and does not keep it.
+	 */
+	cv::Mat toMatrix() const;
+
+	unsigned long memoryUsed() const;
+
+private:
+	/// Where a column sits in values_, and how much room it was given: a column rebuilt into
+	/// more values than it has room for is moved to the end, leaving its room behind until
+	/// compact() recovers it.
+	struct Column
+	{
+		size_t offset = 0;
+		size_t size = 0;
+		size_t capacity = 0;
+	};
+
+	/// update() when @p ids is the ids() it was built for with more appended: every location
+	/// keeps its index, so the columns are updated where they are.
+	bool updateAppended(const PredictionModel & model, const Memory * memory,
+			const std::vector<int> & ids, NeighborsCache & cache);
+
+	/// update() when locations were removed, or came back in the middle of the ones already
+	/// there: the index of a location moves, so the columns are laid out again.
+	bool updateRemapped(const PredictionModel & model, const Memory * memory,
+			const std::vector<int> & ids, NeighborsCache & cache);
+
+	/// Builds one column, from the neighborhood of the location it is for.
+	void buildColumn(const PredictionModel & model, const Memory * memory, int id, int index,
+			const std::vector<int> & ids, const IdToIndexMap & idToIndex,
+			std::vector<float> & buffer, NeighborsCache & cache);
+
+	void takeColumn(std::vector<float> & column, int index, bool withRoomToGrow);
+	void compact();
+
+	std::vector<Column> columns_;
+	std::vector<std::pair<int, float> > values_;
+	size_t used_ = 0;               ///< How many of values_ belong to a column.
+	std::vector<int> ids_;
+};
+
+} // namespace bayes
+} // namespace rtabmap
+
+#endif /* RTABMAP_BAYES_SPARSEPREDICTION_H_ */

corelib/src/optimizer/OptimizerG2O.cpp

@@ -2242,6 +2242,27 @@ bool OptimizerG2O::loadGraph(
 	std::vector<VertexEntry> verticesList;
 	std::vector<EdgeEntry> edgesList;
 
+	// The type of a link, which saveGraph() appends as a column past the fields the
+	// format defines: g2o's own loader reads the fields it knows and ignores what
+	// follows, so the column travels with the file without breaking it. A file written
+	// by anything else has no such column, and the type stays the one its tag implies.
+	// This is the only place the type of an edge can come from: the format has no field
+	// for it, so a loop closure and an odometry link are otherwise the same EDGE_SE2.
+	const auto readType = [](const std::vector<std::string> & v, size_t definedSize, Link::Type fallback)
+	{
+		if(v.size() > definedSize)
+		{
+			const int type = atoi(v[definedSize].c_str());
+			if(type >= 0 && type < Link::kEnd)
+			{
+				return (Link::Type)type;
+			}
+			UWARN("Ignoring link type \"%s\", not one of the %d types.",
+					v[definedSize].c_str(), (int)Link::kEnd);
+		}
+		return fallback;
+	};
+
 	char line[2048];
 	while(fgets(line, 2048, file) != NULL)
 	{
@@ -2301,7 +2322,7 @@ bool OptimizerG2O::loadGraph(
 			e.definitelyLandmark = true;
 			verticesList.push_back(e);
 		}
-		else if(tag == "EDGE_SE2" && v.size() == 12)
+		else if(tag == "EDGE_SE2" && v.size() >= 12)
 		{
 			EdgeEntry e;
 			e.from = atoi(v[1].c_str());
@@ -2314,12 +2335,13 @@ bool OptimizerG2O::loadGraph(
 			e.info.at<double>(1, 1) = uStr2Double(v[9]);
 			e.info.at<double>(1, 5) = e.info.at<double>(5, 1) = uStr2Double(v[10]);
 			e.info.at<double>(5, 5) = uStr2Double(v[11]);
-			e.type = Link::kUndef; // disambiguated after we know landmarkOffset
+			// kUndef is disambiguated after we know landmarkOffset
+			e.type = readType(v, 12, Link::kUndef);
 			e.isPrior = false;
 			e.hasLandmarkEndpoint = false;
 			edgesList.push_back(e);
 		}
-		else if(tag == "EDGE_SE2_XY" && v.size() == 8)
+		else if(tag == "EDGE_SE2_XY" && v.size() >= 8)
 		{
 			EdgeEntry e;
 			e.from = atoi(v[1].c_str());
@@ -2329,12 +2351,12 @@ bool OptimizerG2O::loadGraph(
 			e.info.at<double>(0, 0) = uStr2Double(v[5]);
 			e.info.at<double>(0, 1) = e.info.at<double>(1, 0) = uStr2Double(v[6]);
 			e.info.at<double>(1, 1) = uStr2Double(v[7]);
-			e.type = Link::kLandmark;
+			e.type = readType(v, 8, Link::kLandmark);
 			e.isPrior = false;
 			e.hasLandmarkEndpoint = true;
 			edgesList.push_back(e);
 		}
-		else if((tag == "EDGE_SE3:QUAT" || tag == "EDGE_SE3") && v.size() == 31)
+		else if((tag == "EDGE_SE3:QUAT" || tag == "EDGE_SE3") && v.size() >= 31)
 		{
 			EdgeEntry e;
 			e.from = atoi(v[1].c_str());
@@ -2353,12 +2375,12 @@ bool OptimizerG2O::loadGraph(
 			}
 			// EDGE_SE3 (no :QUAT) is the landmark variant emitted by saveGraph
 			bool landmarkTag = (tag == "EDGE_SE3");
-			e.type = landmarkTag ? Link::kLandmark : Link::kUndef;
+			e.type = readType(v, 31, landmarkTag ? Link::kLandmark : Link::kUndef);
 			e.isPrior = false;
 			e.hasLandmarkEndpoint = landmarkTag;
 			edgesList.push_back(e);
 		}
-		else if(tag == "EDGE_SE3_TRACKXYZ" && v.size() == 13)
+		else if(tag == "EDGE_SE3_TRACKXYZ" && v.size() >= 13)
 		{
 			EdgeEntry e;
 			e.from = atoi(v[1].c_str());
@@ -2372,12 +2394,12 @@ bool OptimizerG2O::loadGraph(
 			e.info.at<double>(1, 1) = uStr2Double(v[10]);
 			e.info.at<double>(1, 2) = e.info.at<double>(2, 1) = uStr2Double(v[11]);
 			e.info.at<double>(2, 2) = uStr2Double(v[12]);
-			e.type = Link::kLandmark;
+			e.type = readType(v, 13, Link::kLandmark);
 			e.isPrior = false;
 			e.hasLandmarkEndpoint = true;
 			edgesList.push_back(e);
 		}
-		else if(tag == "EDGE_PRIOR_SE2" && v.size() == 11)
+		else if(tag == "EDGE_PRIOR_SE2" && v.size() >= 11)
 		{
 			EdgeEntry e;
 			e.from = atoi(v[1].c_str());
@@ -2390,12 +2412,12 @@ bool OptimizerG2O::loadGraph(
 			e.info.at<double>(1, 1) = uStr2Double(v[8]);
 			e.info.at<double>(1, 5) = e.info.at<double>(5, 1) = uStr2Double(v[9]);
 			e.info.at<double>(5, 5) = uStr2Double(v[10]);
-			e.type = Link::kPosePrior;
+			e.type = readType(v, 11, Link::kPosePrior);
 			e.isPrior = true;
 			e.hasLandmarkEndpoint = false;
 			edgesList.push_back(e);
 		}
-		else if(tag == "EDGE_PRIOR_SE2_XY" && v.size() == 7)
+		else if(tag == "EDGE_PRIOR_SE2_XY" && v.size() >= 7)
 		{
 			EdgeEntry e;
 			e.from = atoi(v[1].c_str());
@@ -2407,12 +2429,12 @@ bool OptimizerG2O::loadGraph(
 			e.info.at<double>(1, 1) = uStr2Double(v[6]);
 			// no orientation info on this prior
 			e.info.at<double>(3, 3) = e.info.at<double>(4, 4) = e.info.at<double>(5, 5) = 1.0 / 9999.0;
-			e.type = Link::kPosePrior;
+			e.type = readType(v, 7, Link::kPosePrior);
 			e.isPrior = true;
 			e.hasLandmarkEndpoint = false;
 			edgesList.push_back(e);
 		}
-		else if(tag == "EDGE_SE3_PRIOR" && v.size() == 31)
+		else if(tag == "EDGE_SE3_PRIOR" && v.size() >= 31)
 		{
 			EdgeEntry e;
 			e.from = atoi(v[1].c_str());
@@ -2430,12 +2452,12 @@ bool OptimizerG2O::loadGraph(
 					if(r != c) e.info.at<double>(c, r) = e.info.at<double>(r, c);
 				}
 			}
-			e.type = Link::kPosePrior;
+			e.type = readType(v, 31, Link::kPosePrior);
 			e.isPrior = true;
 			e.hasLandmarkEndpoint = false;
 			edgesList.push_back(e);
 		}
-		else if(tag == "EDGE_POINTXYZ_PRIOR" && v.size() == 11)
+		else if(tag == "EDGE_POINTXYZ_PRIOR" && v.size() >= 11)
 		{
 			EdgeEntry e;
 			e.from = atoi(v[1].c_str());
@@ -2450,12 +2472,12 @@ bool OptimizerG2O::loadGraph(
 			e.info.at<double>(2, 2) = uStr2Double(v[10]);
 			// no orientation info on this prior
 			e.info.at<double>(3, 3) = e.info.at<double>(4, 4) = e.info.at<double>(5, 5) = 1.0 / 9999.0;
-			e.type = Link::kPosePrior;
+			e.type = readType(v, 11, Link::kPosePrior);
 			e.isPrior = true;
 			e.hasLandmarkEndpoint = false;
 			edgesList.push_back(e);
 		}
-		else if(tag == "EDGE_SE2_SWITCHABLE" && v.size() == 13)
+		else if(tag == "EDGE_SE2_SWITCHABLE" && v.size() >= 13)
 		{
 			EdgeEntry e;
 			e.from = atoi(v[1].c_str());
@@ -2469,12 +2491,12 @@ bool OptimizerG2O::loadGraph(
 			e.info.at<double>(1, 1) = uStr2Double(v[10]);
 			e.info.at<double>(1, 5) = e.info.at<double>(5, 1) = uStr2Double(v[11]);
 			e.info.at<double>(5, 5) = uStr2Double(v[12]);
-			e.type = Link::kUndef;
+			e.type = readType(v, 13, Link::kUndef);
 			e.isPrior = false;
 			e.hasLandmarkEndpoint = false;
 			edgesList.push_back(e);
 		}
-		else if(tag == "EDGE_SE3_SWITCHABLE" && v.size() == 32)
+		else if(tag == "EDGE_SE3_SWITCHABLE" && v.size() >= 32)
 		{
 			EdgeEntry e;
 			e.from = atoi(v[1].c_str());
@@ -2492,7 +2514,7 @@ bool OptimizerG2O::loadGraph(
 					if(r != c) e.info.at<double>(c, r) = e.info.at<double>(r, c);
 				}
 			}
-			e.type = Link::kUndef;
+			e.type = readType(v, 32, Link::kUndef);
 			e.isPrior = false;
 			e.hasLandmarkEndpoint = false;
 			edgesList.push_back(e);
@@ -2743,8 +2765,35 @@ bool OptimizerG2O::saveGraph(
 		}
 
 		int virtualVertexId = landmarkOffset - (poses.size()&&poses.rbegin()->first<0?poses.rbegin()->first:0);
+
+		// A link is stored on both of the nodes it connects, so a caller iterating them
+		// hands us each one twice, once per direction. g2o has no notion of a reverse
+		// edge: it would read the two lines as two independent constraints and count the
+		// information of every link twice. Only the first direction of a pair is written,
+		// which is also half the file. Links on a single node (a prior, gravity) are not
+		// pairs and are left alone.
+		std::set<std::pair<int, int> > writtenPairs;
+
 		for(std::multimap<int, Link>::const_iterator iter = edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
 		{
+			if(iter->second.from() != iter->second.to())
+			{
+				const std::pair<int, int> pair(
+						std::min(iter->second.from(), iter->second.to()),
+						std::max(iter->second.from(), iter->second.to()));
+				if(!writtenPairs.insert(pair).second)
+				{
+					continue;
+				}
+			}
+
+			// The type of the link, as a column past the fields the format defines. g2o's
+			// own loader reads the fields it knows and ignores what follows, so this
+			// travels with the file without breaking it, and loadGraph() reads it back.
+			// Without it the type is lost on export, and the type is what tells a loop
+			// closure from an odometry link.
+			const std::string typeSuffix = uFormat(" %d", (int)iter->second.type());
+
 			if (iter->second.type() == Link::kLandmark)
 			{
 				if (this->landmarksIgnored())
@@ -2760,7 +2809,7 @@ bool OptimizerG2O::saveGraph(
 					if(uValue(isLandmarkWithRotation, landmarkId, false))
 					{
 						// EDGE_SE2 observed_vertex_id observing_vertex_id x y qx qy qz qw inf_11 inf_12 inf_13 inf_22 inf_23 inf_33
-						fprintf(file, "EDGE_SE2 %d %d %f %f %f %f %f %f %f %f %f\n",
+						fprintf(file, "EDGE_SE2 %d %d %f %f %f %f %f %f %f %f %f%s\n",
 								iter->second.from()<0?landmarkOffset-iter->second.from():iter->second.from(),
 								iter->second.to()<0?landmarkOffset-iter->second.to():iter->second.to(),
 								iter->second.transform().x(),
@@ -2771,19 +2820,21 @@ bool OptimizerG2O::saveGraph(
 								iter->second.infMatrix().at<double>(0, 5),
 								iter->second.infMatrix().at<double>(1, 1),
 								iter->second.infMatrix().at<double>(1, 5),
-								iter->second.infMatrix().at<double>(5, 5));
+								iter->second.infMatrix().at<double>(5, 5),
+								typeSuffix.c_str());
 					}
 					else
 					{
 						// EDGE_SE2_XY observed_vertex_id observing_vertex_id x y inf_11 inf_12 inf_22
-						fprintf(file, "EDGE_SE2_XY %d %d %f %f %f %f %f\n",
+						fprintf(file, "EDGE_SE2_XY %d %d %f %f %f %f %f%s\n",
 							iter->second.from()<0?landmarkOffset-iter->second.from():iter->second.from(),
 							iter->second.to()<0?landmarkOffset-iter->second.to():iter->second.to(),
 							iter->second.transform().x(),
 							iter->second.transform().y(),
 							iter->second.infMatrix().at<double>(0, 0),
 							iter->second.infMatrix().at<double>(0, 1),
-							iter->second.infMatrix().at<double>(1, 1));
+							iter->second.infMatrix().at<double>(1, 1),
+							typeSuffix.c_str());
 					}
 				}
 				else
@@ -2792,7 +2843,7 @@ bool OptimizerG2O::saveGraph(
 					{
 						// EDGE_SE3 observed_vertex_id observing_vertex_id x y z qx qy qz qw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
 						Eigen::Quaternionf q = iter->second.transform().getQuaternionf();
-						fprintf(file, "EDGE_SE3 %d %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
+						fprintf(file, "EDGE_SE3 %d %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f%s\n",
 								iter->second.from()<0?landmarkOffset-iter->second.from():iter->second.from(),
 								iter->second.to()<0?landmarkOffset-iter->second.to():iter->second.to(),
 								iter->second.transform().x(),
@@ -2822,12 +2873,13 @@ bool OptimizerG2O::saveGraph(
 								iter->second.infMatrix().at<double>(3, 5),
 								iter->second.infMatrix().at<double>(4, 4),
 								iter->second.infMatrix().at<double>(4, 5),
-								iter->second.infMatrix().at<double>(5, 5));
+								iter->second.infMatrix().at<double>(5, 5),
+								typeSuffix.c_str());
 					}
 					else
 					{
 						// EDGE_SE3_TRACKXYZ observed_vertex_id observing_vertex_id param_offset x y z inf_11 inf_12 inf_13 inf_22 inf_23 inf_33
-						fprintf(file, "EDGE_SE3_TRACKXYZ %d %d %d %f %f %f %f %f %f %f %f %f\n",
+						fprintf(file, "EDGE_SE3_TRACKXYZ %d %d %d %f %f %f %f %f %f %f %f %f%s\n",
 							iter->second.from()<0?landmarkOffset-iter->second.from():iter->second.from(),
 							iter->second.to()<0?landmarkOffset-iter->second.to():iter->second.to(),
 							PARAM_OFFSET,
@@ -2839,7 +2891,8 @@ bool OptimizerG2O::saveGraph(
 							iter->second.infMatrix().at<double>(0, 2),
 							iter->second.infMatrix().at<double>(1, 1),
 							iter->second.infMatrix().at<double>(1, 2),
-							iter->second.infMatrix().at<double>(2, 2));
+							iter->second.infMatrix().at<double>(2, 2),
+							typeSuffix.c_str());
 					}
 				}
 				continue;
@@ -2911,7 +2964,7 @@ bool OptimizerG2O::saveGraph(
 				{
 					// EDGE_SE2 observed_vertex_id observing_vertex_id x y qx qy qz qw inf_11 inf_12 inf_13 inf_22 inf_23 inf_33
 					// EDGE_SE2_PRIOR observed_vertex_id x y qx qy qz qw inf_11 inf_12 inf_13 inf_22 inf_23 inf_33
-					fprintf(file, "%s %d%s%s %f %f %f %f %f %f %f %f %f\n",
+					fprintf(file, "%s %d%s%s %f %f %f %f %f %f %f %f %f%s\n",
         					prefix.c_str(),
         					iter->second.from(),
         					to.c_str(),
@@ -2924,13 +2977,14 @@ bool OptimizerG2O::saveGraph(
         					iter->second.infMatrix().at<double>(0, 5),
         					iter->second.infMatrix().at<double>(1, 1),
         					iter->second.infMatrix().at<double>(1, 5),
-        					iter->second.infMatrix().at<double>(5, 5));
+        					iter->second.infMatrix().at<double>(5, 5),
+        					typeSuffix.c_str());
 				}
 				else
 				{
 					// EDGE_XY observed_vertex_id observing_vertex_id x y inf_11 inf_12 inf_22
 					// EDGE_POINTXY_PRIOR x y inf_11 inf_12 inf_22
-					fprintf(file, "%s %d%s%s %f %f %f %f %f\n",
+					fprintf(file, "%s %d%s%s %f %f %f %f %f%s\n",
         					prefix.c_str(),
         					iter->second.from(),
         					to.c_str(),
@@ -2939,7 +2993,8 @@ bool OptimizerG2O::saveGraph(
         					iter->second.transform().y(),
         					iter->second.infMatrix().at<double>(0, 0),
         					iter->second.infMatrix().at<double>(0, 1),
-        					iter->second.infMatrix().at<double>(1, 1));
+        					iter->second.infMatrix().at<double>(1, 1),
+        					typeSuffix.c_str());
 				}
 			}
 			else
@@ -2949,7 +3004,7 @@ bool OptimizerG2O::saveGraph(
 					// EDGE_SE3 observed_vertex_id observing_vertex_id x y z qx qy qz qw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
 					// EDGE_SE3_PRIOR observed_vertex_id offset_parameter_id x y z qx qy qz qw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
 					Eigen::Quaternionf q = iter->second.transform().getQuaternionf();
-					fprintf(file, "%s %d%s%s %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
+					fprintf(file, "%s %d%s%s %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f%s\n",
         					prefix.c_str(),
         					iter->second.from(),
         					to.c_str(),
@@ -2981,13 +3036,14 @@ bool OptimizerG2O::saveGraph(
         					iter->second.infMatrix().at<double>(3, 5),
         					iter->second.infMatrix().at<double>(4, 4),
         					iter->second.infMatrix().at<double>(4, 5),
-        					iter->second.infMatrix().at<double>(5, 5));
+        					iter->second.infMatrix().at<double>(5, 5),
+        					typeSuffix.c_str());
 				}
 				else
 				{
 					// EDGE_XYZ observed_vertex_id observing_vertex_id x y z qx qy qz qw inf_11 inf_12 .. inf_13 inf_22 .. inf_33
 					// EDGE_POINTXYZ_PRIOR observed_vertex_id x y z inf_11 inf_12 .. inf_13 inf_22 .. inf_33
-					fprintf(file, "%s %d%s%s %f %f %f %f %f %f %f %f %f\n",
+					fprintf(file, "%s %d%s%s %f %f %f %f %f %f %f %f %f%s\n",
         					prefix.c_str(),
         					iter->second.from(),
         					to.c_str(),
@@ -3000,7 +3056,8 @@ bool OptimizerG2O::saveGraph(
         					iter->second.infMatrix().at<double>(0, 2),
         					iter->second.infMatrix().at<double>(1, 1),
         					iter->second.infMatrix().at<double>(1, 2),
-        					iter->second.infMatrix().at<double>(2, 2));
+        					iter->second.infMatrix().at<double>(2, 2),
+        					typeSuffix.c_str());
 				}
 			}
 		}

corelib/test/CMakeLists.txt

@@ -119,6 +119,24 @@ IF(BUILD_PERF_TESTS)
 	set_tests_properties(test_flann_index_perf PROPERTIES
 			TIMEOUT ${_perf_timeout}
 			LABELS  "performance")
+
+	# Comparison of the dense and the sparse prediction x posterior multiplication of
+	# BayesFilter (Bayes/SparsePrediction), against the size of the map, how connected
+	# its graph is and the depth of the prediction model. Over synthetic graphs, and over
+	# the graph of a real map read from data/tests/large_reduced_graph.g2o, whose link
+	# types decide how much of the map a column of the prediction holds:
+	#   bin/test_bayesfilter_perf
+	#   bin/test_bayesfilter_perf --gtest_filter=-*LargeMap*:-*RealMap*
+	# Its own executable: it spends its time benchmarking rather than asserting, and the
+	# largest maps it builds allocate a gigabyte for the dense prediction matrix,
+	# which in a unit test shard would look like a leak.
+	add_executable(test_bayesfilter_perf perf_bayesfilter.cpp)
+	target_link_libraries(test_bayesfilter_perf gtest_main rtabmap_core)
+
+	add_test(NAME test_bayesfilter_perf COMMAND test_bayesfilter_perf)
+	set_tests_properties(test_bayesfilter_perf PROPERTIES
+			TIMEOUT ${_perf_timeout}
+			LABELS  "performance")
 ENDIF(BUILD_PERF_TESTS)
 
 # Rtabmap end-to-end replay of sample DBs (test data fetched by

corelib/test/perf_bayesfilter.cpp

@@ -0,0 +1,736 @@
+// Comparison of the dense and the sparse (Parameters::kBayesSparsePrediction())
+// multiplication of the prediction matrix with the last posterior, which is where
+// BayesFilter::computePosterior() spends nearly all of its time on a large map.
+//
+// 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_bayesfilter_perf --gtest_filter=*Growing*
+//
+// The times are reported rather than asserted on, as they depend on the machine.
+// What is asserted is that both multiplications give the same posterior, so that
+// the numbers below compare two ways of computing the same thing.
+#include <gtest/gtest.h>
+#include <rtabmap/core/BayesFilter.h>
+#include <rtabmap/core/Graph.h>
+#include <rtabmap/core/Link.h>
+#include <rtabmap/core/Memory.h>
+#include <rtabmap/core/Optimizer.h>
+#include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/SensorData.h>
+#include <rtabmap/core/Signature.h>
+#include <rtabmap/core/Transform.h>
+#include <rtabmap/utilite/UFile.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <algorithm>
+#include <cmath>
+#include <cstdio>
+#include <iostream>
+#include <map>
+#include <vector>
+
+using namespace rtabmap;
+
+namespace {
+
+// Sizes of the maps compared. The dense prediction matrix is n x n floats, so the
+// largest one below already allocates 244 MB.
+static const int MAP_SIZES[] = {1000, 4000, 8000};
+
+// How many values of the prediction model decides how deep in the graph a column of
+// the prediction matrix reaches, and thus how many values it holds. The default model
+// has 18, so it stores neighbors up to 17 links away, with probabilities down to
+// 6.9e-23. Truncating it to 8 keeps every value above 1e-4 and stops there. The
+// difference is given to the loop closure probability so that the values still sum to
+// slightly more than 1: below 1, normalize() spreads what is missing over every zero
+// of a column and the matrix is no longer sparse at all.
+static const char PREDICTION_DEFAULT[] =
+		"0.1 0.36 0.30 0.16 0.062 0.0151 0.00255 0.000324 2.5e-05 1e-06 4.8e-08 "
+		"1.2e-09 1.9e-11 2.2e-13 1.7e-15 8.5e-18 2.9e-20 6.9e-23";
+static const char PREDICTION_TRUNCATED[] =
+		"0.1 0.36003 0.30 0.16 0.062 0.0151 0.00255 0.000324";
+
+// A chain of signatures linked by odometry, with a global loop closure every
+// loopEvery nodes back to the node loopSpan earlier. The loop closures matter here:
+// Memory::getNeighborsId() follows them, so each one is a shortcut that widens the
+// neighborhood a column of the prediction matrix holds. They are what decides how
+// sparse the matrix is, so they are a knob of these benchmarks rather than a detail.
+class SyntheticMap
+{
+public:
+	// Built by a mapping session, then turned to localization mode unless asked
+	// otherwise: the graph is then fixed, which is what the sparse prediction is built
+	// for, and what the dense one is compared against here.
+	SyntheticMap(int nodes, int loopEvery, int loopSpan, bool localization = true)
+	{
+		ParametersMap params;
+		// No features extracted: the graph is what the prediction matrix is built from.
+		params.insert(ParametersPair(Parameters::kKpMaxFeatures(), "-1"));
+		// Only the latest signature stays in STM, the rest are WM nodes the filter uses.
+		params.insert(ParametersPair(Parameters::kMemSTMSize(), "1"));
+		params.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0"));
+		params.insert(ParametersPair(Parameters::kMemBinDataKept(), "false"));
+		memory_ = new Memory(params);
+
+		const cv::Mat image(8, 8, CV_8UC1, cv::Scalar(128));
+		const cv::Mat covariance = cv::Mat::eye(6, 6, CV_64FC1) * 0.01;
+		const cv::Mat information = cv::Mat::eye(6, 6, CV_64FC1);
+
+		UTimer timer;
+		std::vector<int> ids;
+		ids.reserve(nodes);
+		for(int i=0; i<nodes; ++i)
+		{
+			SensorData data(image);
+			UASSERT(memory_->update(data, Transform(float(i), 0.0f, 0.0f, 0, 0, 0), covariance));
+			ids.push_back(memory_->getLastSignatureId());
+			if(loopEvery > 0 && i >= loopSpan && i % loopEvery == 0)
+			{
+				UASSERT(memory_->addLink(Link(
+						ids.back(),
+						ids[ids.size()-1-loopSpan],
+						Link::kGlobalClosure,
+						Transform::getIdentity(),
+						information)));
+				++loopClosures_;
+			}
+		}
+		buildTime_ = timer.ticks();
+
+		if(localization)
+		{
+			ParametersMap localizationParams;
+			localizationParams.insert(ParametersPair(Parameters::kMemIncrementalMemory(), "false"));
+			memory_->parseParameters(localizationParams);
+			UASSERT(!memory_->isIncremental());
+		}
+	}
+
+	~SyntheticMap()
+	{
+		delete memory_;
+	}
+
+	const Memory * memory() const {return memory_;}
+	int loopClosures() const {return loopClosures_;}
+	double buildTime() const {return buildTime_;}
+
+	// What Rtabmap passes to the filter: the virtual place (new location hypothesis)
+	// followed by the WM locations that are not in STM.
+	std::vector<int> bayesIds() const
+	{
+		std::vector<int> ids;
+		ids.push_back(Memory::kIdVirtual);
+		const std::set<int> & stm = memory_->getStMem();
+		for(std::map<int, double>::const_iterator iter=memory_->getWorkingMem().begin();
+			iter!=memory_->getWorkingMem().end();
+			++iter)
+		{
+			if(iter->first > 0 && stm.find(iter->first) == stm.end())
+			{
+				ids.push_back(iter->first);
+			}
+		}
+		return ids;
+	}
+
+	// Uniform, which is the worst case for the sparse multiplication: no location is
+	// ruled out, so no column of the prediction can be skipped whole.
+	std::map<int, float> uniformLikelihood(const std::vector<int> & ids) const
+	{
+		std::map<int, float> likelihood;
+		for(size_t i=0; i<ids.size(); ++i)
+		{
+			likelihood.insert(std::make_pair(ids[i], 1.0f));
+		}
+		return likelihood;
+	}
+
+private:
+	Memory * memory_ = nullptr;
+	int loopClosures_ = 0;
+	double buildTime_ = 0.0;
+};
+
+// A real map's graph, read from the g2o file it was exported to. What makes it worth
+// measuring against the synthetic graphs above is its link types: the file holds mostly
+// merged neighbor links, which cost a margin like an ordinary neighbor, and only a few
+// hundred closures that Memory::getNeighborsId() follows without spending one. How many
+// of those there are is what decides how much of the map a column of the prediction
+// holds, so a real graph's answer is not a synthetic one's.
+//
+// The graph is rebuilt in a Memory rather than optimized: Memory::update() creates a
+// signature per pose and links each to the previous one, so the links the file does not
+// have are removed and the ones it has are added with their own type.
+// A real map's graph, read from the g2o file it was exported to. What makes it worth
+// measuring against the synthetic graphs above is its link types: how many links
+// Memory::getNeighborsId() follows without spending a margin is what decides how much of
+// the map a column of the prediction holds, and a real graph's answer is not a synthetic
+// one's. A graph that went through the reduction holds mostly merged neighbor links,
+// which cost a margin like an ordinary neighbor; one that did not holds none.
+struct RealGraph
+{
+	std::vector<int> ids;                              // the locations, in the order they were created
+	std::map<int, Transform> poses;
+	// The links between two locations, as indices into ids, each on the later of the two:
+	// that is the one a mapping session adds them on.
+	std::vector<std::vector<std::pair<size_t, Link::Type> > > linksTo;
+	std::map<int, int> byType;
+	int skippedLinks = 0;
+	double loadTime = 0.0;
+};
+
+bool loadRealGraph(const std::string & path, RealGraph & graph)
+{
+	UTimer timer;
+	std::map<int, Transform> poses;
+	std::multimap<int, Link> links;
+	if(!graph::importPoses(path, 4 /*g2o*/, poses, &links))
+	{
+		return false;
+	}
+	graph.loadTime = timer.ticks();
+	graph.poses = poses;
+
+	for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+	{
+		if(iter->first > 0)
+		{
+			graph.ids.push_back(iter->first);
+		}
+	}
+	std::sort(graph.ids.begin(), graph.ids.end());
+	std::map<int, size_t> indexOf;
+	for(size_t i=0; i<graph.ids.size(); ++i)
+	{
+		indexOf.insert(std::make_pair(graph.ids[i], i));
+	}
+
+	// One entry per pair of locations. Links on a single location (a prior, gravity) and
+	// landmark observations are left out: getNeighborsId() doesn't walk the first, and the
+	// second would need the landmark index of a memory that mapped them.
+	graph.linksTo.resize(graph.ids.size());
+	std::set<std::pair<size_t, size_t> > seen;
+	for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
+	{
+		const Link & link = iter->second;
+		if(link.from() == link.to() || link.from() < 0 || link.to() < 0)
+		{
+			++graph.skippedLinks;
+			continue;
+		}
+		const size_t a = indexOf.at(link.from()), b = indexOf.at(link.to());
+		if(!seen.insert(std::make_pair(std::min(a,b), std::max(a,b))).second)
+		{
+			continue;
+		}
+		graph.linksTo[std::max(a,b)].push_back(std::make_pair(std::min(a,b), link.type()));
+		++graph.byType[link.type()];
+	}
+	return true;
+}
+
+// Adds one location and the links the graph has between it and the ones already there.
+// Memory::update() links each new signature to the previous one as a kNeighbor, so that
+// one is dropped when the graph does not have it, or has it with another type.
+void addRealNode(
+		Memory * memory,
+		const RealGraph & graph,
+		size_t index,
+		std::vector<int> & newIds,
+		int * removedLinks = 0)
+{
+	static const cv::Mat image(8, 8, CV_8UC1, cv::Scalar(128));
+	static const cv::Mat covariance = cv::Mat::eye(6, 6, CV_64FC1) * 0.0001;
+	static const cv::Mat information = cv::Mat::eye(6, 6, CV_64FC1);
+
+	SensorData data(image);
+	UASSERT(memory->update(data, graph.poses.at(graph.ids[index]), covariance));
+	newIds.push_back(memory->getLastSignatureId());
+
+	bool previousLinked = false;
+	for(size_t i=0; i<graph.linksTo[index].size(); ++i)
+	{
+		const size_t other = graph.linksTo[index][i].first;
+		const Link::Type type = graph.linksTo[index][i].second;
+		if(other == index-1 && type == Link::kNeighbor)
+		{
+			previousLinked = true;   // update() already made this one
+			continue;
+		}
+		memory->addLink(Link(newIds[index], newIds[other], type,
+				Transform::getIdentity(), information));
+	}
+	if(index > 0 && !previousLinked)
+	{
+		// Either the graph has no link between these two, or it has one of another type
+		// which the loop above has just added.
+		memory->removeLink(newIds[index-1], newIds[index]);
+		if(removedLinks)
+		{
+			++(*removedLinks);
+		}
+	}
+}
+
+Memory * newRealMemory()
+{
+	ParametersMap params;
+	params.insert(ParametersPair(Parameters::kKpMaxFeatures(), "-1"));
+	params.insert(ParametersPair(Parameters::kMemSTMSize(), "1"));
+	params.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0"));
+	params.insert(ParametersPair(Parameters::kMemBinDataKept(), "false"));
+	return new Memory(params);
+}
+
+// What Rtabmap passes to the filter: the virtual place followed by the locations of the
+// working memory that are not in the short term memory.
+std::vector<int> bayesIdsOf(const Memory * memory)
+{
+	std::vector<int> ids;
+	ids.push_back(Memory::kIdVirtual);
+	const std::set<int> & stm = memory->getStMem();
+	for(std::map<int, double>::const_iterator iter=memory->getWorkingMem().begin();
+		iter!=memory->getWorkingMem().end();
+		++iter)
+	{
+		if(iter->first > 0 && stm.find(iter->first) == stm.end())
+		{
+			ids.push_back(iter->first);
+		}
+	}
+	return ids;
+}
+
+std::map<int, float> uniformLikelihoodOf(const std::vector<int> & ids)
+{
+	std::map<int, float> likelihood;
+	for(size_t i=0; i<ids.size(); ++i)
+	{
+		likelihood.insert(std::make_pair(ids[i], 1.0f));
+	}
+	return likelihood;
+}
+
+const char * linkTypeName(int type)
+{
+	switch(type)
+	{
+	case Link::kNeighbor:           return "kNeighbor";
+	case Link::kGlobalClosure:      return "kGlobalClosure";
+	case Link::kLocalSpaceClosure:  return "kLocalSpaceClosure";
+	case Link::kLocalTimeClosure:   return "kLocalTimeClosure";
+	case Link::kUserClosure:        return "kUserClosure";
+	case Link::kVirtualClosure:     return "kVirtualClosure";
+	case Link::kNeighborMerged:     return "kNeighborMerged";
+	case Link::kPosePrior:          return "kPosePrior";
+	case Link::kLandmark:           return "kLandmark";
+	case Link::kGravity:            return "kGravity";
+	default:                        return "other";
+	}
+}
+
+void printGraph(const std::string & path, const RealGraph & graph, int removedLinks)
+{
+	std::cout << "[          ] " << path << ": read in " << graph.loadTime << "s, "
+			  << graph.ids.size() << " locations, links:";
+	for(std::map<int,int>::const_iterator iter=graph.byType.begin(); iter!=graph.byType.end(); ++iter)
+	{
+		std::cout << " " << linkTypeName(iter->first) << "=" << iter->second;
+	}
+	std::cout << " (" << graph.skippedLinks << " on a single location or on a landmark not rebuilt, "
+			  << removedLinks << " gaps in the chain)" << std::endl;
+}
+// The posterior as a map, which the filter no longer builds: it holds the locations and
+// their probabilities as two vectors, and a test reads them more easily as a map.
+static std::map<int, float> posteriorOf(const BayesFilter & filter)
+{
+	const std::vector<int> & ids = filter.getPosteriorIds();
+	const std::vector<float> & values = filter.getPosteriorValues();
+	std::map<int, float> posterior;
+	for(size_t i = 0; i < ids.size(); ++i)
+	{
+		posterior.insert(posterior.end(), std::make_pair(ids[i], values[i]));
+	}
+	return posterior;
+}
+
+struct Result
+{
+	double firstIteration = 0.0;   // includes generating the prediction, sparse or dense
+	double steadyState = 0.0;      // fastest of the following iterations, the prediction being unchanged
+	unsigned long memoryUsed = 0;
+	std::map<int, float> posterior;
+};
+
+Result run(const Memory * memory,
+		const std::vector<int> & ids,
+		const std::map<int, float> & likelihood,
+		const char * predictionLC,
+		bool sparse,
+		int iterations)
+{
+	ParametersMap params;
+	params.insert(ParametersPair(Parameters::kBayesPredictionLC(), predictionLC));
+	params.insert(ParametersPair(Parameters::kBayesSparsePrediction(), sparse?"true":"false"));
+	BayesFilter filter(params);
+
+	Result result;
+	UTimer timer;
+	filter.computePosterior(memory, likelihood);
+	result.firstIteration = timer.ticks();
+
+	// A few untimed iterations to let the caches and the processor clock settle before
+	// measuring. A fixed count, the same whatever the mode: the filter is recursive, so
+	// how many iterations it has run decides where its posterior is, and warming up for a
+	// fixed duration instead would run hundreds of them in the fast mode against one in
+	// the slow one and leave the two posteriors nowhere near each other.
+	for(int i=0; i<3; ++i)
+	{
+		filter.computePosterior(memory, likelihood);
+	}
+
+	// The fastest iteration rather than the mean or the median of them. Everything that
+	// makes an iteration slower than its own best is the machine rather than the code
+	// being measured, and the dense multiplication reads the whole prediction matrix from
+	// memory, which makes it sensitive to whatever else is using that memory. The fastest
+	// is the one measurement of the run that is the least of it, so it is the one that
+	// compares between runs and between machines.
+	double best = 0.0;
+	for(int i=1; i<iterations; ++i)
+	{
+		timer.restart();
+		filter.computePosterior(memory, likelihood);
+		const double elapsed = timer.ticks();
+		if(best == 0.0 || elapsed < best)
+		{
+			best = elapsed;
+		}
+	}
+	result.steadyState = best > 0.0 ? best : result.firstIteration;
+	result.memoryUsed = filter.getMemoryUsed();
+	result.posterior = posteriorOf(filter);
+	return result;
+}
+
+// The two multiplications sum the products of a row in a different order, so the
+// posteriors differ by the rounding of a few thousand float additions rather than
+// being bit identical. Reported relative to the largest probability, which is the
+// scale Rtabmap compares hypotheses at.
+double maxPosteriorDifference(const std::map<int, float> & a, const std::map<int, float> & b)
+{
+	if(a.size() != b.size())
+	{
+		return 1.0;
+	}
+	double maxDiff = 0.0;
+	double maxValue = 0.0;
+	for(std::map<int, float>::const_iterator iter=a.begin(); iter!=a.end(); ++iter)
+	{
+		std::map<int, float>::const_iterator jter = b.find(iter->first);
+		if(jter == b.end())
+		{
+			return 1.0;
+		}
+		maxDiff = std::max(maxDiff, std::fabs(double(iter->second) - double(jter->second)));
+		maxValue = std::max(maxValue, std::fabs(double(iter->second)));
+	}
+	return maxValue > 0.0 ? maxDiff/maxValue : maxDiff;
+}
+
+void report(const char * name, const Result & result)
+{
+	printf("[          ]   %-7s first iteration %9.1f ms, fastest iteration %8.2f ms, filter memory %7.1f MB\n",
+			name, result.firstIteration*1000.0, result.steadyState*1000.0, result.memoryUsed/1048576.0);
+}
+
+// sparseFirst measures the sparse mode before the dense one. It matters on a large map:
+// the dense mode allocates the prediction matrix, and running it first leaves the
+// allocator holding hundreds of megabytes, which the sparse measurement that follows then
+// pays for. Measuring the two in separate processes is the only way to have both clean;
+// within one, the cheaper mode is the one to protect.
+void compare(const Memory * memory,
+		const std::vector<int> & ids,
+		const std::map<int, float> & likelihood,
+		const char * predictionLC,
+		int iterations,
+		bool sparseFirst = false)
+{
+	const size_t size = ids.size();
+	Result dense, sparse;
+	if(sparseFirst)
+	{
+		sparse = run(memory, ids, likelihood, predictionLC, true, iterations);
+		dense = run(memory, ids, likelihood, predictionLC, false, iterations);
+	}
+	else
+	{
+		dense = run(memory, ids, likelihood, predictionLC, false, iterations);
+		sparse = run(memory, ids, likelihood, predictionLC, true, iterations);
+	}
+
+	report("dense", dense);
+	report("sparse", sparse);
+	printf("[          ]   steady state speedup x%.1f, dense/sparse memory x%.2f, "
+		   "relative posterior difference %.1e\n",
+			sparse.steadyState > 0.0 ? dense.steadyState/sparse.steadyState : 0.0,
+			sparse.memoryUsed > 0 ? double(dense.memoryUsed)/double(sparse.memoryUsed) : 0.0,
+			maxPosteriorDifference(dense.posterior, sparse.posterior));
+
+	EXPECT_EQ(dense.posterior.size(), size);
+	// Same probabilities up to the rounding of the sums, which the iterations compound:
+	// the sums are of a few thousand floats spanning the whole range of the model, down
+	// to 6.9e-23 for the default one, and each iteration starts from the previous
+	// posterior. Which location comes out highest is not compared: the likelihood is
+	// uniform here, so the visited locations are all within rounding of each other and
+	// the highest is whichever the rounding favors.
+	EXPECT_LT(maxPosteriorDifference(dense.posterior, sparse.posterior), 1e-3);
+}
+
+} // namespace
+
+// Both modes on the same graph, over maps of growing size. The dense multiplication
+// reads the whole n x n matrix on every iteration, so its cost grows with the square
+// of the number of nodes, while the sparse one grows with the number of values the
+// graph actually puts in the matrix.
+TEST(BayesFilterPerfTest, DenseVsSparsePredictionOnGrowingMaps)
+{
+	// The steady state of the smaller maps is a fraction of a millisecond, so enough
+	// iterations for the fastest of them to be a stable number.
+	const int iterations = 30;
+
+	for(size_t s=0; s<sizeof(MAP_SIZES)/sizeof(int); ++s)
+	{
+		const int nodes = MAP_SIZES[s];
+		// A moderately connected graph, so that this measures the effect of the size.
+		// How much the connectivity itself matters is measured by the test below.
+		SyntheticMap map(nodes, 100, 500);
+		const size_t size = map.bayesIds().size();
+
+		std::cout << "[          ] " << nodes << " nodes (" << size << " ids with the virtual place), "
+				  << map.loopClosures() << " loop closures, graph built in " << map.buildTime()
+				  << "s, dense matrix = " << (size*size*sizeof(float))/1048576 << " MB" << std::endl;
+
+		const std::vector<int> ids = map.bayesIds();
+		compare(map.memory(), ids, map.uniformLikelihood(ids), PREDICTION_DEFAULT, iterations);
+	}
+}
+
+// How sparse the prediction matrix is, and so how much the sparse multiplication can
+// win, is decided by two things: how connected the graph is, every loop closure being
+// a shortcut that getNeighborsId() follows, and how deep the prediction model reaches.
+// The default 18 values model stores neighbors up to 17 links away with probabilities
+// down to 6.9e-23, which are numerically irrelevant next to the 0.36 of the first
+// level but fill most of the matrix.
+TEST(BayesFilterPerfTest, SparsityAgainstGraphConnectivityAndModelDepth)
+{
+	const int nodes = 4000;
+	const int iterations = 30;
+
+	struct Connectivity { const char * name; int loopEvery; int loopSpan; };
+	const Connectivity connectivities[] = {
+		{"chain only, no loop closure", 0, 0},
+		{"a loop closure every 100 nodes, spanning 500", 100, 500},
+		{"a loop closure every 20 nodes, spanning 200", 20, 200},
+	};
+
+	for(size_t c=0; c<sizeof(connectivities)/sizeof(Connectivity); ++c)
+	{
+		SyntheticMap map(nodes, connectivities[c].loopEvery, connectivities[c].loopSpan);
+		std::cout << "[          ] " << nodes << " nodes, " << connectivities[c].name
+				  << " (" << map.loopClosures() << " loop closures)" << std::endl;
+
+		const std::vector<int> ids = map.bayesIds();
+		const std::map<int, float> likelihood = map.uniformLikelihood(ids);
+		std::cout << "[          ]  18 values model (default, depth 17):" << std::endl;
+		compare(map.memory(), ids, likelihood, PREDICTION_DEFAULT, iterations);
+		std::cout << "[          ]   8 values model (depth 7, every value above 1e-4):" << std::endl;
+		compare(map.memory(), ids, likelihood, PREDICTION_TRUNCATED, iterations);
+	}
+}
+
+// The size of a real large map, over which a localization session iterates without
+// ever changing the graph: the prediction matrix is generated once and every
+// following iteration reuses it, so the sparse view is built once too. This is the
+// case the sparse multiplication is for.
+//
+// The dense matrix alone is a gigabyte at that size, and takes seconds to generate;
+// exclude this one with
+//   bin/test_bayesfilter_perf --gtest_filter=-*LargeMap*
+TEST(BayesFilterPerfTest, DenseVsSparsePredictionOnALargeMap)
+{
+	const int nodes = 16384;
+	const int iterations = 10;
+
+	SyntheticMap map(nodes, 100, 500);
+	const size_t size = map.bayesIds().size();
+	std::cout << "[          ] " << nodes << " nodes (" << size << " ids with the virtual place), "
+			  << map.loopClosures() << " loop closures, graph built in " << map.buildTime()
+			  << "s, dense matrix = " << (size*size*sizeof(float))/1048576 << " MB" << std::endl;
+
+	const std::vector<int> ids = map.bayesIds();
+	const std::map<int, float> likelihood = map.uniformLikelihood(ids);
+	std::cout << "[          ]  18 values model (default, depth 17):" << std::endl;
+	compare(map.memory(), ids, likelihood, PREDICTION_DEFAULT, iterations);
+	std::cout << "[          ]   8 values model (depth 7, every value above 1e-4):" << std::endl;
+	compare(map.memory(), ids, likelihood, PREDICTION_TRUNCATED, iterations);
+}
+
+// Mapping mode, over a graph that has stopped growing: the matrix is kept, because
+// updatePrediction() needs it to carry its unchanged columns over whenever the graph does
+// grow, and the sparse form is taken from it rather than built instead of it. It is worth
+// taking here because the prediction outlasts an iteration, which is what
+// DenseVsSparsePredictionWhileMappingARealSession does not have: there a location is added
+// on every iteration and the sparse form is never built at all.
+TEST(BayesFilterPerfTest, DenseVsSparsePredictionWhileMapping)
+{
+	const int nodes = 4000;
+	const int iterations = 30;
+
+	SyntheticMap map(nodes, 100, 500, false /*stay in mapping mode*/);
+	const size_t size = map.bayesIds().size();
+	std::cout << "[          ] " << nodes << " nodes, mapping mode (the matrix is kept), "
+			  << map.loopClosures() << " loop closures, dense matrix = "
+			  << (size*size*sizeof(float))/1048576 << " MB" << std::endl;
+
+	const std::vector<int> ids = map.bayesIds();
+	compare(map.memory(), ids, map.uniformLikelihood(ids), PREDICTION_DEFAULT, iterations);
+}
+
+// The graphs of real maps, against the synthetic ones above, in localization mode where
+// the graph is fixed. Two of them: one that went through the graph reduction, whose merged
+// neighbor links cost a margin like ordinary neighbors, and one that did not.
+//
+// Needs the same ~1 GB as the largest synthetic map for the dense prediction, and reads
+// the graphs from data/tests. Exclude with
+//   bin/test_bayesfilter_perf --gtest_filter=-*RealMap*
+TEST(BayesFilterPerfTest, DenseVsSparsePredictionOnRealMaps)
+{
+	const int iterations = 10;
+	if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
+	{
+		GTEST_SKIP() << "g2o optimizer not built in, needed to read the graphs";
+	}
+
+	const char * files[] = {"large_reduced_graph.g2o", "large_mapping_session.g2o"};
+	const char * labels[] = {"graph reduction applied", "no graph reduction"};
+	for(size_t f=0; f<sizeof(files)/sizeof(const char *); ++f)
+	{
+		const std::string path = std::string(RTABMAP_TEST_DATA_ROOT) + "/tests/" + files[f];
+		if(!UFile::exists(path))
+		{
+			std::cout << "[          ] " << path << " not found, skipped" << std::endl;
+			continue;
+		}
+
+		RealGraph graph;
+		ASSERT_TRUE(loadRealGraph(path, graph)) << "could not read " << path;
+
+		Memory * memory = newRealMemory();
+		std::vector<int> newIds;
+		int removedLinks = 0;
+		UTimer timer;
+		for(size_t i=0; i<graph.ids.size(); ++i)
+		{
+			addRealNode(memory, graph, i, newIds, &removedLinks);
+		}
+		const double buildTime = timer.ticks();
+
+		ParametersMap localization;
+		localization.insert(ParametersPair(Parameters::kMemIncrementalMemory(), "false"));
+		memory->parseParameters(localization);
+		ASSERT_FALSE(memory->isIncremental());
+
+		const std::vector<int> ids = bayesIdsOf(memory);
+		std::cout << "[          ] " << files[f] << " (" << labels[f] << ")" << std::endl;
+		printGraph(path, graph, removedLinks);
+		std::cout << "[          ] rebuilt in " << buildTime << "s, " << ids.size()
+				  << " locations (with the virtual place), dense matrix = "
+				  << (ids.size()*ids.size()*sizeof(float))/1048576 << " MB" << std::endl;
+
+		const std::map<int, float> likelihood = uniformLikelihoodOf(ids);
+		std::cout << "[          ]  18 values model (default, depth 17):" << std::endl;
+		compare(memory, ids, likelihood, PREDICTION_DEFAULT, iterations, /*sparseFirst=*/true);
+		std::cout << "[          ]   8 values model (depth 7, every value above 1e-4):" << std::endl;
+		compare(memory, ids, likelihood, PREDICTION_TRUNCATED, iterations, /*sparseFirst=*/true);
+		delete memory;
+	}
+}
+
+// A mapping session as it runs: a location added, then an iteration of the filter, over and
+// over. Every added location changes the prediction, so this is the case the sparse form
+// cannot amortize -- unlike localization, where it is built once and reused for the rest of
+// the session. What it costs to keep it up to date against what its multiplication saves is
+// what this measures.
+//
+// The session is replayed from its end: the locations before the window are added without
+// running the filter, so the per-location cost is measured at the size the map really
+// reaches rather than at the sizes it passes through.
+TEST(BayesFilterPerfTest, DenseVsSparsePredictionWhileMappingARealSession)
+{
+	const size_t window = 15;   // locations added one at a time, with an iteration each
+	if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
+	{
+		GTEST_SKIP() << "g2o optimizer not built in, needed to read the graph";
+	}
+	const std::string path = std::string(RTABMAP_TEST_DATA_ROOT) + "/tests/large_mapping_session.g2o";
+	if(!UFile::exists(path))
+	{
+		GTEST_SKIP() << path << " not found";
+	}
+
+	RealGraph graph;
+	ASSERT_TRUE(loadRealGraph(path, graph)) << "could not read " << path;
+	ASSERT_GT(graph.ids.size(), window);
+	const size_t prepared = graph.ids.size() - window;
+
+	std::cout << "[          ] large_mapping_session.g2o, mapping mode: " << prepared
+			  << " locations already mapped, " << window
+			  << " more added one at a time with an iteration of the filter each" << std::endl;
+
+	std::map<int, float> lastPosterior[2];
+	for(int sparse=1; sparse>=0; --sparse)   // the sparse mode first, see compare()
+	{
+		Memory * memory = newRealMemory();
+		std::vector<int> newIds;
+		int removedLinks = 0;
+		for(size_t i=0; i<prepared; ++i)
+		{
+			addRealNode(memory, graph, i, newIds, &removedLinks);
+		}
+
+		ParametersMap params;
+		params.insert(ParametersPair(Parameters::kBayesSparsePrediction(), sparse?"true":"false"));
+		BayesFilter filter(params);
+
+		// The first iteration generates the whole prediction, as it does at the start of a
+		// session; the ones after it are what a mapping session pays per location.
+		std::vector<int> ids = bayesIdsOf(memory);
+		UTimer timer;
+		filter.computePosterior(memory, uniformLikelihoodOf(ids));
+		const double first = timer.ticks();
+
+		double total = 0.0, best = 0.0, worst = 0.0;
+		for(size_t i=prepared; i<graph.ids.size(); ++i)
+		{
+			addRealNode(memory, graph, i, newIds, &removedLinks);
+			ids = bayesIdsOf(memory);
+			timer.restart();
+			filter.computePosterior(memory, uniformLikelihoodOf(ids));
+			const double elapsed = timer.ticks();
+			total += elapsed;
+			if(best == 0.0 || elapsed < best) best = elapsed;
+			if(elapsed > worst) worst = elapsed;
+		}
+		lastPosterior[sparse] = posteriorOf(filter);
+
+		printf("[          ]   %-6s first iteration %8.1f ms, then per added location: "
+			   "fastest %8.1f ms, mean %8.1f ms, slowest %8.1f ms, filter memory %7.1f MB\n",
+				sparse?"sparse":"dense", first*1000.0, best*1000.0, total*1000.0/double(window),
+				worst*1000.0, filter.getMemoryUsed()/1048576.0);
+		delete memory;
+	}
+
+	EXPECT_LT(maxPosteriorDifference(lastPosterior[0], lastPosterior[1]), 1e-3);
+}

corelib/test/test_graph.cpp

@@ -8,6 +8,8 @@
 #include <cmath>
 #include <string>
 #include <vector>
+#include <fstream>
+#include <sstream>
 
 using namespace rtabmap;
 
@@ -922,10 +924,10 @@ void expectLinksNearEqual(
 	const auto idxB = index(b);
 	ASSERT_EQ(idxA.size(), idxB.size())
 			<< label << " unique (from,to) link pair count differs";
-	// Note: graph file formats (TORO / g2o) store edges generically and
+	// Note: TORO's text format stores edges generically and doesn't preserve
-	// don't preserve rtabmap's Link::Type tag, so we only round-trip
+	// rtabmap's Link::Type tag, so from / to / transform / infMatrix are all
-	// from / to / transform / infMatrix here. The loader assigns a
+	// that round-trip there. g2o carries the type in a column of its own,
-	// placeholder type for ordinary edges.
+	// which G2oRoundTripPreservesLinkTypes below checks.
 	//
 	// Landmark links in g2o are written as EDGE_SE3_TRACKXYZ (3D point
 	// observation): only the translation and the 3x3 translation block
@@ -1260,3 +1262,254 @@ INSTANTIATE_TEST_SUITE_P(
 					+ "_"
 					+ (std::get<2>(info.param) ? "rotPrior" : "posPrior");
 		});
+
+// -------------------------------------------------------------------------
+// The type of a link (a loop closure against an odometry link, and which kind
+// of loop closure) decides how the graph is traversed: Memory::getNeighborsId()
+// follows a global closure without spending any depth, skips a proximity one
+// and spends a depth on a neighbor. The g2o format defines no field for it, so
+// OptimizerG2O writes it as a column past the ones it defines, which its own
+// loader reads back and g2o's ignores.
+//
+// Also checks that a link handed over in both directions, which is how Memory
+// stores it, is written once: g2o reads two lines as two constraints and would
+// count the information of the link twice.
+// -------------------------------------------------------------------------
+TEST(GraphG2oTest, G2oRoundTripPreservesLinkTypes)
+{
+	if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
+	{
+		GTEST_SKIP() << "g2o optimizer not built in";
+	}
+
+	const Link::Type types[] = {
+		Link::kNeighbor,
+		Link::kNeighborMerged,
+		Link::kGlobalClosure,
+		Link::kLocalSpaceClosure,
+		Link::kLocalTimeClosure,
+		Link::kUserClosure,
+	};
+	const size_t typeCount = sizeof(types)/sizeof(Link::Type);
+
+	std::map<int, Transform> poses;
+	for(size_t i=0; i<=typeCount; ++i)
+	{
+		poses.insert(std::make_pair((int)i+1, Transform((float)i, 0.0f, 0.0f, 0, 0, 0)));
+	}
+
+	const cv::Mat infMatrix = cv::Mat::eye(6, 6, CV_64F) * 100.0;
+	std::multimap<int, Link> links;
+	for(size_t i=0; i<typeCount; ++i)
+	{
+		const int from = (int)i+1, to = (int)i+2;
+		const Transform t = poses.at(from).inverse() * poses.at(to);
+		// Both directions, as Memory holds them: one link stored on each of the
+		// two nodes it connects.
+		links.insert(std::make_pair(from, Link(from, to, types[i], t, infMatrix)));
+		links.insert(std::make_pair(to, Link(to, from, types[i], t.inverse(), infMatrix)));
+	}
+
+	const std::string path = test::tempPath(
+			uFormat("rtabmap_graph_link_types_%d.g2o", test::getPid()));
+	UFile::erase(path);
+	ASSERT_TRUE(graph::exportPoses(path, 4 /*g2o*/, poses, links));
+	ASSERT_TRUE(UFile::exists(path));
+
+	// One line per link, not two, and each one carrying its type last.
+	std::ifstream file(path.c_str());
+	std::string line;
+	std::map<std::pair<int,int>, int> written;
+	while(std::getline(file, line))
+	{
+		if(line.compare(0, 5, "EDGE_") != 0)
+		{
+			continue;
+		}
+		std::istringstream in(line);
+		std::string tag;
+		int from = 0, to = 0;
+		in >> tag >> from >> to;
+		std::string last;
+		while(in >> last) {}
+		const std::pair<int,int> pair(std::min(from,to), std::max(from,to));
+		EXPECT_TRUE(written.insert(std::make_pair(pair, atoi(last.c_str()))).second)
+				<< "link " << from << "->" << to << " written more than once";
+	}
+	ASSERT_EQ(written.size(), typeCount);
+	for(size_t i=0; i<typeCount; ++i)
+	{
+		EXPECT_EQ(written.at(std::make_pair((int)i+1, (int)i+2)), (int)types[i])
+				<< "type column of link " << i+1 << "->" << i+2;
+	}
+
+	// And read back as the types they were.
+	std::map<int, Transform> posesOut;
+	std::multimap<int, Link> linksOut;
+	ASSERT_TRUE(graph::importPoses(path, 4 /*g2o*/, posesOut, &linksOut));
+	ASSERT_EQ(linksOut.size(), typeCount);
+	std::map<std::pair<int,int>, Link::Type> loaded;
+	for(std::multimap<int, Link>::const_iterator iter=linksOut.begin(); iter!=linksOut.end(); ++iter)
+	{
+		loaded.insert(std::make_pair(
+				std::make_pair(std::min(iter->second.from(), iter->second.to()),
+							   std::max(iter->second.from(), iter->second.to())),
+				iter->second.type()));
+	}
+	for(size_t i=0; i<typeCount; ++i)
+	{
+		const std::pair<int,int> pair((int)i+1, (int)i+2);
+		ASSERT_TRUE(loaded.find(pair) != loaded.end()) << "link " << i+1 << "->" << i+2 << " missing";
+		EXPECT_EQ(loaded.at(pair), types[i]) << "type of link " << i+1 << "->" << i+2;
+	}
+	UFile::erase(path);
+}
+
+// A file without the type column, which is every file g2o itself writes and
+// every one rtabmap wrote before, still loads: the type stays the one its tag
+// implies, as it did.
+TEST(GraphG2oTest, G2oWithoutTypeColumnStillLoads)
+{
+	if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
+	{
+		GTEST_SKIP() << "g2o optimizer not built in";
+	}
+
+	const std::string path = test::tempPath(
+			uFormat("rtabmap_graph_no_type_column_%d.g2o", test::getPid()));
+	UFile::erase(path);
+	{
+		std::ofstream file(path.c_str());
+		file << "VERTEX_SE2 1 0 0 0\n";
+		file << "VERTEX_SE2 2 1 0 0\n";
+		file << "EDGE_SE2 1 2 1 0 0 100 0 0 100 0 100\n";
+	}
+
+	std::map<int, Transform> poses;
+	std::multimap<int, Link> links;
+	ASSERT_TRUE(graph::importPoses(path, 4 /*g2o*/, poses, &links));
+	EXPECT_EQ(poses.size(), 2u);
+	ASSERT_EQ(links.size(), 1u);
+	EXPECT_EQ(links.begin()->second.from(), 1);
+	EXPECT_EQ(links.begin()->second.to(), 2);
+	UFile::erase(path);
+}
+
+// The switchable edges of vertigo, which saveGraph() writes for a link that is not a neighbor
+// when Optimizer/Robust is enabled: the tag inserts the id of a switch vertex of its own
+// before the fields of the link, so the type column lands one field further than on the
+// ordinary tags and the writer and the loader have to agree on where it sits.
+TEST(GraphG2oTest, G2oRoundTripsSwitchableEdges)
+{
+	if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
+	{
+		GTEST_SKIP() << "g2o optimizer not built in";
+	}
+
+	// EDGE_SE2_SWITCHABLE in 2D, EDGE_SE3_SWITCHABLE in 3D.
+	for(int slam2d = 1; slam2d >= 0; --slam2d)
+	{
+		SCOPED_TRACE(slam2d ? "slam2d" : "slam3d");
+
+		std::map<int, Transform> poses;
+		poses.insert(std::make_pair(1, Transform(0.0f, 0.0f, 0.0f, 0, 0, 0)));
+		poses.insert(std::make_pair(2, Transform(1.0f, 0.0f, 0.0f, 0, 0, 0)));
+		poses.insert(std::make_pair(3, Transform(2.0f, 0.0f, 0.0f, 0, 0, 0)));
+
+		const cv::Mat infMatrix = cv::Mat::eye(6, 6, CV_64F) * 100.0;
+		std::multimap<int, Link> links;
+		// The odometry links are written as the ordinary tag whatever Optimizer/Robust is,
+		// and the loop closure as the switchable one.
+		const int pairs[][3] = {
+			{1, 2, Link::kNeighbor},
+			{2, 3, Link::kNeighbor},
+			{1, 3, Link::kGlobalClosure}};
+		for(size_t i = 0; i < sizeof(pairs)/sizeof(pairs[0]); ++i)
+		{
+			const int from = pairs[i][0], to = pairs[i][1];
+			const Link::Type type = (Link::Type)pairs[i][2];
+			const Transform t = poses.at(from).inverse() * poses.at(to);
+			links.insert(std::make_pair(from, Link(from, to, type, t, infMatrix)));
+			links.insert(std::make_pair(to, Link(to, from, type, t.inverse(), infMatrix)));
+		}
+
+		ParametersMap params;
+		params.insert(ParametersPair(Parameters::kOptimizerRobust(), "true"));
+		params.insert(ParametersPair(Parameters::kRegForce3DoF(), slam2d ? "true" : "false"));
+
+		const std::string path = test::tempPath(
+				uFormat("rtabmap_graph_switchable_%d_%d.g2o", slam2d, test::getPid()));
+		UFile::erase(path);
+		ASSERT_TRUE(graph::exportPoses(path, 4 /*g2o*/, poses, links, std::map<int, double>(), params));
+
+		// Written as the switchable tag, which is what puts the type column one field further.
+		const std::string switchableTag = slam2d ? "EDGE_SE2_SWITCHABLE" : "EDGE_SE3_SWITCHABLE";
+		int switchableLines = 0;
+		{
+			std::ifstream file(path.c_str());
+			std::string line;
+			while(std::getline(file, line))
+			{
+				if(line.compare(0, switchableTag.size(), switchableTag) == 0)
+				{
+					++switchableLines;
+				}
+			}
+		}
+		EXPECT_EQ(switchableLines, 1);
+
+		std::map<int, Transform> posesOut;
+		std::multimap<int, Link> linksOut;
+		ASSERT_TRUE(graph::importPoses(path, 4 /*g2o*/, posesOut, &linksOut));
+		EXPECT_EQ(posesOut.size(), poses.size()); // the switch vertices are none of the poses
+		ASSERT_EQ(linksOut.size(), sizeof(pairs)/sizeof(pairs[0]));
+
+		std::map<std::pair<int,int>, Link::Type> loaded;
+		for(std::multimap<int, Link>::const_iterator iter=linksOut.begin(); iter!=linksOut.end(); ++iter)
+		{
+			loaded.insert(std::make_pair(
+					std::make_pair(std::min(iter->second.from(), iter->second.to()),
+								   std::max(iter->second.from(), iter->second.to())),
+					iter->second.type()));
+		}
+		for(size_t i = 0; i < sizeof(pairs)/sizeof(pairs[0]); ++i)
+		{
+			const std::pair<int,int> pair(pairs[i][0], pairs[i][1]);
+			ASSERT_TRUE(loaded.find(pair) != loaded.end())
+					<< "link " << pair.first << "->" << pair.second << " missing";
+			EXPECT_EQ(loaded.at(pair), (Link::Type)pairs[i][2])
+					<< "type of link " << pair.first << "->" << pair.second;
+		}
+		UFile::erase(path);
+	}
+}
+
+// A type column holding something that is not one of the types, which nothing rtabmap writes
+// but another writer of the same format could: it is ignored and the type stays the one the
+// tag implies, so a column that means something else elsewhere cannot turn a link into a type
+// it is not.
+TEST(GraphG2oTest, G2oOutOfRangeTypeColumnIsIgnored)
+{
+	if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
+	{
+		GTEST_SKIP() << "g2o optimizer not built in";
+	}
+
+	const std::string path = test::tempPath(
+			uFormat("rtabmap_graph_bad_type_column_%d.g2o", test::getPid()));
+	UFile::erase(path);
+	{
+		std::ofstream file(path.c_str());
+		file << "VERTEX_SE2 1 0 0 0\n";
+		file << "VERTEX_SE2 2 1 0 0\n";
+		file << "EDGE_SE2 1 2 1 0 0 100 0 0 100 0 100 4242\n";
+	}
+
+	std::map<int, Transform> poses;
+	std::multimap<int, Link> links;
+	ASSERT_TRUE(graph::importPoses(path, 4 /*g2o*/, poses, &links));
+	EXPECT_EQ(poses.size(), 2u);
+	ASSERT_EQ(links.size(), 1u);
+	EXPECT_EQ(links.begin()->second.type(), Link::kUndef); // the type EDGE_SE2 implies
+	UFile::erase(path);
+}

corelib/test/test_rtabmap_integration.cpp

@@ -14,6 +14,7 @@
 // pass.
 
 #include <gtest/gtest.h>
+#include <rtabmap/core/DBDriver.h>
 #include <rtabmap/core/DBReader.h>
 #include <rtabmap/core/Features2d.h>
 #include <rtabmap/core/camera/CameraImages.h>
@@ -45,6 +46,7 @@
 #include "TestUtils.h"
 #include <opencv2/imgcodecs.hpp>
 #include <algorithm>
+#include <cmath>
 #include <iostream>
 #include <memory>
 #include <string>
@@ -94,6 +96,7 @@ struct ReplayResult
 	int finalGlobalGraphSize = 0;  // poses returned by Rtabmap::getGraph(global=true)
 	std::map<int, Transform> finalLocalPoses;   // optimized, global=false
 	std::map<int, Transform> finalGlobalPoses;  // optimized, global=true
+	std::multimap<int, Link> finalGlobalLinks;  // constraints of the global graph
 	// Occupancy-grid cell counts after assembling the global grid from per-
 	// node local maps (only populated when RGBD/CreateOccupancyGrid=true).
 	int gridEmptyCells = 0;
@@ -110,6 +113,19 @@ struct ReplayResult
 	// Wall-clock seconds spent inside Odometry::process across all
 	// frames; divide by framesRead to get per-frame average.
 	double odomTotalSeconds = 0.0;
+	// The highest loop closure hypothesis of each node added, id and probability: the curve
+	// the Bayes filter draws over a session.
+	std::map<int, std::pair<int, float> > highestHypothesis;
+	// Timing/Posterior_computation (ms) over the frames that reported it: the prediction and
+	// the multiplication of the Bayes filter.
+	double posteriorMsSum = 0.0;
+	float posteriorMsMin = -1.0f;
+	float posteriorMsMax = 0.0f;
+	int posteriorSamples = 0;
+	float posteriorMsAvg() const
+	{
+		return posteriorSamples > 0 ? (float)(posteriorMsSum/(double)posteriorSamples) : -1.0f;
+	}
 };
 
 // Synchronous replay: DBReader -> Odometry::process -> Rtabmap::process.
@@ -415,8 +431,7 @@ ReplayResult replayDatabase(
 		rtabmap.getGraph(result.finalLocalPoses, constraints,
 				/*optimized=*/true, /*global=*/false);
 		result.finalLocalGraphSize = (int)result.finalLocalPoses.size();
-		constraints.clear();
+		rtabmap.getGraph(result.finalGlobalPoses, result.finalGlobalLinks,
-		rtabmap.getGraph(result.finalGlobalPoses, constraints,
 				/*optimized=*/true, /*global=*/true);
 		result.finalGlobalGraphSize = (int)result.finalGlobalPoses.size();
 	}
@@ -550,7 +565,13 @@ ReplayResult replayDatabaseWithStoredOdom(
 		// When >0, points beyond this range (in meters) are dropped from
 		// the LaserScan of each SensorData after dbReader.takeData(),
 		// simulating a lidar with a tighter max range.
-		float scanMaxRange = 0.0f)
+		float scanMaxRange = 0.0f,
+		// Starts a new map on every frame whose stored odom covariance is the 9999 of a
+		// session start, as rtabmap-reprocess does. Off by default: a test wanting its own
+		// boundaries uses the frame above.
+		bool triggerNewMapOnSessionStart = false,
+		// Filled with the number of sessions replayed: one plus the boundaries triggered on.
+		int * sessionsReplayed = 0)
 {
 	ReplayResult result;
 
@@ -656,6 +677,20 @@ ReplayResult replayDatabaseWithStoredOdom(
 			continue;
 		}
 
+		// Session boundary: the 9999 covariance of a session's first frame says its pose does
+		// not continue the previous one. Started before that frame is processed, so it is the
+		// first of the new session rather than the last of the old. Same as rtabmap-reprocess.
+		if(triggerNewMapOnSessionStart
+				&& result.framesProcessed > 0
+				&& info.odomCovariance.at<double>(0, 0) >= 9999.0)
+		{
+			rtabmap.triggerNewMap();
+			if(sessionsReplayed)
+			{
+				++*sessionsReplayed;
+			}
+		}
+
 		SensorData rtabmapData = data;
 		if(throttle)
 		{
@@ -705,6 +740,24 @@ ReplayResult replayDatabaseWithStoredOdom(
 		{
 			result.translationalRmseFinal = rmseIt->second;
 		}
+		const auto hypIdIt = stats.data().find(Statistics::kLoopHighest_hypothesis_id());
+		const auto hypValIt = stats.data().find(Statistics::kLoopHighest_hypothesis_value());
+		if(hypIdIt != stats.data().end() && hypValIt != stats.data().end() && stats.refImageId()>0)
+		{
+			result.highestHypothesis[stats.refImageId()] =
+					std::make_pair((int)hypIdIt->second, hypValIt->second);
+		}
+		const auto postIt = stats.data().find(Statistics::kTimingPosterior_computation());
+		if(postIt != stats.data().end())
+		{
+			result.posteriorMsSum += postIt->second;
+			result.posteriorMsMax = std::max(result.posteriorMsMax, postIt->second);
+			if(result.posteriorMsMin < 0.0f || postIt->second < result.posteriorMsMin)
+			{
+				result.posteriorMsMin = postIt->second;
+			}
+			++result.posteriorSamples;
+		}
 		data = dbReader.takeData(&info);
 		applyScanRangeFilter(data);
 	}
@@ -714,8 +767,7 @@ ReplayResult replayDatabaseWithStoredOdom(
 		rtabmap.getGraph(result.finalLocalPoses, constraints,
 				/*optimized=*/true, /*global=*/false);
 		result.finalLocalGraphSize = (int)result.finalLocalPoses.size();
-		constraints.clear();
+		rtabmap.getGraph(result.finalGlobalPoses, result.finalGlobalLinks,
-		rtabmap.getGraph(result.finalGlobalPoses, constraints,
 				/*optimized=*/true, /*global=*/true);
 		result.finalGlobalGraphSize = (int)result.finalGlobalPoses.size();
 	}
@@ -733,12 +785,202 @@ ReplayResult replayDatabaseWithStoredOdom(
 			<< " localGraph=" << result.finalLocalGraphSize
 			<< " globalGraph=" << result.finalGlobalGraphSize
 			<< " rmse=" << result.translationalRmseFinal << "m"
+			<< " posterior(avg/min/max)=" << result.posteriorMsAvg()
+			<< "/" << result.posteriorMsMin << "/" << result.posteriorMsMax << "ms"
 			<< " wall=" << result.replayWallSeconds << "s"
 			<< std::endl;
 
 	return result;
 }
 
+// Union-find root, halving the path on the way up.
+int graphComponentRoot(std::map<int, int> & parent, int id)
+{
+	while(parent.at(id) != id)
+	{
+		const int up = parent.at(id);
+		parent.at(id) = parent.at(up);
+		id = up;
+	}
+	return id;
+}
+
+// How many pieces a graph is in. Two sessions that never closed a loop with each other are
+// two pieces.
+int countConnectedComponents(
+		const std::map<int, Transform> & poses,
+		const std::multimap<int, Link> & links)
+{
+	std::map<int, int> parent;
+	for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+	{
+		parent.insert(std::make_pair(iter->first, iter->first));
+	}
+	for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
+	{
+		// A link to a landmark, or to a node the graph does not hold, joins nothing.
+		if(parent.find(iter->second.from()) == parent.end() ||
+		   parent.find(iter->second.to()) == parent.end())
+		{
+			continue;
+		}
+		const int a = graphComponentRoot(parent, iter->second.from());
+		const int b = graphComponentRoot(parent, iter->second.to());
+		if(a != b)
+		{
+			parent.at(a) = b;
+		}
+	}
+	std::set<int> roots;
+	for(std::map<int, int>::const_iterator iter=parent.begin(); iter!=parent.end(); ++iter)
+	{
+		roots.insert(graphComponentRoot(parent, iter->first));
+	}
+	return (int)roots.size();
+}
+
+// What replaying a database asks for on top of the parameters it was recorded with, kept as
+// little as possible: the dictionary and the searches stay as they were recorded, so what the
+// replay does can be held against what the session did.
+ParametersMap replayParams(const ParametersMap & recordedWith, const std::string & srcPath)
+{
+	ParametersMap params = recordedWith;
+	// Its nodes are already the ones its detection rate kept, so every frame is processed.
+	uInsert(params, ParametersPair(Parameters::kRtabmapDetectionRate(), "0"));
+	// The features stored with each node: this database keeps no images to extract them from.
+	uInsert(params, ParametersPair(Parameters::kMemUseOdomFeatures(), "true"));
+	uInsert(params, ParametersPair(Parameters::kRGBDCreateOccupancyGrid(), "false"));
+	return params;
+}
+
+// The highest loop closure hypothesis the recorded session saw at each node, id and value,
+// from the statistics it saved. That is the curve a replay is compared against.
+std::map<int, std::pair<int, float> > loadDatabaseHighestHypothesis(const std::string & path)
+{
+	std::map<int, std::pair<int, float> > hypothesis;
+	DBDriver * driver = DBDriver::create();
+	if(!driver->openConnection(path))
+	{
+		delete driver;
+		return hypothesis;
+	}
+	std::set<int> ids;
+	driver->getAllNodeIds(ids);
+	for(std::set<int>::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter)
+	{
+		double stamp = 0.0;
+		const std::map<std::string, float> stats = driver->getStatistics(*iter, stamp);
+		const std::map<std::string, float>::const_iterator idIter =
+				stats.find(Statistics::kLoopHighest_hypothesis_id());
+		const std::map<std::string, float>::const_iterator valueIter =
+				stats.find(Statistics::kLoopHighest_hypothesis_value());
+		if(idIter != stats.end() && valueIter != stats.end())
+		{
+			hypothesis[*iter] = std::make_pair((int)idIter->second, valueIter->second);
+		}
+	}
+	driver->closeConnection(false);
+	delete driver;
+	return hypothesis;
+}
+
+// How a replay's highest hypothesis per node stands against the recorded one.
+struct HypothesisComparison
+{
+	int nodes = 0;              ///< nodes compared, of the ones both curves have
+	int sameId = 0;             ///< of those, the ones pointing at the very same location
+	int samePlace = 0;          ///< and the ones pointing at a location within a meter of it
+	int alsoOverThreshold = 0;  ///< and the ones the replay also took past the threshold
+	float meanAbsValue = 0.0f;  ///< mean |value - golden value|
+	float maxAbsValue = 0.0f;
+	float sameIdRatio() const {return nodes>0 ? float(sameId)/float(nodes) : 0.0f;}
+	float samePlaceRatio() const {return nodes>0 ? float(samePlace)/float(nodes) : 0.0f;}
+	float overThresholdRatio() const {return nodes>0 ? float(alsoOverThreshold)/float(nodes) : 0.0f;}
+};
+
+// Two hypotheses are on the same place when the locations they point at are this close in the
+// optimized graph. Ids cannot say that on a map of three passes over the same trajectory: the
+// same corner is a node of each pass, hundreds of ids apart. The nodes are ~0.3 m apart along
+// the path, so a meter is a handful of them, and the map is only 24 m by 33 m: a wider radius
+// would call most of it the same place.
+const float kHypothesisSamePlaceRadius = 1.0f;   // meters
+
+// Only the nodes where the recorded session had a hypothesis at or past Rtabmap/LoopThr are
+// compared: under it the value is spread thinly over the working memory and which location
+// comes out highest is noise.
+HypothesisComparison compareHighestHypothesis(
+		const std::map<int, std::pair<int, float> > & golden,
+		const std::map<int, std::pair<int, float> > & replayed,
+		const std::map<int, Transform> & poses,
+		float loopThreshold)
+{
+	HypothesisComparison c;
+	double sum = 0.0;
+	for(std::map<int, std::pair<int, float> >::const_iterator iter=golden.begin(); iter!=golden.end(); ++iter)
+	{
+		if(iter->second.first <= 0 || iter->second.second < loopThreshold)
+		{
+			continue;
+		}
+		const std::map<int, std::pair<int, float> >::const_iterator jter = replayed.find(iter->first);
+		if(jter == replayed.end())
+		{
+			continue;
+		}
+		++c.nodes;
+		if(jter->second.second >= loopThreshold)
+		{
+			++c.alsoOverThreshold;
+		}
+		const int goldenId = iter->second.first;
+		const int replayedId = jter->second.first;
+		if(goldenId == replayedId)
+		{
+			++c.sameId;
+		}
+		bool samePlace = goldenId == replayedId;
+		if(!samePlace && goldenId > 0 && replayedId > 0)
+		{
+			const std::map<int, Transform>::const_iterator goldenPose = poses.find(goldenId);
+			const std::map<int, Transform>::const_iterator replayedPose = poses.find(replayedId);
+			if(goldenPose != poses.end() && replayedPose != poses.end())
+			{
+				samePlace = goldenPose->second.getDistance(replayedPose->second)
+						< kHypothesisSamePlaceRadius;
+			}
+		}
+		if(samePlace)
+		{
+			++c.samePlace;
+		}
+		const float diff = fabs(iter->second.second - jter->second.second);
+		sum += diff;
+		c.maxAbsValue = std::max(c.maxAbsValue, diff);
+	}
+	c.meanAbsValue = c.nodes>0 ? (float)(sum/(double)c.nodes) : 0.0f;
+	return c;
+}
+
+// The optimized graph a database converged to, which a replay is compared against, and the
+// parameters it was recorded with, which the replay runs with.
+bool loadDatabaseGraphAndParameters(
+		const std::string & path,
+		std::map<int, Transform> & optimizedPoses,
+		ParametersMap & parameters)
+{
+	DBDriver * driver = DBDriver::create();
+	if(!driver->openConnection(path))
+	{
+		delete driver;
+		return false;
+	}
+	optimizedPoses = driver->loadOptimizedPoses();
+	parameters = driver->getLastParameters();
+	driver->closeConnection(false);
+	delete driver;
+	return !optimizedPoses.empty();
+}
+
 ParametersMap baseRtabmapParams()
 {
 	ParametersMap params;
@@ -1990,16 +2232,302 @@ TEST_F(RtabmapIntegrationFixture, Loop3ItGps)
 	}
 }
 
+
 // ---------------------------------------------------------------------------
-// Appearance-only loop closure on the 84-image `data/samples` set with the
+// Multi-session 2D lidar + SIFT (3 sessions, 935 nodes, ~270 m).
-// shipped `data/samples_GT.bmp` ground truth. Measures recall at 100%
+//
-// precision (the rtabmap "max recall while no false positive has appeared
+// The reference is the optimized graph the database holds: the same frames with the same
-// yet" metric — same definition as the legacy MATLAB getPrecisionRecall.m
+// parameters have to find about as many loop closures. The replay reuses the features stored
-// script) for every Features2D detector strategy that is available in this
+// with each node, the database keeping no images, and the ICP registration verifies loop
-// build. Detector strategies for which Feature2D::create() silently
+// closures on the scans it does keep.
-// substitutes a different backend (e.g. SURF -> SIFT without nonfree,
+//
-// SuperPointTorch -> GFTT/ORB without RTABMAP_TORCH) are skipped.
+// Run over both forms of the Bayes prediction, matrix and sparse: the same probabilities, so
+// a real session has to come out the same either way.
 // ---------------------------------------------------------------------------
+TEST_F(RtabmapIntegrationFixture, Multisession3It)
+{
+	const std::string srcPath = testDataPath("multisession_3it.db");
+	SKIP_IF_MISSING(srcPath);
+
+	std::map<int, Transform> goldenPoses;
+	ParametersMap dbParams;
+	ASSERT_TRUE(loadDatabaseGraphAndParameters(srcPath, goldenPoses, dbParams))
+			<< "No optimized graph in " << srcPath;
+	std::cerr << "[          ] Reference graph: " << goldenPoses.size()
+			  << " poses, recorded with " << dbParams.size() << " parameters\n";
+
+	for(const bool sparsePrediction : {false, true})
+	{
+		const std::string label = sparsePrediction ? "sparse" : "dense";
+		SCOPED_TRACE(label);
+
+		ParametersMap params = replayParams(dbParams, srcPath);
+		uInsert(params, ParametersPair(Parameters::kBayesSparsePrediction(),
+				sparsePrediction ? "true" : "false"));
+		// The database carries the cap it was recorded with; this test is the run without it.
+		uInsert(params, ParametersPair(Parameters::kRtabmapMemoryThr(), "0"));
+
+		const std::string workDb = test::tempPath(uFormat(
+				"rtabmap_integration_Multisession3It_%s.db", label.c_str()));
+		std::cerr << "Working DB for " << label << ": " << workDb << "\n";
+
+		int sessions = 1;
+		const ReplayResult result = replayDatabaseWithStoredOdom(
+				srcPath, workDb, params,
+				/*triggerNewMapAfterFrame=*/-1,
+				/*overrideOdomAngularVariance=*/-1.0,
+				/*overrideOdomLinearVariance=*/-1.0,
+				/*scanMaxRange=*/0.0f,
+				/*triggerNewMapOnSessionStart=*/true,
+				&sessions);
+
+		ASSERT_GT(result.framesProcessed, 0) << label << " produced no frames";
+		ASSERT_GT(result.finalGlobalGraphSize, 0) << label << " produced an empty graph";
+
+		float tRmse=0, tMean=0, tMed=0, tStd=0, tMin=0, tMax=0;
+		float rRmse=0, rMean=0, rMed=0, rStd=0, rMin=0, rMax=0;
+		graph::calcRMSE(goldenPoses, result.finalGlobalPoses,
+				tRmse, tMean, tMed, tStd, tMin, tMax,
+				rRmse, rMean, rMed, rStd, rMin, rMax,
+				/*align2D=*/false);
+		std::cerr << "[" << label << "] sessions=" << sessions
+				  << " nodes=" << result.finalGlobalGraphSize
+				  << " loops=" << result.loopClosuresAccepted
+				  << " rejected=" << result.loopClosuresRejected
+				  << " proximity=" << result.proximityDetections
+				  << " trans rmse=" << tRmse << "m max=" << tMax << "m"
+				  << " rot rmse=" << rRmse << "deg max=" << rMax << "deg"
+				  << " posterior avg=" << result.posteriorMsAvg()
+				  << "ms min=" << result.posteriorMsMin
+				  << "ms max=" << result.posteriorMsMax << "ms\n";
+
+		// Every frame is a node the database kept, so the replay makes as many, in 3 sessions.
+		EXPECT_EQ(sessions, 3) << label;
+		EXPECT_EQ(result.finalGlobalGraphSize, (int)goldenPoses.size()) << label;
+
+		// Loop closures are what this compares; the trajectory is printed but not asserted,
+		// following from which closures a run happens to find. And a run is never the same
+		// twice: the registration accepts or rejects a hypothesis sitting on its threshold
+		// from one run to the next, and one closure changes the next ones. Hence a band.
+		EXPECT_GT(result.loopClosuresAccepted, 265) << label << " found too few loop closures";
+		EXPECT_LT(result.loopClosuresAccepted, 305) << label << " found more loop closures than the reference";
+	}
+}
+
+// ---------------------------------------------------------------------------
+// The same replay under memory management: Rtabmap/MemoryThr caps the working memory, so the
+// oldest nodes go to long-term memory as the map grows and only a window is ever held. The
+// global optimized graph still covers every node, and the loop closures found from a limited
+// working memory are fewer.
+// ---------------------------------------------------------------------------
+TEST_F(RtabmapIntegrationFixture, Multisession3ItMemoryThr)
+{
+	const std::string srcPath = testDataPath("multisession_3it.db");
+	SKIP_IF_MISSING(srcPath);
+
+	std::map<int, Transform> goldenPoses;
+	ParametersMap dbParams;
+	ASSERT_TRUE(loadDatabaseGraphAndParameters(srcPath, goldenPoses, dbParams))
+			<< "No optimized graph in " << srcPath;
+
+	// The database was recorded with the same 300-node cap, so the highest hypothesis it saw
+	// at each node is the curve to compare against.
+	const std::map<int, std::pair<int, float> > goldenHypothesis =
+			loadDatabaseHighestHypothesis(srcPath);
+	ASSERT_GT(goldenHypothesis.size(), 900u) << "No saved statistics in " << srcPath;
+
+	// Under this a hypothesis is not worth comparing.
+	float loopThreshold = Parameters::defaultRtabmapLoopThr();
+	Parameters::parse(dbParams, Parameters::kRtabmapLoopThr(), loopThreshold);
+	ASSERT_GT(loopThreshold, 0.0f);
+	std::cerr << "[          ] Comparing hypotheses at or over " << Parameters::kRtabmapLoopThr()
+			  << "=" << loopThreshold << "\n";
+
+	struct Variant
+	{
+		// "" leaves Bayes/SparsePrediction at its default, which is the sparse form.
+		std::string sparsePrediction;
+		// "" leaves the retrieval parameter at its default, which is 2 locations.
+		std::string maxLocalRetrieved;
+		std::string maxRetrieved;
+		int minLoops;
+		int maxLoops;
+		int minLocalGraph;
+		int maxLocalGraph;
+		// Of the nodes the recorded session had a hypothesis on, the least that must point at
+		// the same place, and the most the probability may differ by on average.
+		float minSamePlaceRatio;
+		float maxMeanValueDiff;
+		std::string label;
+	};
+	// The first two are the same run over both forms of the prediction, sharing their band:
+	// which form holds it must not change what the session does. The last three hold the form
+	// at its default and take the retrieval apart: none, local only, and both, which is what a
+	// session runs with out of the box.
+	//
+	// The bands come from six runs of each. Observed:
+	//
+	//   variant                loops     working    same place   also over   value diff
+	//                                    memory     as recorded  threshold   mean
+	//   sparse                 268-288   242-263    80-85%       86-91%      0.051-0.075
+	//   dense                  267-284   253-266    81-84%       86-91%      0.052-0.084
+	//   no-retrieval           204-213   229-256    68-71%       81-86%      0.103-0.119
+	//   local-retrieval-only   240-259   262-267    76-81%       85-91%      0.071-0.081
+	//   both-retrieval         265-287   261-268    82-84%       89-92%      0.031-0.053
+	//
+	// The closest to the recorded session is the one configured like it, both retrievals on,
+	// and the furthest is the one with none: what comes back into the working memory is what
+	// the hypotheses are drawn over.
+	//
+	// The loop counts move with the environment, the feature extraction not seeing quite the
+	// same thing: local-retrieval-only has since been seen at 259 and at 271, over the 265-287
+	// of both-retrieval, which is why the two are no longer ordered on the count. The value
+	// diff keeps its order everywhere it has been run.
+	//
+	// The posterior time each run prints is left unasserted: it is what these variants are
+	// measured for, but also what a loaded runner moves most.
+	const std::vector<Variant> variants = {
+		{"true",  "0", "",  245, 310, 225, 290, 0.72f, 0.11f, "sparse"              },
+		{"false", "0", "",  245, 310, 225, 290, 0.72f, 0.11f, "dense"               },
+		{"",      "0", "0", 180, 240, 210, 280, 0.60f, 0.16f, "no-retrieval"        },
+		{"",      "2", "0", 215, 285, 240, 290, 0.68f, 0.11f, "local-retrieval-only"},
+		{"",      "2", "2", 240, 310, 240, 295, 0.74f, 0.08f, "both-retrieval"      },
+	};
+
+	// Loop closures and hypothesis agreement per variant, for the ordering between them.
+	std::map<std::string, int> loopsPerVariant;
+	std::map<std::string, float> valueDiffPerVariant;
+
+	for(const Variant & v : variants)
+	{
+		SCOPED_TRACE(v.label);
+
+		ParametersMap params = replayParams(dbParams, srcPath);
+		uInsert(params, ParametersPair(Parameters::kRtabmapMemoryThr(), "300"));
+		if(!v.sparsePrediction.empty())
+		{
+			uInsert(params, ParametersPair(Parameters::kBayesSparsePrediction(), v.sparsePrediction));
+		}
+		if(!v.maxLocalRetrieved.empty())
+		{
+			uInsert(params, ParametersPair(Parameters::kRGBDMaxLocalRetrieved(), v.maxLocalRetrieved));
+		}
+		if(!v.maxRetrieved.empty())
+		{
+			uInsert(params, ParametersPair(Parameters::kRtabmapMaxRetrieved(), v.maxRetrieved));
+		}
+
+		const std::string workDb = test::tempPath(uFormat(
+				"rtabmap_integration_Multisession3ItMemoryThr_%s.db", v.label.c_str()));
+		std::cerr << "Working DB for " << v.label << ": " << workDb << "\n";
+
+		int sessions = 1;
+		const ReplayResult result = replayDatabaseWithStoredOdom(
+				srcPath, workDb, params,
+				/*triggerNewMapAfterFrame=*/-1,
+				/*overrideOdomAngularVariance=*/-1.0,
+				/*overrideOdomLinearVariance=*/-1.0,
+				/*scanMaxRange=*/0.0f,
+				/*triggerNewMapOnSessionStart=*/true,
+				&sessions);
+
+		ASSERT_GT(result.framesProcessed, 0) << v.label << " produced no frames";
+		ASSERT_GT(result.finalGlobalGraphSize, 0) << v.label << " produced an empty graph";
+
+		const int components = countConnectedComponents(
+				result.finalGlobalPoses, result.finalGlobalLinks);
+
+		float tRmse=0, tMean=0, tMed=0, tStd=0, tMin=0, tMax=0;
+		float rRmse=0, rMean=0, rMed=0, rStd=0, rMin=0, rMax=0;
+		graph::calcRMSE(goldenPoses, result.finalGlobalPoses,
+				tRmse, tMean, tMed, tStd, tMin, tMax,
+				rRmse, rMean, rMed, rStd, rMin, rMax,
+				/*align2D=*/false);
+		std::cerr << "[" << v.label << "] sessions=" << sessions
+				  << " nodes=" << result.finalGlobalGraphSize
+				  << " components=" << components
+				  << " localGraph=" << result.finalLocalGraphSize
+				  << " loops=" << result.loopClosuresAccepted
+				  << " rejected=" << result.loopClosuresRejected
+				  << " proximity=" << result.proximityDetections
+				  << " trans rmse=" << tRmse << "m max=" << tMax << "m"
+				  << " rot rmse=" << rRmse << "deg max=" << rMax << "deg"
+				  << " posterior avg=" << result.posteriorMsAvg()
+				  << "ms min=" << result.posteriorMsMin
+				  << "ms max=" << result.posteriorMsMax << "ms\n";
+
+		// The highest hypothesis of each node against the one the recorded session saw:
+		// the same locations pointed at, with the same probability on them.
+		const HypothesisComparison hyp = compareHighestHypothesis(
+				goldenHypothesis, result.highestHypothesis, goldenPoses, loopThreshold);
+		std::cerr << "[" << v.label << "] hypothesis over " << hyp.nodes
+				  << " nodes the recorded session had one on: same id " << hyp.sameId
+				  << " (" << 100.0f*hyp.sameIdRatio() << "%), same place " << hyp.samePlace
+				  << " (" << 100.0f*hyp.samePlaceRatio() << "%), also over the threshold "
+				  << hyp.alsoOverThreshold << " (" << 100.0f*hyp.overThresholdRatio()
+				  << "%), value diff mean=" << hyp.meanAbsValue
+				  << " max=" << hyp.maxAbsValue << "\n";
+
+		// Nothing is lost: the nodes go to long-term memory and the global graph still covers
+		// every one, in one piece. The three sessions are three passes over the same
+		// trajectory and 300 nodes is wide enough to still hold the end of one when the next
+		// starts over the same place, so the link across the boundary is found even with
+		// nothing coming back.
+		EXPECT_EQ(sessions, 3) << v.label;
+		EXPECT_EQ(result.finalGlobalGraphSize, (int)goldenPoses.size()) << v.label;
+		EXPECT_EQ(components, 1) << v.label << " graph came out in pieces";
+
+		// The capped part stays capped. Anything near 935 would mean the cap never took effect
+		// and the test is no longer about memory management.
+		EXPECT_GT(result.finalLocalGraphSize, v.minLocalGraph) << v.label;
+		EXPECT_LT(result.finalLocalGraphSize, v.maxLocalGraph)
+				<< v.label << " working memory was not capped";
+
+		// Loop closures are only found against what the working memory holds.
+		EXPECT_GT(result.loopClosuresAccepted, v.minLoops) << v.label << " found too few loop closures";
+		EXPECT_LT(result.loopClosuresAccepted, v.maxLoops) << v.label << " found more loop closures than expected";
+
+		// The trajectory is printed but not asserted, following from which closures a run
+		// happens to find.
+
+		// The recorded session had a hypothesis worth the name on 562 of its 935 nodes; the
+		// replay is held against those. Not the same one every time -- the sessions part ways
+		// as soon as they accept a different closure, and what a capped working memory holds
+		// follows from that -- but the same place on three quarters or more, and past the
+		// threshold on nine tenths.
+		EXPECT_GT(hyp.nodes, 400) << v.label << " compared too few nodes";
+		EXPECT_GT(hyp.samePlaceRatio(), v.minSamePlaceRatio)
+				<< v.label << " points at other places than the recorded session";
+		EXPECT_LT(hyp.meanAbsValue, v.maxMeanValueDiff)
+				<< v.label << " hypothesis probabilities drifted from the recorded session";
+		EXPECT_GT(hyp.overThresholdRatio(), 0.70f)
+				<< v.label << " left the recorded session's hypotheses under the threshold";
+
+		loopsPerVariant[v.label] = result.loopClosuresAccepted;
+		valueDiffPerVariant[v.label] = hyp.meanAbsValue;
+	}
+
+	// Whatever a run does inside its band, retrieving nothing finds the fewest closures: the
+	// hypotheses are drawn over the working memory, and nothing comes back into it.
+	ASSERT_EQ(loopsPerVariant.size(), 5u);
+	EXPECT_GT(loopsPerVariant.at("local-retrieval-only"), loopsPerVariant.at("no-retrieval"));
+	EXPECT_GT(loopsPerVariant.at("both-retrieval"), loopsPerVariant.at("no-retrieval"));
+	EXPECT_GT(loopsPerVariant.at("sparse"), loopsPerVariant.at("no-retrieval"));
+	EXPECT_GT(loopsPerVariant.at("dense"), loopsPerVariant.at("no-retrieval"));
+
+	// Local retrieval against both is not asserted on the count: the upper tail of the one
+	// reaches into the band of the other, which a run on another machine walks into (271
+	// against 269) while both stay inside their own bands. Only a collapse is caught here.
+	EXPECT_GE(loopsPerVariant.at("both-retrieval"), loopsPerVariant.at("local-retrieval-only") - 15);
+
+	// What the retrieval buys is asserted on the hypotheses, where the three are ordered with
+	// room to spare -- the gaps are twice the spread of a variant: what comes back into the
+	// working memory is what the hypotheses are drawn over, so bringing back what the
+	// likelihood points at lands closest to the recorded session.
+	EXPECT_LT(valueDiffPerVariant.at("both-retrieval"), valueDiffPerVariant.at("local-retrieval-only"));
+	EXPECT_LT(valueDiffPerVariant.at("local-retrieval-only"), valueDiffPerVariant.at("no-retrieval"));
+}
+
 TEST_F(RtabmapIntegrationFixture, AppearanceOnly_PrecisionRecall)
 {
 	const std::string samplesDir = std::string(RTABMAP_TEST_DATA_ROOT) + "/samples";

coverage-report.sh

@@ -124,10 +124,15 @@ echo "Running tests..."
 )
 
 echo "Capturing coverage..."
+# The whole of corelib/utilite, test objects included, not just their src/. An inline
+# function defined in a header is emitted in whichever translation unit wins comdat
+# folding, which is often a test one, and its counters then live in that unit's .gcda:
+# capturing only src/ reports such a function as uncovered however often it is called.
+# The test sources themselves are dropped below, after the counters are read.
 lcov --gcov-tool "$GCOV_TOOL" "${LCOV_IGNORE[@]}" \
 	--capture \
-	--directory "$BUILD_DIR/corelib/src" \
+	--directory "$BUILD_DIR/corelib" \
-	--directory "$BUILD_DIR/utilite/src" \
+	--directory "$BUILD_DIR/utilite" \
 	--output-file "$INFO_FILE"
 
 echo "Filtering coverage data (repo sources only)..."
@@ -144,6 +149,7 @@ lcov "${LCOV_IGNORE[@]}" --remove "$INFO_FILE" \
 	'*/rtflann/*' \
 	'*/corelib/test/*' \
 	'*/utilite/test/*' \
+	'*/_deps/*' \
 	--output-file "$INFO_FILE"
 
 echo "Generating HTML..."

data/tests/.gitignore

@@ -1,6 +1,8 @@
 # Test data fetched by scripts/fetch_test_data.sh -- never tracked.
 # The manifest (basename / source URL / sha256) is the source of truth.
 *.db
+*.7z
+*.zip
 *.pt
 *.pth
 *.py

data/tests/manifest.txt

@@ -1,14 +1,20 @@
 # Test data assets fetched by scripts/fetch_test_data.sh.
-# Format: <basename>\t<source>\t<sha256>
+# Format: <basename>\t<source>\t<sha256>[\t<extracted>]
 # - basename is the file's name on disk under data/tests/
 # - source is one of:
 #     * a Google Drive file ID (from https://drive.google.com/file/d/<ID>/...);
 #       assembled into a direct-download URL by the fetch script.
 #     * a full http(s):// URL; used as-is. Append "&resourcekey=<key>" to a
 #       GDrive URL for legacy files (uploaded before 2017) that require it.
-# - sha256 must match the downloaded bytes (compute with `sha256sum file.db`).
+# - sha256 must match the bytes of the file the tests read (compute with
-#   Leave as TODO_FILL_SHA256 to bypass the integrity check on first download
+#   `sha256sum file.db`), which for an archive is the file it holds rather than
-#   then paste the actual hash back in.
+#   the archive itself: a corrupt archive is caught by the extractor. Leave as
+#   TODO_FILL_SHA256 to bypass the integrity check on first download then paste
+#   the actual hash back in.
+# - extracted is optional and only for archives (.7z, .zip): the file the
+#   archive holds, which the fetch script extracts into data/tests/ and which
+#   the tests open. The archive is removed once extracted, so this file is both
+#   what the sha256 is of and what tells the script the asset is already there.
 #
 # Lines starting with '#' and blank lines are ignored.
 netherdrone_lidar3d_sample_15s.db	1IBE8aMgY1_kmme_7Kb9W2wz6yBMLl63i	3fedb26d76d080e687d3926ab80cadee27d7c0115638562a75c1d9f08cc1c885
@@ -19,6 +25,7 @@ pr2_scan2d_corridor_50s.db	18ClEPTRM98icorrlDYBzKTWRwVOU8-bh	f518cb4d9284b24a150
 robust_graph_optimization_stereo.db	https://raw.githubusercontent.com/wiki/introlab/rtabmap/doc/Tutorials/RobustGraphOptimization/robust_graph_optimization_stereo.db	247694b5bdb82168ebe88bb6bb5bc31c7142234f8c64567afd93e464418cf02e
 loop_3it_gps.db	https://raw.githubusercontent.com/wiki/introlab/rtabmap/doc/Tutorials/RobustGraphOptimization/loop_3it_gps.db	7aefeb573107a27b0a7826cb87ea03db495367aa56868b4d2b2543e6cacad3c8
 stereo_20Hz.db	https://github.com/introlab/rtabmap/releases/download/0.23.1/stereo_20Hz.db	94e219e1c96e540cbb1490cc23c81c65b9e7b132e8d61eda05e7990bc13393b7
+multisession_3it.7z	10ulRDMqQy5V_3_kx_IeNzAJ8iUMdkkBZ	3d7b139e6b4c608185e44774288ce078b6364bfaeecd0c46c51007c95230c987	multisession_3it.db
 superpoint_v1.pth	https://github.com/magicleap/SuperPointPretrainedNetwork/raw/refs/heads/master/superpoint_v1.pth	52b6708629640ca883673b5d5c097c4ddad37d8048b33f09c8ca0d69db12c40e
 superpoint_v6_from_tf.pth	https://github.com/rpautrat/SuperPoint/raw/refs/heads/master/weights/superpoint_v6_from_tf.pth	cd5d19a5061848e248c17728878ea166b66512076d43c77dbcf27f4a88a56084
 demo_superpoint.py	https://raw.githubusercontent.com/magicleap/SuperPointPretrainedNetwork/master/demo_superpoint.py	613706ae7e9ce3fbc2cfe042fc3f37d739838cc2e1dc8ddd08f8ec037765df04

guilib/src/PreferencesDialog.cpp

@@ -1132,6 +1132,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->general_doubleSpinBox_vp->setObjectName(Parameters::kBayesVirtualPlacePriorThr().c_str());
 	_ui->lineEdit_bayes_predictionLC->setObjectName(Parameters::kBayesPredictionLC().c_str());
 	_ui->checkBox_bayes_fullPredictionUpdate->setObjectName(Parameters::kBayesFullPredictionUpdate().c_str());
+	_ui->checkBox_bayes_sparsePrediction->setObjectName(Parameters::kBayesSparsePrediction().c_str());
 	connect(_ui->lineEdit_bayes_predictionLC, SIGNAL(textChanged(const QString &)), this, SLOT(updatePredictionPlot()));
 
 	//Keypoint-based
@@ -5601,7 +5602,11 @@ void PreferencesDialog::updatePredictionPlot()
 	QVector<qreal> dataX((values.size()-2)*2 + 1);
 	QVector<qreal> dataY((values.size()-2)*2 + 1);
 	double value;
-	double sum = 0;
+	// Summed as PredictionModel does it, into a float: that is the sum the filter decides on,
+	// and under 1 of it is what has normalize() spread the difference over every other
+	// location. A double would land a few 1e-8 from a float on a list like the default one,
+	// and say 1 where the filter says otherwise.
+	float sum = 0.0f;
 	int lvl = 1;
 	bool ok = false;
 	bool error = false;
@@ -5639,7 +5644,7 @@ void PreferencesDialog::updatePredictionPlot()
 	{
 		_ui->label_prediction_sum->setText(QString("<font color=#FF0000>") + _ui->label_prediction_sum->text() + "</font>");
 	}
-	else if(sum == 1.0)
+	else if(sum == 1.0f)
 	{
 		_ui->label_prediction_sum->setText(QString("<font color=#00FF00>") + _ui->label_prediction_sum->text() + "</font>");
 	}

guilib/src/ui/preferencesDialog.ui

@@ -63,9 +63,9 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-1365</y>
+            <y>0</y>
             <width>684</width>
-            <height>5343</height>
+            <height>5315</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -95,7 +95,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>5</number>
+                <number>11</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,0">
@@ -12591,14 +12591,14 @@ see Sqlite3 doc 'PRAGMA temp_store'.</string>
                     </item>
                     <item>
                      <layout class="QGridLayout" name="gridLayout_22" columnstretch="0,1">
-                      <item row="3" column="0">
+                      <item row="4" column="0">
                        <widget class="QCheckBox" name="checkBox_rtabmap_loopGPS">
                         <property name="text">
                          <string/>
                         </property>
                        </widget>
                       </item>
-                      <item row="5" column="0">
+                      <item row="6" column="0">
                        <widget class="QComboBox" name="comboBox_globalDescriptorExtractor">
                         <property name="sizeAdjustPolicy">
                          <enum>QComboBox::AdjustToContents</enum>
@@ -12628,7 +12628,7 @@ see Sqlite3 doc 'PRAGMA temp_store'.</string>
                         </property>
                        </widget>
                       </item>
-                      <item row="3" column="1">
+                      <item row="4" column="1">
                        <widget class="QLabel" name="label_502">
                         <property name="text">
                          <string>Use GPS to filter likelihood (if GPS is recorded). Only locations inside the Local Radius (see RGBD-SLAM panel) of the current GPS location are considered for loop closure detection.</string>
@@ -12648,7 +12648,30 @@ see Sqlite3 doc 'PRAGMA temp_store'.</string>
                         </property>
                        </widget>
                       </item>
-                      <item row="2" column="0">
+                      <item row="1" column="0">
+                       <widget class="QCheckBox" name="checkBox_bayes_sparsePrediction">
+                        <property name="text">
+                         <string/>
+                        </property>
+                        <property name="checked">
+                         <bool>true</bool>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="1" column="1">
+                       <widget class="QLabel" name="label_bayes_sparsePrediction">
+                        <property name="text">
+                         <string>Sparse prediction. Use a sparse representation of the prediction instead of a dense matrix, which significantly reduces memory usage and processing time on large maps. Ignored when the values above sum to less than 1, as the prediction is then not sparse.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="3" column="0">
                        <widget class="QCheckBox" name="checkBox_kp_tfIdfLikelihoodUsed">
                         <property name="text">
                          <string/>
@@ -12658,7 +12681,7 @@ see Sqlite3 doc 'PRAGMA temp_store'.</string>
                         </property>
                        </widget>
                       </item>
-                      <item row="1" column="0">
+                      <item row="2" column="0">
                        <widget class="QDoubleSpinBox" name="general_doubleSpinBox_vp">
                         <property name="maximum">
                          <double>1.000000000000000</double>
@@ -12671,7 +12694,7 @@ see Sqlite3 doc 'PRAGMA temp_store'.</string>
                         </property>
                        </widget>
                       </item>
-                      <item row="1" column="1">
+                      <item row="2" column="1">
                        <widget class="QLabel" name="label_113">
                         <property name="text">
                          <string>The VP here is the probability to be in a new place since no loop closure was found on the last iteration.</string>
@@ -12684,7 +12707,7 @@ see Sqlite3 doc 'PRAGMA temp_store'.</string>
                         </property>
                        </widget>
                       </item>
-                      <item row="2" column="1">
+                      <item row="3" column="1">
                        <widget class="QLabel" name="label_82">
                         <property name="text">
                          <string>Use the tf-idf method to compute the likelihood. Otherwise, images are compared with each other.</string>
@@ -12697,7 +12720,7 @@ see Sqlite3 doc 'PRAGMA temp_store'.</string>
                         </property>
                        </widget>
                       </item>
-                      <item row="5" column="1">
+                      <item row="6" column="1">
                        <widget class="QLabel" name="label_6031">
                         <property name="text">
                          <string>Global descriptor extractor.</string>
@@ -12710,7 +12733,7 @@ see Sqlite3 doc 'PRAGMA temp_store'.</string>
                         </property>
                        </widget>
                       </item>
-                      <item row="4" column="1">
+                      <item row="5" column="1">
                        <widget class="QLabel" name="label_757">
                         <property name="text">
                          <string>Likelihood ratio for VP.</string>
@@ -12723,7 +12746,7 @@ see Sqlite3 doc 'PRAGMA temp_store'.</string>
                         </property>
                        </widget>
                       </item>
-                      <item row="4" column="0">
+                      <item row="5" column="0">
                        <widget class="QComboBox" name="comboBox_virtualPlaceLikelihoodRatio">
                         <property name="sizeAdjustPolicy">
                          <enum>QComboBox::AdjustToContentsOnFirstShow</enum>

package.xml

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

patches/gtsam_4_3a0-ros.patch

@@ -1,5 +1,5 @@
 diff --git a/cmake/Config.cmake.in b/cmake/Config.cmake.in
-index 338ff8500..0b738ea3c 100644
+index 338ff85..0b738ea 100644
 --- a/cmake/Config.cmake.in
 +++ b/cmake/Config.cmake.in
 @@ -28,7 +28,7 @@ if(@GTSAM_USE_TBB@)
@@ -11,3 +11,43 @@ index 338ff8500..0b738ea3c 100644
  endif()
  
  # Load exports
+diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake
+index cf0dfdf..75c390e 100644
+--- a/cmake/GtsamBuildTypes.cmake
++++ b/cmake/GtsamBuildTypes.cmake
+@@ -131,7 +131,7 @@ else()
+   endif()
+ 
+   set(GTSAM_COMPILE_OPTIONS_PRIVATE_COMMON
+-    -Werror                                        # Enable warnings as errors
++    #-Werror                                        # Enable warnings as errors
+     -Wall                                          # Enable common warnings
+     -Wpedantic                                     # Enable pedantic warnings
+     $<$<COMPILE_LANGUAGE:CXX>:-Wextra -Wno-unused-parameter> # Enable extra warnings, but ignore no-unused-parameter (as we ifdef out chunks of code)
+diff --git a/cmake/HandleBoost.cmake b/cmake/HandleBoost.cmake
+index 0325112..f6a73f5 100644
+--- a/cmake/HandleBoost.cmake
++++ b/cmake/HandleBoost.cmake
+@@ -23,12 +23,12 @@ endif()
+ 
+ # Store these in variables so they are automatically replicated in GTSAMConfig.cmake and such.
+ set(BOOST_FIND_MINIMUM_VERSION 1.65)
+-set(BOOST_FIND_MINIMUM_COMPONENTS serialization system filesystem thread program_options date_time timer chrono regex)
++set(BOOST_FIND_MINIMUM_COMPONENTS serialization filesystem thread program_options date_time timer chrono regex)
+ 
+ find_package(Boost ${BOOST_FIND_MINIMUM_VERSION} COMPONENTS ${BOOST_FIND_MINIMUM_COMPONENTS} REQUIRED)
+ 
+ # Required components
+-if(NOT Boost_SERIALIZATION_LIBRARY OR NOT Boost_SYSTEM_LIBRARY OR NOT Boost_FILESYSTEM_LIBRARY OR
++if(NOT Boost_SERIALIZATION_LIBRARY OR NOT Boost_FILESYSTEM_LIBRARY OR
+     NOT Boost_THREAD_LIBRARY OR NOT Boost_DATE_TIME_LIBRARY)
+   message(FATAL_ERROR "Missing required Boost components >= v1.65, please install/upgrade Boost or configure your search paths.")
+ endif()
+@@ -37,7 +37,6 @@ option(GTSAM_DISABLE_NEW_TIMERS "Disables using Boost.chrono for timing" OFF)
+ # Allow for not using the timer libraries on boost < 1.48 (GTSAM timing code falls back to old timer library)
+ set(GTSAM_BOOST_LIBRARIES
+   Boost::serialization
+-  Boost::system
+   Boost::filesystem
+   Boost::thread
+   Boost::date_time

scripts/fetch_test_data.sh

@@ -1,13 +1,16 @@
 #!/usr/bin/env bash
 # Fetch test data assets listed in data/tests/manifest.txt. Each entry's
-# source can be either a bare Google Drive file ID (assembled into the
+# source can be either a bare Google Drive file ID (assembled into a
-# uc?export=download&id=... URL) or a full http(s):// URL (used as-is).
+# direct-download URL) or a full http(s):// URL (used as-is). An entry with a
-# Skips files that are already present and whose SHA-256 matches the manifest.
+# fourth column is an archive (.7z or .zip), which is extracted into data/tests/
-# Intended for CI and local first-time setup.
+# and then removed, its SHA-256 being the one of the file it holds. Skips files
+# that are already present and whose SHA-256 matches the manifest. Intended for
+# CI and local first-time setup.
 #
-# All linked files are public and under ~100 MB, so the direct GDrive download
+# All linked files are public. The GDrive URL carries confirm=t, which is what
-# URL streams the bytes directly without the virus-scan interstitial that
+# answers the interstitial page Drive puts in front of a file it did not scan
-# would otherwise need a tool like gdown.
+# for viruses (it never scans an archive, whatever its size), so the bytes come
+# down without needing a tool like gdown.
 set -euo pipefail
 
 REPO_ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
@@ -30,6 +33,51 @@ else
 	exit 1
 fi
 
+# Extracts an archive into DEST_DIR. Which tool is there varies: every CI runner
+# image ships 7-Zip, but it is 7z on some and 7zz on the ones carrying Debian's
+# 7zip package, the slim ROS images have neither until the workflow installs one,
+# and bsdtar reads 7z through libarchive where no 7-Zip is to be found (macOS tar
+# is bsdtar). Same for zip: unzip where there is one, python3's zipfile where
+# there is not (Git-Bash-on-Windows ships without unzip).
+extract_archive() {
+	local archive="$1"
+	case "$archive" in
+	*.7z)
+		local sevenzip=""
+		for candidate in 7z 7zz 7za 7zr; do
+			if command -v "$candidate" >/dev/null 2>&1; then
+				sevenzip="$candidate"
+				break
+			fi
+		done
+		if [[ -n "$sevenzip" ]]; then
+			"$sevenzip" x -y -o"$DEST_DIR" "$archive" >/dev/null
+		elif command -v bsdtar >/dev/null 2>&1; then
+			bsdtar -x -f "$archive" -C "$DEST_DIR"
+		elif tar --version 2>/dev/null | grep -qi bsdtar; then
+			tar -x -f "$archive" -C "$DEST_DIR"
+		else
+			echo "Error: no 7z, 7zz, 7za, 7zr or bsdtar on PATH to extract $archive" >&2
+			return 1
+		fi
+		;;
+	*.zip)
+		if command -v unzip >/dev/null 2>&1; then
+			unzip -o -q "$archive" -d "$DEST_DIR"
+		elif command -v python3 >/dev/null 2>&1; then
+			python3 -m zipfile -e "$archive" "$DEST_DIR"
+		else
+			echo "Error: neither unzip nor python3 is on PATH to extract $archive" >&2
+			return 1
+		fi
+		;;
+	*)
+		echo "Error: $archive is not an archive this script extracts (.7z, .zip)" >&2
+		return 1
+		;;
+	esac
+}
+
 verify_sha() {
 	local file="$1" expected="$2"
 	if [[ "$expected" == "TODO_FILL_SHA256" || -z "$expected" ]]; then
@@ -44,7 +92,7 @@ verify_sha() {
 	fi
 }
 
-while IFS=$'\t' read -r name source expected_sha; do
+while IFS=$'\t' read -r name source expected_sha extracted; do
 	# Strip trailing CR so the script works when manifest.txt is checked out
 	# with CRLF line endings (default on Windows Git unless core.autocrlf=input).
 	# Without this, expected_sha keeps a trailing \r and even a byte-for-byte
@@ -52,12 +100,17 @@ while IFS=$'\t' read -r name source expected_sha; do
 	name="${name%$'\r'}"
 	source="${source%$'\r'}"
 	expected_sha="${expected_sha%$'\r'}"
+	extracted="${extracted-}"
+	extracted="${extracted%$'\r'}"
 	# Skip comments and blank lines.
 	[[ -z "${name// }" || "$name" =~ ^# ]] && continue
 
 	target="$DEST_DIR/$name"
-	if [[ -f "$target" ]] && verify_sha "$target" "$expected_sha" 2>/dev/null; then
+	# An archive is not kept once it is extracted, so what has to be there, and
-		echo "Already up-to-date: $name"
+	# what the sha is of, is the file it held.
+	kept="${extracted:-$name}"
+	if [[ -f "$DEST_DIR/$kept" ]] && verify_sha "$DEST_DIR/$kept" "$expected_sha" 2>/dev/null; then
+		echo "Already up-to-date: $kept"
 		continue
 	fi
 
@@ -66,17 +119,38 @@ while IFS=$'\t' read -r name source expected_sha; do
 	if [[ "$source" =~ ^https?:// ]]; then
 		url="$source"
 	else
-		url="https://drive.google.com/uc?export=download&id=${source}"
+		url="https://drive.usercontent.google.com/download?id=${source}&export=download&confirm=t"
 	fi
 	echo "Fetching $name <- $url"
 	mkdir -p "$(dirname "$target")"
 	# -L follows the redirect, -f fails on HTTP errors, -S shows errors on stderr.
 	curl -fsSL "$url" -o "$target.partial"
-	if ! verify_sha "$target.partial" "$expected_sha"; then
+
-		rm -f "$target.partial"
+	if [[ -z "$extracted" ]]; then
-		exit 1
+		if ! verify_sha "$target.partial" "$expected_sha"; then
+			rm -f "$target.partial"
+			exit 1
+		fi
+		mv -f "$target.partial" "$target"
+	else
+		mv -f "$target.partial" "$target"
+		echo "  Extracting $name"
+		if ! extract_archive "$target"; then
+			rm -f "$target"
+			exit 1
+		fi
+		# The archive has served its purpose, and these are large files.
+		rm -f "$target"
+		if [[ ! -f "$DEST_DIR/$extracted" ]]; then
+			echo "  $name does not hold $extracted" >&2
+			exit 1
+		fi
+		if ! verify_sha "$DEST_DIR/$extracted" "$expected_sha"; then
+			rm -f "$DEST_DIR/$extracted"
+			exit 1
+		fi
+		echo "  Extracted $extracted ($(du -h "$DEST_DIR/$extracted" | cut -f1))"
 	fi
-	mv -f "$target.partial" "$target"
 done < "$MANIFEST"
 
 echo "Test data ready under $DEST_DIR"