Fixed #940

Removed vtk default iOS simulator build

.github/workflows/cmake-ros.yml

@@ -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}}
       

.github/workflows/docker.yml

@@ -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

CMakeLists.txt

@@ -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})
   

README.md

@@ -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>
+ 

app/android/jni/RTABMapApp.cpp

@@ -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)));
 				}
 			}
 		}

app/ios/RTABMapApp/.gitignore

@@ -0,0 +1 @@
+Libraries

app/ios/RTABMapApp/Libraries/.gitignore

@@ -1,6 +0,0 @@
-# Ignore everything
-*
-
-# But not these files...
-!.gitignore
-!install_deps.sh

app/ios/RTABMapApp/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

corelib/include/rtabmap/core/DBReader.h

@@ -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;

corelib/include/rtabmap/core/Parameters.h

@@ -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.");

corelib/include/rtabmap/core/Rtabmap.h

@@ -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;

corelib/include/rtabmap/core/RtabmapEvent.h

@@ -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,

corelib/include/rtabmap/core/RtabmapThread.h

@@ -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;

corelib/include/rtabmap/core/Statistics.h

@@ -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;

corelib/include/rtabmap/core/camera/CameraDepthAI.h

@@ -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_;

corelib/include/rtabmap/core/impl/util3d_mapping.hpp

@@ -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;

corelib/src/DBDriverSqlite3.cpp

@@ -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());
 	}
 }
 

corelib/src/DBReader.cpp

@@ -52,7 +52,9 @@ DBReader::DBReader(const std::string & databasePath,
 				   int stopId,
 				   bool intermediateNodesIgnored,
 				   bool landmarksIgnored,
-				   bool featuresIgnored) :
+				   bool featuresIgnored,
+				   int startMapId,
+				   int stopMapId) :
 	Camera(frameRate),
 	_paths(uSplit(databasePath, ';')),
 	_odometryIgnored(odometryIgnored),
@@ -64,6 +66,8 @@ DBReader::DBReader(const std::string & databasePath,
 	_intermediateNodesIgnored(intermediateNodesIgnored),
 	_landmarksIgnored(landmarksIgnored),
 	_featuresIgnored(featuresIgnored),
+	_startMapId(startMapId),
+	_stopMapId(stopMapId),
 	_dbDriver(0),
 	_currentId(_ids.end()),
 	_previousMapId(-1),
@@ -75,6 +79,11 @@ DBReader::DBReader(const std::string & databasePath,
 	{
 		_stopId = _startId;
 	}
+
+	if(_stopMapId>-1 && _stopMapId<_startMapId)
+	{
+		_stopMapId = _startMapId;
+	}
 }
 
 DBReader::DBReader(const std::list<std::string> & databasePaths,
@@ -87,7 +96,9 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
 				   int stopId,
 				   bool intermediateNodesIgnored,
 				   bool landmarksIgnored,
-				   bool featuresIgnored) :
+				   bool featuresIgnored,
+				   int startMapId,
+				   int stopMapId) :
 	Camera(frameRate),
    _paths(databasePaths),
 	_odometryIgnored(odometryIgnored),
@@ -99,6 +110,8 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
 	_intermediateNodesIgnored(intermediateNodesIgnored),
 	_landmarksIgnored(landmarksIgnored),
 	_featuresIgnored(featuresIgnored),
+	_startMapId(startMapId),
+	_stopMapId(stopMapId),
 	_dbDriver(0),
 	_currentId(_ids.end()),
 	_previousMapId(-1),
@@ -110,6 +123,11 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
 	{
 		_stopId = _startId;
 	}
+
+	if(_stopMapId>-1 && _stopMapId<_startMapId)
+	{
+		_stopMapId = _startMapId;
+	}
 }
 
 DBReader::~DBReader()
@@ -368,6 +386,15 @@ SensorData DBReader::getNextData(CameraInfo * info)
 
 			if(_intermediateNodesIgnored && s->getWeight() == -1)
 			{
+				UDEBUG("Ignoring node %d (intermediate nodes ignored)", s->id());
+				++_currentId;
+				delete s;
+				continue;
+			}
+
+			if(s->mapId() < _startMapId || (_stopMapId>=0 && s->mapId() > _stopMapId))
+			{
+				UDEBUG("Ignoring node %d (map id=%d, min=%d max=%d)", s->id(), s->mapId(), _startMapId, _stopMapId);
 				++_currentId;
 				delete s;
 				continue;
@@ -439,15 +466,7 @@ SensorData DBReader::getNextData(CameraInfo * info)
 				}
 				else
 				{
-					// if localization data saved in database, covariance will be set in a prior link
-					_dbDriver->loadLinks(*_currentId, links, Link::kPosePrior);
-					if(links.size())
-					{
-						// assume the first is the backward neighbor, take its variance
-						infMatrix = links.begin()->second.infMatrix();
-						_previousInfMatrix = infMatrix;
-					}
-					else if(_previousMapId != s->mapId())
+					if(_previousMapId != s->mapId())
 					{
 						// first node, set high variance to make rtabmap trigger a new map
 						infMatrix /= 9999.0;
@@ -455,12 +474,23 @@ SensorData DBReader::getNextData(CameraInfo * info)
 					}
 					else
 					{
-						if(_previousInfMatrix.empty())
+						// if localization data saved in database, covariance will be set in a prior link
+						_dbDriver->loadLinks(*_currentId, links, Link::kPosePrior);
+						if(links.size())
 						{
-							_previousInfMatrix = cv::Mat::eye(6,6,CV_64FC1);
+							// assume the first is the backward neighbor, take its variance
+							infMatrix = links.begin()->second.infMatrix();
+							_previousInfMatrix = infMatrix;
+						}
+						else
+						{
+							if(_previousInfMatrix.empty())
+							{
+								_previousInfMatrix = cv::Mat::eye(6,6,CV_64FC1);
+							}
+							// we have a node not linked to map, use last variance
+							infMatrix = _previousInfMatrix;
 						}
-						// we have a node not linked to map, use last variance
-						infMatrix = _previousInfMatrix;
 					}
 				}
 				_previousMapId = s->mapId();
@@ -545,7 +575,7 @@ SensorData DBReader::getNextData(CameraInfo * info)
 			data.setId(seq);
 			data.setStamp(s->getStamp());
 			data.setGroundTruth(s->getGroundTruthPose());
-			if(globalPose.isNull())
+			if(!globalPose.isNull())
 			{
 				data.setGlobalPose(globalPose, globalPoseCov);
 			}

corelib/src/Graph.cpp

