bump 0.20.23

DBViewer: ignore marker constraints in refine, add marker priors when optimizing the graph
fix bigobj under MSVC, fix yaml-cpp link. (#957 )
2026-09-08 12:30:20 +08:00 · 2023-01-21 16:09:41 -08:00 · 2023-01-21 15:52:01 -08:00 · 2023-01-21 15:50:02 -08:00 · 2023-01-16 16:41:48 -08:00 · 2023-01-16 14:14:25 -08:00
57 changed files with 2618 additions and 1370 deletions
@@ -18,50 +18,47 @@ jobs:
    # well on Windows or Mac.  You can convert this to a matrix build if you need
    # cross-platform coverage.
    # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix
-    name: Build on ros ${{ matrix.ros_distro }} and ${{ matrix.os }}
+    name: Build on ros ${{ matrix.ros_distribution }} and ${{ matrix.os }}
    runs-on: ${{ matrix.os }}
    strategy:
      matrix:
-        os: [ubuntu-20.04, ubuntu-18.04]
+        ros_distribution: [melodic, noetic, foxy, humble, rolling]
        include:
-        - os: ubuntu-20.04
-          ros_distro: 'noetic'
-        - os: ubuntu-18.04
-          ros_distro: 'melodic'
+        - ros_distribution: 'melodic' 
+          os: ubuntu-18.04
+        - ros_distribution: 'noetic'
+          os: ubuntu-20.04
+        - ros_distribution: 'foxy'
+          os: ubuntu-20.04
+        - ros_distribution: 'humble'
+          os: ubuntu-22.04
+        - ros_distribution: 'rolling'
+          os: ubuntu-22.04

    steps:
-    - uses: ros-tooling/setup-ros@v0.2
+    - uses: ros-tooling/setup-ros@v0.4
      with:
-        required-ros-distributions: ${{ matrix.ros_distro }}
+        required-ros-distributions: ${{ matrix.ros_distribution }}
    
    - name: Install dependencies
      run: |
        sudo apt-get update
-        sudo apt-get -y install ros-${{ matrix.ros_distro }}-rtabmap-ros
-        sudo apt-get -y remove ros-${{ matrix.ros_distro }}-rtabmap
+        sudo apt-get -y install ros-${{ matrix.ros_distribution }}-rtabmap
+        sudo apt-get -y remove ros-${{ matrix.ros_distribution }}-rtabmap

    - uses: actions/checkout@v2

    - name: Configure CMake
-      # Configure CMake in a 'build' subdirectory. `CMAKE_BUILD_TYPE` is only required if you are using a single-configuration generator such as make.
-      # See https://cmake.org/cmake/help/latest/variable/CMAKE_BUILD_TYPE.html?highlight=cmake_build_type
      run: |
-        source /opt/ros/${{ matrix.ros_distro }}/setup.bash
+        source /opt/ros/${{ matrix.ros_distribution }}/setup.bash
        cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}}

    - name: Build
-      # Build your program with the given configuration
      run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}}

    - name: Info
      working-directory: ${{github.workspace}}/build/bin
      run: |
-        source /opt/ros/${{ matrix.ros_distro }}/setup.bash
+        source /opt/ros/${{ matrix.ros_distribution }}/setup.bash
        ./rtabmap-console --version
-
-#    - name: Test
-#      working-directory: ${{github.workspace}}/build
-#      # Execute tests defined by the CMake configuration.  
-#      # See https://cmake.org/cmake/help/latest/manual/ctest.1.html for more detail
-#      run: ctest -C ${{env.BUILD_TYPE}}
      
@@ -24,7 +24,7 @@ jobs:
      run: |
        DEBIAN_FRONTEND=noninteractive
        sudo apt-get update
-        sudo apt-get -y install libopencv-dev libpcl-dev git cmake software-properties-common
+        sudo apt-get -y install libopencv-dev libpcl-dev git cmake software-properties-common libyaml-cpp-dev

    - uses: actions/checkout@v2

@@ -17,6 +17,8 @@ jobs:
          docker_tags: |
            introlab3it/rtabmap:xenial
            introlab3it/rtabmap:16.04
+          docker_args: |
+            NOT_USED=0
          docker_platforms: |
            linux/amd64
          docker_path: 'xenial'
@@ -24,6 +26,8 @@ jobs:
          docker_tags: |
            introlab3it/rtabmap:bionic
            introlab3it/rtabmap:18.04
+          docker_args: |
+            NOT_USED=0
          docker_platforms: |
            linux/amd64
            linux/arm64
@@ -33,6 +37,8 @@ jobs:
            introlab3it/rtabmap:focal
            introlab3it/rtabmap:20.04
            introlab3it/rtabmap:latest
+          docker_args: |
+            NOT_USED=0
          docker_platforms: |
            linux/amd64
            linux/arm64
@@ -20,7 +20,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 20)
-SET(RTABMAP_PATCH_VERSION 21)
+SET(RTABMAP_PATCH_VERSION 23)
 SET(RTABMAP_VERSION
  ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
  
@@ -106,6 +106,7 @@ if(MSVC)
      SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /MP${N}")
    endif()
  endif()
+  add_compile_options("/bigobj")
 endif()

 # [Eclipse] Automatic Discovery of Include directories (Optional, but handy) 
@@ -315,7 +316,11 @@ IF(WITH_QT)
 			IF(value EQUAL -1)
                list(FIND PCL_LIBRARIES vtkGUISupportQt value)
                IF(value EQUAL -1)
-                    SET(PCL_LIBRARIES "${PCL_LIBRARIES};vtkGUISupportQt")
+                    IF("${VTK_MAJOR_VERSION}" GREATER 8)
+                        SET(PCL_LIBRARIES "${PCL_LIBRARIES};VTK::GUISupportQt")
+                    ELSE()
+                        SET(PCL_LIBRARIES "${PCL_LIBRARIES};vtkGUISupportQt")
+                    ENDIF()
                    SET(ADD_VTK_GUI_SUPPORT_QT_TO_CONF TRUE)
                ENDIF(value EQUAL -1)
 		    ENDIF(value EQUAL -1)
@@ -1019,7 +1024,11 @@ IF(NOT WITH_PYTHON OR NOT Python3_FOUND)
 ENDIF()
 IF(ADD_VTK_GUI_SUPPORT_QT_TO_CONF)
   SET(CONF_VTK_QT true)
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} vtkGUISupportQt)
+   IF("${VTK_MAJOR_VERSION}" GREATER 8)
+      SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} VTK::GUISupportQt)
+   ELSE()
+      SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} vtkGUISupportQt)
+   ENDIF()
 ELSE()
   SET(CONF_VTK_QT false)
 ENDIF()
@@ -6,9 +6,6 @@ rtabmap
 [![Release][release-image]][releases]
 [![License][license-image]][license]

- * Linux: [![Build Status](https://github.com/introlab/rtabmap/actions/workflows/cmake.yml/badge.svg)](https://github.com/introlab/rtabmap/actions/workflows/cmake.yml) [![Build Status](https://github.com/introlab/rtabmap/actions/workflows/cmake-ros.yml/badge.svg)](https://github.com/introlab/rtabmap/actions/workflows/cmake-ros.yml) [![docker](https://github.com/introlab/rtabmap/actions/workflows/docker.yml/badge.svg)](https://github.com/introlab/rtabmap/actions/workflows/docker.yml) 
- * Windows: [![Build status](https://ci.appveyor.com/api/projects/status/hr73xspix9oqa26h/branch/master?svg=true)](https://ci.appveyor.com/project/matlabbe/rtabmap/branch/master)
-
 [release-image]: https://img.shields.io/badge/release-0.20.16-green.svg?style=flat
 [releases]: https://github.com/introlab/rtabmap/releases

@@ -28,3 +25,52 @@ This project is supported by [IntRoLab - Intelligent / Interactive / Integrated
 <a href="https://introlab.3it.usherbrooke.ca/">
 <img src="https://github.com/introlab/16SoundsUSB/blob/master/images/IntRoLab.png" alt="IntRoLab" height="100">
 </a>
+
+#### CI Latest
+
+  <table>
+    <tbody>
+        <tr>
+           <td>Linux</td>
+           <td><a href="https://github.com/introlab/rtabmap/actions/workflows/cmake.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/cmake.yml/badge.svg" alt="Build Status"/> <br> <a href="https://github.com/introlab/rtabmap/actions/workflows/cmake-ros.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/cmake-ros.yml/badge.svg" alt="Build Status"/> <br> <a href="https://github.com/introlab/rtabmap/actions/workflows/docker.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/docker.yml/badge.svg" alt="Build Status"/>
+           </td>
+        </tr>
+        <tr>
+           <td>Windows</td>
+           <td><a href="https://ci.appveyor.com/project/matlabbe/rtabmap/branch/master"><img src="https://ci.appveyor.com/api/projects/status/hr73xspix9oqa26h/branch/master?svg=true" alt="Build Status"/>
+           </td>
+        </tr>
+     </tbody>
+  </table>
+ 
+ #### ROS Binaries
+ 
+ `ros-$ROS_DISTRO-rtabmap`
+ 
+ <table>
+    <tbody>
+        <tr>
+           <td rowspan="2">ROS 1</td>
+           <td>Melodic</td>
+            <td><a href="http://build.ros.org/job/Mbin_ubv8_uBv8__rtabmap__ubuntu_bionic_arm64__binary/"><img src="http://build.ros.org/buildStatus/icon?job=Mbin_ubv8_uBv8__rtabmap__ubuntu_bionic_arm64__binary" alt="Build Status"/></td>
+        </tr>
+        <tr>
+            <td>Noetic</td>
+            <td><a href="http://build.ros.org/job/Nbin_ufv8_uFv8__rtabmap__ubuntu_focal_arm64__binary/"><img src="http://build.ros.org/buildStatus/icon?job=Nbin_ufv8_uFv8__rtabmap__ubuntu_focal_arm64__binary" alt="Build Status"/></td>
+        </tr>
+        <tr>
+            <td rowspan="3">ROS 2</td>
+            <td>Foxy</td>
+            <td><a href="http://build.ros2.org/job/Fbin_uF64__rtabmap__ubuntu_focal_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Fbin_uF64__rtabmap__ubuntu_focal_amd64__binary" alt="Build Status"/></td>
+        </tr>
+        <tr>
+            <td>Humble</td>
+            <td><a href="http://build.ros2.org/job/Hbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary" alt="Build Status"/></td>
+        </tr>
+        <tr>
+            <td>Rolling</td>
+            <td><a href="http://build.ros2.org/job/Rbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary" alt="Build Status"/></td>
+        </tr>
+    </tbody>
+</table>
+ 
@@ -2130,7 +2130,7 @@ int RTABMapApp::Render()
 				if(camera_!=0 && lastPoseEventTime_>0.0 && UTimer::now()-lastPoseEventTime_ > 1.0)
 				{
 					UERROR("TangoPoseEventNotReceived");
-					UEventsManager::post(new rtabmap::CameraInfoEvent(10, "TangoPoseEventNotReceived", uNumber2Str(UTimer::now()-lastPoseEventTime_)));
+					UEventsManager::post(new rtabmap::CameraInfoEvent(10, "TangoPoseEventNotReceived", uNumber2Str(UTimer::now()-lastPoseEventTime_, 6)));
 				}
 			}
 		}
@@ -0,0 +1 @@
+Libraries
@@ -1,6 +0,0 @@
-# Ignore everything
-*
-
-# But not these files...
-!.gitignore
-!install_deps.sh
@@ -1,7 +1,11 @@
 #!/bin/bash

+set -euxo pipefail
+
 # Tested on Apple Silicon Mac, with cmake 3.19.2.

+mkdir Libraries
+cd Libraries
 pwd=$(pwd)
 prefix=$pwd
 sysroot=iphoneos
@@ -107,7 +111,7 @@ git checkout tags/v8.2.0
 git cherry-pick bf3ae8072df2393c7270509bae41be0776826346
 mkdir build
 cd build
-cmake -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_FRAMEWORK_INSTALL_PREFIX=$prefix/lib -DBUILD_EXAMPLES=OFF -DBUILD_TESTING=OFF -DVTK_IOS_BUILD=ON -DIOS_SIMULATOR_ARCHITECTURES=arm64 -DModule_vtkFiltersModeling=ON ..
+cmake -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_FRAMEWORK_INSTALL_PREFIX=$prefix/lib -DIOS_DEVICE_ARCHITECTURES="arm64" -DIOS_SIMULATOR_ARCHITECTURES="" -DBUILD_EXAMPLES=OFF -DBUILD_TESTING=OFF -DVTK_IOS_BUILD=ON -DModule_vtkFiltersModeling=ON ..
 # For iphonesimulator: add -DIOS_DEVICE_ARCHITECTURES=""
 cmake --build . --config Release
 cd $pwd
@@ -55,7 +55,9 @@ public:
 			 int stopId = 0,
 			 bool intermediateNodesIgnored = false,
 			 bool landmarksIgnored = false,
-			 bool featuresIgnored = false);
+			 bool featuresIgnored = false,
+			 int startMapId = 0,
+			 int stopMapId = -1);
 	DBReader(const std::list<std::string> & databasePaths,
 			 float frameRate = 0.0f, // -1 = use Database stamps, 0 = inf
 			 bool odometryIgnored = false,
@@ -66,7 +68,9 @@ public:
 			 int stopId = 0,
 			 bool intermediateNodesIgnored = false,
 			 bool landmarksIgnored = false,
-			 bool featuresIgnored = false);
+			 bool featuresIgnored = false,
+			 int startMapId = 0,
+			 int stopMapId = -1);
 	virtual ~DBReader();

 	virtual bool init(
@@ -77,6 +81,8 @@ public:
 	virtual std::string getSerial() const;
 	virtual bool odomProvided() const {return !_odometryIgnored;}

+	const DBDriver * driver() const {return _dbDriver;}
+
 protected:
 	virtual SensorData captureImage(CameraInfo * info = 0);

@@ -94,6 +100,8 @@ private:
 	bool _intermediateNodesIgnored;
 	bool _landmarksIgnored;
 	bool _featuresIgnored;
+	int _startMapId;
+	int _stopMapId;

 	DBDriver * _dbDriver;
 	UTimer _timer;
@@ -183,7 +183,8 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(Rtabmap, ImageBufferSize,          unsigned int, 1, "Data buffer size (0 min inf).");
    RTABMAP_PARAM(Rtabmap, CreateIntermediateNodes,      bool, false, uFormat("Create intermediate nodes between loop closure detection. Only used when %s>0.", kRtabmapDetectionRate().c_str()));
    RTABMAP_PARAM_STR(Rtabmap, WorkingDirectory,         "",          "Working directory.");
-    RTABMAP_PARAM(Rtabmap, MaxRetrieved,             unsigned int, 2, "Maximum locations retrieved at the same time from LTM.");
+    RTABMAP_PARAM(Rtabmap, MaxRetrieved,             unsigned int, 2, "Maximum nodes retrieved at the same time from LTM.");
+    RTABMAP_PARAM(Rtabmap, MaxRepublished,           unsigned int, 2, uFormat("Maximum nodes republished when requesting missing data. When %s=false, only loop closure data is republished, otherwise the closest nodes from the current localization are republished first. Ignored if %s=false.", kRGBDEnabled().c_str(), kRtabmapPublishLastSignature().c_str()));
    RTABMAP_PARAM(Rtabmap, StatisticLogsBufferedInRAM,   bool, true,  "Statistic logs buffered in RAM instead of written to hard drive after each iteration.");
    RTABMAP_PARAM(Rtabmap, StatisticLogged,              bool, false, "Logging enabled.");
    RTABMAP_PARAM(Rtabmap, StatisticLoggedHeaders,       bool, true,  "Add column header description to log files.");
@@ -223,6 +223,7 @@ public:
 	int refineLinks();
 	bool addLink(const Link & link);
 	cv::Mat getInformation(const cv::Mat & covariance) const;
+	void addNodesToRepublish(const std::vector<int> & ids);

 	int getPathStatus() const {return _pathStatus;} // -1=failed 0=idle/executing 1=success
 	void clearPath(int status); // -1=failed 0=idle/executing 1=success
@@ -284,6 +285,7 @@ private:
 	bool _verifyLoopClosureHypothesis;
 	unsigned int _maxRetrieved;
 	unsigned int _maxLocalRetrieved;
+	unsigned int _maxRepublished;
 	bool _rawDataKept;
 	bool _statisticLogsBufferedInRAM;
 	bool _statisticLogged;
@@ -368,6 +370,8 @@ private:
 	std::vector<float> _odomCorrectionAcc;
 	std::map<int, Transform> _markerPriors;

+	std::set<int> _nodesToRepublish;
+
 	// Planning stuff
 	int _pathStatus;
 	std::vector<std::pair<int,Transform> > _path;
@@ -59,15 +59,18 @@ class RtabmapEventCmd : public UEvent
 public:
 	enum dummy {d}; // Hack, to fix Eclipse complaining about not defined Cmd enum ?!
 	enum Cmd {
+			kCmdUndef,
 			kCmdInit,             // params: [string] database path + ParametersMap
 			kCmdResetMemory,
 			kCmdClose,            // params: [bool] database saved (default true), [string] output database path (empty=use same database to save, only work when Db/Sqlite3InMemory=true)
+			kCmdUpdateParams,     // params: ParametersMap
 			kCmdDumpMemory,
 			kCmdDumpPrediction,
 			kCmdGenerateDOTGraph, // params: [bool] global, [string] path, if global=false: [int] id, [int] margin
 			kCmdExportPoses,      // params: [bool] global, [bool] optimized, [string] path, [int] type (0=raw format, 1=RGBD-SLAM format, 2=KITTI format, 3=TORO, 4=g2o)
 			kCmdCleanDataBuffer,
 			kCmdPublish3DMap,     // params: [bool] global, [bool] optimized, [bool] graphOnly
+			kCmdRepublishData,    // params: [vector<int>] ids
 			kCmdTriggerNewMap,
 			kCmdPause,
 			kCmdResume,
@@ -54,22 +54,8 @@ class RTABMAP_EXP RtabmapThread :
 {
 public:
 	enum State {
-		kStateInit,
 		kStateDetecting,
-		kStateReseting,
-		kStateClose,
-		kStateChangingParameters,
-		kStateDumpingMemory,
-		kStateDumpingPrediction,
-		kStateExportingDOTGraph,
-		kStateExportingPoses,
-		kStateCleanDataBuffer,
-		kStatePublishingMap,
-		kStateTriggeringMap,
-		kStateSettingGoal,
-		kStateCancellingGoal,
-		kStateLabelling,
-		kStateRemovingLabel
+		kStateProcessCommand
 	};

 public:
@@ -105,13 +91,13 @@ private:
 	void process();
 	void addData(const OdometryEvent & odomEvent);
 	bool getData(OdometryEvent & data);
-	void pushNewState(State newState, const ParametersMap & parameters = ParametersMap());
+	void pushNewState(State newState, const RtabmapEventCmd & cmdEvent = RtabmapEventCmd(RtabmapEventCmd::kCmdUndef));
 	void publishMap(bool optimized, bool full, bool graphOnly) const;

 private:
 	UMutex _stateMutex;
 	std::queue<State> _state;
-	std::queue<ParametersMap> _stateParam;
+	std::queue<RtabmapEventCmd> _stateParam;

 	std::list<OdometryEvent> _dataBuffer;
 	std::list<double> _newMapEvents;
@@ -241,7 +241,9 @@ public:
 	void setProximityDetectionMapId(int id) {_proximiyDetectionMapId = id;}
 	void setStamp(double stamp) {_stamp = stamp;}

-	void setLastSignatureData(const Signature & data) {_lastSignatureData = data;}
+	RTABMAP_DEPRECATED(void setLastSignatureData(const Signature & data), "Use addSignatureData() instead.");
+	void addSignatureData(const Signature & data) {_signaturesData.insert(std::make_pair(data.id(), data));}
+	void setSignaturesData(const std::map<int, Signature> & data) {_signaturesData = data;}

 	void setPoses(const std::map<int, Transform> & poses) {_poses = poses;}
 	void setConstraints(const std::multimap<int, Link> & constraints) {_constraints = constraints;}
@@ -270,7 +272,8 @@ public:
 	int proximityDetectionMapId() const {return _proximiyDetectionMapId;}
 	double stamp() const {return _stamp;}

-	const Signature & getLastSignatureData() const {return _lastSignatureData;}
+	const Signature & getLastSignatureData() const {return _signaturesData.empty()?_dummyEmptyData:_signaturesData.rbegin()->second;}
+	const std::map<int, Signature> & getSignaturesData() const {return _signaturesData;}

 	const std::map<int, Transform> & poses() const {return _poses;}
 	const std::multimap<int, Link> & constraints() const {return _constraints;}
@@ -302,7 +305,8 @@ private:
 	int _proximiyDetectionMapId;
 	double _stamp;

-	Signature _lastSignatureData;
+	std::map<int, Signature> _signaturesData;
+	Signature _dummyEmptyData;

 	std::map<int, Transform> _poses;
 	std::multimap<int, Link> _constraints;
@@ -59,6 +59,7 @@ public:

 	void setOutputDepth(bool enabled, int confidence = 200);
 	void setIMUFirmwareUpdate(bool enabled);
+	void setIMUPublished(bool published);

 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
 	virtual bool isCalibrated() const;
@@ -76,6 +77,7 @@ private:
 	int depthConfidence_;
 	int resolution_;
 	bool imuFirmwareUpdate_;
+	bool imuPublished_;
 	std::shared_ptr<dai::Device> device_;
 	std::shared_ptr<dai::DataOutputQueue> leftQueue_;
 	std::shared_ptr<dai::DataOutputQueue> rightOrDepthQueue_;
@@ -109,7 +109,7 @@ void segmentObstaclesFromGround(
 					{
 						Eigen::Vector4f min,max;
 						pcl::getMinMax3D(*cloud, *clusteredFlatSurfaces.at(i), min, max);
-						if(min[2]<maxGroundHeight && clusteredFlatSurfaces.size() > points)
+						if(min[2]<maxGroundHeight && clusteredFlatSurfaces.at(i)->size() > points)
 						{
 							points = clusteredFlatSurfaces.at(i)->size();
 							biggestFlatSurfaceIndex = i;
@@ -500,6 +500,7 @@ void DBDriverSqlite3::disconnectDatabaseQuery(bool save, const std::string & out
 				UERROR("Failed to rename just closed db %s to %s", this->getUrl().c_str(), outputUrl.c_str());
 			}
 		}
+		UINFO("Disconnected database %s!", this->getUrl().c_str());
 	}
 }

@@ -52,7 +52,9 @@ DBReader::DBReader(const std::string & databasePath,
 				   int stopId,
 				   bool intermediateNodesIgnored,
 				   bool landmarksIgnored,
-				   bool featuresIgnored) :
+				   bool featuresIgnored,
+				   int startMapId,
+				   int stopMapId) :
 	Camera(frameRate),
 	_paths(uSplit(databasePath, ';')),
 	_odometryIgnored(odometryIgnored),
@@ -64,6 +66,8 @@ DBReader::DBReader(const std::string & databasePath,
 	_intermediateNodesIgnored(intermediateNodesIgnored),
 	_landmarksIgnored(landmarksIgnored),
 	_featuresIgnored(featuresIgnored),
+	_startMapId(startMapId),
+	_stopMapId(stopMapId),
 	_dbDriver(0),
 	_currentId(_ids.end()),
 	_previousMapId(-1),
@@ -75,6 +79,11 @@ DBReader::DBReader(const std::string & databasePath,
 	{
 		_stopId = _startId;
 	}
+
+	if(_stopMapId>-1 && _stopMapId<_startMapId)
+	{
+		_stopMapId = _startMapId;
+	}
 }

 DBReader::DBReader(const std::list<std::string> & databasePaths,
@@ -87,7 +96,9 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
 				   int stopId,
 				   bool intermediateNodesIgnored,
 				   bool landmarksIgnored,
-				   bool featuresIgnored) :
+				   bool featuresIgnored,
+				   int startMapId,
+				   int stopMapId) :
 	Camera(frameRate),
   _paths(databasePaths),
 	_odometryIgnored(odometryIgnored),
@@ -99,6 +110,8 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
 	_intermediateNodesIgnored(intermediateNodesIgnored),
 	_landmarksIgnored(landmarksIgnored),
 	_featuresIgnored(featuresIgnored),
+	_startMapId(startMapId),
+	_stopMapId(stopMapId),
 	_dbDriver(0),
 	_currentId(_ids.end()),
 	_previousMapId(-1),
@@ -110,6 +123,11 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
 	{
 		_stopId = _startId;
 	}
+
+	if(_stopMapId>-1 && _stopMapId<_startMapId)
+	{
+		_stopMapId = _startMapId;
+	}
 }

 DBReader::~DBReader()
@@ -368,6 +386,15 @@ SensorData DBReader::getNextData(CameraInfo * info)

 			if(_intermediateNodesIgnored && s->getWeight() == -1)
 			{
+				UDEBUG("Ignoring node %d (intermediate nodes ignored)", s->id());
+				++_currentId;
+				delete s;
+				continue;
+			}
+
+			if(s->mapId() < _startMapId || (_stopMapId>=0 && s->mapId() > _stopMapId))
+			{
+				UDEBUG("Ignoring node %d (map id=%d, min=%d max=%d)", s->id(), s->mapId(), _startMapId, _stopMapId);
 				++_currentId;
 				delete s;
 				continue;
@@ -439,15 +466,7 @@ SensorData DBReader::getNextData(CameraInfo * info)
 				}
 				else
 				{
-					// if localization data saved in database, covariance will be set in a prior link
-					_dbDriver->loadLinks(*_currentId, links, Link::kPosePrior);
-					if(links.size())
-					{
-						// assume the first is the backward neighbor, take its variance
-						infMatrix = links.begin()->second.infMatrix();
-						_previousInfMatrix = infMatrix;
-					}
-					else if(_previousMapId != s->mapId())
+					if(_previousMapId != s->mapId())
 					{
 						// first node, set high variance to make rtabmap trigger a new map
 						infMatrix /= 9999.0;
@@ -455,12 +474,23 @@ SensorData DBReader::getNextData(CameraInfo * info)
 					}
 					else
 					{
-						if(_previousInfMatrix.empty())
+						// if localization data saved in database, covariance will be set in a prior link
+						_dbDriver->loadLinks(*_currentId, links, Link::kPosePrior);
+						if(links.size())
 						{
-							_previousInfMatrix = cv::Mat::eye(6,6,CV_64FC1);
+							// assume the first is the backward neighbor, take its variance
+							infMatrix = links.begin()->second.infMatrix();
+							_previousInfMatrix = infMatrix;
+						}
+						else
+						{
+							if(_previousInfMatrix.empty())
+							{
+								_previousInfMatrix = cv::Mat::eye(6,6,CV_64FC1);
+							}
+							// we have a node not linked to map, use last variance
+							infMatrix = _previousInfMatrix;
 						}
-						// we have a node not linked to map, use last variance
-						infMatrix = _previousInfMatrix;
 					}
 				}
 				_previousMapId = s->mapId();
@@ -545,7 +575,7 @@ SensorData DBReader::getNextData(CameraInfo * info)
 			data.setId(seq);
 			data.setStamp(s->getStamp());
 			data.setGroundTruth(s->getGroundTruthPose());
-			if(globalPose.isNull())
+			if(!globalPose.isNull())
 			{
 				data.setGlobalPose(globalPose, globalPoseCov);
 			}
@@ -56,7 +56,7 @@ bool exportPoses(
 		int format, // 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame, 11=10+ID), 2=KITTI, 3=TORO, 4=g2o
 		const std::map<int, Transform> & poses,
 		const std::multimap<int, Link> & constraints, // required for formats 3 and 4
-		const std::map<int, double> & stamps, // required for format 1
+		const std::map<int, double> & stamps, // required for format 1, 10 and 11
 		const ParametersMap & parameters) // optional for formats 3 and 4
 {
 	UDEBUG("%s", filePath.c_str());
@@ -524,9 +524,9 @@ bool exportGPS(
 			for(std::map<int, GPS>::const_iterator iter=gpsValues.begin(); iter!=gpsValues.end(); ++iter)
 			{
 				values += uFormat("%s,%s,%s ",
-                                  uReplaceChar(uNumber2Str(iter->second.longitude()), ',', '.').c_str(),
-                                  uReplaceChar(uNumber2Str(iter->second.latitude()), ',', '.').c_str(),
-                                  uReplaceChar(uNumber2Str(iter->second.altitude()), ',', '.').c_str());
+                                  uReplaceChar(uNumber2Str(iter->second.longitude(), 8, true), ',', '.').c_str(),
+                                  uReplaceChar(uNumber2Str(iter->second.latitude(), 8, true), ',', '.').c_str(),
+                                  uReplaceChar(uNumber2Str(iter->second.altitude(), 8, true), ',', '.').c_str());
 			}

 			// switch argb (Qt format) -> abgr
@@ -580,13 +580,13 @@ bool exportGPS(
 			fprintf(fout, "# stamp longitude latitude altitude error bearing\n");
 			for(std::map<int, GPS>::const_iterator iter=gpsValues.begin(); iter!=gpsValues.end(); ++iter)
 			{
-				fprintf(fout, "%f %f %f %f %f %f\n",
-						iter->second.stamp(),
-						iter->second.longitude(),
-						iter->second.latitude(),
-						iter->second.altitude(),
-						iter->second.error(),
-						iter->second.bearing());
+				fprintf(fout, "%f %.*f %.*f %.*f %.*f %.*f\n",
+				    iter->second.stamp(),
+				    8, iter->second.longitude(),
+				    8, iter->second.latitude(),
+				    8, iter->second.altitude(),
+				    8, iter->second.error(),
+				    8, iter->second.bearing());
 			}
 		}

@@ -39,12 +39,16 @@ MarkerDetector::MarkerDetector(const ParametersMap & parameters)
 	maxRange_ = Parameters::defaultMarkerMaxRange();
 	minRange_ = Parameters::defaultMarkerMinRange();
 	dictionaryId_ = Parameters::defaultMarkerDictionary();
-#if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=2)
+#if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7)
+    detectorParams_.reset(new cv::aruco::DetectorParameters());
+#elif CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=2)
 	detectorParams_ = cv::aruco::DetectorParameters::create();
 #else
 	detectorParams_.reset(new cv::aruco::DetectorParameters());
 #endif
-#if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=3)
+#if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7)
+    detectorParams_->cornerRefinementMethod = (cv::aruco::CornerRefineMethod) Parameters::defaultMarkerCornerRefinementMethod();
+#elif CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=3)
 	detectorParams_->cornerRefinementMethod = Parameters::defaultMarkerCornerRefinementMethod();
 #else
 	detectorParams_->doCornerRefinement = Parameters::defaultMarkerCornerRefinementMethod()!=0;
@@ -70,7 +74,11 @@ void MarkerDetector::parseParameters(const ParametersMap & parameters)
 	detectorParams_->minCornerDistanceRate = 0.05;
 	detectorParams_->minDistanceToBorder = 3;
 	detectorParams_->minMarkerDistanceRate = 0.05;
-#if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=3)
+#if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7)
+    int cornerRefinementMethod;
+    Parameters::parse(parameters, Parameters::kMarkerCornerRefinementMethod(), cornerRefinementMethod);
+    detectorParams_->cornerRefinementMethod = (cv::aruco::CornerRefineMethod)cornerRefinementMethod;
+#elif CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=3)
 	Parameters::parse(parameters, Parameters::kMarkerCornerRefinementMethod(), detectorParams_->cornerRefinementMethod);
 #else
 	int doCornerRefinement = detectorParams_->doCornerRefinement?1:0;
@@ -103,7 +111,10 @@ void MarkerDetector::parseParameters(const ParametersMap & parameters)
 		dictionaryId_ = Parameters::defaultMarkerDictionary();
 	}
 #endif
-#if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=2)
+#if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7)
+    dictionary_.reset(new cv::aruco::Dictionary());
+    *dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::PredefinedDictionaryType(dictionaryId_));
+#elif CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=2)
 	dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::PREDEFINED_DICTIONARY_NAME(dictionaryId_));
 #else
 	dictionary_.reset(new cv::aruco::Dictionary());
