Fixed #940

Update install_deps.sh
Removed vtk default iOS simulator build
2026-09-08 12:30:20 +08:00 · 2022-12-11 01:12:21 -08:00 · 2022-12-07 17:59:11 -08:00 · 2022-12-05 20:32:53 -08:00 · 2022-12-04 17:23:33 -08:00 · 2022-11-27 16:20:56 -08:00
51 changed files with 2418 additions and 1339 deletions
@@ -18,50 +18,49 @@ 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, galactic, 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: 'galactic'
+          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}}
-      
@@ -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 22)
 SET(RTABMAP_VERSION
  ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
  
@@ -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,56 @@ 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="4">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>Galactic</td>
+            <td><a href="http://build.ros2.org/job/Gbin_uF64__rtabmap__ubuntu_focal_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Gbin_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) {_signaturesData.insert(std::make_pair(data.id(), 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;
@@ -438,6 +465,14 @@ SensorData DBReader::getNextData(CameraInfo * info)
 					_previousInfMatrix = infMatrix;
 				}
 				else
+				{
+					if(_previousMapId != s->mapId())
+					{
+						// first node, set high variance to make rtabmap trigger a new map
+						infMatrix /= 9999.0;
+						UDEBUG("First node of map %d, variance set to 9999", s->mapId());
+					}
+					else
 					{
 						// if localization data saved in database, covariance will be set in a prior link
 						_dbDriver->loadLinks(*_currentId, links, Link::kPosePrior);
@@ -447,12 +482,6 @@ SensorData DBReader::getNextData(CameraInfo * info)
 							infMatrix = links.begin()->second.infMatrix();
 							_previousInfMatrix = infMatrix;
 						}
-					else if(_previousMapId != s->mapId())
-					{
-						// first node, set high variance to make rtabmap trigger a new map
-						infMatrix /= 9999.0;
-						UDEBUG("First node of map %d, variance set to 9999", s->mapId());
-					}
 						else
 						{
 							if(_previousInfMatrix.empty())
@@ -463,6 +492,7 @@ SensorData DBReader::getNextData(CameraInfo * info)
 							infMatrix = _previousInfMatrix;
 						}
 					}
+				}
 				_previousMapId = s->mapId();
 			}
 			else
@@ -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",
+				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());
+				    8, iter->second.longitude(),
+				    8, iter->second.latitude(),
+				    8, iter->second.altitude(),
+				    8, iter->second.error(),
+				    8, iter->second.bearing());
 			}
 		}

@@ -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)
@@ -1245,13 +1248,15 @@ bool OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 					UDEBUG("first pose= %d last pose=%d", poses.begin()->first, poses.rbegin()->first);
 				}
 				for(std::list<std::pair<int, Transform> >::const_iterator kter = poses.begin(); kter!=poses.end(); ++kter)
+				{
+					std::map<int, cv::Mat >::iterator iter = emptyLocalMaps.find(kter->first);
+					std::map<int, cv::Mat >::iterator jter = occupiedLocalMaps.find(kter->first);
+					if(iter != emptyLocalMaps.end() || jter!=occupiedLocalMaps.end())
 					{
 						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)
 						{
@@ -1449,6 +1454,7 @@ bool OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 							}
 						}
 					}
+				}

 				if(footprintRadius_ >= cellSize_*1.5f || incrementalGraphUpdate)
 				{
@@ -202,6 +202,7 @@ Odometry::~Odometry()

 void Odometry::reset(const Transform & initialPose)
 {
+	UDEBUG("");
 	UASSERT(!initialPose.isNull());
 	previousVelocities_.clear();
 	velocityGuess_.setNull();
@@ -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,82 +198,116 @@ 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);
+	case kStateProcessCommand:
+		if(cmd == RtabmapEventCmd::kCmdInit)
+		{
+			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(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:
+			_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(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())
+			_rtabmap->close(cmdEvent.value1().toBool(), cmdEvent.value2().toStr());
+		}
+		else if(cmd == RtabmapEventCmd::kCmdUpdateParams)
 		{
-			id = _rtabmap->getMemory()->getSignatureIdByLabel(parameters.at("label"));
+			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();
+		}
+		else if(cmd == RtabmapEventCmd::kCmdDumpMemory)
+		{
+			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())
+			{
+				id = _rtabmap->getMemory()->getSignatureIdByLabel(cmdEvent.value1().toStr());
 				if(id <= 0)
 				{
-				UERROR("Failed to find a node with label \"%s\".", parameters.at("label").c_str());
+					UERROR("Failed to find a node with label \"%s\".", cmdEvent.value1().toStr().c_str());
 				}
 			}
-		else if(id < 0)
+			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);
 			}