@@ -56,7 +56,7 @@ bool exportPoses(
 		int format, // 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame, 11=10+ID), 2=KITTI, 3=TORO, 4=g2o
 		const std::map<int, Transform> & poses,
 		const std::multimap<int, Link> & constraints, // required for formats 3 and 4
-		const std::map<int, double> & stamps, // required for format 1
+		const std::map<int, double> & stamps, // required for format 1, 10 and 11
 		const ParametersMap & parameters) // optional for formats 3 and 4
 {
 	UDEBUG("%s", filePath.c_str());
@@ -524,9 +524,9 @@ bool exportGPS(
 			for(std::map<int, GPS>::const_iterator iter=gpsValues.begin(); iter!=gpsValues.end(); ++iter)
 			{
 				values += uFormat("%s,%s,%s ",
-                                  uReplaceChar(uNumber2Str(iter->second.longitude()), ',', '.').c_str(),
-                                  uReplaceChar(uNumber2Str(iter->second.latitude()), ',', '.').c_str(),
-                                  uReplaceChar(uNumber2Str(iter->second.altitude()), ',', '.').c_str());
+                                  uReplaceChar(uNumber2Str(iter->second.longitude(), 8, true), ',', '.').c_str(),
+                                  uReplaceChar(uNumber2Str(iter->second.latitude(), 8, true), ',', '.').c_str(),
+                                  uReplaceChar(uNumber2Str(iter->second.altitude(), 8, true), ',', '.').c_str());
 			}
 
 			// switch argb (Qt format) -> abgr
@@ -580,13 +580,13 @@ bool exportGPS(
 			fprintf(fout, "# stamp longitude latitude altitude error bearing\n");
 			for(std::map<int, GPS>::const_iterator iter=gpsValues.begin(); iter!=gpsValues.end(); ++iter)
 			{
-				fprintf(fout, "%f %f %f %f %f %f\n",
-						iter->second.stamp(),
-						iter->second.longitude(),
-						iter->second.latitude(),
-						iter->second.altitude(),
-						iter->second.error(),
-						iter->second.bearing());
+				fprintf(fout, "%f %.*f %.*f %.*f %.*f %.*f\n",
+				    iter->second.stamp(),
+				    8, iter->second.longitude(),
+				    8, iter->second.latitude(),
+				    8, iter->second.altitude(),
+				    8, iter->second.error(),
+				    8, iter->second.bearing());
 			}
 		}
 

corelib/src/OccupancyGrid.cpp

@@ -770,7 +770,7 @@ void OccupancyGrid::addToCache(
 		const cv::Mat & obstacles,
 		const cv::Mat & empty)
 {
-	UDEBUG("nodeId=%d", nodeId);
+	UDEBUG("nodeId=%d (ground=%d obstacles=%d empty=%d)", nodeId, ground.cols, obstacles.cols, empty.cols);
 	if(nodeId < 0)
 	{
 		UWARN("Cannot add nodes with negative id (nodeId=%d)", nodeId);
@@ -1026,6 +1026,7 @@ bool OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 
 		if(!cache_.empty())
 		{
+			UDEBUG("Updating from cache");
 			for(std::list<std::pair<int, Transform> >::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
 			{
 				if(uContains(cache_, iter->first))
@@ -1035,14 +1036,18 @@ bool OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 					UDEBUG("Adding grid %d: ground=%d obstacles=%d empty=%d", iter->first, pair.first.first.cols, pair.first.second.cols, pair.second.cols);
 
 					//ground
+					cv::Mat ground;
+					if(pair.first.first.cols || pair.second.cols)
+					{
+						ground = cv::Mat(1, pair.first.first.cols+pair.second.cols, CV_32FC2);
+					}
 					if(pair.first.first.cols)
 					{
 						if(pair.first.first.rows > 1 && pair.first.first.cols == 1)
 						{
 							UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", pair.first.first.rows, pair.first.first.cols);
 						}
-						cv::Mat ground(1, pair.first.first.cols, CV_32FC2);
-						for(int i=0; i<ground.cols; ++i)
+						for(int i=0; i<pair.first.first.cols; ++i)
 						{
 							const float * vi = pair.first.first.ptr<float>(0,i);
 							float * vo = ground.ptr<float>(0,i);
@@ -1067,7 +1072,6 @@ bool OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 							else if(maxY < vo[1])
 								maxY = vo[1];
 						}
-						uInsert(emptyLocalMaps, std::make_pair(iter->first, ground));
 
 						if(cloudAssembling_)
 						{
@@ -1083,11 +1087,10 @@ bool OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 						{
 							UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", pair.second.rows, pair.second.cols);
 						}
-						cv::Mat ground(1, pair.second.cols, CV_32FC2);
-						for(int i=0; i<ground.cols; ++i)
+						for(int i=0; i<pair.second.cols; ++i)
 						{
 							const float * vi = pair.second.ptr<float>(0,i);
-							float * vo = ground.ptr<float>(0,i);
+							float * vo = ground.ptr<float>(0,i+pair.first.first.cols);
 							cv::Point3f vt;
 							if(pair.second.channels() != 2 && pair.second.channels() != 5)
 							{
@@ -1109,7 +1112,6 @@ bool OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 							else if(maxY < vo[1])
 								maxY = vo[1];
 						}
-						uInsert(emptyLocalMaps, std::make_pair(iter->first, ground));
 
 						if(cloudAssembling_)
 						{
@@ -1117,6 +1119,7 @@ bool OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 							assembledEmptyCellsUpdated = true;
 						}
 					}
+					uInsert(emptyLocalMaps, std::make_pair(iter->first, ground));
 
 					//obstacles
 					if(pair.first.second.cols)
@@ -1246,205 +1249,208 @@ bool OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 				}
 				for(std::list<std::pair<int, Transform> >::const_iterator kter = poses.begin(); kter!=poses.end(); ++kter)
 				{
-					if(kter->first > 0)
-					{
-						uInsert(addedNodes_, *kter);
-					}
 					std::map<int, cv::Mat >::iterator iter = emptyLocalMaps.find(kter->first);
 					std::map<int, cv::Mat >::iterator jter = occupiedLocalMaps.find(kter->first);
-					std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(kter->first);
-					if(cter == cellCount_.end() && kter->first > 0)
+					if(iter != emptyLocalMaps.end() || jter!=occupiedLocalMaps.end())
 					{
-						cter = cellCount_.insert(std::make_pair(kter->first, std::pair<int,int>(0,0))).first;
-					}
-					if(iter!=emptyLocalMaps.end())
-					{
-						for(int i=0; i<iter->second.cols; ++i)
+						if(kter->first > 0)
 						{
-							float * ptf = iter->second.ptr<float>(0,i);
-							cv::Point2i pt((ptf[0]-xMin)/cellSize_, (ptf[1]-yMin)/cellSize_);
-							UASSERT_MSG(pt.y >=0 && pt.y < map.rows && pt.x >= 0 && pt.x < map.cols,
-									uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d (graph modified=%d)",
-											kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, iter->second.channels(), mapInfo.channels()-1, (graphOptimized || graphChanged)?1:0).c_str());
-							char & value = map.at<char>(pt.y, pt.x);
-							if(value != -2 && (!incrementalGraphUpdate || value==-1))
-							{
-								float * info = mapInfo.ptr<float>(pt.y, pt.x);
-								int nodeId = (int)info[0];
-								if(value != -1)
-								{
-									if(kter->first > 0 && (kter->first <  nodeId || nodeId < 0))
-									{
-										// cannot rewrite on cells referred by more recent nodes
-										continue;
-									}
-									if(nodeId > 0)
-									{
-										std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
-										UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
-										if(value == 0)
-										{
-											eter->second.first -= 1;
-										}
-										else if(value == 100)
-										{
-											eter->second.second -= 1;
-										}
-										if(kter->first < 0)
-										{
-											eter->second.first += 1;
-										}
-									}
-								}
-								if(kter->first > 0)
-								{
-									info[0] = (float)kter->first;
-									info[1] = ptf[0];
-									info[2] = ptf[1];
-									cter->second.first+=1;
-								}
-								value = 0; // free space
-
-								// update odds
-								if(nodeId != kter->first)
-								{
-									info[3] += probMiss_;
-									if (info[3] < probClampingMin_)
-									{
-										info[3] = probClampingMin_;
-									}
-									if (info[3] > probClampingMax_)
-									{
-										info[3] = probClampingMax_;
-									}
-								}
-							}
+							uInsert(addedNodes_, *kter);
 						}
-					}
-
-					if(footprintRadius_ >= cellSize_*1.5f)
-					{
-						// place free space under the footprint of the robot
-						cv::Point2i ptBegin((kter->second.x()-footprintRadius_-xMin)/cellSize_, (kter->second.y()-footprintRadius_-yMin)/cellSize_);
-						cv::Point2i ptEnd((kter->second.x()+footprintRadius_-xMin)/cellSize_, (kter->second.y()+footprintRadius_-yMin)/cellSize_);
-						if(ptBegin.x < 0)
-							ptBegin.x = 0;
-						if(ptEnd.x >= map.cols)
-							ptEnd.x = map.cols-1;
-
-						if(ptBegin.y < 0)
-							ptBegin.y = 0;
-						if(ptEnd.y >= map.rows)
-							ptEnd.y = map.rows-1;
-
-						for(int i=ptBegin.x; i<ptEnd.x; ++i)
+						std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(kter->first);
+						if(cter == cellCount_.end() && kter->first > 0)
 						{
-							for(int j=ptBegin.y; j<ptEnd.y; ++j)
-							{
-								UASSERT(j < map.rows && i < map.cols);
-								char & value = map.at<char>(j, i);
-								float * info = mapInfo.ptr<float>(j, i);
-								int nodeId = (int)info[0];
-								if(value != -1)
-								{
-									if(kter->first > 0 && (kter->first <  nodeId || nodeId < 0))
-									{
-										// cannot rewrite on cells referred by more recent nodes
-										continue;
-									}
-									if(nodeId>0)
-									{
-										std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
-										UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
-										if(value == 0)
-										{
-											eter->second.first -= 1;
-										}
-										else if(value == 100)
-										{
-											eter->second.second -= 1;
-										}
-										if(kter->first < 0)
-										{
-											eter->second.first += 1;
-										}
-									}
-								}
-								if(kter->first > 0)
-								{
-									info[0] = (float)kter->first;
-									info[1] = float(i) * cellSize_ + xMin;
-									info[2] = float(j) * cellSize_ + yMin;
-									info[3] = probClampingMin_;
-									cter->second.first+=1;
-								}
-								value = -2; // free space (footprint)
-							}
+							cter = cellCount_.insert(std::make_pair(kter->first, std::pair<int,int>(0,0))).first;
 						}
-					}
-
-					if(jter!=occupiedLocalMaps.end())
-					{
-						for(int i=0; i<jter->second.cols; ++i)
+						if(iter!=emptyLocalMaps.end())
 						{
-							float * ptf = jter->second.ptr<float>(0,i);
-							cv::Point2i pt((ptf[0]-xMin)/cellSize_, (ptf[1]-yMin)/cellSize_);
-							UASSERT_MSG(pt.y>=0 && pt.y < map.rows && pt.x>=0 && pt.x < map.cols,
+							for(int i=0; i<iter->second.cols; ++i)
+							{
+								float * ptf = iter->second.ptr<float>(0,i);
+								cv::Point2i pt((ptf[0]-xMin)/cellSize_, (ptf[1]-yMin)/cellSize_);
+								UASSERT_MSG(pt.y >=0 && pt.y < map.rows && pt.x >= 0 && pt.x < map.cols,
 										uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d (graph modified=%d)",
-												kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, jter->second.channels(), mapInfo.channels()-1, (graphOptimized || graphChanged)?1:0).c_str());
-							char & value = map.at<char>(pt.y, pt.x);
-							if(value != -2)
+												kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, iter->second.channels(), mapInfo.channels()-1, (graphOptimized || graphChanged)?1:0).c_str());
+								char & value = map.at<char>(pt.y, pt.x);
+								if(value != -2 && (!incrementalGraphUpdate || value==-1))
+								{
+									float * info = mapInfo.ptr<float>(pt.y, pt.x);
+									int nodeId = (int)info[0];
+									if(value != -1)
+									{
+										if(kter->first > 0 && (kter->first <  nodeId || nodeId < 0))
+										{
+											// cannot rewrite on cells referred by more recent nodes
+											continue;
+										}
+										if(nodeId > 0)
+										{
+											std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
+											UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
+											if(value == 0)
+											{
+												eter->second.first -= 1;
+											}
+											else if(value == 100)
+											{
+												eter->second.second -= 1;
+											}
+											if(kter->first < 0)
+											{
+												eter->second.first += 1;
+											}
+										}
+									}
+									if(kter->first > 0)
+									{
+										info[0] = (float)kter->first;
+										info[1] = ptf[0];
+										info[2] = ptf[1];
+										cter->second.first+=1;
+									}
+									value = 0; // free space
+
+									// update odds
+									if(nodeId != kter->first)
+									{
+										info[3] += probMiss_;
+										if (info[3] < probClampingMin_)
+										{
+											info[3] = probClampingMin_;
+										}
+										if (info[3] > probClampingMax_)
+										{
+											info[3] = probClampingMax_;
+										}
+									}
+								}
+							}
+						}
+
+						if(footprintRadius_ >= cellSize_*1.5f)
+						{
+							// place free space under the footprint of the robot
+							cv::Point2i ptBegin((kter->second.x()-footprintRadius_-xMin)/cellSize_, (kter->second.y()-footprintRadius_-yMin)/cellSize_);
+							cv::Point2i ptEnd((kter->second.x()+footprintRadius_-xMin)/cellSize_, (kter->second.y()+footprintRadius_-yMin)/cellSize_);
+							if(ptBegin.x < 0)
+								ptBegin.x = 0;
+							if(ptEnd.x >= map.cols)
+								ptEnd.x = map.cols-1;
+
+							if(ptBegin.y < 0)
+								ptBegin.y = 0;
+							if(ptEnd.y >= map.rows)
+								ptEnd.y = map.rows-1;
+
+							for(int i=ptBegin.x; i<ptEnd.x; ++i)
 							{
-								float * info = mapInfo.ptr<float>(pt.y, pt.x);
-								int nodeId = (int)info[0];
-								if(value != -1)
+								for(int j=ptBegin.y; j<ptEnd.y; ++j)
 								{
-									if(kter->first > 0 && (kter->first <  nodeId || nodeId < 0))
+									UASSERT(j < map.rows && i < map.cols);
+									char & value = map.at<char>(j, i);
+									float * info = mapInfo.ptr<float>(j, i);
+									int nodeId = (int)info[0];
+									if(value != -1)
 									{
-										// cannot rewrite on cells referred by more recent nodes
-										continue;
-									}
-									if(nodeId>0)
-									{
-										std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
-										UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
-										if(value == 0)
+										if(kter->first > 0 && (kter->first <  nodeId || nodeId < 0))
 										{
-											eter->second.first -= 1;
+											// cannot rewrite on cells referred by more recent nodes
+											continue;
 										}
-										else if(value == 100)
+										if(nodeId>0)
 										{
-											eter->second.second -= 1;
-										}
-										if(kter->first < 0)
-										{
-											eter->second.second += 1;
+											std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
+											UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
+											if(value == 0)
+											{
+												eter->second.first -= 1;
+											}
+											else if(value == 100)
+											{
+												eter->second.second -= 1;
+											}
+											if(kter->first < 0)
+											{
+												eter->second.first += 1;
+											}
 										}
 									}
-								}
-								if(kter->first > 0)
-								{
-									info[0] = (float)kter->first;
-									info[1] = ptf[0];
-									info[2] = ptf[1];
-									cter->second.second+=1;
-								}
-
-								// update odds
-								if(nodeId != kter->first || value!=100)
-								{
-									info[3] += probHit_;
-									if (info[3] < probClampingMin_)
+									if(kter->first > 0)
 									{
+										info[0] = (float)kter->first;
+										info[1] = float(i) * cellSize_ + xMin;
+										info[2] = float(j) * cellSize_ + yMin;
 										info[3] = probClampingMin_;
+										cter->second.first+=1;
 									}
-									if (info[3] > probClampingMax_)
-									{
-										info[3] = probClampingMax_;
-									}
+									value = -2; // free space (footprint)
 								}
+							}
+						}
 
-								value = 100; // obstacles
+						if(jter!=occupiedLocalMaps.end())
+						{
+							for(int i=0; i<jter->second.cols; ++i)
+							{
+								float * ptf = jter->second.ptr<float>(0,i);
+								cv::Point2i pt((ptf[0]-xMin)/cellSize_, (ptf[1]-yMin)/cellSize_);
+								UASSERT_MSG(pt.y>=0 && pt.y < map.rows && pt.x>=0 && pt.x < map.cols,
+											uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d (graph modified=%d)",
+													kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, jter->second.channels(), mapInfo.channels()-1, (graphOptimized || graphChanged)?1:0).c_str());
+								char & value = map.at<char>(pt.y, pt.x);
+								if(value != -2)
+								{
+									float * info = mapInfo.ptr<float>(pt.y, pt.x);
+									int nodeId = (int)info[0];
+									if(value != -1)
+									{
+										if(kter->first > 0 && (kter->first <  nodeId || nodeId < 0))
+										{
+											// cannot rewrite on cells referred by more recent nodes
+											continue;
+										}
+										if(nodeId>0)
+										{
+											std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
+											UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
+											if(value == 0)
+											{
+												eter->second.first -= 1;
+											}
+											else if(value == 100)
+											{
+												eter->second.second -= 1;
+											}
+											if(kter->first < 0)
+											{
+												eter->second.second += 1;
+											}
+										}
+									}
+									if(kter->first > 0)
+									{
+										info[0] = (float)kter->first;
+										info[1] = ptf[0];
+										info[2] = ptf[1];
+										cter->second.second+=1;
+									}
+
+									// update odds
+									if(nodeId != kter->first || value!=100)
+									{
+										info[3] += probHit_;
+										if (info[3] < probClampingMin_)
+										{
+											info[3] = probClampingMin_;
+										}
+										if (info[3] > probClampingMax_)
+										{
+											info[3] = probClampingMax_;
+										}
+									}
+
+									value = 100; // obstacles
+								}
 							}
 						}
 					}

corelib/src/Odometry.cpp

@@ -202,6 +202,7 @@ Odometry::~Odometry()
 
 void Odometry::reset(const Transform & initialPose)
 {
+	UDEBUG("");
 	UASSERT(!initialPose.isNull());
 	previousVelocities_.clear();
 	velocityGuess_.setNull();

corelib/src/Rtabmap.cpp

@@ -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);

corelib/src/RtabmapThread.cpp

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

corelib/src/camera/CameraDepthAI.cpp

@@ -56,11 +56,12 @@ CameraDepthAI::CameraDepthAI(
 	outputDepth_(false),
 	depthConfidence_(200),
 	resolution_(resolution),
-	imuFirmwareUpdate_(false)
+	imuFirmwareUpdate_(false),
+	imuPublished_(true)
 #endif
 {
 #ifdef RTABMAP_DEPTHAI
-	UASSERT(resolution_>=0 && resolution_<=2);
+	UASSERT(resolution_>=(int)dai::MonoCameraProperties::SensorResolution::THE_720_P && resolution_<=(int)dai::MonoCameraProperties::SensorResolution::THE_1200_P);
 #endif
 }
 
@@ -96,6 +97,15 @@ void CameraDepthAI::setIMUFirmwareUpdate(bool enabled)
 #endif
 }
 
+void CameraDepthAI::setIMUPublished(bool published)
+{
+#ifdef RTABMAP_DEPTHAI
+	imuPublished_ = published;
+#else
+	UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
+#endif
+}
+
 bool CameraDepthAI::init(const std::string & calibrationFolder, const std::string & cameraName)
 {
 	UDEBUG("");
@@ -139,21 +149,27 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 
 	// look for calibration files
 	stereoModel_ = StereoCameraModel();
-	cv::Size targetSize(resolution_<2?1280:640, resolution_==0?720:resolution_==1?800:400);
+	cv::Size targetSize(resolution_<2?1280:resolution_==4?1920:640, resolution_==0?720:resolution_==1?800:resolution_==2?400:resolution_==3?480:1200);
 
 	dai::Pipeline p;
 	auto monoLeft  = p.create<dai::node::MonoCamera>();
 	auto monoRight = p.create<dai::node::MonoCamera>();
 	auto stereo    = p.create<dai::node::StereoDepth>();
-	auto imu       = p.create<dai::node::IMU>();
+	std::shared_ptr<dai::node::IMU> imu;
+	if(imuPublished_)
+		imu = p.create<dai::node::IMU>();
+
 	auto xoutLeft = p.create<dai::node::XLinkOut>();
 	auto xoutDepthOrRight = p.create<dai::node::XLinkOut>();
-	auto xoutIMU = p.create<dai::node::XLinkOut>();
+	std::shared_ptr<dai::node::XLinkOut> xoutIMU;
+	if(imuPublished_)
+		xoutIMU = p.create<dai::node::XLinkOut>();
 
 	// XLinkOut
 	xoutLeft->setStreamName("rectified_left");
 	xoutDepthOrRight->setStreamName(outputDepth_?"depth":"rectified_right");
-	xoutIMU->setStreamName("imu");
+	if(imuPublished_)
+		xoutIMU->setStreamName("imu");
 
 	// MonoCamera
 	monoLeft->setResolution((dai::MonoCameraProperties::SensorResolution)resolution_);
@@ -193,19 +209,22 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 		stereo->rectifiedRight.link(xoutDepthOrRight->input);
 	}
 
-	// enable ACCELEROMETER_RAW and GYROSCOPE_RAW at 200 hz rate
-	imu->enableIMUSensor({dai::IMUSensor::ACCELEROMETER_RAW, dai::IMUSensor::GYROSCOPE_RAW}, 200);
-	// above this threshold packets will be sent in batch of X, if the host is not blocked and USB bandwidth is available
-	imu->setBatchReportThreshold(1);
-	// maximum number of IMU packets in a batch, if it's reached device will block sending until host can receive it
-	// if lower or equal to batchReportThreshold then the sending is always blocking on device
-	// useful to reduce device's CPU load  and number of lost packets, if CPU load is high on device side due to multiple nodes
-	imu->setMaxBatchReports(10);
+	if(imuPublished_)
+	{
+		// enable ACCELEROMETER_RAW and GYROSCOPE_RAW at 200 hz rate
+		imu->enableIMUSensor({dai::IMUSensor::ACCELEROMETER_RAW, dai::IMUSensor::GYROSCOPE_RAW}, 200);
+		// above this threshold packets will be sent in batch of X, if the host is not blocked and USB bandwidth is available
+		imu->setBatchReportThreshold(1);
+		// maximum number of IMU packets in a batch, if it's reached device will block sending until host can receive it
+		// if lower or equal to batchReportThreshold then the sending is always blocking on device
+		// useful to reduce device's CPU load  and number of lost packets, if CPU load is high on device side due to multiple nodes
+		imu->setMaxBatchReports(10);
 
-	// Link plugins IMU -> XLINK
-	imu->out.link(xoutIMU->input);
+		// Link plugins IMU -> XLINK
+		imu->out.link(xoutIMU->input);
 
-	imu->enableFirmwareUpdate(imuFirmwareUpdate_);
+		imu->enableFirmwareUpdate(imuFirmwareUpdate_);
+	}
 
 	device_.reset(new dai::Device(p, deviceToUse));
 
@@ -221,23 +240,33 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 	UINFO("left: fx=%f fy=%f cx=%f cy=%f baseline=%f", fx, fy, cx, cy, baseline);
 	stereoModel_ = StereoCameraModel(device_->getMxId(), fx, fy, cx, cy, baseline, this->getLocalTransform(), targetSize);
 
-	// Cannot test the following, I get "IMU calibration data is not available on device yet." with my camera
-	// Update: now (as March 6, 2022) it crashes in "dai::CalibrationHandler::getImuToCameraExtrinsics(dai::CameraBoardSocket, bool)"
-	//matrix = calibHandler.getImuToCameraExtrinsics(dai::CameraBoardSocket::LEFT);
-	//imuLocalTransform_ = Transform(
-	//		matrix[0][0], matrix[0][1], matrix[0][2], matrix[0][3],
-	//		matrix[1][0], matrix[1][1], matrix[1][2], matrix[1][3],
-	//		matrix[2][0], matrix[2][1], matrix[2][2], matrix[2][3]);
-	// Hard-coded: x->down, y->left, z->forward
-	imuLocalTransform_ = Transform(
-			 0, 0, 1, 0,
-			 0, 1, 0, 0,
-			-1 ,0, 0, 0);
-	UINFO("IMU local transform = %s", imuLocalTransform_.prettyPrint().c_str());
+	if(imuPublished_)
+	{
+		// Cannot test the following, I get "IMU calibration data is not available on device yet." with my camera
+		// Update: now (as March 6, 2022) it crashes in "dai::CalibrationHandler::getImuToCameraExtrinsics(dai::CameraBoardSocket, bool)"
+		//matrix = calibHandler.getImuToCameraExtrinsics(dai::CameraBoardSocket::LEFT);
+		//imuLocalTransform_ = Transform(
+		//		matrix[0][0], matrix[0][1], matrix[0][2], matrix[0][3],
+		//		matrix[1][0], matrix[1][1], matrix[1][2], matrix[1][3],
+		//		matrix[2][0], matrix[2][1], matrix[2][2], matrix[2][3]);
+		// Hard-coded: x->down, y->left, z->forward
+		imuLocalTransform_ = Transform(
+				 0, 0, 1, 0,
+				 0, 1, 0, 0,
+				-1 ,0, 0, 0);
+		UINFO("IMU local transform = %s", imuLocalTransform_.prettyPrint().c_str());
+	}
+	else
+	{
+		UINFO("IMU disabled");
+	}
 
-	leftQueue_ = device_->getOutputQueue("rectified_left", 8, false);
-	rightOrDepthQueue_ = device_->getOutputQueue(outputDepth_?"depth":"rectified_right", 8, false);
-	imuQueue_ = device_->getOutputQueue("imu", 50, false);
+	if(imuPublished_)
+	{
+		imuQueue_ = device_->getOutputQueue("imu", 50, false);
+	}
+	leftQueue_ = device_->getOutputQueue("rectified_left", 1, false);
+	rightOrDepthQueue_ = device_->getOutputQueue(outputDepth_?"depth":"rectified_right", 1, false);
 
 	uSleep(2000); // avoid bad frames on start
 
@@ -304,35 +333,42 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info)
 			}
 
 			//get imu
-			int added=  0;
-			while(1)
+			double stampStart = UTimer::now();
+			while(imuPublished_ && imuQueue_.get())
 			{
-				auto imuData = imuQueue_->get<dai::IMUData>();
-
-				auto imuPackets = imuData->packets;
-				double accStamp = 0.0;
-				double gyroStamp = 0.0;
-				for(auto& imuPacket : imuPackets) {
-					auto& acceleroValues = imuPacket.acceleroMeter;
-					auto& gyroValues = imuPacket.gyroscope;
-
-					accStamp = double(acceleroValues.timestamp.get().time_since_epoch().count())/10e8;
-					gyroStamp = double(gyroValues.timestamp.get().time_since_epoch().count())/10e8;
-					accBuffer_.insert(accBuffer_.end(), std::make_pair(accStamp, cv::Vec3f(acceleroValues.x, acceleroValues.y, acceleroValues.z)));
-					gyroBuffer_.insert(gyroBuffer_.end(), std::make_pair(gyroStamp, cv::Vec3f(gyroValues.x, gyroValues.y, gyroValues.z)));
-					if(accBuffer_.size() > 1000)
-					{
-						accBuffer_.erase(accBuffer_.begin());
-					}
-					if(gyroBuffer_.size() > 1000)
-					{
-						gyroBuffer_.erase(gyroBuffer_.begin());
-					}
-					++added;
-				}
-				if(accStamp >= stamp && gyroStamp >= stamp)
+				if(imuQueue_->has())
 				{
-					break;
+					auto imuData = imuQueue_->get<dai::IMUData>();
+
+					auto imuPackets = imuData->packets;
+					double accStamp = 0.0;
+					double gyroStamp = 0.0;
+					for(auto& imuPacket : imuPackets) {
+						auto& acceleroValues = imuPacket.acceleroMeter;
+						auto& gyroValues = imuPacket.gyroscope;
+
+						accStamp = double(acceleroValues.timestamp.get().time_since_epoch().count())/10e8;
+						gyroStamp = double(gyroValues.timestamp.get().time_since_epoch().count())/10e8;
+						accBuffer_.insert(accBuffer_.end(), std::make_pair(accStamp, cv::Vec3f(acceleroValues.x, acceleroValues.y, acceleroValues.z)));
+						gyroBuffer_.insert(gyroBuffer_.end(), std::make_pair(gyroStamp, cv::Vec3f(gyroValues.x, gyroValues.y, gyroValues.z)));
+						if(accBuffer_.size() > 1000)
+						{
+							accBuffer_.erase(accBuffer_.begin());
+						}
+						if(gyroBuffer_.size() > 1000)
+						{
+							gyroBuffer_.erase(gyroBuffer_.begin());
+						}
+					}
+					if(accStamp >= stamp && gyroStamp >= stamp)
+					{
+						break;
+					}
+				}
+				if((UTimer::now() - stampStart) > 0.01)
+				{
+					UWARN("Could not received IMU after 10 ms! Disabling IMU!");
+					imuPublished_ = false;
 				}
 			}
 

corelib/src/odometry/OdometryFLOAM.cpp

@@ -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{

corelib/src/odometry/OdometryLOAM.cpp

@@ -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_);

corelib/src/optimizer/OptimizerG2O.cpp

@@ -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;
 		}
 

corelib/src/optimizer/OptimizerGTSAM.cpp

@@ -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;

docker/bionic/Dockerfile

@@ -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 && \

docker/bionic/android/deps.bash

@@ -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

docker/bionic/android/rtabmap_apiXX/rtabmap.bash

@@ -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

docker/focal/Dockerfile

@@ -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 && \

guilib/include/rtabmap/gui/DatabaseViewer.h

@@ -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,

guilib/include/rtabmap/gui/MainWindow.h

@@ -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();

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -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;

guilib/src/DatabaseViewer.cpp

@@ -1830,7 +1830,6 @@ void DatabaseViewer::updateIds()
 		previousPose=p;
 
 		//links
-		bool addPose = links.find(ids_[i]) == links.end();
 		for(std::multimap<int, Link>::iterator jter=links.find(ids_[i]); jter!=links.end() && jter->first == ids_[i]; ++jter)
 		{
 			if(jter->second.type() == Link::kNeighborMerged)
@@ -1856,34 +1855,27 @@ void DatabaseViewer::updateIds()
 				{
 					links_.insert(std::make_pair(ids_[i], jter->second));
 				}
-				addPose = true;
-			}
-			else if(graph::findLink(links_, jter->second.from(), jter->second.to()) != links_.end())
-			{
-				addPose = true;
 			}
 		}