@@ -770,7 +770,7 @@ void OccupancyGrid::addToCache(
 		const cv::Mat & obstacles,
 		const cv::Mat & empty)
 {
-	UDEBUG("nodeId=%d", nodeId);
+	UDEBUG("nodeId=%d (ground=%d obstacles=%d empty=%d)", nodeId, ground.cols, obstacles.cols, empty.cols);
 	if(nodeId < 0)
 	{
 		UWARN("Cannot add nodes with negative id (nodeId=%d)", nodeId);
@@ -1026,6 +1026,7 @@ bool OccupancyGrid::update(const std::map<int, Transform> & posesIn)

 		if(!cache_.empty())
 		{
+			UDEBUG("Updating from cache");
 			for(std::list<std::pair<int, Transform> >::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
 			{
 				if(uContains(cache_, iter->first))
@@ -1035,14 +1036,18 @@ bool OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 					UDEBUG("Adding grid %d: ground=%d obstacles=%d empty=%d", iter->first, pair.first.first.cols, pair.first.second.cols, pair.second.cols);

 					//ground
+					cv::Mat ground;
+					if(pair.first.first.cols || pair.second.cols)
+					{
+						ground = cv::Mat(1, pair.first.first.cols+pair.second.cols, CV_32FC2);
+					}
 					if(pair.first.first.cols)
 					{
 						if(pair.first.first.rows > 1 && pair.first.first.cols == 1)
 						{
 							UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", pair.first.first.rows, pair.first.first.cols);
 						}
-						cv::Mat ground(1, pair.first.first.cols, CV_32FC2);
-						for(int i=0; i<ground.cols; ++i)
+						for(int i=0; i<pair.first.first.cols; ++i)
 						{
 							const float * vi = pair.first.first.ptr<float>(0,i);
 							float * vo = ground.ptr<float>(0,i);
@@ -1067,7 +1072,6 @@ bool OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 							else if(maxY < vo[1])
 								maxY = vo[1];
 						}
-						uInsert(emptyLocalMaps, std::make_pair(iter->first, ground));

 						if(cloudAssembling_)
 						{
@@ -1083,11 +1087,10 @@ bool OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 						{
 							UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", pair.second.rows, pair.second.cols);
 						}
-						cv::Mat ground(1, pair.second.cols, CV_32FC2);
-						for(int i=0; i<ground.cols; ++i)
+						for(int i=0; i<pair.second.cols; ++i)
 						{
 							const float * vi = pair.second.ptr<float>(0,i);
-							float * vo = ground.ptr<float>(0,i);
+							float * vo = ground.ptr<float>(0,i+pair.first.first.cols);
 							cv::Point3f vt;
 							if(pair.second.channels() != 2 && pair.second.channels() != 5)
 							{
@@ -1109,7 +1112,6 @@ bool OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 							else if(maxY < vo[1])
 								maxY = vo[1];
 						}
-						uInsert(emptyLocalMaps, std::make_pair(iter->first, ground));

 						if(cloudAssembling_)
 						{
@@ -1117,6 +1119,7 @@ bool OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 							assembledEmptyCellsUpdated = true;
 						}
 					}
+					uInsert(emptyLocalMaps, std::make_pair(iter->first, ground));

 					//obstacles
 					if(pair.first.second.cols)
@@ -1246,205 +1249,208 @@ bool OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 				}
 				for(std::list<std::pair<int, Transform> >::const_iterator kter = poses.begin(); kter!=poses.end(); ++kter)
 				{
-					if(kter->first > 0)
-					{
-						uInsert(addedNodes_, *kter);
-					}
 					std::map<int, cv::Mat >::iterator iter = emptyLocalMaps.find(kter->first);
 					std::map<int, cv::Mat >::iterator jter = occupiedLocalMaps.find(kter->first);
-					std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(kter->first);
-					if(cter == cellCount_.end() && kter->first > 0)
+					if(iter != emptyLocalMaps.end() || jter!=occupiedLocalMaps.end())
 					{
-						cter = cellCount_.insert(std::make_pair(kter->first, std::pair<int,int>(0,0))).first;
-					}
-					if(iter!=emptyLocalMaps.end())
-					{
-						for(int i=0; i<iter->second.cols; ++i)
+						if(kter->first > 0)
 						{
-							float * ptf = iter->second.ptr<float>(0,i);
-							cv::Point2i pt((ptf[0]-xMin)/cellSize_, (ptf[1]-yMin)/cellSize_);
-							UASSERT_MSG(pt.y >=0 && pt.y < map.rows && pt.x >= 0 && pt.x < map.cols,
-									uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d (graph modified=%d)",
-											kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, iter->second.channels(), mapInfo.channels()-1, (graphOptimized || graphChanged)?1:0).c_str());
-							char & value = map.at<char>(pt.y, pt.x);
-							if(value != -2 && (!incrementalGraphUpdate || value==-1))
-							{
-								float * info = mapInfo.ptr<float>(pt.y, pt.x);
-								int nodeId = (int)info[0];
-								if(value != -1)
-								{
-									if(kter->first > 0 && (kter->first <  nodeId || nodeId < 0))
-									{
-										// cannot rewrite on cells referred by more recent nodes
-										continue;
-									}
-									if(nodeId > 0)
-									{
-										std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
-										UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
-										if(value == 0)
-										{
-											eter->second.first -= 1;
-										}
-										else if(value == 100)
-										{
-											eter->second.second -= 1;
-										}
-										if(kter->first < 0)
-										{
-											eter->second.first += 1;
-										}
-									}
-								}
-								if(kter->first > 0)
-								{
-									info[0] = (float)kter->first;
-									info[1] = ptf[0];
-									info[2] = ptf[1];
-									cter->second.first+=1;
-								}
-								value = 0; // free space
-
-								// update odds
-								if(nodeId != kter->first)
-								{
-									info[3] += probMiss_;
-									if (info[3] < probClampingMin_)
-									{
-										info[3] = probClampingMin_;
-									}
-									if (info[3] > probClampingMax_)
-									{
-										info[3] = probClampingMax_;
-									}
-								}
-							}
+							uInsert(addedNodes_, *kter);
 						}
-					}
-
-					if(footprintRadius_ >= cellSize_*1.5f)
-					{
-						// place free space under the footprint of the robot
-						cv::Point2i ptBegin((kter->second.x()-footprintRadius_-xMin)/cellSize_, (kter->second.y()-footprintRadius_-yMin)/cellSize_);
-						cv::Point2i ptEnd((kter->second.x()+footprintRadius_-xMin)/cellSize_, (kter->second.y()+footprintRadius_-yMin)/cellSize_);
-						if(ptBegin.x < 0)
-							ptBegin.x = 0;
-						if(ptEnd.x >= map.cols)
-							ptEnd.x = map.cols-1;
-
-						if(ptBegin.y < 0)
-							ptBegin.y = 0;
-						if(ptEnd.y >= map.rows)
-							ptEnd.y = map.rows-1;
-
-						for(int i=ptBegin.x; i<ptEnd.x; ++i)
+						std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(kter->first);
+						if(cter == cellCount_.end() && kter->first > 0)
 						{
-							for(int j=ptBegin.y; j<ptEnd.y; ++j)
-							{
-								UASSERT(j < map.rows && i < map.cols);
-								char & value = map.at<char>(j, i);
-								float * info = mapInfo.ptr<float>(j, i);
-								int nodeId = (int)info[0];
-								if(value != -1)
-								{
-									if(kter->first > 0 && (kter->first <  nodeId || nodeId < 0))
-									{
-										// cannot rewrite on cells referred by more recent nodes
-										continue;
-									}
-									if(nodeId>0)
-									{
-										std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
-										UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
-										if(value == 0)
-										{
-											eter->second.first -= 1;
-										}
-										else if(value == 100)
-										{
-											eter->second.second -= 1;
-										}
-										if(kter->first < 0)
-										{
-											eter->second.first += 1;
-										}
-									}
-								}
-								if(kter->first > 0)
-								{
-									info[0] = (float)kter->first;
-									info[1] = float(i) * cellSize_ + xMin;
-									info[2] = float(j) * cellSize_ + yMin;
-									info[3] = probClampingMin_;
-									cter->second.first+=1;
-								}
-								value = -2; // free space (footprint)
-							}
+							cter = cellCount_.insert(std::make_pair(kter->first, std::pair<int,int>(0,0))).first;
 						}
-					}
-
-					if(jter!=occupiedLocalMaps.end())
-					{
-						for(int i=0; i<jter->second.cols; ++i)
+						if(iter!=emptyLocalMaps.end())
 						{
-							float * ptf = jter->second.ptr<float>(0,i);
-							cv::Point2i pt((ptf[0]-xMin)/cellSize_, (ptf[1]-yMin)/cellSize_);
-							UASSERT_MSG(pt.y>=0 && pt.y < map.rows && pt.x>=0 && pt.x < map.cols,
+							for(int i=0; i<iter->second.cols; ++i)
+							{
+								float * ptf = iter->second.ptr<float>(0,i);
+								cv::Point2i pt((ptf[0]-xMin)/cellSize_, (ptf[1]-yMin)/cellSize_);
+								UASSERT_MSG(pt.y >=0 && pt.y < map.rows && pt.x >= 0 && pt.x < map.cols,
 										uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d (graph modified=%d)",
-												kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, jter->second.channels(), mapInfo.channels()-1, (graphOptimized || graphChanged)?1:0).c_str());
-							char & value = map.at<char>(pt.y, pt.x);
-							if(value != -2)
+												kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, iter->second.channels(), mapInfo.channels()-1, (graphOptimized || graphChanged)?1:0).c_str());
+								char & value = map.at<char>(pt.y, pt.x);
+								if(value != -2 && (!incrementalGraphUpdate || value==-1))
+								{
+									float * info = mapInfo.ptr<float>(pt.y, pt.x);
+									int nodeId = (int)info[0];
+									if(value != -1)
+									{
+										if(kter->first > 0 && (kter->first <  nodeId || nodeId < 0))
+										{
+											// cannot rewrite on cells referred by more recent nodes
+											continue;
+										}
+										if(nodeId > 0)
+										{
+											std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
+											UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
+											if(value == 0)
+											{
+												eter->second.first -= 1;
+											}
+											else if(value == 100)
+											{
+												eter->second.second -= 1;
+											}
+											if(kter->first < 0)
+											{
+												eter->second.first += 1;
+											}
+										}
+									}
+									if(kter->first > 0)
+									{
+										info[0] = (float)kter->first;
+										info[1] = ptf[0];
+										info[2] = ptf[1];
+										cter->second.first+=1;
+									}
+									value = 0; // free space
+
+									// update odds
+									if(nodeId != kter->first)
+									{
+										info[3] += probMiss_;
+										if (info[3] < probClampingMin_)
+										{
+											info[3] = probClampingMin_;
+										}
+										if (info[3] > probClampingMax_)
+										{
+											info[3] = probClampingMax_;
+										}
+									}
+								}
+							}
+						}
+
+						if(footprintRadius_ >= cellSize_*1.5f)
+						{
+							// place free space under the footprint of the robot
+							cv::Point2i ptBegin((kter->second.x()-footprintRadius_-xMin)/cellSize_, (kter->second.y()-footprintRadius_-yMin)/cellSize_);
+							cv::Point2i ptEnd((kter->second.x()+footprintRadius_-xMin)/cellSize_, (kter->second.y()+footprintRadius_-yMin)/cellSize_);
+							if(ptBegin.x < 0)
+								ptBegin.x = 0;
+							if(ptEnd.x >= map.cols)
+								ptEnd.x = map.cols-1;
+
+							if(ptBegin.y < 0)
+								ptBegin.y = 0;
+							if(ptEnd.y >= map.rows)
+								ptEnd.y = map.rows-1;
+
+							for(int i=ptBegin.x; i<ptEnd.x; ++i)
 							{
-								float * info = mapInfo.ptr<float>(pt.y, pt.x);
-								int nodeId = (int)info[0];
-								if(value != -1)
+								for(int j=ptBegin.y; j<ptEnd.y; ++j)
 								{
-									if(kter->first > 0 && (kter->first <  nodeId || nodeId < 0))
+									UASSERT(j < map.rows && i < map.cols);
+									char & value = map.at<char>(j, i);
+									float * info = mapInfo.ptr<float>(j, i);
+									int nodeId = (int)info[0];
+									if(value != -1)
 									{
-										// cannot rewrite on cells referred by more recent nodes
-										continue;
-									}
-									if(nodeId>0)
-									{
-										std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
-										UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
-										if(value == 0)
+										if(kter->first > 0 && (kter->first <  nodeId || nodeId < 0))
 										{
-											eter->second.first -= 1;
+											// cannot rewrite on cells referred by more recent nodes
+											continue;
 										}
-										else if(value == 100)
+										if(nodeId>0)
 										{
-											eter->second.second -= 1;
-										}
-										if(kter->first < 0)
-										{
-											eter->second.second += 1;
+											std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
+											UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
+											if(value == 0)
+											{
+												eter->second.first -= 1;
+											}
+											else if(value == 100)
+											{
+												eter->second.second -= 1;
+											}
+											if(kter->first < 0)
+											{
+												eter->second.first += 1;
+											}
 										}
 									}
-								}
-								if(kter->first > 0)
-								{
-									info[0] = (float)kter->first;
-									info[1] = ptf[0];
-									info[2] = ptf[1];
-									cter->second.second+=1;
-								}
-
-								// update odds
-								if(nodeId != kter->first || value!=100)
-								{
-									info[3] += probHit_;
-									if (info[3] < probClampingMin_)
+									if(kter->first > 0)
 									{
+										info[0] = (float)kter->first;
+										info[1] = float(i) * cellSize_ + xMin;
+										info[2] = float(j) * cellSize_ + yMin;
 										info[3] = probClampingMin_;
+										cter->second.first+=1;
 									}
-									if (info[3] > probClampingMax_)
-									{
-										info[3] = probClampingMax_;
-									}
+									value = -2; // free space (footprint)
 								}
+							}
+						}

-								value = 100; // obstacles
+						if(jter!=occupiedLocalMaps.end())
+						{
+							for(int i=0; i<jter->second.cols; ++i)
+							{
+								float * ptf = jter->second.ptr<float>(0,i);
+								cv::Point2i pt((ptf[0]-xMin)/cellSize_, (ptf[1]-yMin)/cellSize_);
+								UASSERT_MSG(pt.y>=0 && pt.y < map.rows && pt.x>=0 && pt.x < map.cols,
+											uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d (graph modified=%d)",
+													kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, jter->second.channels(), mapInfo.channels()-1, (graphOptimized || graphChanged)?1:0).c_str());
+								char & value = map.at<char>(pt.y, pt.x);
+								if(value != -2)
+								{
+									float * info = mapInfo.ptr<float>(pt.y, pt.x);
+									int nodeId = (int)info[0];
+									if(value != -1)
+									{
+										if(kter->first > 0 && (kter->first <  nodeId || nodeId < 0))
+										{
+											// cannot rewrite on cells referred by more recent nodes
+											continue;
+										}
+										if(nodeId>0)
+										{
+											std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
+											UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
+											if(value == 0)
+											{
+												eter->second.first -= 1;
+											}
+											else if(value == 100)
+											{
+												eter->second.second -= 1;
+											}
+											if(kter->first < 0)
+											{
+												eter->second.second += 1;
+											}
+										}
+									}
+									if(kter->first > 0)
+									{
+										info[0] = (float)kter->first;
+										info[1] = ptf[0];
+										info[2] = ptf[1];
+										cter->second.second+=1;
+									}
+
+									// update odds
+									if(nodeId != kter->first || value!=100)
+									{
+										info[3] += probHit_;
+										if (info[3] < probClampingMin_)
+										{
+											info[3] = probClampingMin_;
+										}
+										if (info[3] > probClampingMax_)
+										{
+											info[3] = probClampingMax_;
+										}
+									}
+
+									value = 100; // obstacles
+								}
 							}
 						}
 					}
@@ -202,6 +202,7 @@ Odometry::~Odometry()

 void Odometry::reset(const Transform & initialPose)
 {
+	UDEBUG("");
 	UASSERT(!initialPose.isNull());
 	previousVelocities_.clear();
 	velocityGuess_.setNull();
@@ -490,9 +490,9 @@ Transform RegistrationVis::computeTransformationImpl(
 				bool guessSet = !guess.isIdentity() && !guess.isNull();
 				if(guessSet)
 				{
-					if(fromSignature.sensorData().cameraModels().size() == 1 || fromSignature.sensorData().cameraModels().size() == 1)
+					if(toSignature.sensorData().cameraModels().size() == 1 || toSignature.sensorData().stereoCameraModels().size() == 1)
 					{
-						Transform localTransform = fromSignature.sensorData().cameraModels().size()?fromSignature.sensorData().cameraModels()[0].localTransform():fromSignature.sensorData().stereoCameraModels()[0].left().localTransform();
+						Transform localTransform = toSignature.sensorData().cameraModels().size()?toSignature.sensorData().cameraModels()[0].localTransform():toSignature.sensorData().stereoCameraModels()[0].left().localTransform();
 						Transform guessCameraRef = (guess * localTransform).inverse();
 						cv::Mat R = (cv::Mat_<double>(3,3) <<
 								(double)guessCameraRef.r11(), (double)guessCameraRef.r12(), (double)guessCameraRef.r13(),
@@ -501,7 +501,7 @@ Transform RegistrationVis::computeTransformationImpl(
 						cv::Mat rvec(1,3, CV_64FC1);
 						cv::Rodrigues(R, rvec);
 						cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guessCameraRef.x(), (double)guessCameraRef.y(), (double)guessCameraRef.z());
-						cv::Mat K = fromSignature.sensorData().cameraModels().size()?fromSignature.sensorData().cameraModels()[0].K():fromSignature.sensorData().stereoCameraModels()[0].left().K();
+						cv::Mat K = toSignature.sensorData().cameraModels().size()?toSignature.sensorData().cameraModels()[0].K():toSignature.sensorData().stereoCameraModels()[0].left().K();
 						cv::projectPoints(kptsFrom3D, rvec, tvec, K, cv::Mat(), cornersTo);
 					}
 					else
@@ -101,6 +101,7 @@ Rtabmap::Rtabmap() :
 	_verifyLoopClosureHypothesis(Parameters::defaultVhEpEnabled()),
 	_maxRetrieved(Parameters::defaultRtabmapMaxRetrieved()),
 	_maxLocalRetrieved(Parameters::defaultRGBDMaxLocalRetrieved()),
+	_maxRepublished(Parameters::defaultRtabmapMaxRepublished()),
 	_rawDataKept(Parameters::defaultMemImageKept()),
 	_statisticLogsBufferedInRAM(Parameters::defaultRtabmapStatisticLogsBufferedInRAM()),
 	_statisticLogged(Parameters::defaultRtabmapStatisticLogged()),
@@ -355,6 +356,7 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
 	_globalScanMapPoses.clear();
 	_odomCachePoses.clear();
 	_odomCacheConstraints.clear();
+	_nodesToRepublish.clear();

 	// Parse all parameters
 	this->parseParameters(allParameters);
@@ -473,6 +475,8 @@ void Rtabmap::close(bool databaseSaved, const std::string & ouputDatabasePath)
 	_globalScanMap.clear();
 	_globalScanMapPoses.clear();

+	_nodesToRepublish.clear();
+
 	flushStatisticLogs();
 	if(_foutFloat)
 	{
@@ -559,6 +563,11 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kVhEpEnabled(), _verifyLoopClosureHypothesis);
 	Parameters::parse(parameters, Parameters::kRtabmapMaxRetrieved(), _maxRetrieved);
 	Parameters::parse(parameters, Parameters::kRGBDMaxLocalRetrieved(), _maxLocalRetrieved);
+	Parameters::parse(parameters, Parameters::kRtabmapMaxRepublished(), _maxRepublished);
+	if(_maxRepublished == 0 || !_publishLastSignatureData)
+	{
+		_nodesToRepublish.clear();
+	}
 	Parameters::parse(parameters, Parameters::kMemImageKept(), _rawDataKept);
 	Parameters::parse(parameters, Parameters::kRGBDEnabled(), _rgbdSlamMode);
 	Parameters::parse(parameters, Parameters::kRGBDLinearUpdate(), _rgbdLinearUpdate);
@@ -1012,7 +1021,7 @@ void Rtabmap::exportPoses(const std::string & path, bool optimized, bool global,
 		}

 		std::map<int, double> stamps;
-		if(format == 1)
+		if(format == 1 || format == 10 || format == 11)
 		{
 			for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
 			{
@@ -1053,6 +1062,7 @@ void Rtabmap::resetMemory()
 	_optimizeFromGraphEndChanged = false;
 	_globalScanMap.clear();
 	_globalScanMapPoses.clear();
+	_nodesToRepublish.clear();
 	this->clearPath(0);

 	if(_memory)
@@ -3667,6 +3677,7 @@ bool Rtabmap::process(

 	// Posterior is empty if a bad signature is detected
 	float vpHypothesis = posterior.size()?posterior.at(Memory::kIdVirtual):0.0f;
+	int loopId = _loopClosureHypothesis.first>0?_loopClosureHypothesis.first:lastProximitySpaceClosureId;

 	// prepare statistics
 	if(_loopClosureHypothesis.first || _publishStats)
@@ -3681,6 +3692,7 @@ bool Rtabmap::process(
 			statistics_.setLoopClosureMapId(_memory->getMapId(_loopClosureHypothesis.first));
 			ULOGGER_INFO("Loop closure detected! With id=%d", _loopClosureHypothesis.first);
 		}
+
 		if(_publishStats)
 		{
 			ULOGGER_INFO("send all stats...");
@@ -3722,7 +3734,6 @@ bool Rtabmap::process(
 			statistics_.setProximityDetectionId(lastProximitySpaceClosureId);
 			statistics_.setProximityDetectionMapId(_memory->getMapId(lastProximitySpaceClosureId));

-			int loopId = _loopClosureHypothesis.first>0?_loopClosureHypothesis.first:lastProximitySpaceClosureId;
 			statistics_.addStatistic(Statistics::kLoopId(), loopId);
 			statistics_.addStatistic(Statistics::kLoopMap_id(), (loopId>0 && sLoop)?sLoop->mapId():-1);

@@ -4187,7 +4198,71 @@ bool Rtabmap::process(
 		if(_publishLastSignatureData)
 		{
 			UINFO("Adding data %d [%d] (rgb/left=%d depth/right=%d)", lastSignatureData.id(), lastSignatureData.mapId(), lastSignatureData.sensorData().imageRaw().empty()?0:1, lastSignatureData.sensorData().depthOrRightRaw().empty()?0:1);
-			statistics_.setLastSignatureData(lastSignatureData);
+			statistics_.addSignatureData(lastSignatureData);
+
+			if(_nodesToRepublish.size())
+			{
+				std::multimap<int, int> missingIds;
+
+				// priority to loopId
+				int tmpId = loopId>0?loopId:_highestHypothesis.first;
+				if(tmpId>0 && _nodesToRepublish.find(tmpId) != _nodesToRepublish.end())
+				{
+					missingIds.insert(std::make_pair(-1, tmpId));
+				}
+
+				if(!_lastLocalizationPose.isNull())
+				{
+					// Republish data from closest nodes of the current localization
+					std::map<int, Transform> nodesOnly(_optimizedPoses.lower_bound(1), _optimizedPoses.end());
+					int id = rtabmap::graph::findNearestNode(nodesOnly, _lastLocalizationPose);
+					if(id>0)
+					{
+						std::map<int, int> ids = _memory->getNeighborsId(id, 0, 0, true, false, true);
+						for(std::map<int, int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
+						{
+							if(iter->first != loopId &&
+									_nodesToRepublish.find(iter->first) != _nodesToRepublish.end())
+							{
+								missingIds.insert(std::make_pair(iter->second, iter->first));
+							}
+						}
+
+						if(_nodesToRepublish.size() != missingIds.size())
+						{
+							// remove requested nodes not anymore in the graph
+							for(std::set<int>::iterator iter=_nodesToRepublish.begin(); iter!=_nodesToRepublish.end();)
+							{
+								if(ids.find(*iter) == ids.end())
+								{
+									iter = _nodesToRepublish.erase(iter);
+								}
+								else
+								{
+									++iter;
+								}
+							}
+						}
+					}
+				}
+
+				int loaded = 0;
+				std::stringstream stream;
+				for(std::multimap<int, int>::iterator iter=missingIds.begin(); iter!=missingIds.end() && loaded<(int)_maxRepublished; ++iter)
+				{
+					statistics_.addSignatureData(getSignatureCopy(iter->second, true, true, true, true, true, true));
+					_nodesToRepublish.erase(iter->second);
+					++loaded;
+					stream << iter->second << " ";
+				}
+				if(loaded)
+				{
+					UWARN("Republishing data of requested node(s) %s(%s=%d)",
+							stream.str().c_str(),
+							Parameters::kRtabmapMaxRepublished().c_str(),
+							_maxRepublished);
+				}
+			}
 		}
 		else
 		{
@@ -4207,7 +4282,7 @@ bool Rtabmap::process(
 			}
 			nodeInfo.sensorData().setGPS(lastSignatureData.sensorData().gps());
 			nodeInfo.sensorData().setEnvSensors(lastSignatureData.sensorData().envSensors());
-			statistics_.setLastSignatureData(nodeInfo);
+			statistics_.addSignatureData(nodeInfo);
 		}
 		UDEBUG("");
 		localGraphSize = (int)poses.size();
@@ -5340,7 +5415,7 @@ int Rtabmap::detectMoreLoopClosures(
 							UASSERT(signatures.find(to) != signatures.end());

 							Transform guess;
-							if(_proximityOdomGuess && uContains(poses, from) && uContains(poses, to))
+							if(_proximityBySpace && uContains(poses, from) && uContains(poses, to))
 							{
 								guess = poses.at(from).inverse() * poses.at(to);
 							}
@@ -5680,7 +5755,7 @@ bool Rtabmap::addLink(const Link & link)
 	}
 	if(t.isNull())
 	{
-		UERROR("Link's transform is null!");
+		UERROR("Link's transform is null! (%d->%d type=%s)", link.from(), link.to(), link.typeName().c_str());
 		return false;
 	}
 	if(_memory->isIncremental())
@@ -6016,6 +6091,26 @@ cv::Mat Rtabmap::getInformation(const cv::Mat & covariance) const
 	return information;
 }

+void Rtabmap::addNodesToRepublish(const std::vector<int> & ids)
+{
+	if(ids.empty())
+	{
+		_nodesToRepublish.clear();
+	}
+	else if(_maxRepublished > 0 && _publishLastSignatureData)
+	{
+		_nodesToRepublish.insert(ids.begin(), ids.end());
+	}
+	else if(_maxRepublished == 0)
+	{
+		UWARN("%s=0, so cannot republish the %d requested nodes.", Parameters::kRtabmapMaxRepublished().c_str(), (int)ids.size());
+	}
+	else //_publishLastSignatureData=false
+	{
+		UWARN("%s=false, so cannot republish the %d requested nodes.", Parameters::kRtabmapPublishLastSignature().c_str(), (int)ids.size());
+	}
+}
+
 void Rtabmap::clearPath(int status)
 {
 	UINFO("status=%d", status);
@@ -66,14 +66,14 @@ RtabmapThread::~RtabmapThread()
 	delete _frameRateTimer;
 }

-void RtabmapThread::pushNewState(State newState, const ParametersMap & parameters)
+void RtabmapThread::pushNewState(State newState, const RtabmapEventCmd & cmdEvent)
 {
 	ULOGGER_DEBUG("to %d", newState);

 	_stateMutex.lock();
 	{
 		_state.push(newState);
-		_stateParam.push(parameters);
+		_stateParam.push(cmdEvent);
 	}
 	_stateMutex.unlock();

@@ -180,7 +180,7 @@ void RtabmapThread::mainLoopKill()
 void RtabmapThread::mainLoop()
 {
 	State state = kStateDetecting;
-	ParametersMap parameters;
+	RtabmapEventCmd cmdEvent(RtabmapEventCmd::kCmdUndef);

 	_stateMutex.lock();
 	{
@@ -188,7 +188,7 @@ void RtabmapThread::mainLoop()
 		{
 			state = _state.front();
 			_state.pop();
-			parameters = _stateParam.front();
+			cmdEvent = _stateParam.front();
 			_stateParam.pop();
 		}
 	}
@@ -198,110 +198,161 @@ void RtabmapThread::mainLoop()
 	cv::Mat userData;
 	UTimer timer;
 	std::string str;
+	RtabmapEventCmd::Cmd cmd = cmdEvent.getCmd();
 	switch(state)
 	{
 	case kStateDetecting:
 		this->process();
 		break;
-	case kStateInit:
-		UASSERT(!parameters.at("RtabmapThread/DatabasePath").empty());
-		str = parameters.at("RtabmapThread/DatabasePath");
-		parameters.erase("RtabmapThread/DatabasePath");
-		Parameters::parse(parameters, Parameters::kRtabmapImageBufferSize(), _dataBufferMaxSize);
-		Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), _rate);
-		Parameters::parse(parameters, Parameters::kRtabmapCreateIntermediateNodes(), _createIntermediateNodes);
-		UASSERT(_rate >= 0.0f);
-		_rtabmap->init(parameters, str);
-		break;
-	case kStateChangingParameters:
-		Parameters::parse(parameters, Parameters::kRtabmapImageBufferSize(), _dataBufferMaxSize);
-		Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), _rate);
-		Parameters::parse(parameters, Parameters::kRtabmapCreateIntermediateNodes(), _createIntermediateNodes);
-		UASSERT(_rate >= 0.0f);
-		_rtabmap->parseParameters(parameters);
-		break;
-	case kStateReseting:
-		_rtabmap->resetMemory();
-		this->clearBufferedData();
-		break;
-	case kStateClose:
-		if(_dataBuffer.size())
+	case kStateProcessCommand:
+		if(cmd == RtabmapEventCmd::kCmdInit)
 		{
-			UWARN("Closing... %d data still buffered! They will be cleared.", (int)_dataBuffer.size());
+			ULOGGER_DEBUG("CMD_INIT");
+			Parameters::parse(cmdEvent.getParameters(), Parameters::kRtabmapImageBufferSize(), _dataBufferMaxSize);
+			Parameters::parse(cmdEvent.getParameters(), Parameters::kRtabmapDetectionRate(), _rate);
+			Parameters::parse(cmdEvent.getParameters(), Parameters::kRtabmapCreateIntermediateNodes(), _createIntermediateNodes);
+			UASSERT(_rate >= 0.0f);
+			_rtabmap->init(cmdEvent.getParameters(), cmdEvent.value1().toStr());
+		}
+		else if(cmd == RtabmapEventCmd::kCmdClose)
+		{
+			ULOGGER_DEBUG("CMD_CLOSE");
+			if(_dataBuffer.size())
+			{
+				UWARN("Closing... %d data still buffered! They will be cleared.", (int)_dataBuffer.size());
+				this->clearBufferedData();
+			}
+			_rtabmap->close(cmdEvent.value1().toBool(), cmdEvent.value2().toStr());
+		}
+		else if(cmd == RtabmapEventCmd::kCmdUpdateParams)
+		{
+			ULOGGER_DEBUG("CMD_UPDATE_PARAMS");
+			Parameters::parse(cmdEvent.getParameters(), Parameters::kRtabmapImageBufferSize(), _dataBufferMaxSize);
+			Parameters::parse(cmdEvent.getParameters(), Parameters::kRtabmapDetectionRate(), _rate);
+			Parameters::parse(cmdEvent.getParameters(), Parameters::kRtabmapCreateIntermediateNodes(), _createIntermediateNodes);
+			UASSERT(_rate >= 0.0f);
+			_rtabmap->parseParameters(cmdEvent.getParameters());
+			break;
+		}
+		else if(cmd == RtabmapEventCmd::kCmdResetMemory)
+		{
+			ULOGGER_DEBUG("CMD_RESET_MEMORY");
+			_rtabmap->resetMemory();
 			this->clearBufferedData();
 		}
-		_rtabmap->close(uStr2Bool(parameters.at("saved")), parameters.at("outputPath"));
-		break;
-	case kStateDumpingMemory:
-		_rtabmap->dumpData();
-		break;
-	case kStateDumpingPrediction:
-		_rtabmap->dumpPrediction();
-		break;
-	case kStateExportingDOTGraph:
-		_rtabmap->generateDOTGraph(
-				parameters.at("path"),
-				atoi(parameters.at("id").c_str()),
-				atoi(parameters.at("margin").c_str()));
-		break;
-	case kStateExportingPoses:
-		_rtabmap->exportPoses(
-				parameters.at("path"),
-				uStr2Bool(parameters.at("optimized")),
-				uStr2Bool(parameters.at("global")),
-				atoi(parameters.at("type").c_str()));
-		break;
-	case kStateCleanDataBuffer:
-		this->clearBufferedData();
-		break;
-	case kStatePublishingMap:
-		this->publishMap(
-				uStr2Bool(parameters.at("optimized")),
-				uStr2Bool(parameters.at("global")),
-				uStr2Bool(parameters.at("graph_only")));
-		break;
-	case kStateTriggeringMap:
-		_rtabmap->triggerNewMap();
-		break;
-	case kStateSettingGoal:
-		id = atoi(parameters.at("id").c_str());
-		if(id == 0 && !parameters.at("label").empty() && _rtabmap->getMemory())
+		else if(cmd == RtabmapEventCmd::kCmdDumpMemory)
 		{
-			id = _rtabmap->getMemory()->getSignatureIdByLabel(parameters.at("label"));
-			if(id <= 0)
+			ULOGGER_DEBUG("CMD_DUMP_MEMORY");
+			_rtabmap->dumpData();
+		}
+		else if(cmd == RtabmapEventCmd::kCmdDumpPrediction)
+		{
+			ULOGGER_DEBUG("CMD_DUMP_PREDICTION");
+			_rtabmap->dumpPrediction();
+		}
+		else if(cmd == RtabmapEventCmd::kCmdGenerateDOTGraph)
+		{
+			ULOGGER_DEBUG("CMD_GENERATE_DOT_GRAPH");
+			_rtabmap->generateDOTGraph(
+					cmdEvent.value2().toStr(),
+					cmdEvent.value1().toBool()?0:cmdEvent.value3().toInt(),
+					cmdEvent.value1().toBool()?0:cmdEvent.value4().toInt());
+		}
+		else if(cmd == RtabmapEventCmd::kCmdExportPoses)
+		{
+			ULOGGER_DEBUG("CMD_EXPORT_POSES");
+			_rtabmap->exportPoses(
+					cmdEvent.value3().toStr(),
+					cmdEvent.value2().toBool(),
+					cmdEvent.value1().toBool(),
+					cmdEvent.value4().toInt());
+
+		}
+		else if(cmd == RtabmapEventCmd::kCmdCleanDataBuffer)
+		{
+			ULOGGER_DEBUG("CMD_CLEAN_DATA_BUFFER");
+			this->clearBufferedData();
+		}
+		else if(cmd == RtabmapEventCmd::kCmdPublish3DMap)
+		{
+			ULOGGER_DEBUG("CMD_PUBLISH_MAP");
+			this->publishMap(
+					cmdEvent.value2().toBool(),
+					cmdEvent.value1().toBool(),
+					cmdEvent.value3().toBool());
+		}
+		else if(cmd == RtabmapEventCmd::kCmdTriggerNewMap)
+		{
+			ULOGGER_DEBUG("CMD_TRIGGER_NEW_MAP");
+			_rtabmap->triggerNewMap();
+		}
+		else if(cmd == RtabmapEventCmd::kCmdPause)
+		{
+			ULOGGER_DEBUG("CMD_PAUSE");
+			_paused = !_paused;
+		}
+		else if(cmd == RtabmapEventCmd::kCmdGoal)
+		{
+			ULOGGER_DEBUG("CMD_GOAL");
+			if(cmdEvent.value1().isStr() && !cmdEvent.value1().toStr().empty() && _rtabmap->getMemory())
 			{
-				UERROR("Failed to find a node with label \"%s\".", parameters.at("label").c_str());
+				id = _rtabmap->getMemory()->getSignatureIdByLabel(cmdEvent.value1().toStr());
+				if(id <= 0)
+				{
+					UERROR("Failed to find a node with label \"%s\".", cmdEvent.value1().toStr().c_str());
+				}
+			}
+			else if(cmdEvent.value1().isInt() || cmdEvent.value1().isUInt())
+			{
+				id = cmdEvent.value1().toInt();
+			}
+
+			if(id < 0)
+			{
+				UERROR("Failed to set a goal. ID (%d) should be positive > 0", id);
+			}
+			timer.start();
+			if(id > 0 && !_rtabmap->computePath(id, true))
+			{
+				UERROR("Failed to compute a path to goal %d.", id);
+			}
+			this->post(new RtabmapGlobalPathEvent(
+					id,
+					cmdEvent.value1().isStr()?cmdEvent.value1().toStr():"",
+					_rtabmap->getPath(),
+					timer.elapsed()));
+			break;
+		}
+		else if(cmd == RtabmapEventCmd::kCmdCancelGoal)
+		{
+			ULOGGER_DEBUG("CMD_CANCEL_GOAL");
+			_rtabmap->clearPath(0);
+		}
+		else if(cmd == RtabmapEventCmd::kCmdLabel)
+		{
+			ULOGGER_DEBUG("CMD_LABEL");
+			if(!_rtabmap->labelLocation(cmdEvent.value2().toInt(), cmdEvent.value1().toStr()))
+			{
+				this->post(new RtabmapLabelErrorEvent(cmdEvent.value2().toInt(), cmdEvent.value1().toStr()));
 			}
 		}
-		else if(id < 0)
+		else if(cmd == RtabmapEventCmd::kCmdRemoveLabel)
 		{
-			UERROR("Failed to set a goal. ID (%d) should be positive > 0", id);
+			ULOGGER_DEBUG("CMD_REMOVE_LABEL");
+			id = _rtabmap->getMemory()->getSignatureIdByLabel(cmdEvent.value1().toStr(), true);
+			if(id <= 0 || !_rtabmap->labelLocation(id, ""))
+			{
+				this->post(new RtabmapLabelErrorEvent(id, cmdEvent.value1().toStr()));
+			}
 		}
-		timer.start();
-		if(id > 0 && !_rtabmap->computePath(id, true))
+		else if(cmd == RtabmapEventCmd::kCmdRepublishData)
 		{
-			UERROR("Failed to compute a path to goal %d.", id);
+			ULOGGER_DEBUG("CMD_REPUBLISH_DATA");
+			_rtabmap->addNodesToRepublish(cmdEvent.value1().toIntArray());
 		}
-		this->post(new RtabmapGlobalPathEvent(
-				id,
-				parameters.at("label"),
-				_rtabmap->getPath(),
-				timer.elapsed()));
-		break;
-	case kStateCancellingGoal:
-		_rtabmap->clearPath(0);
-		break;
-	case kStateLabelling:
-		if(!_rtabmap->labelLocation(atoi(parameters.at("id").c_str()), parameters.at("label")))
+		else
 		{
-			this->post(new RtabmapLabelErrorEvent(atoi(parameters.at("id").c_str()), parameters.at("label")));
-		}
-		break;
-	case kStateRemovingLabel:
-		id = _rtabmap->getMemory()->getSignatureIdByLabel(parameters.at("label"), true);
-		if(!_rtabmap->labelLocation(id, ""))
-		{
-			this->post(new RtabmapLabelErrorEvent(id, parameters.at("label")));
+			UWARN("Cmd %d unknown!", cmd);
 		}
 		break;
 	default:
@@ -397,127 +448,12 @@ bool RtabmapThread::handleEvent(UEvent* event)
 		else if(event->getClassName().compare("RtabmapEventCmd") == 0)
 		{
 			RtabmapEventCmd * rtabmapEvent = (RtabmapEventCmd*)event;
-			RtabmapEventCmd::Cmd cmd = rtabmapEvent->getCmd();
-			if(cmd == RtabmapEventCmd::kCmdInit)
-			{
-				ULOGGER_DEBUG("CMD_INIT");
-				ParametersMap parameters = ((RtabmapEventCmd*)event)->getParameters();
-				UASSERT(rtabmapEvent->value1().isStr());
-				UASSERT(parameters.insert(ParametersPair("RtabmapThread/DatabasePath", rtabmapEvent->value1().toStr())).second);
-				pushNewState(kStateInit, parameters);
-			}
-			else if(cmd == RtabmapEventCmd::kCmdClose)
-			{
-				ULOGGER_DEBUG("CMD_CLOSE");
-				UASSERT(rtabmapEvent->value1().isUndef() || rtabmapEvent->value1().isBool());
-				ParametersMap param;
-				param.insert(ParametersPair("saved", uBool2Str(rtabmapEvent->value1().isUndef() || rtabmapEvent->value1().toBool())));
-				param.insert(ParametersPair("outputPath", rtabmapEvent->value2().toStr()));
-				pushNewState(kStateClose, param);
-			}
-			else if(cmd == RtabmapEventCmd::kCmdResetMemory)
-			{
-				ULOGGER_DEBUG("CMD_RESET_MEMORY");
-				pushNewState(kStateReseting);
-			}
-			else if(cmd == RtabmapEventCmd::kCmdDumpMemory)
-			{
-				ULOGGER_DEBUG("CMD_DUMP_MEMORY");
-				pushNewState(kStateDumpingMemory);
-			}
-			else if(cmd == RtabmapEventCmd::kCmdDumpPrediction)
-			{
-				ULOGGER_DEBUG("CMD_DUMP_PREDICTION");
-				pushNewState(kStateDumpingPrediction);
-			}
-			else if(cmd == RtabmapEventCmd::kCmdGenerateDOTGraph)
-			{
-				ULOGGER_DEBUG("CMD_GENERATE_DOT_GRAPH");
-				UASSERT(rtabmapEvent->value1().isBool());
-				UASSERT(rtabmapEvent->value2().isStr());
-				UASSERT(rtabmapEvent->value1().toBool() || rtabmapEvent->value3().isInt() || rtabmapEvent->value3().isUInt());
-				UASSERT(rtabmapEvent->value1().toBool() || rtabmapEvent->value4().isInt() || rtabmapEvent->value4().isUInt());
-				ParametersMap param;
-				param.insert(ParametersPair("path", rtabmapEvent->value2().toStr()));
-				param.insert(ParametersPair("id", !rtabmapEvent->value1().toBool()?rtabmapEvent->value3().toStr():"0"));
-				param.insert(ParametersPair("margin", !rtabmapEvent->value1().toBool()?rtabmapEvent->value4().toStr():"0"));
-				pushNewState(kStateExportingDOTGraph, param);
-			}
-			else if(cmd == RtabmapEventCmd::kCmdExportPoses)
-			{
-				ULOGGER_DEBUG("CMD_EXPORT_POSES");
-				UASSERT(rtabmapEvent->value1().isBool());
-				UASSERT(rtabmapEvent->value2().isBool());
-				UASSERT(rtabmapEvent->value3().isStr());
-				UASSERT(rtabmapEvent->value4().isUndef() || rtabmapEvent->value4().isInt() || rtabmapEvent->value4().isUInt());
-				ParametersMap param;
-				param.insert(ParametersPair("global", rtabmapEvent->value1().toStr()));
-				param.insert(ParametersPair("optimized", rtabmapEvent->value1().toStr()));
-				param.insert(ParametersPair("path", rtabmapEvent->value3().toStr()));
-				param.insert(ParametersPair("type", rtabmapEvent->value4().isInt()?rtabmapEvent->value4().toStr():"0"));
-				pushNewState(kStateExportingPoses, param);
-
-			}
-			else if(cmd == RtabmapEventCmd::kCmdCleanDataBuffer)
-			{
-				ULOGGER_DEBUG("CMD_CLEAN_DATA_BUFFER");
-				pushNewState(kStateCleanDataBuffer);
-			}
-			else if(cmd == RtabmapEventCmd::kCmdPublish3DMap)
-			{
-				ULOGGER_DEBUG("CMD_PUBLISH_MAP");
-				UASSERT(rtabmapEvent->value1().isBool());
-				UASSERT(rtabmapEvent->value2().isBool());
-				UASSERT(rtabmapEvent->value3().isBool());
-				ParametersMap param;
-				param.insert(ParametersPair("global", rtabmapEvent->value1().toStr()));
-				param.insert(ParametersPair("optimized", rtabmapEvent->value2().toStr()));
-				param.insert(ParametersPair("graph_only", rtabmapEvent->value3().toStr()));
-				pushNewState(kStatePublishingMap, param);
-			}
-			else if(cmd == RtabmapEventCmd::kCmdTriggerNewMap)
-			{
-				ULOGGER_DEBUG("CMD_TRIGGER_NEW_MAP");
-				pushNewState(kStateTriggeringMap);
-			}
-			else if(cmd == RtabmapEventCmd::kCmdPause)
-			{
-				ULOGGER_DEBUG("CMD_PAUSE");
-				_paused = !_paused;
-			}
-			else if(cmd == RtabmapEventCmd::kCmdGoal)
-			{
-				ULOGGER_DEBUG("CMD_GOAL");
-				UASSERT(rtabmapEvent->value1().isStr() || rtabmapEvent->value1().isInt() || rtabmapEvent->value1().isUInt());
-				ParametersMap param;
-				param.insert(ParametersPair("label", rtabmapEvent->value1().isStr()?rtabmapEvent->value1().toStr():""));
-				param.insert(ParametersPair("id", !rtabmapEvent->value1().isStr()?rtabmapEvent->value1().toStr():"0"));
-				pushNewState(kStateSettingGoal, param);
-			}
-			else if(cmd == RtabmapEventCmd::kCmdCancelGoal)
-			{
-				ULOGGER_DEBUG("CMD_CANCEL_GOAL");
-				pushNewState(kStateCancellingGoal);
-			}
-			else if(cmd == RtabmapEventCmd::kCmdLabel)
-			{
-				ULOGGER_DEBUG("CMD_LABEL");
-				UASSERT(rtabmapEvent->value1().isStr());
-				UASSERT(rtabmapEvent->value2().isUndef() || rtabmapEvent->value2().isInt() || rtabmapEvent->value2().isUInt());
-				ParametersMap param;
-				param.insert(ParametersPair("label", rtabmapEvent->value1().toStr()));
-				param.insert(ParametersPair("id", rtabmapEvent->value2().isUndef()?"0":rtabmapEvent->value2().toStr()));
-				pushNewState(kStateLabelling, param);
-			}
-			else
-			{
-				UWARN("Cmd %d unknown!", cmd);
-			}
+			pushNewState(kStateProcessCommand, *rtabmapEvent);
 		}
 		else if(event->getClassName().compare("ParamEvent") == 0)
 		{
 			ULOGGER_DEBUG("changing parameters");
-			pushNewState(kStateChangingParameters, ((ParamEvent*)event)->getParameters());
+			pushNewState(kStateProcessCommand, RtabmapEventCmd(RtabmapEventCmd::kCmdUpdateParams, ((ParamEvent*)event)->getParameters()));
 		}
 	}
 	return false;
@@ -95,4 +95,10 @@ void Statistics::addStatistic(const std::string & name, float value)
 	uInsert(_data, std::pair<std::string, float>(name, value));
 }

+//deprecated
+void Statistics::setLastSignatureData(const Signature & data)
+{
+	_signaturesData.insert(std::make_pair(data.id(), data));
+}
+
 }
@@ -56,11 +56,12 @@ CameraDepthAI::CameraDepthAI(
 	outputDepth_(false),
 	depthConfidence_(200),
 	resolution_(resolution),
-	imuFirmwareUpdate_(false)
+	imuFirmwareUpdate_(false),
+	imuPublished_(true)
 #endif
 {
 #ifdef RTABMAP_DEPTHAI
-	UASSERT(resolution_>=0 && resolution_<=2);
+	UASSERT(resolution_>=(int)dai::MonoCameraProperties::SensorResolution::THE_720_P && resolution_<=(int)dai::MonoCameraProperties::SensorResolution::THE_1200_P);
 #endif
 }

@@ -96,6 +97,15 @@ void CameraDepthAI::setIMUFirmwareUpdate(bool enabled)
 #endif
 }

+void CameraDepthAI::setIMUPublished(bool published)
+{
+#ifdef RTABMAP_DEPTHAI
+	imuPublished_ = published;
+#else
+	UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
+#endif
+}
+
 bool CameraDepthAI::init(const std::string & calibrationFolder, const std::string & cameraName)
 {
 	UDEBUG("");
@@ -139,21 +149,27 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin

 	// look for calibration files
 	stereoModel_ = StereoCameraModel();
-	cv::Size targetSize(resolution_<2?1280:640, resolution_==0?720:resolution_==1?800:400);
+	cv::Size targetSize(resolution_<2?1280:resolution_==4?1920:640, resolution_==0?720:resolution_==1?800:resolution_==2?400:resolution_==3?480:1200);

 	dai::Pipeline p;
 	auto monoLeft  = p.create<dai::node::MonoCamera>();
 	auto monoRight = p.create<dai::node::MonoCamera>();
 	auto stereo    = p.create<dai::node::StereoDepth>();
-	auto imu       = p.create<dai::node::IMU>();
+	std::shared_ptr<dai::node::IMU> imu;
+	if(imuPublished_)
+		imu = p.create<dai::node::IMU>();
+
 	auto xoutLeft = p.create<dai::node::XLinkOut>();
 	auto xoutDepthOrRight = p.create<dai::node::XLinkOut>();
-	auto xoutIMU = p.create<dai::node::XLinkOut>();
+	std::shared_ptr<dai::node::XLinkOut> xoutIMU;
+	if(imuPublished_)
+		xoutIMU = p.create<dai::node::XLinkOut>();

 	// XLinkOut
 	xoutLeft->setStreamName("rectified_left");
 	xoutDepthOrRight->setStreamName(outputDepth_?"depth":"rectified_right");
-	xoutIMU->setStreamName("imu");
+	if(imuPublished_)
+		xoutIMU->setStreamName("imu");

 	// MonoCamera
 	monoLeft->setResolution((dai::MonoCameraProperties::SensorResolution)resolution_);
@@ -193,19 +209,22 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 		stereo->rectifiedRight.link(xoutDepthOrRight->input);
 	}

-	// enable ACCELEROMETER_RAW and GYROSCOPE_RAW at 200 hz rate
-	imu->enableIMUSensor({dai::IMUSensor::ACCELEROMETER_RAW, dai::IMUSensor::GYROSCOPE_RAW}, 200);
-	// above this threshold packets will be sent in batch of X, if the host is not blocked and USB bandwidth is available
-	imu->setBatchReportThreshold(1);
-	// maximum number of IMU packets in a batch, if it's reached device will block sending until host can receive it
-	// if lower or equal to batchReportThreshold then the sending is always blocking on device
-	// useful to reduce device's CPU load  and number of lost packets, if CPU load is high on device side due to multiple nodes
-	imu->setMaxBatchReports(10);
+	if(imuPublished_)
+	{
+		// enable ACCELEROMETER_RAW and GYROSCOPE_RAW at 200 hz rate
+		imu->enableIMUSensor({dai::IMUSensor::ACCELEROMETER_RAW, dai::IMUSensor::GYROSCOPE_RAW}, 200);
+		// above this threshold packets will be sent in batch of X, if the host is not blocked and USB bandwidth is available
+		imu->setBatchReportThreshold(1);
+		// maximum number of IMU packets in a batch, if it's reached device will block sending until host can receive it
+		// if lower or equal to batchReportThreshold then the sending is always blocking on device
+		// useful to reduce device's CPU load  and number of lost packets, if CPU load is high on device side due to multiple nodes
+		imu->setMaxBatchReports(10);

-	// Link plugins IMU -> XLINK
-	imu->out.link(xoutIMU->input);
+		// Link plugins IMU -> XLINK
+		imu->out.link(xoutIMU->input);

-	imu->enableFirmwareUpdate(imuFirmwareUpdate_);
+		imu->enableFirmwareUpdate(imuFirmwareUpdate_);
+	}

 	device_.reset(new dai::Device(p, deviceToUse));

@@ -221,23 +240,33 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 	UINFO("left: fx=%f fy=%f cx=%f cy=%f baseline=%f", fx, fy, cx, cy, baseline);
 	stereoModel_ = StereoCameraModel(device_->getMxId(), fx, fy, cx, cy, baseline, this->getLocalTransform(), targetSize);

-	// Cannot test the following, I get "IMU calibration data is not available on device yet." with my camera
-	// Update: now (as March 6, 2022) it crashes in "dai::CalibrationHandler::getImuToCameraExtrinsics(dai::CameraBoardSocket, bool)"
-	//matrix = calibHandler.getImuToCameraExtrinsics(dai::CameraBoardSocket::LEFT);
-	//imuLocalTransform_ = Transform(
-	//		matrix[0][0], matrix[0][1], matrix[0][2], matrix[0][3],
-	//		matrix[1][0], matrix[1][1], matrix[1][2], matrix[1][3],
-	//		matrix[2][0], matrix[2][1], matrix[2][2], matrix[2][3]);
-	// Hard-coded: x->down, y->left, z->forward
-	imuLocalTransform_ = Transform(
-			 0, 0, 1, 0,
-			 0, 1, 0, 0,
-			-1 ,0, 0, 0);
-	UINFO("IMU local transform = %s", imuLocalTransform_.prettyPrint().c_str());
+	if(imuPublished_)
+	{
+		// Cannot test the following, I get "IMU calibration data is not available on device yet." with my camera
+		// Update: now (as March 6, 2022) it crashes in "dai::CalibrationHandler::getImuToCameraExtrinsics(dai::CameraBoardSocket, bool)"
+		//matrix = calibHandler.getImuToCameraExtrinsics(dai::CameraBoardSocket::LEFT);
+		//imuLocalTransform_ = Transform(
+		//		matrix[0][0], matrix[0][1], matrix[0][2], matrix[0][3],
+		//		matrix[1][0], matrix[1][1], matrix[1][2], matrix[1][3],
+		//		matrix[2][0], matrix[2][1], matrix[2][2], matrix[2][3]);
+		// Hard-coded: x->down, y->left, z->forward
+		imuLocalTransform_ = Transform(
+				 0, 0, 1, 0,
+				 0, 1, 0, 0,
+				-1 ,0, 0, 0);
+		UINFO("IMU local transform = %s", imuLocalTransform_.prettyPrint().c_str());
+	}
+	else
+	{
+		UINFO("IMU disabled");
+	}

-	leftQueue_ = device_->getOutputQueue("rectified_left", 8, false);
-	rightOrDepthQueue_ = device_->getOutputQueue(outputDepth_?"depth":"rectified_right", 8, false);
-	imuQueue_ = device_->getOutputQueue("imu", 50, false);
+	if(imuPublished_)
+	{
+		imuQueue_ = device_->getOutputQueue("imu", 50, false);
+	}
+	leftQueue_ = device_->getOutputQueue("rectified_left", 1, false);
+	rightOrDepthQueue_ = device_->getOutputQueue(outputDepth_?"depth":"rectified_right", 1, false);

 	uSleep(2000); // avoid bad frames on start

@@ -304,35 +333,42 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info)
 			}

 			//get imu
-			int added=  0;
-			while(1)
+			double stampStart = UTimer::now();
+			while(imuPublished_ && imuQueue_.get())
 			{
-				auto imuData = imuQueue_->get<dai::IMUData>();
-
-				auto imuPackets = imuData->packets;
-				double accStamp = 0.0;
-				double gyroStamp = 0.0;
-				for(auto& imuPacket : imuPackets) {
-					auto& acceleroValues = imuPacket.acceleroMeter;
-					auto& gyroValues = imuPacket.gyroscope;
-
-					accStamp = double(acceleroValues.timestamp.get().time_since_epoch().count())/10e8;
-					gyroStamp = double(gyroValues.timestamp.get().time_since_epoch().count())/10e8;
-					accBuffer_.insert(accBuffer_.end(), std::make_pair(accStamp, cv::Vec3f(acceleroValues.x, acceleroValues.y, acceleroValues.z)));
-					gyroBuffer_.insert(gyroBuffer_.end(), std::make_pair(gyroStamp, cv::Vec3f(gyroValues.x, gyroValues.y, gyroValues.z)));
-					if(accBuffer_.size() > 1000)
-					{
-						accBuffer_.erase(accBuffer_.begin());
-					}
-					if(gyroBuffer_.size() > 1000)
-					{
-						gyroBuffer_.erase(gyroBuffer_.begin());
-					}
-					++added;
-				}
-				if(accStamp >= stamp && gyroStamp >= stamp)
+				if(imuQueue_->has())
 				{
-					break;
+					auto imuData = imuQueue_->get<dai::IMUData>();
+
+					auto imuPackets = imuData->packets;
+					double accStamp = 0.0;
+					double gyroStamp = 0.0;
+					for(auto& imuPacket : imuPackets) {
+						auto& acceleroValues = imuPacket.acceleroMeter;
+						auto& gyroValues = imuPacket.gyroscope;
+
+						accStamp = double(acceleroValues.timestamp.get().time_since_epoch().count())/10e8;
+						gyroStamp = double(gyroValues.timestamp.get().time_since_epoch().count())/10e8;
+						accBuffer_.insert(accBuffer_.end(), std::make_pair(accStamp, cv::Vec3f(acceleroValues.x, acceleroValues.y, acceleroValues.z)));
+						gyroBuffer_.insert(gyroBuffer_.end(), std::make_pair(gyroStamp, cv::Vec3f(gyroValues.x, gyroValues.y, gyroValues.z)));
+						if(accBuffer_.size() > 1000)
+						{
+							accBuffer_.erase(accBuffer_.begin());
+						}
+						if(gyroBuffer_.size() > 1000)
+						{
+							gyroBuffer_.erase(gyroBuffer_.begin());
+						}
+					}
+					if(accStamp >= stamp && gyroStamp >= stamp)
+					{
+						break;
+					}
+				}
+				if((UTimer::now() - stampStart) > 0.01)
+				{
+					UWARN("Could not received IMU after 10 ms! Disabling IMU!");
+					imuPublished_ = false;
 				}
 			}

@@ -70,7 +70,6 @@ OdometryFLOAM::OdometryFLOAM(const ParametersMap & parameters) :
 	Parameters::parse(parameters, Parameters::kIcpRangeMin(), min_dis);
 	Parameters::parse(parameters, Parameters::kOdomLOAMResolution(), map_resolution);

-	UASSERT(scan_period>0.0f);
 	Parameters::parse(parameters, Parameters::kOdomLOAMLinVar(), linVar_);
 	UASSERT(linVar_>0.0f);
 	Parameters::parse(parameters, Parameters::kOdomLOAMAngVar(), angVar_);
@@ -140,6 +139,12 @@ Transform OdometryFLOAM::computeTransform(
 		laserProcessing_->featureExtraction(laserCloudInPtr,pointcloud_edge,pointcloud_surf);
 		UDEBUG("Feature extraction: %fs", timer.ticks());

+		// Put back the laser scan filtered
+		pcl::PointCloud<pcl::PointXYZI>::Ptr pointcloud_filtered(new pcl::PointCloud<pcl::PointXYZI>());
+		*pointcloud_filtered+=*pointcloud_edge;
+		*pointcloud_filtered+=*pointcloud_surf;
+		data.setLaserScan(util3d::laserScanFromPointCloud(*pointcloud_filtered));
+
 		if(this->framesProcessed() == 0){
 			odomEstimation_->initMapWithPoints(pointcloud_edge, pointcloud_surf);
 		}else{
@@ -56,7 +56,6 @@ OdometryLOAM::OdometryLOAM(const ParametersMap & parameters) :
 	float mapResolution  = Parameters::defaultOdomLOAMResolution();
 	Parameters::parse(parameters, Parameters::kOdomLOAMSensor(), velodyneType);
 	Parameters::parse(parameters, Parameters::kOdomLOAMScanPeriod(), scanPeriod_);
-	UASSERT(scanPeriod_>0.0f);
 	Parameters::parse(parameters, Parameters::kOdomLOAMResolution(), mapResolution);
 	UASSERT(mapResolution>0.0f);
 	Parameters::parse(parameters, Parameters::kOdomLOAMLinVar(), linVar_);
@@ -503,7 +503,7 @@ Transform OdometryVINS::computeTransform(
 	{
 		UERROR("VINS-Fusion requires stereo images!");
 	}
-	else
+	else if(data.imu().empty())
 	{
 		UERROR("VINS-Fusion requires stereo images (and only one stereo camera with valid calibration)!");
 	}
@@ -950,14 +950,17 @@ std::map<int, Transform> OptimizerG2O::optimize(
 		UDEBUG("Initial optimization...");
 		optimizer.initializeOptimization();

-		UASSERT_MSG(optimizer.verifyInformationMatrices(true),
-				"This error can be caused by (1) bad covariance matrix "
+		if(!optimizer.verifyInformationMatrices(true))
+		{
+			UERROR("This error can be caused by (1) bad covariance matrix "
 				"set in odometry messages "
 				"(see requirements in g2o::OptimizableGraph::verifyInformationMatrices() function) "
 				"or that (2) PCL and g2o hadn't "
 				"been built both with or without \"-march=native\" compilation "
 				"flag (if one library is built with this flag and not the other, "
 				"this is causing Eigen to not work properly, resulting in segmentation faults).");
+			return optimizedPoses;
+		}

 		UINFO("g2o optimizing begin (max iterations=%d, robust=%d)", iterations(), isRobust()?1:0);
 		int it = 0;
@@ -1085,7 +1088,7 @@ std::map<int, Transform> OptimizerG2O::optimize(

 				if(i>0 && optimizer.activeRobustChi2() > 1000000000000.0)
 				{
-					UWARN("g2o: Large optimimzation error detected (%f), aborting optimization!");
+					UERROR("g2o: Large optimimzation error detected (%f), aborting optimization!");
 					return optimizedPoses;
 				}

@@ -1128,7 +1131,7 @@ std::map<int, Transform> OptimizerG2O::optimize(

 		if(optimizer.activeRobustChi2() > 1000000000000.0)
 		{
-			UWARN("g2o: Large optimimzation error detected (%f), aborting optimization!");
+			UERROR("g2o: Large optimimzation error detected (%f), aborting optimization!");
 			return optimizedPoses;
 		}

@@ -303,8 +303,8 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 							Eigen::Matrix<double, 6, 6> mgtsam = Eigen::Matrix<double, 6, 6>::Identity();
 							mgtsam.block(0,0,3,3) = information.block(3,3,3,3); // cov rotation
 							mgtsam.block(3,3,3,3) = information.block(0,0,3,3); // cov translation
-							mgtsam.block(0,3,3,3) = information.block(0,3,3,3); // off diagonal
-							mgtsam.block(3,0,3,3) = information.block(3,0,3,3); // off diagonal
+							mgtsam.block(0,3,3,3) = information.block(3,0,3,3); // off diagonal
+							mgtsam.block(3,0,3,3) = information.block(0,3,3,3); // off diagonal
 							gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(mgtsam);

 							graph.add(gtsam::PriorFactor<gtsam::Pose3>(id1, gtsam::Pose3(iter->second.transform().toEigen4d()), model));
@@ -383,8 +383,8 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 							Eigen::Matrix<double, 6, 6> mgtsam = Eigen::Matrix<double, 6, 6>::Identity();
 							mgtsam.block(0,0,3,3) = information.block(3,3,3,3); // cov rotation
 							mgtsam.block(3,3,3,3) = information.block(0,0,3,3); // cov translation
-							mgtsam.block(0,3,3,3) = information.block(0,3,3,3); // off diagonal
-							mgtsam.block(3,0,3,3) = information.block(3,0,3,3); // off diagonal
+							mgtsam.block(0,3,3,3) = information.block(3,0,3,3); // off diagonal
+							mgtsam.block(3,0,3,3) = information.block(0,3,3,3); // off diagonal
 							gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(mgtsam);
 							graph.add(gtsam::BetweenFactor<gtsam::Pose3>(id1, id2, gtsam::Pose3(t.toEigen4d()), model));
 						}
@@ -471,8 +471,8 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 					Eigen::Matrix<double, 6, 6> mgtsam = Eigen::Matrix<double, 6, 6>::Identity();
 					mgtsam.block(0,0,3,3) = information.block(3,3,3,3); // cov rotation
 					mgtsam.block(3,3,3,3) = information.block(0,0,3,3); // cov translation
-					mgtsam.block(0,3,3,3) = information.block(0,3,3,3); // off diagonal
-					mgtsam.block(3,0,3,3) = information.block(3,0,3,3); // off diagonal
+					mgtsam.block(0,3,3,3) = information.block(3,0,3,3); // off diagonal
+					mgtsam.block(3,0,3,3) = information.block(0,3,3,3); // off diagonal
 					gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(mgtsam);

 #ifdef RTABMAP_VERTIGO
@@ -708,22 +708,25 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 				Eigen::Matrix<double, 6, 6> mgtsam = Eigen::Matrix<double, 6, 6>::Identity();
 				mgtsam.block(3,3,3,3) = info.block(0,0,3,3); // cov rotation
 				mgtsam.block(0,0,3,3) = info.block(3,3,3,3); // cov translation
-				mgtsam.block(0,3,3,3) = info.block(0,3,3,3); // off diagonal
-				mgtsam.block(3,0,3,3) = info.block(3,0,3,3); // off diagonal
+				mgtsam.block(0,3,3,3) = info.block(3,0,3,3); // off diagonal
+				mgtsam.block(3,0,3,3) = info.block(0,3,3,3); // off diagonal
 				memcpy(outputCovariance.data, mgtsam.data(), outputCovariance.total()*sizeof(double));
 			}
 			else
 			{
-				UWARN("GTSAM: Could not compute marginal covariance!");
+				UERROR("GTSAM: Could not compute marginal covariance!");
+				optimizedPoses.clear(); // Failed optimization
 			}
 		}
 		catch(gtsam::IndeterminantLinearSystemException & e)
 		{
-			UWARN("GTSAM exception caught: %s", e.what());
+			UERROR("GTSAM exception caught: %s", e.what());
+			optimizedPoses.clear(); // Failed optimization
 		}
 		catch(std::exception& e)
 		{
-			UWARN("GTSAM exception caught: %s", e.what());
+			UERROR("GTSAM exception caught: %s", e.what());
+			optimizedPoses.clear(); // Failed optimization
 		}

 		delete optimizer;
@@ -92,6 +92,7 @@ RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/
 RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/alicevision/AliceVision.git --recursive && \
    cd AliceVision && \
    git checkout 0f6115b6af6183c524aa7fcf26141337c1cf3872 && \
+    git submodule update -i && \
    wget https://gist.githubusercontent.com/matlabbe/1df724465106c056ca4cc195c81d8cf0/raw/b3ed4cb8f9b270833a40d57d870a259eabfa4415/alicevision_0f6115b.patch && \
    git apply alicevision_0f6115b.patch && \
    mkdir build && \
@@ -1,5 +1,7 @@
 #!/bin/bash

+set -e
+
 if [ $# -ne 1 ]; then
    echo "deps.bash android_install_prefix   # Example: setup.bash /opt/android"
    exit 1
@@ -1,5 +1,7 @@
 #!/bin/bash

+set -e
+
 if [ $# -ne 2 ]; then
    echo "rtabmap.bash android_install_prefix api_level (23 for tango, 24 for arengine)   # Example: build.bash /opt/android 24"
    exit 1
@@ -144,6 +144,7 @@ RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/
 RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/alicevision/AliceVision.git --recursive && \
    cd AliceVision && \
    git checkout 0f6115b6af6183c524aa7fcf26141337c1cf3872 && \
+    git submodule update -i && \
    wget https://gist.githubusercontent.com/matlabbe/1df724465106c056ca4cc195c81d8cf0/raw/b3ed4cb8f9b270833a40d57d870a259eabfa4415/alicevision_0f6115b.patch && \
    git apply alicevision_0f6115b.patch && \
    mkdir build && \
@@ -165,10 +165,12 @@ private:
 				QLabel * labelId,
 				QLabel * labelMapId,
 				QLabel * labelPose,
+				QLabel * labelOptPose,
 				QLabel * labelVelocity,
 				QLabel * labelCalib,
 				QLabel * labelScan,
 				QLabel * labelGravity,
+				QLabel * labelPrior,
 				QLabel * labelGps,
 				QLabel * labelGt,
 				QLabel * labelSensors,
@@ -187,11 +187,13 @@ protected Q_SLOTS:
 	void selectStereoTara();
 	void selectStereoUsb();
 	void selectMyntEyeS();
-	void selectDepthAI();
+	void selectDepthAIOAKD();
+	void selectDepthAIOAKDLite();
 	void dumpTheMemory();
 	void dumpThePrediction();
 	void sendGoal();
 	void sendWaypoints();
+	void postGoal();
 	void postGoal(const QString & goal);
 	void cancelGoal();
 	void label();
@@ -162,6 +162,7 @@ public:
 	bool isPosteriorGraphView() const;
 	bool isWordsCountGraphView() const;
 	bool isLocalizationsCountGraphView() const;
+	bool isRelocalizationColorOdomCacheGraphView() const;
 	int getOdomRegistrationApproach() const;
 	double getOdomF2MGravitySigma() const;
 	bool isOdomDisabled() const;
@@ -285,6 +286,7 @@ public:

 	//
 	bool isImagesKept() const;
+	bool isMissingCacheRepublished() const;
 	bool isCloudsKept() const;
 	float getTimeLimit() const;
 	float getDetectionRate() const;
@@ -1830,7 +1830,6 @@ void DatabaseViewer::updateIds()
 		previousPose=p;

 		//links
-		bool addPose = links.find(ids_[i]) == links.end();
 		for(std::multimap<int, Link>::iterator jter=links.find(ids_[i]); jter!=links.end() && jter->first == ids_[i]; ++jter)
 		{
 			if(jter->second.type() == Link::kNeighborMerged)
@@ -1856,34 +1855,27 @@ void DatabaseViewer::updateIds()
 				{
 					links_.insert(std::make_pair(ids_[i], jter->second));
 				}
-				addPose = true;
-			}
-			else if(graph::findLink(links_, jter->second.from(), jter->second.to()) != links_.end())
-			{
-				addPose = true;
 			}
 		}
-		if(addPose)
+		// Add pose
+		odomPoses_.insert(std::make_pair(ids_[i], p));
+		if(!g.isNull())
 		{
-			odomPoses_.insert(std::make_pair(ids_[i], p));
-			if(!g.isNull())
-			{
-				groundTruthPoses_.insert(std::make_pair(ids_[i], g));
-			}
-			if(gps.stamp() > 0.0)
-			{
-				gpsValues_.insert(std::make_pair(ids_[i], gps));
+			groundTruthPoses_.insert(std::make_pair(ids_[i], g));
+		}
+		if(gps.stamp() > 0.0)
+		{
+			gpsValues_.insert(std::make_pair(ids_[i], gps));

-				cv::Point3f p(0.0f,0.0f,0.0f);
-				if(!gpsPoses_.empty())
-				{
-					GeodeticCoords coords = gps.toGeodeticCoords();
-					GPS originGPS = gpsValues_.begin()->second;
-					p = coords.toENU_WGS84(originGPS.toGeodeticCoords());
-				}
-				Transform pose(p.x, p.y, p.z, 0.0f, 0.0f, (float)((-(gps.bearing()-90))*M_PI/180.0));
-				gpsPoses_.insert(std::make_pair(ids_[i], pose));
+			cv::Point3f p(0.0f,0.0f,0.0f);
+			if(!gpsPoses_.empty())
+			{
+				GeodeticCoords coords = gps.toGeodeticCoords();
+				GPS originGPS = gpsValues_.begin()->second;
+				p = coords.toENU_WGS84(originGPS.toGeodeticCoords());
 			}
+			Transform pose(p.x, p.y, p.z, 0.0f, 0.0f, (float)((-(gps.bearing()-90))*M_PI/180.0));
+			gpsPoses_.insert(std::make_pair(ids_[i], pose));
 		}
 	}

@@ -3234,17 +3226,57 @@ void DatabaseViewer::regenerateSavedMap()
 		return;
 	}

+	//
+#ifdef RTABMAP_OCTOMAP
+	QStringList types;
+	types.push_back("Default occupancy grid");
+	types.push_back("From OctoMap projection");
+	bool ok;
+	QString type = QInputDialog::getItem(this, tr("Which type?"), tr("Type:"), types, 0, false, &ok);
+	if(!ok)
+	{
+		return;
+	}
+#endif

 	//update scans
 	UINFO("Update local maps list...");
-	OccupancyGrid grid(ui_->parameters_toolbox->getParameters());
-	for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=localMaps_.begin(); iter!=localMaps_.end(); ++iter)
-	{
-		grid.addToCache(iter->first, iter->second.first.first, iter->second.first.second, iter->second.second);
-	}
-	grid.update(graphes_.back());
 	float xMin, yMin;
-	cv::Mat map = grid.getMap(xMin, yMin);
+	cv::Mat map;
+	float gridCellSize = Parameters::defaultGridCellSize();
+	Parameters::parse(ui_->parameters_toolbox->getParameters(), Parameters::kGridCellSize(), gridCellSize);
+#ifdef RTABMAP_OCTOMAP
+	if(type.compare("From OctoMap projection") == 0)
+	{
+		//create local octomap
+		OctoMap octomap(ui_->parameters_toolbox->getParameters());
+		for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=localMaps_.begin(); iter!=localMaps_.end(); ++iter)
+		{
+			if(iter->second.first.first.channels() == 2 || iter->second.first.second.channels() == 2)
+			{
+				QMessageBox::warning(this, tr(""),
+						tr("Some local occupancy grids are 2D, but OctoMap requires 3D local "
+							"occupancy grids. Select default occupancy grid or generate "
+							"3D local occupancy grids (\"Grid/3D\" core parameter)."));
+				return;
+			}
+			octomap.addToCache(iter->first, iter->second.first.first, iter->second.first.second, iter->second.second, localMapsInfo_.at(iter->first).second);
+		}
+
+		octomap.update(graphes_.back());
+		map = octomap.createProjectionMap(xMin, yMin, gridCellSize, 0);
+	}
+	else
+#endif
+	{
+		OccupancyGrid grid(ui_->parameters_toolbox->getParameters());
+		for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=localMaps_.begin(); iter!=localMaps_.end(); ++iter)
+		{
+			grid.addToCache(iter->first, iter->second.first.first, iter->second.first.second, iter->second.second);
+		}
+		grid.update(graphes_.back());
+		map = grid.getMap(xMin, yMin);
+	}

 	if(map.empty())
 	{
@@ -3252,7 +3284,7 @@ void DatabaseViewer::regenerateSavedMap()
 	}
 	else
 	{
-		dbDriver_->save2DMap(map, xMin, yMin, grid.getCellSize());
+		dbDriver_->save2DMap(map, xMin, yMin, gridCellSize);
 		Transform lastlocalizationPose;
 		dbDriver_->loadOptimizedPoses(&lastlocalizationPose);
 		if(lastlocalizationPose.isNull() && !graphes_.back().empty())
@@ -4332,9 +4364,15 @@ void DatabaseViewer::refineAllLinks(const QList<Link> & links)
 		{
 			int from = links[i].from();
 			int to = links[i].to();
-			this->refineConstraint(links[i].from(), links[i].to(), true);
-
-			progressDialog->appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(links.size()));
+			if(from > 0 && to > 0)
+			{
+				this->refineConstraint(links[i].from(), links[i].to(), true);
+				progressDialog->appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(links.size()));
+			}
+			else
+			{
+				progressDialog->appendText(tr("Ignored link %1->%2 (landmark)").arg(from).arg(to));
+			}
 			progressDialog->incrementStep();
 			QApplication::processEvents();
 			if(progressDialog->isCanceled())
@@ -4381,10 +4419,12 @@ void DatabaseViewer::sliderAValueChanged(int value)
 			ui_->label_idA,
 			ui_->label_mapA,
 			ui_->label_poseA,
+			ui_->label_optposeA,
 			ui_->label_velA,
 			ui_->label_calibA,
 			ui_->label_scanA,
 			ui_->label_gravityA,
+			ui_->label_priorA,
 			ui_->label_gpsA,
 			ui_->label_gtA,
 			ui_->label_sensorsA,
@@ -4404,10 +4444,12 @@ void DatabaseViewer::sliderBValueChanged(int value)
 			ui_->label_idB,
 			ui_->label_mapB,
 			ui_->label_poseB,
+			ui_->label_optposeB,
 			ui_->label_velB,
 			ui_->label_calibB,
 			ui_->label_scanB,
 			ui_->label_gravityB,
+			ui_->label_priorB,
 			ui_->label_gpsB,
 			ui_->label_gtB,
 			ui_->label_sensorsB,
@@ -4425,10 +4467,12 @@ void DatabaseViewer::update(int value,
 						QLabel * labelId,
 						QLabel * labelMapId,
 						QLabel * labelPose,
+						QLabel * labelOptPose,
 						QLabel * labelVelocity,
 						QLabel * labelCalib,
 						QLabel * labelScan,
 						QLabel * labelGravity,
+						QLabel * labelPrior,
 						QLabel * labelGps,
 						QLabel * labelGt,
 						QLabel * labelSensors,
@@ -4444,11 +4488,13 @@ void DatabaseViewer::update(int value,
 	label->clear();
 	labelMapId->clear();
 	labelPose->clear();
+	labelOptPose->clear();
 	labelVelocity->clear();
 	stamp->clear();
 	labelCalib->clear();
 	labelScan ->clear();
 	labelGravity->clear();
+	labelPrior->clear();
 	labelGps->clear();
 	labelGt->clear();
 	labelSensors->clear();
@@ -4553,9 +4599,17 @@ void DatabaseViewer::update(int value,
 				float x,y,z,roll,pitch,yaw;
 				odomPose.getTranslationAndEulerAngles(x,y,z,roll, pitch,yaw);
 				labelPose->setText(QString("%1xyz=(%2,%3,%4)\nrpy=(%5,%6,%7)").arg(odomPose.isIdentity()?"* ":"").arg(x).arg(y).arg(z).arg(roll).arg(pitch).arg(yaw));
-				if(odomPoses_.size() && odomPoses_.find(id) == odomPoses_.end())
+				if(graphes_.size())
 				{
-					labelPose->setText(labelPose->text() + "\n<Not in graph>");
+					if(graphes_.back().find(id) == graphes_.back().end())
+					{
+						labelOptPose->setText("<Not in optimized graph>");
+					}
+					else
+					{
+						graphes_.back().find(id)->second.getTranslationAndEulerAngles(x,y,z,roll, pitch,yaw);
+						labelOptPose->setText(QString("xyz=(%1,%2,%3)\nrpy=(%4,%5,%6)").arg(x).arg(y).arg(z).arg(roll).arg(pitch).arg(yaw));
+					}
 				}
 				if(s!=0.0)
 				{
@@ -4578,6 +4632,17 @@ void DatabaseViewer::update(int value,
 					labelGravity->setToolTip(QString("roll=%1 pitch=%2 yaw=%3").arg(roll).arg(pitch).arg(yaw));
 				}

+				std::multimap<int, Link> priorLink;
+				dbDriver_->loadLinks(id, priorLink, Link::kPosePrior);
+				if(!priorLink.empty())
+				{
+					priorLink.begin()->second.transform().getTranslationAndEulerAngles(x,y,z,roll, pitch,yaw);
+					labelPrior->setText(QString("xyz=(%1,%2,%3)\nrpy=(%4,%5,%6)").arg(x).arg(y).arg(z).arg(roll).arg(pitch).arg(yaw));
+					std::stringstream out;
+					out << priorLink.begin()->second.infMatrix().inv();
+					labelPrior->setToolTip(QString("%1").arg(out.str().c_str()));
+				}
+
 				if(gps.stamp()>0.0)
 				{
 					labelGps->setText(QString("stamp=%1 longitude=%2 latitude=%3 altitude=%4m error=%5m bearing=%6deg").arg(QString::number(gps.stamp(), 'f')).arg(gps.longitude()).arg(gps.latitude()).arg(gps.altitude()).arg(gps.error()).arg(gps.bearing()));
@@ -5609,28 +5674,21 @@ void DatabaseViewer::updateWordsMatching(const std::vector<int> & inliers)
 					float scaleAX = ui_->graphicsView_A->viewScale();
 					float scaleBX = ui_->graphicsView_B->viewScale();

-					float scaleDiff = ui_->graphicsView_A->viewScale() / ui_->graphicsView_B->viewScale();
-					float deltaAX = 0;
-					float deltaAY = 0;
+					float marginAX = (ui_->graphicsView_A->width()   - ui_->graphicsView_A->sceneRect().width()*scaleAX)/2.0f;
+					float marginAY = (ui_->graphicsView_A->height()  - ui_->graphicsView_A->sceneRect().height()*scaleAX)/2.0f;
+					float marginBX   = (ui_->graphicsView_B->width()   - ui_->graphicsView_B->sceneRect().width()*scaleBX)/2.0f;
+					float marginBY   = (ui_->graphicsView_B->height()  - ui_->graphicsView_B->sceneRect().height()*scaleBX)/2.0f;
+
+					float deltaX = 0;
+					float deltaY = 0;

 					if(ui_->actionVertical_Layout->isChecked())
 					{
-						deltaAY = ui_->graphicsView_A->height()/scaleAX;
+						deltaY = ui_->graphicsView_A->height();
 					}
 					else
 					{
-						deltaAX = ui_->graphicsView_A->width()/scaleAX;
-					}
-					float deltaBX = 0;
-					float deltaBY = 0;
-
-					if(ui_->actionVertical_Layout->isChecked())
-					{
-						deltaBY = ui_->graphicsView_B->height()/scaleBX;
-					}
-					else
-					{
-						deltaBX = ui_->graphicsView_A->width()/scaleBX;
+						deltaX = ui_->graphicsView_A->width();
 					}

 					const KeypointItem * kptA = wordsA.value(ids[i]);
@@ -5652,17 +5710,17 @@ void DatabaseViewer::updateWordsMatching(const std::vector<int> & inliers)
 					}

 					ui_->graphicsView_A->addLine(
-							kptA->rect().x()+kptA->rect().width()/2,
-							kptA->rect().y()+kptA->rect().height()/2,
-							kptB->rect().x()/scaleDiff+kptB->rect().width()/scaleDiff/2+deltaAX,
-							kptB->rect().y()/scaleDiff+kptB->rect().height()/scaleDiff/2+deltaAY,
+							kptA->keypoint().pt.x,
+							kptA->keypoint().pt.y,
+							(kptB->keypoint().pt.x*scaleBX+marginBX+deltaX-marginAX)/scaleAX,
+							(kptB->keypoint().pt.y*scaleBX+marginBY+deltaY-marginAY)/scaleAX,
 							cA);

 					ui_->graphicsView_B->addLine(
-							kptA->rect().x()*scaleDiff+kptA->rect().width()*scaleDiff/2-deltaBX,
-							kptA->rect().y()*scaleDiff+kptA->rect().height()*scaleDiff/2-deltaBY,
-							kptB->rect().x()+kptB->rect().width()/2,
-							kptB->rect().y()+kptB->rect().height()/2,
+							(kptA->keypoint().pt.x*scaleAX+marginAX-deltaX-marginBX)/scaleBX,
+							(kptA->keypoint().pt.y*scaleAX+marginAY-deltaY-marginBY)/scaleBX,
+							kptB->keypoint().pt.x,
+							kptB->keypoint().pt.y,
 							cB);
 				}
 			}
@@ -5981,10 +6039,12 @@ void DatabaseViewer::updateConstraintView(
 						ui_->label_idA,
 						ui_->label_mapA,
 						ui_->label_poseA,
+						ui_->label_optposeA,
 						ui_->label_velA,
 						ui_->label_calibA,
 						ui_->label_scanA,
 						ui_->label_gravityA,
+						ui_->label_priorA,
 						ui_->label_gpsA,
 						ui_->label_gtA,
 						ui_->label_sensorsA,
@@ -6002,10 +6062,12 @@ void DatabaseViewer::updateConstraintView(
 						ui_->label_idB,
 						ui_->label_mapB,
 						ui_->label_poseB,
+						ui_->label_optposeB,
 						ui_->label_velB,
 						ui_->label_calibB,
 						ui_->label_scanB,
 						ui_->label_gravityB,
+						ui_->label_priorB,
 						ui_->label_gpsB,
 						ui_->label_gtB,
 						ui_->label_sensorsB,
@@ -6526,6 +6588,17 @@ void DatabaseViewer::updateConstraintButtons()
 				ui_->pushButton_add->setEnabled(true);
 			}
 		}
+		else if(ui_->checkBox_enableForAll->isChecked())
+		{
+			if(odomPoses_.find(from) == odomPoses_.end())
+			{
+				UWARN("Button \"Add\" cannot be enabled even if \"all\" checkbox is checked, as node %d doesn't have odometry set.", from);
+			}
+			else if(odomPoses_.find(to) == odomPoses_.end())
+			{
+				UWARN("Button \"Add\" cannot be enabled even if \"all\" checkbox is checked, as node %d doesn't have odometry set.", to);
+			}
+		}

 		currentLink = findActiveLink(from ,to);
 	}
@@ -6674,7 +6747,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 						localMapsInfo.insert(*localMapsInfo_.find(ids[i]));
 					}
 				}
-				else
+				else if(ids.at(i)>0)
 				{
 					SensorData data;
 					dbDriver_->getNodeData(ids.at(i), data, false, false, false);
@@ -7150,6 +7223,49 @@ void DatabaseViewer::updateGraphView()
 			}
 		}

+		// Marker priors parameters
+		double markerPriorsLinearVariance = Parameters::defaultMarkerPriorsVarianceLinear();
+		double markerPriorsAngularVariance = Parameters::defaultMarkerPriorsVarianceAngular();
+		std::map<int, Transform> markerPriors;
+		ParametersMap parameters = ui_->parameters_toolbox->getParameters();
+		Parameters::parse(parameters, Parameters::kMarkerPriorsVarianceLinear(), markerPriorsLinearVariance);
+		UASSERT(markerPriorsLinearVariance>0.0f);
+		Parameters::parse(parameters, Parameters::kMarkerPriorsVarianceAngular(), markerPriorsAngularVariance);
+		UASSERT(markerPriorsAngularVariance>0.0f);
+		std::string markerPriorsStr;
+		if(Parameters::parse(parameters, Parameters::kMarkerPriors(), markerPriorsStr))
+		{
+			std::list<std::string> strList = uSplit(markerPriorsStr, '|');
+			for(std::list<std::string>::iterator iter=strList.begin(); iter!=strList.end(); ++iter)
+			{
+				std::string markerStr = *iter;
+				while(!markerStr.empty() && !uIsDigit(markerStr[0]))
+				{
+					markerStr.erase(markerStr.begin());
+				}
+				if(!markerStr.empty())
+				{
+					std::string idStr = uSplitNumChar(markerStr).front();
+					int id = uStr2Int(idStr);
+					Transform prior = Transform::fromString(markerStr.substr(idStr.size()));
+					if(!prior.isNull() && id>0)
+					{
+						markerPriors.insert(std::make_pair(-id, prior));
+						UDEBUG("Added landmark prior %d: %s", id, prior.prettyPrint().c_str());
+					}
+					else
+					{
+						UERROR("Failed to parse element \"%s\" in parameter %s", markerStr.c_str(), Parameters::kMarkerPriors().c_str());
+					}
+				}
+				else if(!iter->empty())
+				{
+					UERROR("Failed to parse parameter %s, value=\"%s\"", Parameters::kMarkerPriors().c_str(), iter->c_str());
+				}
+			}
+		}
+
+
 		// filter links
 		int totalNeighbor = 0;
 		int totalNeighborMerged = 0;
@@ -7224,6 +7340,19 @@ void DatabaseViewer::updateGraphView()
 				}
 				loopLinks_.push_back(iter->second);
 				++totalLandmarks;
+
+				// add landmark priors if there are some
+				int markerId = iter->second.to();
+				if(markerPriors.find(markerId) != markerPriors.end())
+				{
+					cv::Mat infMatrix = cv::Mat::eye(6, 6, CV_64FC1);
+					infMatrix(cv::Range(0,3), cv::Range(0,3)) /= markerPriorsLinearVariance;
+					infMatrix(cv::Range(3,6), cv::Range(3,6)) /= markerPriorsAngularVariance;
+					links.insert(std::make_pair(markerId, Link(markerId, markerId, Link::kPosePrior, markerPriors.at(markerId), infMatrix)));
+					UDEBUG("Added prior %d : %s (variance: lin=%f ang=%f)", markerId, markerPriors.at(markerId).prettyPrint().c_str(),
+							markerPriorsLinearVariance, markerPriorsAngularVariance);
+					++totalPriors;
+				}
 			}
 			else if(iter->second.type() == Link::kPosePrior)
 			{
@@ -7285,7 +7414,7 @@ void DatabaseViewer::updateGraphView()

 		graphes_.push_back(poses);

-		Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters());
+		Optimizer * optimizer = Optimizer::create(parameters);

 		std::map<int, rtabmap::Transform> posesOut;
 		std::multimap<int, rtabmap::Link> linksOut;
@@ -8237,7 +8366,18 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 			}
 		}

-		t = reg->computeTransformationMod(*fromS, *toS, guess, &info);
+		if(switchedIds)
+		{
+			t = reg->computeTransformationMod(*toS, *fromS, guess.isNull()?guess:guess.inverse(), &info);
+			if(!t.isNull())
+			{
+				t = t.inverse();
+			}
+		}
+		else
+		{
+			t = reg->computeTransformationMod(*fromS, *toS, guess, &info);
+		}
 		delete reg;
 		UDEBUG("");

@@ -624,7 +624,14 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 				}
 				else if(iter->second.type() == Link::kUserClosure)
 				{
-					linkItem->setColor(_loopClosureUserColor);
+					if(_intraInterSessionColors)
+					{
+						linkItem->setColor(interSessionClosure?_loopInterSessionColor:_loopIntraSessionColor);
+					}
+					else
+					{
+						linkItem->setColor(_loopClosureUserColor);
+					}
 				}
 				else if(iter->second.type() == Link::kLandmark)
 				{
@@ -707,7 +714,7 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,

 	if(_nodeItems.size())
 	{
-		(--_nodeItems.end()).value()->setColor(Qt::green);
+		(--_nodeItems.end()).value()->setColor(_nodeOdomCacheColor);
 	}

 	this->scene()->setSceneRect(this->scene()->itemsBoundingRect());  // Re-shrink the scene to it's bounding contents
@@ -1064,10 +1071,6 @@ void GraphViewer::updatePosterior(const std::map<int, float> & posterior, float
 				iter.value()->setColor(QColor::fromHsvF((1-v/max)*240.0f/360.0f, 1, 1, 1)); //0=red 240=blue
 				iter.value()->setZValue(iter.value()->zValue()+zValueOffset);
 			}
-			else if(iter.key() > 0)
-			{
-				iter.value()->setColor(QColor::fromHsvF(240.0f/360.0f, 1, 1, 1)); // blue
-			}
 		}
 	}
 }