@@ -284,24 +318,41 @@ void RtabmapThread::mainLoop()
 			}
 			this->post(new RtabmapGlobalPathEvent(
 					id,
-				parameters.at("label"),
+					cmdEvent.value1().isStr()?cmdEvent.value1().toStr():"",
 					_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")))
-		{
-			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, ""))
+		else if(cmd == RtabmapEventCmd::kCmdCancelGoal)
 		{
-			this->post(new RtabmapLabelErrorEvent(id, parameters.at("label")));
+			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(cmd == RtabmapEventCmd::kCmdRemoveLabel)
+		{
+			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()));
+			}
+		}
+		else if(cmd == RtabmapEventCmd::kCmdRepublishData)
+		{
+			ULOGGER_DEBUG("CMD_REPUBLISH_DATA");
+			_rtabmap->addNodesToRepublish(cmdEvent.value1().toIntArray());
+		}
+		else
+		{
+			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;
@@ -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,20 +149,26 @@ 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");
+	if(imuPublished_)
 		xoutIMU->setStreamName("imu");

 	// MonoCamera
@@ -193,6 +209,8 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 		stereo->rectifiedRight.link(xoutDepthOrRight->input);
 	}

+	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
@@ -206,6 +224,7 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 		imu->out.link(xoutIMU->input);

 		imu->enableFirmwareUpdate(imuFirmwareUpdate_);
+	}

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

@@ -221,6 +240,8 @@ 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);

+	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);
@@ -234,10 +255,18 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 				 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);
+	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,8 +333,10 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info)
 			}

 			//get imu
-			int added=  0;
-			while(1)
+			double stampStart = UTimer::now();
+			while(imuPublished_ && imuQueue_.get())
+			{
+				if(imuQueue_->has())
 				{
 					auto imuData = imuQueue_->get<dai::IMUData>();

@@ -328,13 +359,18 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info)
 						{
 							gyroBuffer_.erase(gyroBuffer_.begin());
 						}
-					++added;
 					}
 					if(accStamp >= stamp && gyroStamp >= stamp)
 					{
 						break;
 					}
 				}
+				if((UTimer::now() - stampStart) > 0.01)
+				{
+					UWARN("Could not received IMU after 10 ms! Disabling IMU!");
+					imuPublished_ = false;
+				}
+			}

 			cv::Vec3d acc, gyro;
 			bool valid = !accBuffer_.empty() && !gyroBuffer_.empty();
@@ -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_);
@@ -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;
 		}

@@ -714,16 +714,19 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 			}
 			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,15 +1855,9 @@ 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())
 		{
@@ -1885,7 +1878,6 @@ void DatabaseViewer::updateIds()
 			gpsPoses_.insert(std::make_pair(ids_[i], pose));
 		}
 	}
-	}

 	progressDialog->appendText("Loading info for all nodes... done!");
 	progressDialog->incrementStep();
@@ -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...");
+	float 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());
-	float xMin, yMin;
-	cv::Mat map = grid.getMap(xMin, yMin);
+		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())
@@ -4381,10 +4413,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 +4438,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 +4461,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 +4482,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 +4593,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 +4626,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 +5668,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 +5704,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 +6033,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 +6056,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 +6582,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 +6741,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);
@@ -8237,7 +8304,18 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 			}
 		}

+		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("");

@@ -623,9 +623,16 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 					linkItem->setColor(_neighborMergedColor);
 				}
 				else if(iter->second.type() == Link::kUserClosure)