-		if(addPose)
+		// Add pose
+		odomPoses_.insert(std::make_pair(ids_[i], p));
+		if(!g.isNull())
 		{
-			odomPoses_.insert(std::make_pair(ids_[i], p));
-			if(!g.isNull())
-			{
-				groundTruthPoses_.insert(std::make_pair(ids_[i], g));
-			}
-			if(gps.stamp() > 0.0)
-			{
-				gpsValues_.insert(std::make_pair(ids_[i], gps));
+			groundTruthPoses_.insert(std::make_pair(ids_[i], g));
+		}
+		if(gps.stamp() > 0.0)
+		{
+			gpsValues_.insert(std::make_pair(ids_[i], gps));
 
-				cv::Point3f p(0.0f,0.0f,0.0f);
-				if(!gpsPoses_.empty())
-				{
-					GeodeticCoords coords = gps.toGeodeticCoords();
-					GPS originGPS = gpsValues_.begin()->second;
-					p = coords.toENU_WGS84(originGPS.toGeodeticCoords());
-				}
-				Transform pose(p.x, p.y, p.z, 0.0f, 0.0f, (float)((-(gps.bearing()-90))*M_PI/180.0));
-				gpsPoses_.insert(std::make_pair(ids_[i], pose));
+			cv::Point3f p(0.0f,0.0f,0.0f);
+			if(!gpsPoses_.empty())
+			{
+				GeodeticCoords coords = gps.toGeodeticCoords();
+				GPS originGPS = gpsValues_.begin()->second;
+				p = coords.toENU_WGS84(originGPS.toGeodeticCoords());
 			}
+			Transform pose(p.x, p.y, p.z, 0.0f, 0.0f, (float)((-(gps.bearing()-90))*M_PI/180.0));
+			gpsPoses_.insert(std::make_pair(ids_[i], pose));
 		}
 	}
 