@@ -1591,7 +1594,8 @@ void GraphViewer::setIntraSessionLoopColor(const QColor & color)
 		{
 			if((iter.value()->linkType() == Link::kGlobalClosure ||
 				iter.value()->linkType() == Link::kLocalSpaceClosure ||
-				iter.value()->linkType() == Link::kLocalTimeClosure) &&
+				iter.value()->linkType() == Link::kLocalTimeClosure ||
+				iter.value()->linkType() == Link::kUserClosure) &&
 				!iter.value()->isInterSession())
 			{
 				iter.value()->setColor(_loopIntraSessionColor);
@@ -1609,7 +1613,8 @@ void GraphViewer::setInterSessionLoopColor(const QColor & color)
 		{
 			if((iter.value()->linkType() == Link::kGlobalClosure ||
 				iter.value()->linkType() == Link::kLocalSpaceClosure ||
-				iter.value()->linkType() == Link::kLocalTimeClosure) &&
+				iter.value()->linkType() == Link::kLocalTimeClosure ||
+				iter.value()->linkType() == Link::kUserClosure) &&
 				iter.value()->isInterSession())
 			{
 				iter.value()->setColor(_loopInterSessionColor);
@@ -1631,6 +1636,7 @@ void GraphViewer::setIntraInterSessionColorsEnabled(bool enabled)
 	{
 		this->setGlobalLoopClosureColor(_loopClosureColor);
 		this->setLocalLoopClosureColor(_loopClosureLocalColor);
+		this->setUserLoopClosureColor(_loopClosureUserColor);
 	}
 }

@@ -42,6 +42,7 @@
        <file>images/mynteyes.png</file>
        <file>images/l515.png</file>
        <file>images/oakd.png</file>
-		<file>images/astra.png</file>
+        <file>images/oakd_lite.png</file>
+        <file>images/astra.png</file>
    </qresource>
 </RCC>
@@ -452,7 +452,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
 	connect(_ui->actionStereoUsb, SIGNAL(triggered()), this, SLOT(selectStereoUsb()));
 	connect(_ui->actionRealSense2_T265, SIGNAL(triggered()), this, SLOT(selectRealSense2Stereo()));
 	connect(_ui->actionMYNT_EYE_S_SDK, SIGNAL(triggered()), this, SLOT(selectMyntEyeS()));
-	connect(_ui->actionDepthAI, SIGNAL(triggered()), this, SLOT(selectDepthAI()));
+	connect(_ui->actionDepthAI_oakd, SIGNAL(triggered()), this, SLOT(selectDepthAIOAKD()));
+	connect(_ui->actionDepthAI_oakdlite, SIGNAL(triggered()), this, SLOT(selectDepthAIOAKDLite()));
 	_ui->actionFreenect->setEnabled(CameraFreenect::available());
 	_ui->actionOpenNI_CV->setEnabled(CameraOpenNICV::available());
 	_ui->actionOpenNI_CV_ASUS->setEnabled(CameraOpenNICV::available());
@@ -475,7 +476,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
 	_ui->actionZed_Open_Capture->setEnabled(CameraStereoZedOC::available());
    _ui->actionStereoTara->setEnabled(CameraStereoTara::available());
    _ui->actionMYNT_EYE_S_SDK->setEnabled(CameraMyntEye::available());
-    _ui->actionDepthAI->setEnabled(CameraDepthAI::available());
+    _ui->actionDepthAI_oakd->setEnabled(CameraDepthAI::available());
+    _ui->actionDepthAI_oakdlite->setEnabled(CameraDepthAI::available());
 	this->updateSelectSourceMenu();

 	connect(_ui->actionPreferences, SIGNAL(triggered()), this, SLOT(openPreferences()));
@@ -1576,7 +1578,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
 		if(_preferencesDialog->isFramesShown())
 		{
-			_cloudViewer->addOrUpdateLine("odom_to_base_link", _odometryCorrection, _odometryCorrection*odom.pose(), qRgb(255, 128, 0), false, false);
+			_cloudViewer->addOrUpdateLine("odom_to_base_link", _odometryCorrection, _odometryCorrection*odom.pose(), qRgb(255, 128, 0), true, false);
 		}
 		else
 		{
@@ -1995,6 +1997,39 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 			}
 		}