+				{
+					if(_intraInterSessionColors)
+					{
+						linkItem->setColor(interSessionClosure?_loopInterSessionColor:_loopIntraSessionColor);
+					}
+					else
 					{
 						linkItem->setColor(_loopClosureUserColor);
 					}
+				}
 				else if(iter->second.type() == Link::kLandmark)
 				{
 					linkItem->setColor(_landmarkColor);
@@ -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/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>
@@ -63,7 +63,7 @@
          <property name="geometry">
           <rect>
            <x>0</x>
-            <y>-853</y>
+            <y>0</y>
            <width>756</width>
            <height>3657</height>
           </rect>
@@ -95,7 +95,7 @@
                <enum>QFrame::Raised</enum>
               </property>
               <property name="currentIndex">
-                <number>21</number>
+                <number>0</number>
               </property>
               <widget class="QWidget" name="page_22">
                <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,0">
@@ -107,15 +107,8 @@
                   <layout class="QVBoxLayout" name="verticalLayout_28">
                    <item>
                     <layout class="QGridLayout" name="gridLayout_39" columnstretch="0,1">
-                      <item row="2" column="0">
-                       <widget class="QCheckBox" name="checkBox_beep">
-                        <property name="text">
-                         <string/>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="5" column="0">
-                       <widget class="QCheckBox" name="checkBox_notifyWhenNewGlobalPathIsReceived">
+                      <item row="0" column="0">
+                       <widget class="QCheckBox" name="general_checkBox_imagesKept">
                        <property name="text">
                         <string/>
                        </property>
@@ -124,7 +117,7 @@
                        </property>
                       </widget>
                      </item>
-                      <item row="3" column="1">
+                      <item row="4" column="1">
                       <widget class="QLabel" name="label_247">
                        <property name="text">
                         <string>Use time for figures' x-axis. Otherwise, sensor data IDs are used for camera/odometry info and for all mapping statistics, node IDs are used.</string>
@@ -137,7 +130,24 @@
                        </property>
                       </widget>
                      </item>
-                      <item row="1" column="1">
+                      <item row="5" column="0">
+                       <widget class="QCheckBox" name="checkBox_cacheStatistics">
+                        <property name="text">
+                         <string/>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="6" column="0">
+                       <widget class="QCheckBox" name="checkBox_notifyWhenNewGlobalPathIsReceived">
+                        <property name="text">
+                         <string/>
+                        </property>
+                        <property name="checked">
+                         <bool>true</bool>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="2" column="1">
                       <widget class="QLabel" name="label_313">
                        <property name="text">
                         <string>Insert created clouds in the GUI cache to avoid regenerating clouds on export.</string>
@@ -150,47 +160,14 @@
                        </property>
                       </widget>
                      </item>
-                      <item row="5" column="1">
-                       <widget class="QLabel" name="label_102">
-                        <property name="text">
-                         <string>Notify when a new global path is received.</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                        <property name="textInteractionFlags">
-                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="0" column="0">
-                       <widget class="QCheckBox" name="general_checkBox_imagesKept">
-                        <property name="text">
-                         <string/>
-                        </property>
-                        <property name="checked">
-                         <bool>true</bool>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="1" column="0">
-                       <widget class="QCheckBox" name="general_checkBox_cloudsKept">
-                        <property name="text">
-                         <string/>
-                        </property>
-                        <property name="checked">
-                         <bool>true</bool>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="3" column="0">
+                      <item row="4" column="0">
                       <widget class="QCheckBox" name="checkBox_stamps">
                        <property name="text">
                         <string/>
                        </property>
                       </widget>
                      </item>
-                      <item row="4" column="1">
+                      <item row="5" column="1">
                       <widget class="QLabel" name="label_342">
                        <property name="text">
                         <string>Cache statistics.</string>
@@ -203,7 +180,40 @@
                        </property>
                       </widget>
                      </item>
-                      <item row="2" column="1">
+                      <item row="3" column="0">
+                       <widget class="QCheckBox" name="checkBox_beep">
+                        <property name="text">
+                         <string/>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="6" column="1">
+                       <widget class="QLabel" name="label_102">
+                        <property name="text">
+                         <string>Notify when a new global path is received.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="7" column="1">
+                       <widget class="QLabel" name="label_239">
+                        <property name="text">
+                         <string>Disable odometry. This option only makes sense in localization mode with a pre-built map and that odometry cannot be computed from the source selected (e.g. RGB source).</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="3" column="1">
                       <widget class="QLabel" name="label_88">
                        <property name="text">
                         <string>Beep! on special events (finished processing the data set, an error has occured, ...).</string>
@@ -229,34 +239,14 @@
                        </property>
                       </widget>
                      </item>
-                      <item row="6" column="0">
+                      <item row="7" column="0">
                       <widget class="QCheckBox" name="checkbox_odomDisabled">
                        <property name="text">
                         <string/>
                        </property>
                       </widget>
                      </item>
-                      <item row="6" column="1">
-                       <widget class="QLabel" name="label_239">
-                        <property name="text">
-                         <string>Disable odometry. This option only makes sense in localization mode with a pre-built map and that odometry cannot be computed from the source selected (e.g. RGB source).</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                        <property name="textInteractionFlags">
-                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="4" column="0">
-                       <widget class="QCheckBox" name="checkBox_cacheStatistics">
-                        <property name="text">
-                         <string/>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="7" column="1">
+                      <item row="8" column="1">
                       <widget class="QLabel" name="label_347">
                        <property name="text">
                         <string>When a ground truth is provided, align it with the map.</string>
@@ -269,13 +259,46 @@
                        </property>
                       </widget>
                      </item>
-                      <item row="7" column="0">
+                      <item row="8" column="0">
                       <widget class="QCheckBox" name="checkbox_groundTruthAlign">
                        <property name="text">
                         <string/>
                        </property>
                       </widget>
                      </item>
+                      <item row="2" column="0">
+                       <widget class="QCheckBox" name="general_checkBox_cloudsKept">
+                        <property name="text">
+                         <string/>
+                        </property>
+                        <property name="checked">
+                         <bool>true</bool>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="1" column="1">
+                       <widget class="QLabel" name="label_651">
+                        <property name="text">
+                         <string>Republish nodes not in GUI cache.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="1" column="0">
+                       <widget class="QCheckBox" name="general_checkBox_missing_cache_republished">
+                        <property name="text">
+                         <string/>
+                        </property>
+                        <property name="checked">
+                         <bool>true</bool>
+                        </property>
+                       </widget>
+                      </item>
                     </layout>
                    </item>
                    <item>
@@ -404,7 +427,23 @@
                      <property name="title">
                       <string>Graph View</string>
                      </property>
+                      <layout class="QVBoxLayout" name="verticalLayout_167">
+                       <item>
+                        <widget class="QGroupBox" name="groupBox_34">
+                         <property name="title">
+                          <string>Map Graph</string>
+                         </property>
                         <layout class="QGridLayout" name="gridLayout_104" columnstretch="0,1">
+                          <item row="0" column="0">
+                           <widget class="QRadioButton" name="radioButton_posteriorGraphView">
+                            <property name="text">
+                             <string/>
+                            </property>
+                            <property name="checked">
+                             <bool>true</bool>
+                            </property>
+                           </widget>
+                          </item>
                          <item row="0" column="1">
                           <widget class="QLabel" name="label_222">
                            <property name="text">
@@ -428,16 +467,6 @@
                            </property>
                           </widget>
                          </item>
-                       <item row="0" column="0">
-                        <widget class="QRadioButton" name="radioButton_posteriorGraphView">
-                         <property name="text">
-                          <string/>
-                         </property>
-                         <property name="checked">
-                          <bool>true</bool>
-                         </property>
-                        </widget>
-                       </item>
                          <item row="1" column="1">
                           <widget class="QLabel" name="label_478">
                            <property name="text">
@@ -451,6 +480,29 @@
                            </property>
                           </widget>
                          </item>
+                          <item row="2" column="0">
+                           <widget class="QRadioButton" name="radioButton_localizationsGraphView">
+                            <property name="text">
+                             <string/>
+                            </property>
+                            <property name="checked">
+                             <bool>false</bool>
+                            </property>
+                           </widget>
+                          </item>
+                          <item row="2" column="1">
+                           <widget class="QLabel" name="label_481">
+                            <property name="text">
+                             <string>Show nodes on which loop closures or localizations happened in red.</string>
+                            </property>
+                            <property name="wordWrap">
+                             <bool>true</bool>
+                            </property>
+                            <property name="textInteractionFlags">
+                             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                            </property>
+                           </widget>
+                          </item>
                          <item row="3" column="0">
                           <widget class="QRadioButton" name="radioButton_nochangeGraphView">
                            <property name="text">
@@ -471,10 +523,29 @@
                            </property>
                           </widget>
                          </item>
-                       <item row="2" column="1">
-                        <widget class="QLabel" name="label_481">
+                         </layout>
+                        </widget>
+                       </item>
+                       <item>
+                        <widget class="QGroupBox" name="groupBox_35">
+                         <property name="title">
+                          <string>Localization Graph</string>
+                         </property>
+                         <layout class="QGridLayout" name="gridLayout_123" columnstretch="0,1">
+                          <item row="0" column="0">
+                           <widget class="QRadioButton" name="radioButton_localizationsGraphViewOdomCache">
                            <property name="text">
-                          <string>Show nodes on which loop closures or localizations happened in red.</string>
+                             <string/>
+                            </property>
+                            <property name="checked">
+                             <bool>false</bool>
+                            </property>
+                           </widget>
+                          </item>
+                          <item row="0" column="1">
+                           <widget class="QLabel" name="label_649">
+                            <property name="text">
+                             <string>Show nodes in green when there is re-localization, red otherwise.</string>
                            </property>
                            <property name="wordWrap">
                             <bool>true</bool>
@@ -484,25 +555,33 @@
                            </property>
                           </widget>
                          </item>
-                       <item row="2" column="0">
-                        <widget class="QRadioButton" name="radioButton_localizationsGraphView">
+                          <item row="1" column="0">
+                           <widget class="QRadioButton" name="radioButton_nochangeGraphViewOdomCache">
                            <property name="text">
                             <string/>
                            </property>
                            <property name="checked">
-                          <bool>false</bool>
+                             <bool>true</bool>
+                            </property>
+                           </widget>
+                          </item>
+                          <item row="1" column="1">
+                           <widget class="QLabel" name="label_648">
+                            <property name="text">
+                             <string>No change of graph color.</string>
+                            </property>
+                            <property name="wordWrap">
+                             <bool>true</bool>
+                            </property>
+                            <property name="textInteractionFlags">
+                             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
                            </property>
                           </widget>
                          </item>
                         </layout>
-                      <zorder>radioButton_posteriorGraphView</zorder>
-                      <zorder>radioButton_wordsGraphView</zorder>
-                      <zorder>label_478</zorder>
-                      <zorder>label_222</zorder>
-                      <zorder>label_479</zorder>
-                      <zorder>radioButton_nochangeGraphView</zorder>
-                      <zorder>label_481</zorder>
-                      <zorder>radioButton_localizationsGraphView</zorder>
+                        </widget>
+                       </item>
+                      </layout>
                     </widget>
                    </item>
                    <item>
@@ -5710,17 +5789,10 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                  <string>DepthAI</string>
                                 </property>
                                 <layout class="QGridLayout" name="gridLayout_120" columnstretch="0,1">
-                                  <item row="1" column="0">
-                                   <widget class="QCheckBox" name="checkBox_depthai_depth">
+                                  <item row="0" column="1">
+                                   <widget class="QLabel" name="label_626">
                                    <property name="text">
-                                     <string/>
-                                    </property>
-                                   </widget>
-                                  </item>
-                                  <item row="1" column="1">
-                                   <widget class="QLabel" name="label_625">
-                                    <property name="text">
-                                     <string>Output depth.</string>
+                                     <string>Resolution.</string>
                                    </property>
                                    <property name="wordWrap">
                                     <bool>true</bool>
@@ -5730,48 +5802,10 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                    </property>
                                   </widget>
                                  </item>
-                                  <item row="0" column="0">
-                                   <widget class="QComboBox" name="comboBox_depthai_resolution">
-                                    <property name="currentIndex">
-                                     <number>1</number>
-                                    </property>
-                                    <property name="sizeAdjustPolicy">
-                                     <enum>QComboBox::AdjustToContents</enum>
-                                    </property>
-                                    <item>
+                                  <item row="1" column="1">
+                                   <widget class="QLabel" name="label_625">
                                    <property name="text">
-                                      <string>720p</string>
-                                     </property>
-                                    </item>
-                                    <item>
-                                     <property name="text">
-                                      <string>800p</string>
-                                     </property>
-                                    </item>
-                                    <item>
-                                     <property name="text">
-                                      <string>400p</string>
-                                     </property>
-                                    </item>
-                                   </widget>
-                                  </item>
-                                  <item row="2" column="0">
-                                   <widget class="QSpinBox" name="spinBox_depthai_confidence">
-                                    <property name="minimum">
-                                     <number>0</number>
-                                    </property>
-                                    <property name="maximum">
-                                     <number>255</number>
-                                    </property>
-                                    <property name="value">
-                                     <number>200</number>
-                                    </property>
-                                   </widget>
-                                  </item>
-                                  <item row="0" column="1">
-                                   <widget class="QLabel" name="label_626">
-                                    <property name="text">
-                                     <string>Resolution.</string>
+                                     <string>Output depth.</string>
                                    </property>
                                    <property name="wordWrap">
                                     <bool>true</bool>
@@ -5794,7 +5828,62 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                    </property>
                                   </widget>
                                  </item>
-                                  <item row="3" column="1">
+                                  <item row="1" column="0">
+                                   <widget class="QCheckBox" name="checkBox_depthai_depth">
+                                    <property name="text">
+                                     <string/>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="0" column="0">
+                                   <widget class="QComboBox" name="comboBox_depthai_resolution">
+                                    <property name="currentIndex">
+                                     <number>0</number>
+                                    </property>
+                                    <property name="sizeAdjustPolicy">
+                                     <enum>QComboBox::AdjustToContents</enum>
+                                    </property>
+                                    <item>
+                                     <property name="text">
+                                      <string>720p</string>
+                                     </property>
+                                    </item>
+                                    <item>
+                                     <property name="text">
+                                      <string>800p</string>
+                                     </property>
+                                    </item>
+                                    <item>
+                                     <property name="text">
+                                      <string>400p</string>
+                                     </property>
+                                    </item>
+                                    <item>
+                                     <property name="text">
+                                      <string>480p</string>
+                                     </property>
+                                    </item>
+                                    <item>
+                                     <property name="text">
+                                      <string>1200p</string>
+                                     </property>
+                                    </item>
+                                   </widget>
+                                  </item>
+                                  <item row="2" column="0">
+                                   <widget class="QSpinBox" name="spinBox_depthai_confidence">
+                                    <property name="minimum">
+                                     <number>0</number>
+                                    </property>
+                                    <property name="maximum">
+                                     <number>255</number>
+                                    </property>
+                                    <property name="value">
+                                     <number>200</number>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="4" column="1">
                                   <widget class="QLabel" name="label_646">
                                    <property name="text">
                                     <string>IMU firmware update</string>
@@ -5807,13 +5896,33 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                    </property>
                                   </widget>
                                  </item>
-                                  <item row="3" column="0">
+                                  <item row="4" column="0">
                                   <widget class="QCheckBox" name="checkBox_depthai_imu_firmware_update">
                                    <property name="text">
                                     <string/>
                                    </property>
                                   </widget>
                                  </item>
+                                  <item row="3" column="1">
+                                   <widget class="QLabel" name="label_650">
+                                    <property name="text">
+                                     <string>IMU published</string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
+                                    </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="3" column="0">
+                                   <widget class="QCheckBox" name="checkBox_depthai_imu_published">
+                                    <property name="text">
+                                     <string/>
+                                    </property>
+                                   </widget>
+                                  </item>
                                 </layout>
                                </widget>
                               </item>
@@ -8221,19 +8330,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                   <layout class="QVBoxLayout" name="verticalLayout_15">
                    <item>
                     <layout class="QGridLayout" name="gridLayout_43" columnstretch="0,1">
-                      <item row="1" column="0">
-                       <widget class="QDoubleSpinBox" name="general_doubleSpinBox_detectionRate">
-                        <property name="suffix">
-                         <string> Hz</string>
-                        </property>
-                        <property name="decimals">
-                         <number>3</number>
-                        </property>
-                        <property name="value">
-                         <double>1.000000000000000</double>
-                        </property>
-                       </widget>
-                      </item>
                      <item row="0" column="0">
                       <widget class="QCheckBox" name="general_checkBox_SLAM_mode">
                        <property name="text">
@@ -8258,13 +8354,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        </property>
                       </widget>
                      </item>
-                      <item row="4" column="0">
-                       <widget class="QCheckBox" name="general_checkBox_startNewMapOnLoopClosure">
-                        <property name="text">
-                         <string/>
+                      <item row="1" column="0">
+                       <widget class="QDoubleSpinBox" name="general_doubleSpinBox_detectionRate">
+                        <property name="suffix">
+                         <string> Hz</string>
                        </property>
-                        <property name="checked">
-                         <bool>true</bool>
+                        <property name="decimals">
+                         <number>3</number>
+                        </property>
+                        <property name="value">
+                         <double>1.000000000000000</double>
                        </property>
                       </widget>
                      </item>
@@ -8291,29 +8390,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        </property>
                       </widget>
                      </item>
-                      <item row="3" column="0">
-                       <widget class="QCheckBox" name="general_checkBox_createIntermediateNodes">
-                        <property name="text">
-                         <string/>
-                        </property>
-                        <property name="checked">
-                         <bool>false</bool>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="4" column="1">
-                       <widget class="QLabel" name="label_185">
-                        <property name="text">
-                         <string>Start a new map only if there is a global loop closure detected first with a previous map. If there is no map in memory, a new map is still created.</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                        <property name="textInteractionFlags">
-                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                        </property>
-                       </widget>
-                      </item>
                      <item row="2" column="1">
                       <widget class="QLabel" name="label_84">
                        <property name="text">
@@ -8327,6 +8403,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        </property>
                       </widget>
                      </item>
+                      <item row="3" column="0">
+                       <widget class="QCheckBox" name="general_checkBox_createIntermediateNodes">
+                        <property name="text">
+                         <string/>
+                        </property>
+                        <property name="checked">
+                         <bool>false</bool>
+                        </property>
+                       </widget>
+                      </item>
                      <item row="3" column="1">
                       <widget class="QLabel" name="label_165">
                        <property name="text">
@@ -8340,10 +8426,20 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        </property>
                       </widget>
                      </item>
-                      <item row="6" column="1">
-                       <widget class="QLabel" name="label_467">
+                      <item row="4" column="0">
+                       <widget class="QCheckBox" name="general_checkBox_startNewMapOnLoopClosure">
                        <property name="text">
-                         <string>Images are already rectified. By default RTAB-Map assumes that received images are rectified. If they are not, they can be rectified by RTAB-Map if this parameter is false.</string>
+                         <string/>
+                        </property>
+                        <property name="checked">
+                         <bool>true</bool>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="4" column="1">
+                       <widget class="QLabel" name="label_185">
+                        <property name="text">
+                         <string>Start a new map only if there is a global loop closure detected first with a previous map. If there is no map in memory, a new map is still created.</string>
                        </property>
                        <property name="wordWrap">
                         <bool>true</bool>
@@ -8353,8 +8449,8 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        </property>
                       </widget>
                      </item>
-                      <item row="6" column="0">
-                       <widget class="QCheckBox" name="general_checkBox_imagesAlreadyRectified">
+                      <item row="5" column="0">
+                       <widget class="QCheckBox" name="general_checkBox_startNewMapOnGoodSignature">
                        <property name="text">
                         <string/>
                        </property>
@@ -8376,8 +8472,31 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        </property>
                       </widget>
                      </item>
-                      <item row="5" column="0">
-                       <widget class="QCheckBox" name="general_checkBox_startNewMapOnGoodSignature">
+                      <item row="6" column="0">
+                       <widget class="QCheckBox" name="general_checkBox_imagesAlreadyRectified">
+                        <property name="text">
+                         <string/>
+                        </property>
+                        <property name="checked">
+                         <bool>true</bool>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="6" column="1">
+                       <widget class="QLabel" name="label_467">
+                        <property name="text">
+                         <string>Images are already rectified. By default RTAB-Map assumes that received images are rectified. If they are not, they can be rectified by RTAB-Map if this parameter is false.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="7" column="0">
+                       <widget class="QCheckBox" name="general_checkBox_rectifyOnlyFeatures">
                        <property name="text">
                         <string/>
                        </property>
@@ -8399,8 +8518,8 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        </property>
                       </widget>
                      </item>
-                      <item row="7" column="0">
-                       <widget class="QCheckBox" name="general_checkBox_rectifyOnlyFeatures">
+                      <item row="8" column="0">
+                       <widget class="QCheckBox" name="general_checkBox_saveLocalizationData">
                        <property name="text">
                         <string/>
                        </property>
@@ -8422,16 +8541,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        </property>
                       </widget>
                      </item>
-                      <item row="8" column="0">
-                       <widget class="QCheckBox" name="general_checkBox_saveLocalizationData">
-                        <property name="text">
-                         <string/>
-                        </property>
-                        <property name="checked">
-                         <bool>true</bool>
-                        </property>
-                       </widget>
-                      </item>
                     </layout>
                    </item>
                    <item>
@@ -8559,7 +8668,27 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                       <bool>true</bool>
                      </property>
                      <layout class="QGridLayout" name="gridLayout_45" columnstretch="0,1">
-                       <item row="1" column="0">
+                       <item row="4" column="0">
+                        <widget class="QCheckBox" name="general_checkBox_publishRMSE">
+                         <property name="text">
+                          <string/>
+                         </property>
+                         <property name="checked">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="6" column="0">
+                        <widget class="QCheckBox" name="general_checkBox_saveWMState">
+                         <property name="text">
+                          <string/>
+                         </property>
+                         <property name="checked">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="2" column="0">
                        <widget class="QCheckBox" name="general_checkBox_publishPdf">
                         <property name="text">
                          <string/>
@@ -8569,7 +8698,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                       <item row="1" column="1">
+                       <item row="2" column="1">
                        <widget class="QLabel" name="label_106">
                         <property name="text">
                          <string>Publish loop closure hypotheses (pdf).</string>
@@ -8579,17 +8708,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                       <item row="2" column="0">
-                        <widget class="QCheckBox" name="general_checkBox_publishLikelihood">
-                         <property name="text">
-                          <string/>
-                         </property>
-                         <property name="checked">
-                          <bool>true</bool>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="5" column="1">
+                       <item row="6" column="1">
                        <widget class="QLabel" name="label_462">
                         <property name="text">
                          <string>Save working memory state after each update.</string>
@@ -8599,6 +8718,36 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
+                       <item row="3" column="1">
+                        <widget class="QLabel" name="label_116">
+                         <property name="text">
+                          <string>Publish loop closure likelihood.</string>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="5" column="1">
+                        <widget class="QLabel" name="label_452">
+                         <property name="text">
+                          <string>Publish RAM usage.</string>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="4" column="1">
+                        <widget class="QLabel" name="label_437">
+                         <property name="text">
+                          <string>Compute RMSE when ground truth is available.</string>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
                       <item row="0" column="0">
                        <widget class="QCheckBox" name="general_checkBox_publishRawData">
                         <property name="text">
@@ -8609,6 +8758,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
+                       <item row="3" column="0">
+                        <widget class="QCheckBox" name="general_checkBox_publishLikelihood">
+                         <property name="text">
+                          <string/>
+                         </property>
+                         <property name="checked">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
                       <item row="0" column="1">
                        <widget class="QLabel" name="label_91">
                         <property name="text">
@@ -8619,27 +8778,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                       <item row="2" column="1">
-                        <widget class="QLabel" name="label_116">
-                         <property name="text">
-                          <string>Publish loop closure likelihood.</string>
-                         </property>
-                         <property name="textInteractionFlags">
-                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="4" column="1">
-                        <widget class="QLabel" name="label_452">
-                         <property name="text">
-                          <string>Publish RAM usage.</string>
-                         </property>
-                         <property name="textInteractionFlags">
-                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="4" column="0">
+                       <item row="5" column="0">
                        <widget class="QCheckBox" name="general_checkBox_publishRAM">
                         <property name="text">
                          <string/>
@@ -8649,33 +8788,26 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                       <item row="3" column="1">
-                        <widget class="QLabel" name="label_437">
+                       <item row="1" column="1">
+                        <widget class="QLabel" name="label_652">
                         <property name="text">
-                          <string>Compute RMSE when ground truth is available.</string>
+                          <string>Maximum nodes republished when requesting missing data. When RGB-D SLAM mode is disabled, only loop closure data is republished, otherwise the closest nodes from the current localization are republished first. Ignored if &quot;Publish signature data&quot; is unchecked.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
                         </property>
                         <property name="textInteractionFlags">
                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
                         </property>
                        </widget>
                       </item>
-                       <item row="3" column="0">
-                        <widget class="QCheckBox" name="general_checkBox_publishRMSE">
-                         <property name="text">
-                          <string/>
+                       <item row="1" column="0">
+                        <widget class="QSpinBox" name="general_spinBox_max_republished">
+                         <property name="maximum">
+                          <number>999</number>
                         </property>
-                         <property name="checked">
-                          <bool>true</bool>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="5" column="0">
-                        <widget class="QCheckBox" name="general_checkBox_saveWMState">
-                         <property name="text">
-                          <string/>
-                         </property>
-                         <property name="checked">
-                          <bool>true</bool>
+                         <property name="value">
+                          <number>0</number>
                         </property>
                        </widget>
                       </item>
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
  <name>rtabmap</name>
-  <version>0.20.21</version>
+  <version>0.20.22</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>
@@ -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())
@@ -69,8 +69,13 @@ void showUsage()
 			"                       arguments, they overwrite those in config file and the database.\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"