@@ -3234,17 +3226,57 @@ void DatabaseViewer::regenerateSavedMap()
 		return;
 	}
 
+	//
+#ifdef RTABMAP_OCTOMAP
+	QStringList types;
+	types.push_back("Default occupancy grid");
+	types.push_back("From OctoMap projection");
+	bool ok;
+	QString type = QInputDialog::getItem(this, tr("Which type?"), tr("Type:"), types, 0, false, &ok);
+	if(!ok)
+	{
+		return;
+	}
+#endif
 
 	//update scans
 	UINFO("Update local maps list...");
-	OccupancyGrid grid(ui_->parameters_toolbox->getParameters());
-	for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=localMaps_.begin(); iter!=localMaps_.end(); ++iter)
-	{
-		grid.addToCache(iter->first, iter->second.first.first, iter->second.first.second, iter->second.second);
-	}
-	grid.update(graphes_.back());
 	float xMin, yMin;
-	cv::Mat map = grid.getMap(xMin, yMin);
+	cv::Mat map;
+	float gridCellSize = Parameters::defaultGridCellSize();
+	Parameters::parse(ui_->parameters_toolbox->getParameters(), Parameters::kGridCellSize(), gridCellSize);
+#ifdef RTABMAP_OCTOMAP
+	if(type.compare("From OctoMap projection") == 0)
+	{
+		//create local octomap
+		OctoMap octomap(ui_->parameters_toolbox->getParameters());
+		for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=localMaps_.begin(); iter!=localMaps_.end(); ++iter)
+		{
+			if(iter->second.first.first.channels() == 2 || iter->second.first.second.channels() == 2)
+			{
+				QMessageBox::warning(this, tr(""),
+						tr("Some local occupancy grids are 2D, but OctoMap requires 3D local "
+							"occupancy grids. Select default occupancy grid or generate "
+							"3D local occupancy grids (\"Grid/3D\" core parameter)."));
+				return;
+			}
+			octomap.addToCache(iter->first, iter->second.first.first, iter->second.first.second, iter->second.second, localMapsInfo_.at(iter->first).second);
+		}
+
+		octomap.update(graphes_.back());
+		map = octomap.createProjectionMap(xMin, yMin, gridCellSize, 0);
+	}
+	else
+#endif
+	{
+		OccupancyGrid grid(ui_->parameters_toolbox->getParameters());
+		for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=localMaps_.begin(); iter!=localMaps_.end(); ++iter)
+		{
+			grid.addToCache(iter->first, iter->second.first.first, iter->second.first.second, iter->second.second);
+		}
+		grid.update(graphes_.back());
+		map = grid.getMap(xMin, yMin);
+	}
 
 	if(map.empty())
 	{
@@ -3252,7 +3284,7 @@ void DatabaseViewer::regenerateSavedMap()
 	}
 	else
 	{
-		dbDriver_->save2DMap(map, xMin, yMin, grid.getCellSize());
+		dbDriver_->save2DMap(map, xMin, yMin, gridCellSize);
 		Transform lastlocalizationPose;
 		dbDriver_->loadOptimizedPoses(&lastlocalizationPose);
 		if(lastlocalizationPose.isNull() && !graphes_.back().empty())
@@ -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)
 			}
 		}
 