+		// Add data
+		for(std::map<int, Signature>::const_iterator iter = stat.getSignaturesData().begin();
+			iter!=stat.getSignaturesData().end();
+			++iter)
+		{
+			if(signature.id() != iter->first &&
+				(!_cachedSignatures.contains(iter->first) ||
+				 (_cachedSignatures.value(iter->first).sensorData().imageCompressed().empty() && !iter->second.sensorData().imageCompressed().empty())))
+			{
+				_cachedSignatures.insert(iter->first, iter->second);
+				_cachedMemoryUsage += iter->second.sensorData().getMemoryUsed();
+				unsigned int count = 0;
+				if(!iter->second.getWords3().empty())
+				{
+					for(std::multimap<int, int>::const_iterator jter=iter->second.getWords().upper_bound(-1); jter!=iter->second.getWords().end(); ++jter)
+					{
+						if(util3d::isFinite(iter->second.getWords3()[jter->second]))
+						{
+							++count;
+						}
+					}
+				}
+				_cachedWordsCount.insert(std::make_pair(iter->first, (float)count));
+				UINFO("Added node data %d [map=%d] to cache", iter->first, iter->second.mapId());
+
+				_currentMapIds.insert(std::make_pair(iter->first, iter->second.mapId()));
+				if(!iter->second.getGroundTruthPose().isNull())
+				{
+					_currentGTPosesMap.insert(std::make_pair(iter->first, iter->second.getGroundTruthPose()));
+				}
+			}
+		}
+
 		// For intermediate empty nodes, keep latest image shown
 		if(signature.getWeight() >= 0)
 		{
@@ -2391,21 +2426,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)

 			UDEBUG("time= %d ms (update gt-gps stuff)", time.restart());