@@ -226,9 +231,13 @@ int main(int argc, char * argv[])
 	bool useDatabaseRate = 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;
@@ -293,6 +302,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 +368,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;
@@ -524,6 +571,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 +580,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 +591,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 +662,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 +706,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);
+		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 +728,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);
@@ -692,7 +808,31 @@ int main(int argc, char * argv[])
 				printf("Failed processing node %d.\n", data.id());
 				globalMapStats.clear();
 			}
-			else if(assemble2dMap || assemble3dMap || assemble2dOctoMap || assemble3dOctoMap)
+			else
+			{
+				if(republishLoopClosures && dbReader->driver())
+				{
+					std::multimap<int, Link> links;
+					dbReader->driver()->loadLinks(data.id(), links);
+					for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
+					{
+						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())
+						{
+							if(!iter->second.transform().isNull() &&
+								rtabmap.getMemory()->getWorkingMem().find(iter->second.to()) != rtabmap.getMemory()->getWorkingMem().end() &&
+								rtabmap.addLink(iter->second))
+							{
+								printf("Added link %d->%d from input database.\n", iter->second.from(), iter->second.to());
+							}
+						}
+					}
+				}
+
+				if(assemble2dMap || assemble3dMap || assemble2dOctoMap || assemble3dOctoMap)
 				{
 					globalMapStats.clear();
 					double timeRtabmap = t.ticks();
@@ -772,6 +912,7 @@ int main(int argc, char * argv[])
 #endif
 				}
 			}
+		}

 		const rtabmap::Statistics & stats = rtabmap.getStatistics();
 		int refId = stats.refImageId();
@@ -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	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