-		t = reg->computeTransformationMod(*fromS, *toS, guess, &info);
+		if(switchedIds)
+		{
+			t = reg->computeTransformationMod(*toS, *fromS, guess.isNull()?guess:guess.inverse(), &info);
+			if(!t.isNull())
+			{
+				t = t.inverse();
+			}
+		}
+		else
+		{
+			t = reg->computeTransformationMod(*fromS, *toS, guess, &info);
+		}
 		delete reg;
 		UDEBUG("");
 

guilib/src/GraphViewer.cpp

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

guilib/src/GuiLib.qrc

@@ -42,6 +42,7 @@
         <file>images/mynteyes.png</file>
         <file>images/l515.png</file>
         <file>images/oakd.png</file>
-		<file>images/astra.png</file>
+        <file>images/oakd_lite.png</file>
+        <file>images/astra.png</file>
     </qresource>
 </RCC>

guilib/src/MainWindow.cpp

@@ -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())

guilib/src/PreferencesDialog.cpp

@@ -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();

guilib/src/ui/DatabaseViewer.ui

@@ -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>

guilib/src/ui/mainWindow.ui

@@ -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>

package.xml

@@ -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>

tools/Export/main.cpp

@@ -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())

tools/Reprocess/main.cpp

@@ -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);
-		printf("Parameter \"%s\" is set to false, initializing RTAB-Map with \"%s\" for localization...\n", Parameters::kMemIncrementalMemory().c_str(), databases.front().c_str());
+		if(!incrementalMemory)
+		{
+			printf("Parameter \"%s\" is set to false, initializing RTAB-Map with \"%s\" for localization...\n", Parameters::kMemIncrementalMemory().c_str(), databases.front().c_str());
+		}
 		databases.pop_front();
 		inputDatabasePath = uJoin(databases, ";");
 	}