-#if PCL_VERSION_COMPARE(>=, 1, 7, 2)
-			if(_preferencesDialog->isFramesShown())
-			{
-				_cloudViewer->addOrUpdateCoordinate("map_frame", Transform::getIdentity(), 0.5, false);
-				_cloudViewer->addOrUpdateCoordinate("odom_frame", _odometryCorrection, 0.35, false);
-				_cloudViewer->addOrUpdateLine("map_to_odom", Transform::getIdentity(), _odometryCorrection, qRgb(255, 128, 0), false, false);
-			}
-			else
-			{
-				_cloudViewer->removeLine("map_to_odom");
-				_cloudViewer->removeCoordinate("odom_frame");
-				_cloudViewer->removeCoordinate("map_frame");
-			}
-#endif
-
 			UDEBUG("%d %d %d", poses.size(), poses.size()?poses.rbegin()->first:0, stat.refImageId());
 			if(!_odometryReceived && poses.size() && poses.rbegin()->first == stat.refImageId())
 			{
@@ -2489,6 +2509,28 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 					_ui->statsToolBox->updateStat(iter->first.c_str(), _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), int(iter->second), _preferencesDialog->isCacheSavedInFigures());
 				}
 			}
+
+#if PCL_VERSION_COMPARE(>=, 1, 7, 2)
+			if(_preferencesDialog->isFramesShown())
+			{
+				_cloudViewer->addOrUpdateCoordinate("map_frame", Transform::getIdentity(), 0.5, false);
+				_cloudViewer->addOrUpdateCoordinate("odom_frame", _odometryCorrection, 0.35, false);
+				_cloudViewer->addOrUpdateLine("map_to_odom", Transform::getIdentity(), _odometryCorrection, qRgb(255, 128, 0), true, false);
+				if(_preferencesDialog->isLabelsShown())
+				{
+					_cloudViewer->addOrUpdateText("map_frame_label", "map",	Transform::getIdentity(), 0.1, Qt::white);
+					_cloudViewer->addOrUpdateText("odom_frame_label", "odom", _odometryCorrection, 0.1, Qt::white);
+				}
+			}
+			else
+			{
+				_cloudViewer->removeLine("map_to_odom");
+				_cloudViewer->removeCoordinate("odom_frame");
+				_cloudViewer->removeCoordinate("map_frame");
+				_cloudViewer->removeText("map_frame_label");
+				_cloudViewer->removeText("odom_frame_label");
+			}
+#endif
 		}

 		if( _ui->graphicsView_graphView->isVisible())
@@ -2512,6 +2554,26 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 					_ui->graphicsView_graphView->updatePosterior(_cachedLocalizationsCount, 1.0f);
 				}
 			}
+			if(_preferencesDialog->isRelocalizationColorOdomCacheGraphView() && !stat.odomCachePoses().empty())
+			{
+				std::map<int, float> colors;
+				colors.insert(std::make_pair(stat.odomCachePoses().rbegin()->first, 240));
+				for(std::multimap<int, Link>::const_iterator iter=stat.odomCacheConstraints().begin(); iter!=stat.odomCacheConstraints().end(); ++iter)
+				{
+					if(iter->second.type() != Link::kNeighbor)
+					{
+						uInsert(colors, std::pair<int,float>(iter->second.from()>iter->second.to()?iter->second.from():iter->second.to(), 120)); //green
+					}
+				}
+				for(std::map<int, Transform>::const_iterator iter=stat.odomCachePoses().begin(); iter!=stat.odomCachePoses().end(); ++iter)
+				{
+					if(stat.poses().find(iter->first) == stat.poses().end())
+					{
+						colors.insert(std::make_pair(iter->first, 240)); //red
+					}
+				}
+				_ui->graphicsView_graphView->updatePosterior(colors, 240);
+			}
 			// update local path on the graph view
 			_ui->graphicsView_graphView->updateLocalPath(stat.localPath());
 			if(stat.localPath().size() == 0)
@@ -2544,6 +2606,37 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 			_cachedMemoryUsage += s.sensorData().getMemoryUsed();
 		}

+		// Check missing cache
+		if(stat.getSignaturesData().size() <= 1)
+		{
+			if(_preferencesDialog->isMissingCacheRepublished() &&
+			   _preferencesDialog->isImagesKept() &&
+			   atoi(_preferencesDialog->getParameter(Parameters::kRtabmapMaxRepublished()).c_str()) > 0)
+			{
+				std::vector<int> missingIds;
+				bool ignoreNewData = smallMovement || fastMovement || signature.getWeight()<0;
+				for(std::map<int, Transform>::const_iterator iter=stat.poses().begin(); iter!=stat.poses().end(); ++iter)
+				{
+					if(!ignoreNewData || stat.refImageId() != iter->first)
+					{
+						QMap<int, Signature>::iterator ster = _cachedSignatures.find(iter->first);
+						if(ster == _cachedSignatures.end() ||
+							(ster.value().getWeight() >=0 && // ignore intermediate nodes
+							 ster.value().sensorData().imageCompressed().empty() &&
+							 ster.value().sensorData().depthOrRightCompressed().empty() &&
+							 ster.value().sensorData().laserScanCompressed().empty()))
+						{
+							missingIds.push_back(iter->first);
+						}
+					}
+				}
+				if(!missingIds.empty())
+				{
+					this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdRepublishData, UVariant(missingIds)));
+				}
+			}
+		}
+
 		UDEBUG("time= %d ms (update cache)", time.restart());
 	}
 	else if(!stat.extended() && stat.loopClosureId()>0)
@@ -4622,8 +4715,7 @@ void MainWindow::processRtabmapGlobalPathEvent(const rtabmap::RtabmapGlobalPathE
 	else if(event.getPoses().empty() && _waypoints.size())
 	{
 		// resend the same goal
-		uSleep(1000);
-		this->postGoal(_waypoints.at(_waypointsIndex % _waypoints.size()));
+		QTimer::singleShot(1000, this, SLOT(postGoal()));
 	}
 }

@@ -5085,7 +5177,8 @@ void MainWindow::updateSelectSourceMenu()
 	_ui->actionStereoUsb->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoUsb);
 	_ui->actionRealSense2_T265->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoRealSense2);
 	_ui->actionMYNT_EYE_S_SDK->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoMyntEye);
-	_ui->actionDepthAI->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoDepthAI);
+	_ui->actionDepthAI_oakd->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoDepthAI);
+	_ui->actionDepthAI_oakdlite->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoDepthAI);
 }

 void MainWindow::changeImgRateSetting()
@@ -5224,6 +5317,8 @@ void MainWindow::newDatabase()
 	_cloudViewer->removeLine("odom_to_base_link");
 	_cloudViewer->removeCoordinate("odom_frame");
 	_cloudViewer->removeCoordinate("map_frame");
+	_cloudViewer->removeText("map_frame_label");
+	_cloudViewer->removeText("odom_frame_label");
 	ULOGGER_DEBUG("");
 	this->clearTheCache();
 	std::string databasePath = (_preferencesDialog->getWorkingDirectory()+QDir::separator()+QString("rtabmap.tmp.db")).toStdString();
@@ -5373,8 +5468,9 @@ void MainWindow::openDatabase(const QString & path, const ParametersMap & overri
 						if(different)
 						{
 							differentParameters.insert(*iter);
-							QString msg = tr("Parameter \"%1\": database=\"%2\" Preferences=\"%3\"")
+							QString msg = tr("Parameter \"%1\": %2=\"%3\" Preferences=\"%4\"")
 											.arg(iter->first.c_str())
+											.arg(overridedParameters.find(iter->first) != overridedParameters.end()?"Arguments":"Database")
 											.arg(iter->second.c_str())
 											.arg(jter->second.c_str());
 							_ui->widget_console->appendMsg(msg);
@@ -5387,9 +5483,11 @@ void MainWindow::openDatabase(const QString & path, const ParametersMap & overri
 				{
 					int r = QMessageBox::question(this,
 							tr("Update parameters..."),
-							tr("The database is using %1 different parameter(s) than "
+							tr("The %1 using %2 different parameter(s) than "
 							   "those currently set in Preferences. Do you want "
-							   "to use database's parameters?").arg(differentParameters.size()),
+							   "to use those parameters?")
+							   .arg(overridedParameters.empty()?tr("database is"):tr("database and input arguments are"))
+							   .arg(differentParameters.size()),
 							QMessageBox::Yes | QMessageBox::No,
 							QMessageBox::Yes);
 					if(r == QMessageBox::Yes)
@@ -6955,9 +7053,14 @@ void MainWindow::selectMyntEyeS()
 	_preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcStereoMyntEye);
 }

-void MainWindow::selectDepthAI()
+void MainWindow::selectDepthAIOAKD()
 {
-	_preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcStereoDepthAI);
+	_preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcStereoDepthAI, 1); // variant 1=IMU
+}
+
+void MainWindow::selectDepthAIOAKDLite()
+{
+	_preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcStereoDepthAI, 0); // variant 0=no IMU
 }

 void MainWindow::dumpTheMemory()
@@ -7005,6 +7108,14 @@ void MainWindow::sendWaypoints()
 	}
 }

+void MainWindow::postGoal()
+{
+	if(!_waypoints.isEmpty())
+	{
+		postGoal(_waypoints.at(_waypointsIndex % _waypoints.size()));
+	}
+}
+
 void MainWindow::postGoal(const QString & goal)
 {
 	if(!goal.isEmpty())
@@ -450,6 +450,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :

 	// General panel
 	connect(_ui->general_checkBox_imagesKept, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
+	connect(_ui->general_checkBox_missing_cache_republished, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
 	connect(_ui->general_checkBox_cloudsKept, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
 	connect(_ui->checkBox_verticalLayoutUsed, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
 	connect(_ui->checkBox_imageRejectedShown, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
@@ -463,6 +464,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->radioButton_posteriorGraphView, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteGeneralPanel()));
 	connect(_ui->radioButton_wordsGraphView, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteGeneralPanel()));
 	connect(_ui->radioButton_localizationsGraphView, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteGeneralPanel()));
+	connect(_ui->radioButton_localizationsGraphViewOdomCache, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteGeneralPanel()));
 	connect(_ui->radioButton_nochangeGraphView, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteGeneralPanel()));
 	connect(_ui->checkbox_odomDisabled, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
 	connect(_ui->odom_registration, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
@@ -795,6 +797,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->comboBox_depthai_resolution, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkBox_depthai_depth, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->spinBox_depthai_confidence, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->checkBox_depthai_imu_published, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkBox_depthai_imu_firmware_update, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));

 	connect(_ui->checkbox_rgbd_colorOnly, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
@@ -886,6 +889,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->general_spinBox_imagesBufferSize->setObjectName(Parameters::kRtabmapImageBufferSize().c_str());
 	_ui->general_checkBox_createIntermediateNodes->setObjectName(Parameters::kRtabmapCreateIntermediateNodes().c_str());
 	_ui->general_spinBox_maxRetrieved->setObjectName(Parameters::kRtabmapMaxRetrieved().c_str());
+	_ui->general_spinBox_max_republished->setObjectName(Parameters::kRtabmapMaxRepublished().c_str());
 	_ui->general_checkBox_startNewMapOnLoopClosure->setObjectName(Parameters::kRtabmapStartNewMapOnLoopClosure().c_str());
 	_ui->general_checkBox_startNewMapOnGoodSignature->setObjectName(Parameters::kRtabmapStartNewMapOnGoodSignature().c_str());
 	_ui->general_checkBox_imagesAlreadyRectified->setObjectName(Parameters::kRtabmapImagesAlreadyRectified().c_str());
@@ -1790,6 +1794,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 	if(groupBox->objectName() == _ui->groupBox_generalSettingsGui0->objectName())
 	{
 		_ui->general_checkBox_imagesKept->setChecked(true);
+		_ui->general_checkBox_missing_cache_republished->setChecked(true);
 		_ui->general_checkBox_cloudsKept->setChecked(true);
 		_ui->checkBox_beep->setChecked(false);
 		_ui->checkBox_stamps->setChecked(true);
@@ -1803,6 +1808,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->radioButton_posteriorGraphView->setChecked(true);
 		_ui->radioButton_wordsGraphView->setChecked(false);
 		_ui->radioButton_localizationsGraphView->setChecked(false);
+		_ui->radioButton_localizationsGraphViewOdomCache->setChecked(false);
 		_ui->radioButton_nochangeGraphView->setChecked(false);
 		_ui->checkbox_odomDisabled->setChecked(false);
 		_ui->checkbox_groundTruthAlign->setChecked(true);
@@ -2042,6 +2048,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->comboBox_depthai_resolution->setCurrentIndex(1);
 		_ui->checkBox_depthai_depth->setChecked(false);
 		_ui->spinBox_depthai_confidence->setValue(200);
+		_ui->checkBox_depthai_imu_published->setChecked(true);
 		_ui->checkBox_depthai_imu_firmware_update->setChecked(false);

 		_ui->checkBox_cameraImages_configForEachFrame->setChecked(false);
@@ -2268,6 +2275,7 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
 	settings.beginGroup("Gui");
 	settings.beginGroup("General");
 	_ui->general_checkBox_imagesKept->setChecked(settings.value("imagesKept", _ui->general_checkBox_imagesKept->isChecked()).toBool());
+	_ui->general_checkBox_missing_cache_republished->setChecked(settings.value("missingRepublished", _ui->general_checkBox_missing_cache_republished->isChecked()).toBool());
 	_ui->general_checkBox_cloudsKept->setChecked(settings.value("cloudsKept", _ui->general_checkBox_cloudsKept->isChecked()).toBool());
 	_ui->comboBox_loggerLevel->setCurrentIndex(settings.value("loggerLevel", _ui->comboBox_loggerLevel->currentIndex()).toInt());
 	_ui->comboBox_loggerEventLevel->setCurrentIndex(settings.value("loggerEventLevel", _ui->comboBox_loggerEventLevel->currentIndex()).toInt());
@@ -2287,6 +2295,7 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
 	_ui->radioButton_posteriorGraphView->setChecked(settings.value("posteriorGraphView", _ui->radioButton_posteriorGraphView->isChecked()).toBool());
 	_ui->radioButton_wordsGraphView->setChecked(settings.value("wordsGraphView", _ui->radioButton_wordsGraphView->isChecked()).toBool());
 	_ui->radioButton_localizationsGraphView->setChecked(settings.value("localizationsGraphView", _ui->radioButton_localizationsGraphView->isChecked()).toBool());
+	_ui->radioButton_localizationsGraphViewOdomCache->setChecked(settings.value("localizationsGraphViewOdomCache", _ui->radioButton_localizationsGraphViewOdomCache->isChecked()).toBool());
 	_ui->radioButton_nochangeGraphView->setChecked(settings.value("nochangeGraphView", _ui->radioButton_nochangeGraphView->isChecked()).toBool());
 	_ui->checkbox_odomDisabled->setChecked(settings.value("odomDisabled", _ui->checkbox_odomDisabled->isChecked()).toBool());
 	_ui->odom_registration->setCurrentIndex(settings.value("odomRegistration", _ui->odom_registration->currentIndex()).toInt());
@@ -2523,6 +2532,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->comboBox_depthai_resolution->setCurrentIndex(settings.value("resolution", _ui->comboBox_depthai_resolution->currentIndex()).toInt());
 	_ui->checkBox_depthai_depth->setChecked(settings.value("depth", _ui->checkBox_depthai_depth->isChecked()).toBool());
 	_ui->spinBox_depthai_confidence->setValue(settings.value("confidence", _ui->spinBox_depthai_confidence->value()).toInt());
+	_ui->checkBox_depthai_imu_published->setChecked(settings.value("imu_published", _ui->checkBox_depthai_imu_published->isChecked()).toBool());
 	_ui->checkBox_depthai_imu_firmware_update->setChecked(settings.value("imu_firmware_update", _ui->checkBox_depthai_imu_firmware_update->isChecked()).toBool());
 	settings.endGroup(); // DepthAI

@@ -2795,6 +2805,7 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const
 	settings.beginGroup("General");
 	settings.remove("");
 	settings.setValue("imagesKept",           _ui->general_checkBox_imagesKept->isChecked());
+	settings.setValue("missingRepublished",   _ui->general_checkBox_missing_cache_republished->isChecked());
 	settings.setValue("cloudsKept",           _ui->general_checkBox_cloudsKept->isChecked());
 	settings.setValue("loggerLevel",          _ui->comboBox_loggerLevel->currentIndex());
 	settings.setValue("loggerEventLevel",     _ui->comboBox_loggerEventLevel->currentIndex());
@@ -2814,6 +2825,7 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const
 	settings.setValue("posteriorGraphView",   _ui->radioButton_posteriorGraphView->isChecked());
 	settings.setValue("wordsGraphView",       _ui->radioButton_wordsGraphView->isChecked());
 	settings.setValue("localizationsGraphView", _ui->radioButton_localizationsGraphView->isChecked());
+	settings.setValue("localizationsGraphViewOdomCache", _ui->radioButton_localizationsGraphViewOdomCache->isChecked());
 	settings.setValue("nochangeGraphView",    _ui->radioButton_nochangeGraphView->isChecked());
 	settings.setValue("odomDisabled",         _ui->checkbox_odomDisabled->isChecked());
 	settings.setValue("odomRegistration",     _ui->odom_registration->currentIndex());
@@ -3046,6 +3058,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("resolution",    _ui->comboBox_depthai_resolution->currentIndex());
 	settings.setValue("depth",         _ui->checkBox_depthai_depth->isChecked());
 	settings.setValue("confidence",    _ui->spinBox_depthai_confidence->value());
+	settings.setValue("imu_published", _ui->checkBox_depthai_imu_published->isChecked());
 	settings.setValue("imu_firmware_update", _ui->checkBox_depthai_imu_firmware_update->isChecked());
 	settings.endGroup(); // DepthAI

@@ -3923,6 +3936,11 @@ void PreferencesDialog::selectSourceDriver(Src src, int variant)
 			// disable IMU filtering (zed sends already quaternion)
 			_ui->comboBox_imuFilter_strategy->setCurrentIndex(0);
 		}
+		else if(src == kSrcStereoDepthAI) // OAK-D (variant==1), OAK-D Lite (variant==0)
+		{
+			_ui->checkBox_depthai_imu_published->setChecked(variant == 1);
+			_ui->comboBox_depthai_resolution->setCurrentIndex(variant == 1?1:3);
+		}
 	}
 	else if(src >= kSrcRGB && src<kSrcDatabase)
 	{
@@ -5331,6 +5349,10 @@ bool PreferencesDialog::isLocalizationsCountGraphView() const
 {
 	return _ui->radioButton_localizationsGraphView->isChecked();
 }
+bool PreferencesDialog::isRelocalizationColorOdomCacheGraphView() const
+{
+	return _ui->radioButton_localizationsGraphViewOdomCache->isChecked();
+}
 bool PreferencesDialog::isOdomDisabled() const
 {
 	return _ui->checkbox_odomDisabled->isChecked();
@@ -6279,6 +6301,7 @@ Camera * PreferencesDialog::createCamera(
 			this->getSourceLocalTransform());
 		((CameraDepthAI*)camera)->setOutputDepth(_ui->checkBox_depthai_depth->isChecked(), _ui->spinBox_depthai_confidence->value());
 		((CameraDepthAI*)camera)->setIMUFirmwareUpdate(_ui->checkBox_depthai_imu_firmware_update->isChecked());
+		((CameraDepthAI*)camera)->setIMUPublished(_ui->checkBox_depthai_imu_published->isChecked());
 	}
 	else if(driver == kSrcUsbDevice)
 	{
@@ -6464,6 +6487,10 @@ bool PreferencesDialog::isImagesKept() const
 {
 	return _ui->general_checkBox_imagesKept->isChecked();
 }
+bool PreferencesDialog::isMissingCacheRepublished() const
+{
+	return _ui->general_checkBox_missing_cache_republished->isChecked();
+}
 bool PreferencesDialog::isCloudsKept() const
 {
 	return _ui->general_checkBox_cloudsKept->isChecked();
@@ -60,21 +60,141 @@
         <property name="geometry">
          <rect>
           <x>0</x>
-           <y>-23</y>
+           <y>0</y>
           <width>293</width>
-           <height>334</height>
+           <height>380</height>
          </rect>
         </property>
         <layout class="QGridLayout" name="gridLayout" columnstretch="0,1">
-          <item row="5" column="0">
-           <widget class="QLabel" name="label_childrenA_12">
+          <item row="2" column="0">
+           <widget class="QLabel" name="label_childrenA_8">
            <property name="text">
-             <string>Pose</string>
+             <string>Weight</string>
            </property>
           </widget>
          </item>
-          <item row="5" column="1">
-           <widget class="QLabel" name="label_poseA">
+          <item row="11" column="0">
+           <widget class="QLabel" name="label_childrenA_24">
+            <property name="text">
+             <string>Gravity</string>
+            </property>
+           </widget>
+          </item>
+          <item row="12" column="1">
+           <widget class="QLabel" name="label_priorA">
+            <property name="text">
+             <string/>
+            </property>
+            <property name="textInteractionFlags">
+             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+            </property>
+           </widget>
+          </item>
+          <item row="1" column="0">
+           <widget class="QLabel" name="label_childrenA_2">
+            <property name="text">
+             <string>Children</string>
+            </property>
+           </widget>
+          </item>
+          <item row="15" column="1">
+           <widget class="QLabel" name="label_sensorsA">
+            <property name="text">
+             <string/>
+            </property>
+            <property name="textInteractionFlags">
+             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+            </property>
+           </widget>
+          </item>
+          <item row="8" column="0">
+           <widget class="QLabel" name="label_childrenA_6">
+            <property name="text">
+             <string>Stamp</string>
+            </property>
+           </widget>
+          </item>
+          <item row="10" column="0">
+           <widget class="QLabel" name="label_childrenA_22">
+            <property name="text">
+             <string>Scan</string>
+            </property>
+           </widget>
+          </item>
+          <item row="15" column="0">
+           <widget class="QLabel" name="label_childrenA_20">
+            <property name="text">
+             <string>Sensors</string>
+            </property>
+           </widget>
+          </item>
+          <item row="10" column="1">
+           <widget class="QLabel" name="label_scanA">
+            <property name="text">
+             <string/>
+            </property>
+            <property name="textInteractionFlags">
+             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+            </property>
+           </widget>
+          </item>
+          <item row="5" column="0">
+           <widget class="QLabel" name="label_childrenA_12">
+            <property name="text">
+             <string>Odometry Pose</string>
+            </property>
+           </widget>
+          </item>
+          <item row="0" column="0">
+           <widget class="QLabel" name="label_parentsA_2">
+            <property name="text">
+             <string>Parents</string>
+            </property>
+           </widget>
+          </item>
+          <item row="2" column="1">
+           <widget class="QLabel" name="label_weightA">
+            <property name="text">
+             <string/>
+            </property>
+            <property name="textInteractionFlags">
+             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+            </property>
+           </widget>
+          </item>
+          <item row="13" column="1">
+           <widget class="QLabel" name="label_gpsA">
+            <property name="text">
+             <string/>
+            </property>
+            <property name="textInteractionFlags">
+             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+            </property>
+           </widget>
+          </item>
+          <item row="9" column="0">
+           <widget class="QLabel" name="label_childrenA_14">
+            <property name="text">
+             <string>Calib</string>
+            </property>
+           </widget>
+          </item>
+          <item row="12" column="0">
+           <widget class="QLabel" name="label_childrenA_28">
+            <property name="text">
+             <string>Prior</string>
+            </property>
+           </widget>
+          </item>
+          <item row="7" column="0">
+           <widget class="QLabel" name="label_childrenA_18">
+            <property name="text">
+             <string>Velocity</string>
+            </property>
+           </widget>
+          </item>
+          <item row="4" column="1">
+           <widget class="QLabel" name="label_mapA">
            <property name="text">
             <string/>
            </property>
@@ -93,14 +213,28 @@
            </property>
           </widget>
          </item>
-          <item row="2" column="0">
-           <widget class="QLabel" name="label_childrenA_8">
+          <item row="4" column="0">
+           <widget class="QLabel" name="label_childrenA_10">
            <property name="text">
-             <string>Weight</string>
+             <string>Map ID</string>
            </property>
           </widget>
          </item>
-          <item row="6" column="1">
+          <item row="13" column="0">
+           <widget class="QLabel" name="label_childrenA_16">
+            <property name="text">
+             <string>GPS</string>
+            </property>
+           </widget>
+          </item>
+          <item row="14" column="0">
+           <widget class="QLabel" name="label_childrenA_27">
+            <property name="text">
+             <string>Ground Truth</string>
+            </property>
+           </widget>
+          </item>
+          <item row="7" column="1">
           <widget class="QLabel" name="label_velA">
            <property name="text">
             <string/>
@@ -110,6 +244,33 @@
            </property>
           </widget>
          </item>
+          <item row="3" column="0">
+           <widget class="QLabel" name="label_childrenA_4">
+            <property name="text">
+             <string>Label</string>
+            </property>
+           </widget>
+          </item>
+          <item row="5" column="1">
+           <widget class="QLabel" name="label_poseA">
+            <property name="text">
+             <string/>
+            </property>
+            <property name="textInteractionFlags">
+             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+            </property>
+           </widget>
+          </item>
+          <item row="9" column="1">
+           <widget class="QLabel" name="label_calibA">
+            <property name="text">
+             <string/>
+            </property>
+            <property name="textInteractionFlags">
+             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+            </property>
+           </widget>
+          </item>
          <item row="3" column="1">
           <widget class="QLabel" name="label_labelA">
            <property name="text">
@@ -120,8 +281,8 @@
            </property>
           </widget>
          </item>
-          <item row="4" column="1">
-           <widget class="QLabel" name="label_mapA">
+          <item row="11" column="1">
+           <widget class="QLabel" name="label_gravityA">
            <property name="text">
             <string/>
            </property>
@@ -130,10 +291,13 @@
            </property>
           </widget>
          </item>
-          <item row="8" column="0">
-           <widget class="QLabel" name="label_childrenA_14">
+          <item row="8" column="1">
+           <widget class="QLabel" name="label_stampA">
            <property name="text">
-             <string>Calib</string>
+             <string/>
+            </property>
+            <property name="textInteractionFlags">
+             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
            </property>
           </widget>
          </item>
@@ -147,8 +311,8 @@
            </property>
           </widget>
          </item>
-          <item row="7" column="1">
-           <widget class="QLabel" name="label_stampA">
+          <item row="14" column="1">
+           <widget class="QLabel" name="label_gtA">
            <property name="text">
             <string/>
            </property>
@@ -157,145 +321,15 @@
            </property>
           </widget>
          </item>
-          <item row="13" column="1">
-           <widget class="QLabel" name="label_sensorsA">
-            <property name="text">
-             <string/>
-            </property>
-            <property name="textInteractionFlags">
-             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-            </property>
-           </widget>
-          </item>
-          <item row="9" column="0">
-           <widget class="QLabel" name="label_childrenA_22">
-            <property name="text">
-             <string>Scan</string>
-            </property>
-           </widget>
-          </item>
-          <item row="10" column="0">
-           <widget class="QLabel" name="label_childrenA_24">
-            <property name="text">
-             <string>Gravity</string>
-            </property>
-           </widget>
-          </item>
-          <item row="11" column="1">
-           <widget class="QLabel" name="label_gpsA">
-            <property name="text">
-             <string/>
-            </property>
-            <property name="textInteractionFlags">
-             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-            </property>
-           </widget>
-          </item>
-          <item row="2" column="1">
-           <widget class="QLabel" name="label_weightA">
-            <property name="text">
-             <string/>
-            </property>
-            <property name="textInteractionFlags">
-             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-            </property>
-           </widget>
-          </item>
-          <item row="13" column="0">
-           <widget class="QLabel" name="label_childrenA_20">
-            <property name="text">
-             <string>Sensors</string>
-            </property>
-           </widget>
-          </item>
-          <item row="11" column="0">
-           <widget class="QLabel" name="label_childrenA_16">
-            <property name="text">
-             <string>GPS</string>
-            </property>
-           </widget>
-          </item>
-          <item row="7" column="0">
-           <widget class="QLabel" name="label_childrenA_6">
-            <property name="text">
-             <string>Stamp</string>
-            </property>
-           </widget>
-          </item>
          <item row="6" column="0">
-           <widget class="QLabel" name="label_childrenA_18">
+           <widget class="QLabel" name="label_childrenA_30">
            <property name="text">
-             <string>Velocity</string>
+             <string>Optimized Pose</string>
            </property>
           </widget>
          </item>
-          <item row="0" column="0">
-           <widget class="QLabel" name="label_parentsA_2">
-            <property name="text">
-             <string>Parents</string>
-            </property>
-           </widget>
-          </item>
-          <item row="10" column="1">
-           <widget class="QLabel" name="label_gravityA">
-            <property name="text">
-             <string/>
-            </property>
-            <property name="textInteractionFlags">
-             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-            </property>
-           </widget>
-          </item>
-          <item row="4" column="0">
-           <widget class="QLabel" name="label_childrenA_10">
-            <property name="text">
-             <string>Map ID</string>
-            </property>
-           </widget>
-          </item>
-          <item row="9" column="1">
-           <widget class="QLabel" name="label_scanA">
-            <property name="text">
-             <string/>
-            </property>
-            <property name="textInteractionFlags">
-             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-            </property>
-           </widget>
-          </item>
-          <item row="3" column="0">
-           <widget class="QLabel" name="label_childrenA_4">
-            <property name="text">
-             <string>Label</string>
-            </property>
-           </widget>
-          </item>
-          <item row="1" column="0">
-           <widget class="QLabel" name="label_childrenA_2">
-            <property name="text">
-             <string>Children</string>
-            </property>
-           </widget>
-          </item>
-          <item row="8" column="1">
-           <widget class="QLabel" name="label_calibA">
-            <property name="text">
-             <string/>
-            </property>
-            <property name="textInteractionFlags">
-             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-            </property>
-           </widget>
-          </item>
-          <item row="12" column="0">
-           <widget class="QLabel" name="label_childrenA_27">
-            <property name="text">
-             <string>Ground Truth</string>
-            </property>
-           </widget>
-          </item>
-          <item row="12" column="1">
-           <widget class="QLabel" name="label_gtA">
+          <item row="6" column="1">
+           <widget class="QLabel" name="label_optposeA">
            <property name="text">
             <string/>
            </property>
@@ -320,16 +354,16 @@
         <property name="geometry">
          <rect>
           <x>0</x>
-           <y>-23</y>
+           <y>0</y>
           <width>292</width>
-           <height>334</height>
+           <height>380</height>
          </rect>
         </property>
         <layout class="QGridLayout" name="gridLayout_2" columnstretch="0,1">
-          <item row="11" column="0">
-           <widget class="QLabel" name="label_childrenA_17">
+          <item row="4" column="0">
+           <widget class="QLabel" name="label_childrenA_11">
            <property name="text">
-             <string>GPS</string>
+             <string>Map ID</string>
            </property>
           </widget>
          </item>
@@ -343,8 +377,36 @@
            </property>
           </widget>
          </item>
-          <item row="11" column="1">
-           <widget class="QLabel" name="label_gpsB">
+          <item row="14" column="0">
+           <widget class="QLabel" name="label_childrenA_26">
+            <property name="text">
+             <string>Ground Truth</string>
+            </property>
+           </widget>
+          </item>
+          <item row="3" column="0">
+           <widget class="QLabel" name="label_childrenA_5">
+            <property name="text">
+             <string>Label</string>
+            </property>
+           </widget>
+          </item>
+          <item row="1" column="0">
+           <widget class="QLabel" name="label_childrenA_3">
+            <property name="text">
+             <string>Children</string>
+            </property>
+           </widget>
+          </item>
+          <item row="15" column="0">
+           <widget class="QLabel" name="label_childrenA_21">
+            <property name="text">
+             <string>Sensors</string>
+            </property>
+           </widget>
+          </item>
+          <item row="1" column="1">
+           <widget class="QLabel" name="label_childrenB">
            <property name="text">
             <string/>
            </property>
@@ -353,27 +415,13 @@
            </property>
           </widget>
          </item>
-          <item row="7" column="0">
+          <item row="8" column="0">
           <widget class="QLabel" name="label_childrenA_7">
            <property name="text">
             <string>Stamp</string>
            </property>
           </widget>
          </item>
-          <item row="2" column="0">
-           <widget class="QLabel" name="label_childrenA_9">
-            <property name="text">
-             <string>Weight</string>
-            </property>
-           </widget>
-          </item>
-          <item row="13" column="0">
-           <widget class="QLabel" name="label_childrenA_21">
-            <property name="text">
-             <string>Sensors</string>
-            </property>
-           </widget>
-          </item>
          <item row="4" column="1">
           <widget class="QLabel" name="label_mapB">
            <property name="text">
@@ -384,56 +432,8 @@
            </property>
           </widget>
          </item>
-          <item row="10" column="0">
-           <widget class="QLabel" name="label_childrenA_25">
-            <property name="text">
-             <string>Gravity</string>
-            </property>
-           </widget>
-          </item>
-          <item row="8" column="0">
-           <widget class="QLabel" name="label_childrenA_15">
-            <property name="text">
-             <string>Calib</string>
-            </property>
-           </widget>
-          </item>
-          <item row="9" column="1">
-           <widget class="QLabel" name="label_scanB">
-            <property name="text">
-             <string/>
-            </property>
-            <property name="textInteractionFlags">
-             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-            </property>
-           </widget>
-          </item>
-          <item row="0" column="1">
-           <widget class="QLabel" name="label_parentsB">
-            <property name="text">
-             <string/>
-            </property>
-            <property name="textInteractionFlags">
-             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-            </property>
-           </widget>
-          </item>
-          <item row="1" column="0">
-           <widget class="QLabel" name="label_childrenA_3">
-            <property name="text">
-             <string>Children</string>
-            </property>
-           </widget>
-          </item>
-          <item row="5" column="0">
-           <widget class="QLabel" name="label_childrenA_13">
-            <property name="text">
-             <string>Pose</string>
-            </property>
-           </widget>
-          </item>
-          <item row="8" column="1">
-           <widget class="QLabel" name="label_calibB">
+          <item row="11" column="1">
+           <widget class="QLabel" name="label_gravityB">
            <property name="text">
             <string/>
            </property>
@@ -452,49 +452,8 @@
            </property>
           </widget>
          </item>
-          <item row="6" column="0">
-           <widget class="QLabel" name="label_childrenA_19">
-            <property name="text">
-             <string>Velocity</string>
-            </property>
-           </widget>
-          </item>
-          <item row="4" column="0">
-           <widget class="QLabel" name="label_childrenA_11">
-            <property name="text">
-             <string>Map ID</string>
-            </property>
-           </widget>
-          </item>
-          <item row="2" column="1">
-           <widget class="QLabel" name="label_weightB">
-            <property name="text">
-             <string/>
-            </property>
-            <property name="textInteractionFlags">
-             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-            </property>
-           </widget>
-          </item>
-          <item row="3" column="0">
-           <widget class="QLabel" name="label_childrenA_5">
-            <property name="text">
-             <string>Label</string>
-            </property>
-           </widget>
-          </item>
-          <item row="1" column="1">
-           <widget class="QLabel" name="label_childrenB">
-            <property name="text">
-             <string/>
-            </property>
-            <property name="textInteractionFlags">
-             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-            </property>
-           </widget>
-          </item>
          <item row="7" column="1">
-           <widget class="QLabel" name="label_stampB">
+           <widget class="QLabel" name="label_velB">
            <property name="text">
             <string/>
            </property>
@@ -503,8 +462,66 @@
            </property>
           </widget>
          </item>
+          <item row="13" column="1">
+           <widget class="QLabel" name="label_gpsB">
+            <property name="text">
+             <string/>
+            </property>
+            <property name="textInteractionFlags">
+             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+            </property>
+           </widget>
+          </item>
+          <item row="5" column="0">
+           <widget class="QLabel" name="label_childrenA_13">
+            <property name="text">
+             <string>Odometry Pose</string>
+            </property>
+           </widget>
+          </item>
+          <item row="10" column="0">
+           <widget class="QLabel" name="label_childrenA_23">
+            <property name="text">
+             <string>Scan</string>
+            </property>
+           </widget>
+          </item>
          <item row="10" column="1">
-           <widget class="QLabel" name="label_gravityB">
+           <widget class="QLabel" name="label_scanB">
+            <property name="text">
+             <string/>
+            </property>
+            <property name="textInteractionFlags">
+             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+            </property>
+           </widget>
+          </item>
+          <item row="9" column="0">
+           <widget class="QLabel" name="label_childrenA_15">
+            <property name="text">
+             <string>Calib</string>
+            </property>
+           </widget>
+          </item>
+          <item row="11" column="0">
+           <widget class="QLabel" name="label_childrenA_25">
+            <property name="text">
+             <string>Gravity</string>
+            </property>
+           </widget>
+          </item>
+          <item row="0" column="1">
+           <widget class="QLabel" name="label_parentsB">
+            <property name="text">
+             <string/>
+            </property>
+            <property name="textInteractionFlags">
+             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+            </property>
+           </widget>
+          </item>
+          <item row="12" column="1">
+           <widget class="QLabel" name="label_priorB">
            <property name="text">
             <string/>
            </property>
@@ -520,15 +537,8 @@
            </property>
           </widget>
          </item>
-          <item row="9" column="0">
-           <widget class="QLabel" name="label_childrenA_23">
-            <property name="text">
-             <string>Scan</string>
-            </property>
-           </widget>
-          </item>
-          <item row="6" column="1">
-           <widget class="QLabel" name="label_velB">
+          <item row="8" column="1">
+           <widget class="QLabel" name="label_stampB">
            <property name="text">
             <string/>
            </property>
@@ -537,7 +547,14 @@
            </property>
           </widget>
          </item>
-          <item row="13" column="1">
+          <item row="7" column="0">
+           <widget class="QLabel" name="label_childrenA_19">
+            <property name="text">
+             <string>Velocity</string>
+            </property>
+           </widget>
+          </item>
+          <item row="15" column="1">
           <widget class="QLabel" name="label_sensorsB">
            <property name="text">
             <string/>
@@ -547,14 +564,24 @@
            </property>
           </widget>
          </item>
-          <item row="12" column="0">
-           <widget class="QLabel" name="label_childrenA_26">
+          <item row="9" column="1">
+           <widget class="QLabel" name="label_calibB">
            <property name="text">
-             <string>Ground Truth</string>
+             <string/>
+            </property>
+            <property name="textInteractionFlags">
+             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
            </property>
           </widget>
          </item>
-          <item row="12" column="1">
+          <item row="2" column="0">
+           <widget class="QLabel" name="label_childrenA_9">
+            <property name="text">
+             <string>Weight</string>
+            </property>
+           </widget>
+          </item>
+          <item row="14" column="1">
           <widget class="QLabel" name="label_gtB">
            <property name="text">
             <string/>
@@ -564,6 +591,47 @@
            </property>
           </widget>
          </item>
+          <item row="13" column="0">
+           <widget class="QLabel" name="label_childrenA_17">
+            <property name="text">
+             <string>GPS</string>
+            </property>
+           </widget>
+          </item>
+          <item row="2" column="1">
+           <widget class="QLabel" name="label_weightB">
+            <property name="text">
+             <string/>
+            </property>
+            <property name="textInteractionFlags">
+             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+            </property>
+           </widget>
+          </item>
+          <item row="12" column="0">
+           <widget class="QLabel" name="label_childrenA_29">
+            <property name="text">
+             <string>Prior</string>
+            </property>
+           </widget>
+          </item>
+          <item row="6" column="0">
+           <widget class="QLabel" name="label_childrenA_31">
+            <property name="text">
+             <string>Optimized Pose</string>
+            </property>
+           </widget>
+          </item>
+          <item row="6" column="1">
+           <widget class="QLabel" name="label_optposeB">
+            <property name="text">
+             <string/>
+            </property>
+            <property name="textInteractionFlags">
+             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+            </property>
+           </widget>
+          </item>
         </layout>
        </widget>
       </widget>
@@ -320,7 +320,17 @@
        <iconset resource="../GuiLib.qrc">
         <normaloff>:/images/oakd.png</normaloff>:/images/oakd.png</iconset>
       </property>
-       <addaction name="actionDepthAI"/>
+       <addaction name="actionDepthAI_oakd"/>
+      </widget>
+      <widget class="QMenu" name="menuOAK_D_Lite">
+       <property name="title">
+        <string>OAK-D Lite</string>
+       </property>
+       <property name="icon">
+        <iconset resource="../GuiLib.qrc">
+         <normaloff>:/images/oakd_lite.png</normaloff>:/images/oakd_lite.png</iconset>
+       </property>
+       <addaction name="actionDepthAI_oakdlite"/>
      </widget>
      <addaction name="menuBumblebee2_2"/>
      <addaction name="menuZed_camera"/>
@@ -328,6 +338,7 @@
      <addaction name="menuRealSense_T265"/>
      <addaction name="menuMynt_Eye_S"/>
      <addaction name="menuOAK_D"/>
+      <addaction name="menuOAK_D_Lite"/>
     </widget>
     <addaction name="menuRGB_D_camera"/>
     <addaction name="menuStereo_camera"/>
@@ -1677,7 +1688,7 @@
    <string>Zed Open Capture</string>
   </property>
  </action>
-  <action name="actionDepthAI">
+  <action name="actionDepthAI_oakd">
   <property name="checkable">
    <bool>true</bool>
   </property>
@@ -1703,6 +1714,14 @@
    <string>Remove label...</string>
   </property>
  </action>
+  <action name="actionDepthAI_oakdlite">
+   <property name="checkable">
+    <bool>true</bool>
+   </property>
+   <property name="text">
+    <string>Depth AI</string>
+   </property>
+  </action>
 </widget>
 <customwidgets>
  <customwidget>
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
  <name>rtabmap</name>
-  <version>0.20.21</version>
+  <version>0.20.23</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>
@@ -45,11 +45,11 @@ SET(LIBRARIES
 	${YAML_CPP_LIBRARIES}
 )

-INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
+INCLUDE_DIRECTORIES(${INCLUDE_DIRS} yaml-cpp)

 ADD_EXECUTABLE(euroc_dataset main.cpp)
  
-TARGET_LINK_LIBRARIES(euroc_dataset ${LIBRARIES})
+TARGET_LINK_LIBRARIES(euroc_dataset ${LIBRARIES} yaml-cpp)

 SET_TARGET_PROPERTIES( euroc_dataset 
    PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-euroc_dataset)
@@ -1290,7 +1290,7 @@ int main(int argc, char * argv[])
 				}
 			}
 		}