@@ -627,7 +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,84 +808,109 @@ int main(int argc, char * argv[])
 				printf("Failed processing node %d.\n", data.id());
 				globalMapStats.clear();
 			}
-			else if(assemble2dMap || assemble3dMap || assemble2dOctoMap || assemble3dOctoMap)
+			else
 			{
-				globalMapStats.clear();
-				double timeRtabmap = t.ticks();
-				double timeUpdateInit = 0.0;
-				double timeUpdateGrid = 0.0;
-#ifdef RTABMAP_OCTOMAP
-				double timeUpdateOctoMap = 0.0;
-#endif
-				const rtabmap::Statistics & stats = rtabmap.getStatistics();
-				if(stats.poses().size() && stats.getLastSignatureData().id())
+				if(republishLoopClosures && dbReader->driver())
 				{
-					int id = stats.poses().rbegin()->first;
-					if(id == stats.getLastSignatureData().id() &&
-					   stats.getLastSignatureData().sensorData().gridCellSize() > 0.0f)
+					std::multimap<int, Link> links;
+					dbReader->driver()->loadLinks(data.id(), links);
+					for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
 					{
-						bool updateGridMap = false;
-						bool updateOctoMap = false;
-						if((assemble2dMap || assemble3dMap) && grid.addedNodes().find(id) == grid.addedNodes().end())
+						if((iter->second.type() == Link::kGlobalClosure ||
+							iter->second.type() == Link::kLocalSpaceClosure ||
+							iter->second.type() == Link::kLocalTimeClosure ||
+							iter->second.type() == Link::kUserClosure) &&
+							iter->second.to() < data.id())
 						{
-							updateGridMap = true;
-						}
-#ifdef RTABMAP_OCTOMAP
-						if((assemble2dOctoMap || assemble3dOctoMap) && octomap.addedNodes().find(id) == octomap.addedNodes().end())
-						{
-							updateOctoMap = true;
-						}
-#endif
-						if(updateGridMap || updateOctoMap)
-						{
-							cv::Mat ground, obstacles, empty;
-							stats.getLastSignatureData().sensorData().uncompressDataConst(0, 0, 0, 0, &ground, &obstacles, &empty);
-
-							timeUpdateInit = t.ticks();
-
-							if(updateGridMap)
+							if(!iter->second.transform().isNull() &&
+								rtabmap.getMemory()->getWorkingMem().find(iter->second.to()) != rtabmap.getMemory()->getWorkingMem().end() &&
+								rtabmap.addLink(iter->second))
 							{
-								grid.addToCache(id, ground, obstacles, empty);
-								grid.update(stats.poses());
-								timeUpdateGrid = t.ticks() + timeUpdateInit;
+								printf("Added link %d->%d from input database.\n", iter->second.from(), iter->second.to());
 							}
-#ifdef RTABMAP_OCTOMAP
-							if(updateOctoMap)
-							{
-								const cv::Point3f & viewpoint = stats.getLastSignatureData().sensorData().gridViewPoint();
-								octomap.addToCache(id, ground, obstacles, empty, viewpoint);
-								octomap.update(stats.poses());
-								timeUpdateOctoMap = t.ticks() + timeUpdateInit;
-							}
-#endif
 						}
 					}
 				}
 
-				globalMapStats.insert(std::make_pair(std::string("GlobalGrid/GridUpdate/ms"), timeUpdateGrid*1000.0f));
+				if(assemble2dMap || assemble3dMap || assemble2dOctoMap || assemble3dOctoMap)
+				{
+					globalMapStats.clear();
+					double timeRtabmap = t.ticks();
+					double timeUpdateInit = 0.0;
+					double timeUpdateGrid = 0.0;
 #ifdef RTABMAP_OCTOMAP
-				//Simulate publishing
-				double timePub2dOctoMap = 0.0;
-				double timePub3dOctoMap = 0.0;
-				if(assemble2dOctoMap)
-				{
-					float xMin, yMin, size;
-					octomap.createProjectionMap(xMin, yMin, size);
-					timePub2dOctoMap = t.ticks();
-				}
-				if(assemble3dOctoMap)
-				{
-					octomap.createCloud();
-					timePub3dOctoMap = t.ticks();
-				}
-
-				globalMapStats.insert(std::make_pair(std::string("GlobalGrid/OctoMapUpdate/ms"), timeUpdateOctoMap*1000.0f));
-				globalMapStats.insert(std::make_pair(std::string("GlobalGrid/OctoMapProjection/ms"), timePub2dOctoMap*1000.0f));
-				globalMapStats.insert(std::make_pair(std::string("GlobalGrid/OctomapToCloud/ms"), timePub3dOctoMap*1000.0f));
-				globalMapStats.insert(std::make_pair(std::string("GlobalGrid/TotalWithRtabmap/ms"), (timeUpdateGrid+timeUpdateOctoMap+timePub2dOctoMap+timePub3dOctoMap+timeRtabmap)*1000.0f));
-#else
-				globalMapStats.insert(std::make_pair(std::string("GlobalGrid/TotalWithRtabmap/ms"), (timeUpdateGrid+timeRtabmap)*1000.0f));
+					double timeUpdateOctoMap = 0.0;
 #endif
+					const rtabmap::Statistics & stats = rtabmap.getStatistics();
+					if(stats.poses().size() && stats.getLastSignatureData().id())
+					{
+						int id = stats.poses().rbegin()->first;
+						if(id == stats.getLastSignatureData().id() &&
+						   stats.getLastSignatureData().sensorData().gridCellSize() > 0.0f)
+						{
+							bool updateGridMap = false;
+							bool updateOctoMap = false;
+							if((assemble2dMap || assemble3dMap) && grid.addedNodes().find(id) == grid.addedNodes().end())
+							{
+								updateGridMap = true;
+							}
+#ifdef RTABMAP_OCTOMAP
+							if((assemble2dOctoMap || assemble3dOctoMap) && octomap.addedNodes().find(id) == octomap.addedNodes().end())
+							{
+								updateOctoMap = true;
+							}
+#endif
+							if(updateGridMap || updateOctoMap)
+							{
+								cv::Mat ground, obstacles, empty;
+								stats.getLastSignatureData().sensorData().uncompressDataConst(0, 0, 0, 0, &ground, &obstacles, &empty);
+
+								timeUpdateInit = t.ticks();
+
+								if(updateGridMap)
+								{
+									grid.addToCache(id, ground, obstacles, empty);
+									grid.update(stats.poses());
+									timeUpdateGrid = t.ticks() + timeUpdateInit;
+								}
+#ifdef RTABMAP_OCTOMAP
+								if(updateOctoMap)
+								{
+									const cv::Point3f & viewpoint = stats.getLastSignatureData().sensorData().gridViewPoint();
+									octomap.addToCache(id, ground, obstacles, empty, viewpoint);
+									octomap.update(stats.poses());
+									timeUpdateOctoMap = t.ticks() + timeUpdateInit;
+								}
+#endif
+							}
+						}
+					}
+
+					globalMapStats.insert(std::make_pair(std::string("GlobalGrid/GridUpdate/ms"), timeUpdateGrid*1000.0f));
+#ifdef RTABMAP_OCTOMAP
+					//Simulate publishing
+					double timePub2dOctoMap = 0.0;
+					double timePub3dOctoMap = 0.0;
+					if(assemble2dOctoMap)
+					{
+						float xMin, yMin, size;
+						octomap.createProjectionMap(xMin, yMin, size);
+						timePub2dOctoMap = t.ticks();
+					}
+					if(assemble3dOctoMap)
+					{
+						octomap.createCloud();
+						timePub3dOctoMap = t.ticks();
+					}
+
+					globalMapStats.insert(std::make_pair(std::string("GlobalGrid/OctoMapUpdate/ms"), timeUpdateOctoMap*1000.0f));
+					globalMapStats.insert(std::make_pair(std::string("GlobalGrid/OctoMapProjection/ms"), timePub2dOctoMap*1000.0f));
+					globalMapStats.insert(std::make_pair(std::string("GlobalGrid/OctomapToCloud/ms"), timePub3dOctoMap*1000.0f));
+					globalMapStats.insert(std::make_pair(std::string("GlobalGrid/TotalWithRtabmap/ms"), (timeUpdateGrid+timeUpdateOctoMap+timePub2dOctoMap+timePub3dOctoMap+timeRtabmap)*1000.0f));
+#else
+					globalMapStats.insert(std::make_pair(std::string("GlobalGrid/TotalWithRtabmap/ms"), (timeUpdateGrid+timeRtabmap)*1000.0f));
+#endif
+				}
 			}
 		}
 

utilite/include/rtabmap/utilite/UConversion.h

@@ -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.

utilite/include/rtabmap/utilite/UVariant.h

@@ -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() {}
 

utilite/src/UConversion.cpp

@@ -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();
 }

utilite/src/UVariant.cpp

@@ -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;
+}