-		if(!depth.empty())
+		if(!depth.empty() && (depth.type() == CV_16UC1 || depth.type() == CV_32FC1))
 		{
 			cameraDepths.insert(std::make_pair(iter->first, depth));
 		}
@@ -1483,6 +1483,46 @@ int main(int argc, char * argv[])
 				cameraModelsProj = cameraModels;
 			}

+			if(exportImages)
+			{
+				printf("Camera projection... projecting cloud to individual cameras (--images option)\n");
+				// projectCloudToCamera requires PCLPointCloud2
+				pcl::PCLPointCloud2::Ptr cloud2(new pcl::PCLPointCloud2);
+				if(!cloudToExport->empty())
+				{
+					pcl::toPCLPointCloud2(*cloudToExport, *cloud2);
+				}
+				else if(!cloudIToExport->empty())
+				{
+					pcl::toPCLPointCloud2(*cloudIToExport, *cloud2);
+				}
+
+				std::string dir = outputDirectory+"/"+baseName+"_depth_from_scan";
+				if(!UDirectory::exists(dir)) {
+					UDirectory::makeDir(dir);
+				}
+
+				for(std::map<int, std::vector<rtabmap::CameraModel> >::iterator iter=cameraModelsProj.begin();
+					iter!=cameraModelsProj.end();
+					++iter)
+				{
+					cv::Mat depth(iter->second.front().imageHeight(), iter->second.front().imageWidth()*iter->second.size(), CV_32FC1);
+					for(size_t i=0; i<iter->second.size(); ++i)
+					{
+						cv::Mat subDepth = util3d::projectCloudToCamera(
+								iter->second.at(i).imageSize(),
+								iter->second.at(i).K(),
+								cloud2,
+								robotPoses.at(iter->first) * iter->second.at(i).localTransform());
+						subDepth.copyTo(depth(cv::Range::all(), cv::Range(i*iter->second.front().imageWidth(), (i+1)*iter->second.front().imageWidth())));
+					}
+
+					depth = rtabmap::util2d::cvtDepthFromFloat(depth);
+					std::string outputPath=dir+"/"+(exportImagesId?uNumber2Str(iter->first):uFormat("%f",cameraStamps.at(iter->first)))+".png";
+					cv::imwrite(outputPath, depth);
+				}
+			}
+
 			cv::Mat projMask;
 			if(!cameraProjMask.empty())
 			{
@@ -1495,6 +1535,7 @@ int main(int argc, char * argv[])
 				}
 			}

+			printf("Camera projection... projecting cloud to all cameras\n");
 			pointToCamId.resize(!cloudToExport->empty()?cloudToExport->size():cloudIToExport->size());
 			std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > pointToPixel;
 			if(!cloudToExport->empty())
@@ -35,6 +35,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Graph.h>
 #include <rtabmap/core/Memory.h>
 #include <rtabmap/core/CameraThread.h>
+#include <rtabmap/core/Odometry.h>
+#include <rtabmap/core/OdometryInfo.h>
 #include <rtabmap/utilite/UFile.h>
 #include <rtabmap/utilite/UDirectory.h>
 #include <rtabmap/utilite/UTimer.h>
@@ -67,10 +69,20 @@ void showUsage()
 			"     -c \"path.ini\"   Configuration file, overwriting parameters read \n"
 			"                       from the database. If custom parameters are also set as \n"
 			"                       arguments, they overwrite those in config file and the database.\n"
+			"     -default    Input database's parameters are ignored, using default ones instead.\n"
+			"     -odom       Recompute odometry. See \"Odom/\" parameters with --params. If -skip option\n"
+			"                 is used, it will be applied to odometry frames, not rtabmap frames. Multi-session\n"
+			"                 cannot be detected in this mode (assuming the database contains continuous frames\n"
+			"                 of a single session).\n"
 			"     -start #    Start from this node ID.\n"
 			"     -stop #     Last node to process.\n"
+			"     -start_s #  Start from this map session ID.\n"
+			"     -stop_s #   Last map session to process.\n"
+			"     -a          Append mode: if Mem/IncrementalMemory is true, RTAB-Map is initialized with the first input database,\n"
+			"                 then next databases are reprocessed on top of the first one.\n"
 			"     -cam #      Camera index to stream. Ignored if a database doesn't contain multi-camera data.\n"
 			"     -nolandmark Don't republish landmarks contained in input database.\n"
+			"     -pub_loops  Republish loop closures contained in input database.\n"
 			"     -loc_null   On localization mode, reset localization pose to null and map correction to identity between sessions.\n"
 			"     -gt         When reprocessing a single database, load its original optimized graph, then \n"
 			"                 set it as ground truth for output database. If there was a ground truth in the input database, it will be ignored.\n"
@@ -224,11 +236,17 @@ int main(int argc, char * argv[])
 	bool assemble2dOctoMap = false;
 	bool assemble3dOctoMap = false;
 	bool useDatabaseRate = false;
+	bool useDefaultParameters = false;
+	bool recomputeOdometry = false;
 	int startId = 0;
 	int stopId = 0;
+	int startMapId = 0;
+	int stopMapId = -1;
+	bool appendMode = false;
 	int cameraIndex = -1;
 	int framesToSkip = 0;
 	bool ignoreLandmarks = false;
+	bool republishLoopClosures = false;
 	bool locNull = false;
 	bool originalGraphAsGT = false;
 	bool scanFromDepth = false;
@@ -265,6 +283,15 @@ int main(int argc, char * argv[])
 				showUsage();
 			}
 		}
+		else if(strcmp(argv[i], "-default") == 0 || strcmp(argv[i], "--default") == 0)
+		{
+			useDefaultParameters = true;
+			printf("Using default parameters.\n");
+		}
+		else if(strcmp(argv[i], "-odom") == 0 || strcmp(argv[i], "--odom") == 0)
+		{
+			recomputeOdometry = true;
+		}
 		else if (strcmp(argv[i], "-start") == 0 || strcmp(argv[i], "--start") == 0)
 		{
 			++i;
@@ -293,6 +320,39 @@ int main(int argc, char * argv[])
 				showUsage();
 			}
 		}
+		else if (strcmp(argv[i], "-start_s") == 0 || strcmp(argv[i], "--start_s") == 0)
+		{
+			++i;
+			if(i < argc - 2)
+			{
+				startMapId = atoi(argv[i]);
+				printf("Start at map session ID = %d.\n", startMapId);
+			}
+			else
+			{
+				printf("-start_s option requires a value\n");
+				showUsage();
+			}
+		}
+		else if (strcmp(argv[i], "-stop_s") == 0 || strcmp(argv[i], "--stop_s") == 0)
+		{
+			++i;
+			if(i < argc - 2)
+			{
+				stopMapId = atoi(argv[i]);
+				printf("Stop at map session ID = %d.\n", stopMapId);
+			}
+			else
+			{
+				printf("-stop option requires a value\n");
+				showUsage();
+			}
+		}
+		else if (strcmp(argv[i], "-a") == 0 || strcmp(argv[i], "--a") == 0)
+		{
+			appendMode = true;
+			printf("Append mode enabled (initialize with first database then reprocess next ones)\n");
+		}
 		else if (strcmp(argv[i], "-cam") == 0 || strcmp(argv[i], "--cam") == 0)
 		{
 			++i;
@@ -326,6 +386,11 @@ int main(int argc, char * argv[])
 			ignoreLandmarks = true;
 			printf("Ignoring landmarks from input database (-nolandmark option).\n");
 		}
+		else if(strcmp(argv[i], "-pub_loops") == 0 || strcmp(argv[i], "--pub_loops") == 0)
+		{
+			republishLoopClosures = true;
+			printf("Republish loop closures from input database (-pub_loops option).\n");
+		}
 		else if(strcmp(argv[i], "-loc_null") == 0 || strcmp(argv[i], "--loc_null") == 0)
 		{
 			locNull = true;
@@ -509,13 +574,19 @@ int main(int argc, char * argv[])
 		return -1;
 	}

-	ParametersMap parameters = dbDriver->getLastParameters();
-	std::string targetVersion = dbDriver->getDatabaseVersion();
-	parameters.insert(ParametersPair(Parameters::kDbTargetVersion(), targetVersion));
-	if(parameters.empty())
+	ParametersMap parameters;
+	std::string targetVersion;
+	if(!useDefaultParameters)
 	{
-		printf("WARNING: Failed getting parameters from database, reprocessing will be done with default parameters! Database version may be too old (%s).\n", dbDriver->getDatabaseVersion().c_str());
+		parameters = dbDriver->getLastParameters();
+		targetVersion = dbDriver->getDatabaseVersion();
+		parameters.insert(ParametersPair(Parameters::kDbTargetVersion(), targetVersion));
+		if(parameters.empty())
+		{
+			printf("WARNING: Failed getting parameters from database, reprocessing will be done with default parameters! Database version may be too old (%s).\n", dbDriver->getDatabaseVersion().c_str());
+		}
 	}
+
 	if(customParameters.size())
 	{
 		printf("Custom parameters:\n");
@@ -524,6 +595,8 @@ int main(int argc, char * argv[])
 			printf("  %s\t= %s\n", iter->first.c_str(), iter->second.c_str());
 		}
 	}
+
+	bool useOdomFeatures = Parameters::defaultMemUseOdomFeatures();
 	if((configParameters.find(Parameters::kKpDetectorStrategy())!=configParameters.end() ||
 		configParameters.find(Parameters::kVisFeatureType())!=configParameters.end() ||
 		customParameters.find(Parameters::kKpDetectorStrategy())!=customParameters.end() ||
@@ -531,7 +604,6 @@ int main(int argc, char * argv[])
 			configParameters.find(Parameters::kMemUseOdomFeatures())==configParameters.end() &&
 			customParameters.find(Parameters::kMemUseOdomFeatures())==customParameters.end())
 	{
-		bool useOdomFeatures = Parameters::defaultMemUseOdomFeatures();
 		Parameters::parse(parameters, Parameters::kMemUseOdomFeatures(), useOdomFeatures);
 		if(useOdomFeatures)
 		{
@@ -543,6 +615,56 @@ int main(int argc, char * argv[])
 					Parameters::kMemUseOdomFeatures().c_str(),
 					Parameters::kMemUseOdomFeatures().c_str());
 			uInsert(parameters, ParametersPair(Parameters::kMemUseOdomFeatures(), "false"));
+			useOdomFeatures = false;
+		}
+	}
+
+	if(useOdomFeatures && databases.size() > 1 &&
+		configParameters.find(Parameters::kMemUseOdomFeatures())==configParameters.end() &&
+		customParameters.find(Parameters::kMemUseOdomFeatures())==customParameters.end())
+	{
+		printf("[Warning] Parameter %s is set to false for convenience as "
+				"there are more than one input database (which could "
+				"contain different features). Set %s "
+				"explicitly to suppress this warning.\n",
+				Parameters::kMemUseOdomFeatures().c_str(),
+				Parameters::kMemUseOdomFeatures().c_str());
+		useOdomFeatures = false;
+	}
+
+	if(republishLoopClosures)
+	{
+		if(databases.size() > 1)
+		{
+			printf("[Warning] \"pub_loops\" option cannot be used with multiple databases input. "
+					"Disabling \"pub_loops\" to avoid mismatched loop closue ids.\n");
+			republishLoopClosures = false;
+		}
+		else
+		{
+			bool generateIds = Parameters::defaultMemGenerateIds();
+			Parameters::parse(parameters, Parameters::kMemGenerateIds(), generateIds);
+			Parameters::parse(configParameters, Parameters::kMemGenerateIds(), generateIds);
+			Parameters::parse(customParameters, Parameters::kMemGenerateIds(), generateIds);
+			if(generateIds)
+			{
+				if(configParameters.find(Parameters::kMemGenerateIds())!=configParameters.end() ||
+				   customParameters.find(Parameters::kMemGenerateIds())!=customParameters.end())
+				{
+					printf("[Warning] \"pub_loops\" option is used but parameter %s is set to true in custom arguments. "
+							"Disabling \"pub_loops\" to avoid mismatched loop closure ids.\n",
+							Parameters::kMemGenerateIds().c_str());
+					republishLoopClosures = false;
+				}
+				else
+				{
+					printf("[Warning] \"pub_loops\" option is used but parameter %s is true in the opened database. "
+							"Setting parameter %s to false for convenience so that republished loop closure ids match.\n",
+							Parameters::kMemGenerateIds().c_str(),
+							Parameters::kMemGenerateIds().c_str());
+					uInsert(parameters, ParametersPair(Parameters::kMemGenerateIds(), "false"));
+				}
+			}
 		}
 	}
 	uInsert(parameters, configParameters);
@@ -564,7 +686,7 @@ int main(int argc, char * argv[])
 		delete dbDriver;
 		return -1;
 	}
-	if(!(!incrementalMemory && databases.size() > 1))
+	if(!((!incrementalMemory || appendMode) && databases.size() > 1))
 	{
 		totalIds = ids.size();
 	}
@@ -608,10 +730,13 @@ int main(int argc, char * argv[])
 	uInsert(parameters, ParametersPair(Parameters::kRtabmapWorkingDirectory(), workingDirectory));
 	uInsert(parameters, ParametersPair(Parameters::kRtabmapPublishStats(), "true")); // to log status below

-	if(!incrementalMemory && databases.size() > 1)
+	if((!incrementalMemory || appendMode ) && databases.size() > 1)
 	{
 		UFile::copy(databases.front(), outputDatabasePath);
-		printf("Parameter \"%s\" is set to false, initializing RTAB-Map with \"%s\" for localization...\n", Parameters::kMemIncrementalMemory().c_str(), databases.front().c_str());
+		if(!incrementalMemory)
+		{
+			printf("Parameter \"%s\" is set to false, initializing RTAB-Map with \"%s\" for localization...\n", Parameters::kMemIncrementalMemory().c_str(), databases.front().c_str());
+		}
 		databases.pop_front();
 		inputDatabasePath = uJoin(databases, ";");
 	}
@@ -627,7 +752,22 @@ int main(int argc, char * argv[])
 	bool rgbdEnabled = Parameters::defaultRGBDEnabled();
 	Parameters::parse(parameters, Parameters::kRGBDEnabled(), rgbdEnabled);
 	bool odometryIgnored = !rgbdEnabled;
-	DBReader * dbReader = new DBReader(inputDatabasePath, useDatabaseRate?-1:0, odometryIgnored, false, false, startId, cameraIndex, stopId, !intermediateNodes, ignoreLandmarks);
+
+	DBReader * dbReader = new DBReader(
+			inputDatabasePath,
+			useDatabaseRate?-1:0,
+			odometryIgnored,
+			false,
+			false,
+			startId,
+			cameraIndex,
+			stopId,
+			!intermediateNodes,
+			ignoreLandmarks,
+			!useOdomFeatures,
+			startMapId,
+			stopMapId);
+
 	dbReader->init();

 	OccupancyGrid grid(parameters);
@@ -641,6 +781,28 @@ int main(int argc, char * argv[])
 	Parameters::parse(parameters, Parameters::kRGBDLinearUpdate(), linearUpdate);
 	Parameters::parse(parameters, Parameters::kRGBDAngularUpdate(), angularUpdate);

+	Odometry * odometry = 0;
+	float rtabmapUpdateRate = Parameters::defaultRtabmapDetectionRate();
+	double lastUpdateStamp = 0;
+	if(recomputeOdometry)
+	{
+		if(odometryIgnored)
+		{
+			printf("odom option is set but %s parameter is false, odometry won't be recomputed...\n", Parameters::kRGBDEnabled().c_str());
+			recomputeOdometry = false;
+		}
+		else
+		{
+			printf("Odometry will be recomputed (odom option is set)\n");
+			Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), rtabmapUpdateRate);
+			if(rtabmapUpdateRate!=0)
+			{
+				rtabmapUpdateRate = 1.0f/rtabmapUpdateRate;
+			}
+			odometry = Odometry::create(parameters);
+		}
+	}
+
 	printf("Reprocessing data of \"%s\"...\n", inputDatabasePath.c_str());
 	std::map<std::string, float> globalMapStats;
 	int processed = 0;
@@ -657,6 +819,44 @@ int main(int argc, char * argv[])
 	bool inMotion = true;
 	while(data.isValid() && g_loopForever)
 	{
+		if(recomputeOdometry)
+		{
+			OdometryInfo odomInfo;
+			Transform pose = odometry->process(data, &odomInfo);
+			printf("Processed %d/%d frames (visual=%d/%d lidar=%f lost=%s)... odometry = %dms\n",
+					processed+1,
+					totalIds,
+					odomInfo.reg.inliers,
+					odomInfo.reg.matches,
+					odomInfo.reg.icpInliersRatio,
+					odomInfo.lost?"true":"false",
+					int(odomInfo.timeEstimation * 1000));
+			if(lastUpdateStamp > 0.0 && data.stamp() < lastUpdateStamp + rtabmapUpdateRate)
+			{
+				if(framesToSkip>0)
+				{
+					int skippedFrames = framesToSkip;
+					while(skippedFrames-- > 0)
+					{
+						++processed;
+						data = dbReader->takeImage();
+					}
+				}
+
+				data = dbReader->takeImage(&info);
+				if(scanFromDepth)
+				{
+					data.setLaserScan(LaserScan());
+				}
+				camThread.postUpdate(&data, &info);
+				++processed;
+				continue;
+			}
+			info.odomPose = pose;
+			info.odomCovariance = odomInfo.reg.covariance;
+			lastUpdateStamp = data.stamp();
+		}
+
 		UTimer iterationTime;
 		std::string status;
 		if(!odometryIgnored && info.odomPose.isNull())
@@ -692,84 +892,109 @@ int main(int argc, char * argv[])
 				printf("Failed processing node %d.\n", data.id());
 				globalMapStats.clear();
 			}
-			else if(assemble2dMap || assemble3dMap || assemble2dOctoMap || assemble3dOctoMap)
+			else
 			{
-				globalMapStats.clear();
-				double timeRtabmap = t.ticks();
-				double timeUpdateInit = 0.0;
-				double timeUpdateGrid = 0.0;
-#ifdef RTABMAP_OCTOMAP
-				double timeUpdateOctoMap = 0.0;
-#endif
-				const rtabmap::Statistics & stats = rtabmap.getStatistics();
-				if(stats.poses().size() && stats.getLastSignatureData().id())
+				if(republishLoopClosures && dbReader->driver())
 				{
-					int id = stats.poses().rbegin()->first;
-					if(id == stats.getLastSignatureData().id() &&
-					   stats.getLastSignatureData().sensorData().gridCellSize() > 0.0f)
+					std::multimap<int, Link> links;
+					dbReader->driver()->loadLinks(data.id(), links);
+					for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
 					{
-						bool updateGridMap = false;
-						bool updateOctoMap = false;
-						if((assemble2dMap || assemble3dMap) && grid.addedNodes().find(id) == grid.addedNodes().end())
+						if((iter->second.type() == Link::kGlobalClosure ||
+							iter->second.type() == Link::kLocalSpaceClosure ||
+							iter->second.type() == Link::kLocalTimeClosure ||
+							iter->second.type() == Link::kUserClosure) &&
+							iter->second.to() < data.id())
 						{
-							updateGridMap = true;
-						}
-#ifdef RTABMAP_OCTOMAP
-						if((assemble2dOctoMap || assemble3dOctoMap) && octomap.addedNodes().find(id) == octomap.addedNodes().end())
-						{
-							updateOctoMap = true;
-						}
-#endif
-						if(updateGridMap || updateOctoMap)
-						{
-							cv::Mat ground, obstacles, empty;
-							stats.getLastSignatureData().sensorData().uncompressDataConst(0, 0, 0, 0, &ground, &obstacles, &empty);
-
-							timeUpdateInit = t.ticks();
-
-							if(updateGridMap)
+							if(!iter->second.transform().isNull() &&
+								rtabmap.getMemory()->getWorkingMem().find(iter->second.to()) != rtabmap.getMemory()->getWorkingMem().end() &&
+								rtabmap.addLink(iter->second))
 							{
-								grid.addToCache(id, ground, obstacles, empty);
-								grid.update(stats.poses());
-								timeUpdateGrid = t.ticks() + timeUpdateInit;
+								printf("Added link %d->%d from input database.\n", iter->second.from(), iter->second.to());
 							}
-#ifdef RTABMAP_OCTOMAP
-							if(updateOctoMap)
-							{
-								const cv::Point3f & viewpoint = stats.getLastSignatureData().sensorData().gridViewPoint();
-								octomap.addToCache(id, ground, obstacles, empty, viewpoint);
-								octomap.update(stats.poses());
-								timeUpdateOctoMap = t.ticks() + timeUpdateInit;
-							}
-#endif
 						}
 					}
 				}

-				globalMapStats.insert(std::make_pair(std::string("GlobalGrid/GridUpdate/ms"), timeUpdateGrid*1000.0f));
+				if(assemble2dMap || assemble3dMap || assemble2dOctoMap || assemble3dOctoMap)
+				{
+					globalMapStats.clear();
+					double timeRtabmap = t.ticks();
+					double timeUpdateInit = 0.0;
+					double timeUpdateGrid = 0.0;
 #ifdef RTABMAP_OCTOMAP
-				//Simulate publishing
-				double timePub2dOctoMap = 0.0;
-				double timePub3dOctoMap = 0.0;
-				if(assemble2dOctoMap)
-				{
-					float xMin, yMin, size;
-					octomap.createProjectionMap(xMin, yMin, size);
-					timePub2dOctoMap = t.ticks();
-				}
-				if(assemble3dOctoMap)
-				{
-					octomap.createCloud();
-					timePub3dOctoMap = t.ticks();
-				}
-
-				globalMapStats.insert(std::make_pair(std::string("GlobalGrid/OctoMapUpdate/ms"), timeUpdateOctoMap*1000.0f));
-				globalMapStats.insert(std::make_pair(std::string("GlobalGrid/OctoMapProjection/ms"), timePub2dOctoMap*1000.0f));
-				globalMapStats.insert(std::make_pair(std::string("GlobalGrid/OctomapToCloud/ms"), timePub3dOctoMap*1000.0f));
-				globalMapStats.insert(std::make_pair(std::string("GlobalGrid/TotalWithRtabmap/ms"), (timeUpdateGrid+timeUpdateOctoMap+timePub2dOctoMap+timePub3dOctoMap+timeRtabmap)*1000.0f));
-#else
-				globalMapStats.insert(std::make_pair(std::string("GlobalGrid/TotalWithRtabmap/ms"), (timeUpdateGrid+timeRtabmap)*1000.0f));
+					double timeUpdateOctoMap = 0.0;
 #endif
+					const rtabmap::Statistics & stats = rtabmap.getStatistics();
+					if(stats.poses().size() && stats.getLastSignatureData().id())
+					{
+						int id = stats.poses().rbegin()->first;
+						if(id == stats.getLastSignatureData().id() &&
+						   stats.getLastSignatureData().sensorData().gridCellSize() > 0.0f)
+						{
+							bool updateGridMap = false;
+							bool updateOctoMap = false;
+							if((assemble2dMap || assemble3dMap) && grid.addedNodes().find(id) == grid.addedNodes().end())
+							{
+								updateGridMap = true;
+							}
+#ifdef RTABMAP_OCTOMAP
+							if((assemble2dOctoMap || assemble3dOctoMap) && octomap.addedNodes().find(id) == octomap.addedNodes().end())
+							{
+								updateOctoMap = true;
+							}
+#endif
+							if(updateGridMap || updateOctoMap)
+							{
+								cv::Mat ground, obstacles, empty;
+								stats.getLastSignatureData().sensorData().uncompressDataConst(0, 0, 0, 0, &ground, &obstacles, &empty);
+
+								timeUpdateInit = t.ticks();
+
+								if(updateGridMap)
+								{
+									grid.addToCache(id, ground, obstacles, empty);
+									grid.update(stats.poses());
+									timeUpdateGrid = t.ticks() + timeUpdateInit;
+								}
+#ifdef RTABMAP_OCTOMAP
+								if(updateOctoMap)
+								{
+									const cv::Point3f & viewpoint = stats.getLastSignatureData().sensorData().gridViewPoint();
+									octomap.addToCache(id, ground, obstacles, empty, viewpoint);
+									octomap.update(stats.poses());
+									timeUpdateOctoMap = t.ticks() + timeUpdateInit;
+								}
+#endif
+							}
+						}
+					}
+
+					globalMapStats.insert(std::make_pair(std::string("GlobalGrid/GridUpdate/ms"), timeUpdateGrid*1000.0f));
+#ifdef RTABMAP_OCTOMAP
+					//Simulate publishing
+					double timePub2dOctoMap = 0.0;
+					double timePub3dOctoMap = 0.0;
+					if(assemble2dOctoMap)
+					{
+						float xMin, yMin, size;
+						octomap.createProjectionMap(xMin, yMin, size);
+						timePub2dOctoMap = t.ticks();
+					}
+					if(assemble3dOctoMap)
+					{
+						octomap.createCloud();
+						timePub3dOctoMap = t.ticks();
+					}
+
+					globalMapStats.insert(std::make_pair(std::string("GlobalGrid/OctoMapUpdate/ms"), timeUpdateOctoMap*1000.0f));
+					globalMapStats.insert(std::make_pair(std::string("GlobalGrid/OctoMapProjection/ms"), timePub2dOctoMap*1000.0f));
+					globalMapStats.insert(std::make_pair(std::string("GlobalGrid/OctomapToCloud/ms"), timePub3dOctoMap*1000.0f));
+					globalMapStats.insert(std::make_pair(std::string("GlobalGrid/TotalWithRtabmap/ms"), (timeUpdateGrid+timeUpdateOctoMap+timePub2dOctoMap+timePub3dOctoMap+timeRtabmap)*1000.0f));
+#else
+					globalMapStats.insert(std::make_pair(std::string("GlobalGrid/TotalWithRtabmap/ms"), (timeUpdateGrid+timeRtabmap)*1000.0f));
+#endif
+				}
 			}
 		}

@@ -845,11 +1070,12 @@ int main(int argc, char * argv[])

 		Transform odomPose = info.odomPose;

-		if(framesToSkip>0)
+		if(framesToSkip>0 && !recomputeOdometry)
 		{
 			int skippedFrames = framesToSkip;
 			while(skippedFrames-- > 0)
 			{
+				processed++;
 				data = dbReader->takeImage(&info);
 				if(!odometryIgnored && !info.odomCovariance.empty() && info.odomCovariance.at<double>(0,0)>=9999)
 				{
@@ -917,6 +1143,8 @@ int main(int argc, char * argv[])
 	rtabmap.close(true);
 	printf("Closing database \"%s\"... done!\n", outputDatabasePath.c_str());

+	delete odometry;
+
 	if(assemble2dMap)
 	{
 		std::string outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_map.pgm";
@@ -109,13 +109,13 @@ std::string UTILITE_EXP uNumber2Str(int number);
 * @param number the number to convert in a string
 * @return the string
 */
-std::string UTILITE_EXP uNumber2Str(float number);
+std::string UTILITE_EXP uNumber2Str(float number, int precision=6, bool fixed = false);
 /**
 * Convert a number (double) to a string.
 * @param number the number to convert in a string
 * @return the string
 */
-std::string UTILITE_EXP uNumber2Str(double number);
+std::string UTILITE_EXP uNumber2Str(double number, int precision=6, bool fixed = false);

 /**
 * Convert a string to an integer.
@@ -41,6 +41,14 @@ public:
 		kFloat,
 		kDouble,
 		kStr,
+		kCharArray,
+		kUCharArray,
+		kShortArray,
+		kUShortArray,
+		kIntArray,
+		kUIntArray,
+		kFloatArray,
+		kDoubleArray,
 		kUndef
 		};
 public:
@@ -56,6 +64,14 @@ public:
 	UVariant(const double & value);
 	UVariant(const char * value);
 	UVariant(const std::string & value);
+	UVariant(const std::vector<char> & value);
+	UVariant(const std::vector<unsigned char> & value);
+	UVariant(const std::vector<short> & value);
+	UVariant(const std::vector<unsigned short> & value);
+	UVariant(const std::vector<int> & value);
+	UVariant(const std::vector<unsigned int> & value);
+	UVariant(const std::vector<float> & value);
+	UVariant(const std::vector<double> & value);

 	Type type() const {return type_;}

@@ -70,6 +86,14 @@ public:
 	bool isFloat() const {return type_ == kFloat;}
 	bool isDouble() const {return type_ == kDouble;}
 	bool isStr() const {return type_ == kStr;}
+	bool isCharArray() const {return type_ == kCharArray;}
+	bool isUCharArray() const {return type_ == kUCharArray;}
+	bool isShortArray() const {return type_ == kShortArray;}
+	bool isUShortArray() const {return type_ == kUShortArray;}
+	bool isIntArray() const {return type_ == kIntArray;}
+	bool isUIntArray() const {return type_ == kUIntArray;}
+	bool isFloatArray() const {return type_ == kFloatArray;}
+	bool isDoubleArray() const {return type_ == kDoubleArray;}

 	bool toBool() const;
 	char toChar(bool * ok = 0) const;
@@ -81,6 +105,14 @@ public:
 	float toFloat(bool * ok = 0) const;
 	double toDouble(bool * ok = 0) const;
 	std::string toStr(bool * ok = 0) const;
+	std::vector<char> toCharArray(bool * ok = 0) const;
+	std::vector<unsigned char> toUCharArray(bool * ok = 0) const;
+	std::vector<short> toShortArray(bool * ok = 0) const;
+	std::vector<unsigned short> toUShortArray(bool * ok = 0) const;
+	std::vector<int> toIntArray(bool * ok = 0) const;
+	std::vector<unsigned int> toUIntArray(bool * ok = 0) const;
+	std::vector<float> toFloatArray(bool * ok = 0) const;
+	std::vector<double> toDoubleArray(bool * ok = 0) const;

 	virtual ~UVariant() {}

@@ -24,6 +24,7 @@
 #include <string.h>
 #include <stdio.h>
 #include <stdlib.h>
+#include <iomanip>

 #ifdef _WIN32
 #include <windows.h>
@@ -101,16 +102,22 @@ std::string uNumber2Str(int number)
 	return s.str();
 }

-std::string uNumber2Str(float number)
+std::string uNumber2Str(float number, int precision, bool fixed)
 {
 	std::stringstream s;
+	s << std::setprecision(precision);
+	if(fixed)
+		s << std::fixed;
 	s << number;
 	return s.str();
 }

-std::string uNumber2Str(double number)
+std::string uNumber2Str(double number, int precision, bool fixed)
 {
 	std::stringstream s;
+	s << std::setprecision(precision);
+	if(fixed)
+		s << std::fixed;
 	s << number;
 	return s.str();
 }
@@ -93,6 +93,54 @@ UVariant::UVariant(const std::string & value) :
 {
 	memcpy(data_.data(), value.data(), value.size()+1);
 }
+UVariant::UVariant(const std::vector<char> & value) :
+	type_(kCharArray),
+	data_(sizeof(char)*value.size())
+{
+	memcpy(data_.data(), value.data(), sizeof(char)*value.size());
+}
+UVariant::UVariant(const std::vector<unsigned char> & value) :
+	type_(kUCharArray),
+	data_(sizeof(unsigned char)*value.size())
+{
+	memcpy(data_.data(), value.data(), sizeof(unsigned char)*value.size());
+}
+UVariant::UVariant(const std::vector<short> & value) :
+	type_(kShortArray),
+	data_(sizeof(short)*value.size())
+{
+	memcpy(data_.data(), value.data(), sizeof(short)*value.size());
+}
+UVariant::UVariant(const std::vector<unsigned short> & value) :
+	type_(kUShortArray),
+	data_(sizeof(unsigned short)*value.size())
+{
+	memcpy(data_.data(), value.data(), sizeof(unsigned short)*value.size());
+}
+UVariant::UVariant(const std::vector<int> & value) :
+	type_(kIntArray),
+	data_(sizeof(int)*value.size())
+{
+	memcpy(data_.data(), value.data(), sizeof(int)*value.size());
+}
+UVariant::UVariant(const std::vector<unsigned int> & value) :
+	type_(kUIntArray),
+	data_(sizeof(unsigned int)*value.size())
+{
+	memcpy(data_.data(), value.data(), sizeof(unsigned int)*value.size());
+}
+UVariant::UVariant(const std::vector<float> & value) :
+	type_(kFloatArray),
+	data_(sizeof(float)*value.size())
+{
+	memcpy(data_.data(), value.data(), sizeof(float)*value.size());
+}
+UVariant::UVariant(const std::vector<double> & value) :
+	type_(kDoubleArray),
+	data_(sizeof(double)*value.size())
+{
+	memcpy(data_.data(), value.data(), sizeof(double)*value.size());
+}

 bool UVariant::toBool() const
 {
@@ -641,3 +689,182 @@ std::string UVariant::toStr(bool * ok) const
 	}
 	return v;
 }
+
+std::vector<char> UVariant::toCharArray(bool * ok) const
+{
+	if(ok)
+	{
+		*ok = false;
+	}
+
+	std::vector<char> v;
+	if(type_ == kCharArray)
+	{
+		if(ok)
+		{
+			*ok = true;
+		}
+		if(data_.size())
+		{
+			v.resize(data_.size() / sizeof(char));
+			memcpy(v.data(), data_.data(), data_.size());
+		}
+	}
+	return v;
+}
+
+std::vector<unsigned char> UVariant::toUCharArray(bool * ok) const
+{
+	if(ok)
+	{
+		*ok = false;
+	}
+
+	std::vector<unsigned char> v;
+	if(type_ == kUCharArray)
+	{
+		if(ok)
+		{
+			*ok = true;
+		}
+		if(data_.size())
+		{
+			v.resize(data_.size() / sizeof(unsigned char));
+			memcpy(v.data(), data_.data(), data_.size());
+		}
+	}
+	return v;
+}
+
+std::vector<short> UVariant::toShortArray(bool * ok) const
+{
+	if(ok)
+	{
+		*ok = false;
+	}
+
+	std::vector<short> v;
+	if(type_ == kShortArray)
+	{
+		if(ok)
+		{
+			*ok = true;
+		}
+		if(data_.size())
+		{
+			v.resize(data_.size() / sizeof(short));
+			memcpy(v.data(), data_.data(), data_.size());
+		}
+	}
+	return v;
+}
+std::vector<unsigned short> UVariant::toUShortArray(bool * ok) const
+{
+	if(ok)
+	{
+		*ok = false;
+	}
+
+	std::vector<unsigned short> v;
+	if(type_ == kUShortArray)
+	{
+		if(ok)
+		{
+			*ok = true;
+		}
+		if(data_.size())
+		{
+			v.resize(data_.size() / sizeof(unsigned short));
+			memcpy(v.data(), data_.data(), data_.size());
+		}
+	}
+	return v;
+}
+std::vector<int> UVariant::toIntArray(bool * ok) const
+{
+	if(ok)
+	{
+		*ok = false;
+	}
+
+	std::vector<int> v;
+	if(type_ == kIntArray)
+	{
+		if(ok)
+		{
+			*ok = true;
+		}
+		if(data_.size())
+		{
+			v.resize(data_.size() / sizeof(int));
+			memcpy(v.data(), data_.data(), data_.size());
+		}
+	}
+	return v;
+}
+std::vector<unsigned int> UVariant::toUIntArray(bool * ok) const
+{
+	if(ok)
+	{
+		*ok = false;
+	}
+
+	std::vector<unsigned int> v;
+	if(type_ == kUIntArray)
+	{
+		if(ok)
+		{
+			*ok = true;
+		}
+		if(data_.size())
+		{
+			v.resize(data_.size() / sizeof(unsigned int));
+			memcpy(v.data(), data_.data(), data_.size());
+		}
+	}
+	return v;
+}
+std::vector<float> UVariant::toFloatArray(bool * ok) const
+{
+	if(ok)
+	{
+		*ok = false;
+	}
+
+	std::vector<float> v;
+	if(type_ == kFloatArray)
+	{
+		if(ok)
+		{
+			*ok = true;
+		}
+		if(data_.size())
+		{
+			v.resize(data_.size() / sizeof(float));
+			memcpy(v.data(), data_.data(), data_.size());
+		}
+	}
+	return v;
+}
+std::vector<double> UVariant::toDoubleArray(bool * ok) const
+{
+	if(ok)
+	{
+		*ok = false;
+	}
+
+	std::vector<double> v;
+	if(type_ == kDoubleArray)
+	{
+		if(ok)
+		{
+			*ok = true;
+		}
+		if(data_.size())
+		{
+			v.resize(data_.size() / sizeof(double));
+			memcpy(v.data(), data_.data(), data_.size());
+		}
+	}
+	return v;
+}
Author	SHA1	Message	Date
matlabbe	95e6a9f039	bump 0.20.23	2023-01-21 16:09:41 -08:00
matlabbe	b5eef4b86a	DBViewer: ignore marker constraints in refine, add marker priors when optimizing the graph	2023-01-21 15:52:01 -08:00
reito	40ab33031b	fix bigobj under MSVC, fix yaml-cpp link. (#957 )	2023-01-21 15:50:02 -08:00
reito	0e908206d0	OpenCV 4.7 moved aruco back to opencv repo, and makes some breaking changes. (#955 )	2023-01-16 16:41:48 -08:00
matlabbe	6b9f7de782	Fixed a -pedantic warning	2023-01-16 14:14:25 -08:00
matlabbe	22fede9335	Reprocess: added --odom option to recompute odometry.	2023-01-15 15:07:24 -08:00
matlabbe	22917cc1c3	VTK9: Fixed vtkGUISupportQt -> VTK::GUISupportQt	2023-01-14 20:12:33 -08:00
matlabbe	98bf3fb184	workflows: removed galactic build (EOF)	2023-01-08 16:59:24 -08:00
matlabbe	8d301afe6c	reprocess: added --default option	2023-01-08 16:29:44 -08:00
matlabbe	e3ceb8a572	VINS-Fusion: Fixed error wrongly logged when only imu is processed by vins.	2023-01-08 15:22:46 -08:00
matlabbe	f12cc83fc2	RegVis: using toSignature camera model for guess reprojection on optical flow mode (#947 )	2022-12-20 20:51:56 -08:00
matlabbe	4a6a765cd1	Fixed #944	2022-12-14 19:42:56 -08:00
matlabbe	a4761ad397	Fixed #940	2022-12-11 01:12:21 -08:00
matlabbe	aa380de2a7	Update install_deps.sh Removed vtk default iOS simulator build	2022-12-07 17:59:11 -08:00
matlabbe	467dc738c7	Fixed deprecated warning not set to right function	2022-12-05 20:32:53 -08:00
matlabbe	64cb7fb6fe	export: when --images and --cam_projection are used, we export depth image from lidar projection to RGB frames	2022-12-04 17:23:33 -08:00
matlabbe	8a8c360f89	fixed compilation warning	2022-11-27 16:20:56 -08:00
matlabbe	83693ed1b9	detectMoreLoopClosures: fixed guess not set for lidar-only registration (https://github.com/introlab/rtabmap_ros/issues/727 )	2022-11-26 20:24:43 -08:00
matlabbe	860c14e0ec	rtabmap: fixed export poses in format 10 and 11 (#883 )	2022-11-26 12:42:04 -08:00
matlabbe	85bc1ee27f	GTSAM: return empty optimized poses in case of exception or error. g2o: don't assert on marginal not computed, log error and reutn empty optimized poses.	2022-11-22 12:26:02 -08:00
matlabbe	05676d1577	ios: Moved install_deps.sh in upper directory to easy cleanup Libraries. Fixed uNumber2Str(double) link error.	2022-11-19 12:41:01 -08:00
matlabbe	6d713c2ccb	uNumberToStr(double): added "fixed" param (default false) to fix ros param double/int conversion issue.	2022-11-11 16:20:59 -08:00
matlabbe	16e3314e64	💄	2022-11-11 14:45:33 -08:00
matlabbe	d59a306ae3	Export: fixed depth image wrongly set for stereo data	2022-11-10 17:18:57 -08:00
matlabbe	bdfab4e0be	reprocess: added "-a" option (append mode)	2022-11-02 15:06:13 -07:00
matlabbe	c9d50ef445	Export GPS poses: set 8 decimals (1 mm accuracy) https://github.com/introlab/rtabmap_ros/issues/833	2022-11-02 11:35:54 -07:00
matlabbe	2889cf8048	GUI: fixed latest data requested on localization mode	2022-10-31 13:21:13 -07:00
matlabbe	def876983d	docker/android: fixed docker not detecting compilation failure. ios: added exit flag for install_deps.sh script	2022-10-30 15:07:55 -07:00
matlabbe	69214b927a	fixed android build	2022-10-30 14:47:34 -07:00
matlabbe	67b2cbe5ca	docker: set dummy build-args for build not required them	2022-10-30 14:39:33 -07:00
matlabbe	86f66cc8b8	Added missing signature info in republished data	2022-10-30 13:59:08 -07:00
matlabbe	d70924a3a4	bump package version 0.20.22	2022-10-28 12:30:01 -07:00
matlabbe	1297714271	Bump version 0.20.22. Refactored RtabmapThread commands handling. MainWindow/RtabmapThread/Statistics/UVariant: added new option to republish missing data on GUI side. MainWindow: fixed 1 sec lag when waypoints are used. Parameters: added Rtabmap/MaxRepublished (moved from rtabmap_ros). OccupancyGrid: fixed ground cells ignored if there are empty cells, also don't add pose in addedNodes if corresponding node was not in cache.	2022-10-28 12:25:48 -07:00
matlabbe	8e30c1c812	GraphView: added user link on inter/intra loop closure color scheme option	2022-10-24 17:45:16 -07:00
matlabbe	df888962f4	depthai: fixed app freezing if oakd device (oak-d lite) doesn't have imu and imu is requested.	2022-10-24 11:13:27 -07:00
matlabbe	a8cdf1f940	reprocess: disable Mem/UseOdomFeatures by default when there are more than 1 input database	2022-10-24 10:34:37 -07:00
matlabbe	769ace86ce	Added oak-d lite menu option (depthai: imu is now optionnal)	2022-10-23 18:15:23 -07:00
matlabbe	97a150b180	DbViewer: added option to regenerate optimized 2d map from OctoMap (https://github.com/introlab/rtabmap_ros/issues/213 )	2022-10-23 12:16:35 -07:00
matlabbe	6e8f43916c	floam: republish input scan with features	2022-10-16 21:31:43 -07:00
matlabbe	5b0047efad	docker: fixed AliceVision build (bionic/focal)	2022-10-15 14:06:20 -07:00
matlabbe	fdb78d33b0	DBViewer: added optimzed and prior poses in node details. Reprocess: add start_s, stop_s and pub_loops options.	2022-10-12 13:05:01 -07:00
matlabbe	2cbd43dfb6	Update cmake-ros.yml Added ros2 distros	2022-10-09 12:47:03 -07:00
matlabbe	1f06386f37	Update README.md	2022-10-09 12:22:09 -07:00
matlabbe	1e9b090624	Gui: Added re-localization colors on odom cache graph. Added frame labels (map, odom).	2022-10-04 16:27:33 -07:00
matlabbe	7e3e5ec50b	DBViewer: fixed matching lines when two images don't have the same number of cameras. Fixed nodes with w=-9 can be used to add constraint (in case localization data is saved in database). Add constraint: compute transform in order of the sliders.	2022-10-04 14:23:12 -07:00