latest update

Co-authored-by: FIRST_NAME LAST_NAME <[email protected]>

.appveyor.yml

@@ -17,6 +17,9 @@ init:
   - call "C:\Program Files (x86)\Microsoft Visual Studio 14.0\VC\vcvarsall.bat" x86_amd64
 
 install:
+  # To download from google drive
+  - set PATH=C:\Python38-x64;C:\Python38-x64\Scripts;%PATH%
+  - ps: py -m pip --disable-pip-version-check install gdown
   # Qt
   - set QTDIR=C:\Qt\5.10.1\msvc2015_64
   # make sure Qt bin path is before cmake bin path to avoid copying qt5 dlls from cmake before qt installation
@@ -73,7 +76,7 @@ install:
   - ps: "ls \"C:/Program Files/PCL\""
   - set PATH=%PATH%;C:\Program Files\PCL\bin
   # zlib
-  - ps: wget 'https://docs.google.com/uc?authuser=0&id=0B46akLGdg-uaYm9MTTI4MUtUcmc&export=download' -outfile zlib-1.2.8-vc2010-x64.zip
+  - ps: gdown -q 0B46akLGdg-uaYm9MTTI4MUtUcmc
   - ps: Expand-Archive zlib-1.2.8-vc2010-x64.zip -DestinationPath 'C:\Program Files'
   - ECHO "Installed zlib:"
   - ps: "ls \"C:/Program Files/zlib\""

CMakeLists.txt

@@ -218,7 +218,7 @@ option(WITH_OPENVINS      "Include OpenVINS support"             OFF)
 option(WITH_MADGWICK      "Include Madgwick IMU filtering support" ON)
 option(WITH_FASTCV        "Include FastCV support"               ON)
 option(WITH_OPENMP        "Include OpenMP support"               ON)
-option(WITH_OPENGV        "Include OpenGV support"               OFF)
+option(WITH_OPENGV        "Include OpenGV support"               ON)
 IF(MOBILE_BUILD)
 option(PCL_OMP            "With PCL OMP implementations"         OFF)
 ELSE()
@@ -228,7 +228,7 @@ ENDIF()
 set(RTABMAP_QT_VERSION AUTO CACHE STRING "Force a specific Qt version.")
 set_property(CACHE RTABMAP_QT_VERSION PROPERTY STRINGS AUTO 4 5 6)
 
-FIND_PACKAGE(OpenCV REQUIRED QUIET COMPONENTS core calib3d imgproc highgui stitching photo video OPTIONAL_COMPONENTS aruco xfeatures2d nonfree gpu cudafeatures2d)
+FIND_PACKAGE(OpenCV REQUIRED QUIET COMPONENTS core calib3d imgproc highgui stitching photo video videoio OPTIONAL_COMPONENTS aruco xfeatures2d nonfree gpu cudafeatures2d)
 
 IF(WITH_QT)
 FIND_PACKAGE(PCL 1.7 REQUIRED QUIET COMPONENTS common io kdtree search surface filters registration sample_consensus segmentation visualization)
@@ -977,10 +977,10 @@ IF(NOT mynteye_FOUND)
    SET(MYNTEYE "//")
 ENDIF(NOT mynteye_FOUND)
 IF(NOT depthai_FOUND)
-   SET(CONF_DEPTH_AI OFF)
+   SET(CONF_WITH_DEPTH_AI 0)
    SET(DEPTHAI "//")
 ELSE()
-   SET(CONF_DEPTH_AI ON) 
+   SET(CONF_WITH_DEPTH_AI 1)
 ENDIF()
 IF(NOT octomap_FOUND)
    SET(OCTOMAP "//")

README.md

@@ -4,11 +4,15 @@ rtabmap
 [![RTAB-Map Logo](https://raw.githubusercontent.com/introlab/rtabmap/master/guilib/src/images/RTAB-Map100.png)](http://introlab.github.io/rtabmap)
 
 [![Release][release-image]][releases]
+[![Downloads][downloads-image]][downloads]
 [![License][license-image]][license]
 
-[release-image]: https://img.shields.io/badge/release-0.20.16-green.svg?style=flat
+[release-image]: https://img.shields.io/badge/release-0.21.0-green.svg?style=flat
 [releases]: https://github.com/introlab/rtabmap/releases
 
+[downloads-image]: https://img.shields.io/github/downloads/introlab/rtabmap/total?label=downloads
+[downloads]: https://github.com/introlab/rtabmap/releases
+
 [license-image]: https://img.shields.io/badge/license-BSD-green.svg?style=flat
 [license]: https://github.com/introlab/rtabmap/blob/master/LICENSE
 
@@ -71,6 +75,12 @@ This project is supported by [IntRoLab - Intelligent / Interactive / Integrated
             <td>Rolling</td>
             <td><a href="http://build.ros2.org/job/Rbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary" alt="Build Status"/></td>
         </tr>
+        <tr>
+           <td>Docker</td>
+           <td>
+             <a href="https://hub.docker.com/r/introlab3it/rtabmap">rtabmap</a>
+           </td>
+           <td><img src="https://img.shields.io/docker/pulls/introlab3it/rtabmap.svg?label=pulls" alt="Docker Pulls"/></td>
+        </tr>
     </tbody>
 </table>
- 

RTABMapConfig.cmake.in

@@ -42,6 +42,10 @@ IF(@CONF_WITH_K4A@)
     ENDIF()
 ENDIF()
 
+IF(@CONF_WITH_DEPTH_AI@)
+ find_dependency(depthai 2)
+ENDIF()
+
 IF(@CONF_WITH_OCTOMAP@)
   find_dependency(octomap)
 ENDIF()

corelib/include/rtabmap/core/IMUThread.h

@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 
 #include <rtabmap/core/Transform.h>
+#include <rtabmap/core/Parameters.h>
 #include <rtabmap/utilite/UThread.h>
 #include <rtabmap/utilite/UEventsSender.h>
 #include <rtabmap/utilite/UTimer.h>
@@ -39,6 +40,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap
 {
 
+class IMUFilter;
+
 /**
  * Class IMUThread
  *
@@ -53,6 +56,8 @@ public:
 
 	bool init(const std::string & path);
 	void setRate(int rate);
+	void enableIMUFiltering(int filteringStrategy=1, const ParametersMap & parameters = ParametersMap(), bool baseFrameConversion = false);
+	void disableIMUFiltering();
 
 private:
 	virtual void mainLoopBegin();
@@ -65,6 +70,8 @@ private:
 	UTimer frameRateTimer_;
 	double captureDelay_;
 	double previousStamp_;
+	IMUFilter * _imuFilter;
+	bool _imuBaseFrameConversion;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/OccupancyGrid.h

@@ -132,6 +132,7 @@ private:
 	bool normalsSegmentation_;
 	bool grid3D_;
 	bool groundIsObstacle_;
+	bool labelUndergroundObstaclesAsGround_;
 	float noiseFilteringRadius_;
 	int noiseFilteringMinNeighbors_;
 	bool scan2dUnknownSpaceFilled_;

corelib/include/rtabmap/core/Parameters.h

@@ -376,6 +376,7 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(RGBD, MarkerDetection,              bool, false,  "Detect static markers to be added as landmarks for graph optimization. If input data have already landmarks, this will be ignored. See \"Marker\" group for parameters.");
     RTABMAP_PARAM(RGBD, LoopCovLimited,               bool, false,  "Limit covariance of non-neighbor links to minimum covariance of neighbor links. In other words, if covariance of a loop closure link is smaller than the minimum covariance of odometry links, its covariance is set to minimum covariance of odometry links.");
     RTABMAP_PARAM(RGBD, MaxOdomCacheSize,             int,  10,      uFormat("Maximum odometry cache size. Used only in localization mode (when %s=false). This is used to get smoother localizations and to verify localization transforms (when %s!=0) to make sure we don't teleport to a location very similar to one we previously localized on. Set 0 to disable caching.", kMemIncrementalMemory().c_str(), kRGBDOptimizeMaxError().c_str()));
+    RTABMAP_PARAM(RGBD, LocalizationSmoothing,        bool, true,    uFormat("Adjust localization constraints based on optimized odometry cache poses (when %s>0).",  kRGBDMaxOdomCacheSize().c_str()));
 
     // Local/Proximity loop closure detection
     RTABMAP_PARAM(RGBD, ProximityByTime,              bool, false, "Detection over all locations in STM.");
@@ -759,6 +760,7 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(Grid, 3D,                      bool,   false,   uFormat("A 3D occupancy grid is required if you want an OctoMap (3D ray tracing). Set to false if you want only a 2D map, the cloud will be projected on xy plane. A 2D map can be still generated if checked, but it requires more memory and time to generate it. Ignored if laser scan is 2D and \"%s\" is 0.", kGridSensor().c_str()));
 #endif
     RTABMAP_PARAM(Grid, GroundIsObstacle,           bool,   false,   uFormat("[%s=true] Ground segmentation (%s) is ignored, all points are obstacles. Use this only if you want an OctoMap with ground identified as an obstacle (e.g., with an UAV).", kGrid3D().c_str(), kGridNormalsSegmentation().c_str()));
+    RTABMAP_PARAM(Grid, UndergroundIsGround,        bool,   false,   uFormat("[%s=true] Label all underground points under largest flat surface detected as ground.", kGridNormalsSegmentation().c_str()));
     RTABMAP_PARAM(Grid, NoiseFilteringRadius,       float,   0.0,    "Noise filtering radius (0=disabled). Done after segmentation.");
     RTABMAP_PARAM(Grid, NoiseFilteringMinNeighbors, int,     5,      "Noise filtering minimum neighbors.");
     RTABMAP_PARAM(Grid, Scan2dUnknownSpaceFilled,   bool,    false,  uFormat("Unknown space filled. Only used with 2D laser scans. Use %s to set maximum range if laser scan max range is to set.", kGridRangeMax().c_str()));

corelib/include/rtabmap/core/Rtabmap.h

@@ -326,6 +326,7 @@ private:
 	bool _loopCovLimited;
 	bool _loopGPS;
 	int _maxOdomCacheSize;
+	bool _localizationSmoothing;
 	bool _createGlobalScanMap;
 	float _markerPriorsLinearVariance;
 	float _markerPriorsAngularVariance;

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

@@ -58,6 +58,9 @@ public:
 	void setOutputDepth(bool enabled, int confidence = 200);
 	void setIMUFirmwareUpdate(bool enabled);
 	void setIMUPublished(bool published);
+	void publishInterIMU(bool enabled);
+	void setLaserDotBrightness(float dotProjectormA = 0.0f);
+	void setFloodLightBrightness(float floodLightmA = 200.0f);
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
 	virtual bool isCalibrated() const;
@@ -76,12 +79,15 @@ private:
 	int resolution_;
 	bool imuFirmwareUpdate_;
 	bool imuPublished_;
+	bool publishInterIMU_;
+	float dotProjectormA_;
+	float floodLightmA_;
 	std::shared_ptr<dai::Device> device_;
 	std::shared_ptr<dai::DataOutputQueue> leftQueue_;
 	std::shared_ptr<dai::DataOutputQueue> rightOrDepthQueue_;
-	std::shared_ptr<dai::DataOutputQueue> imuQueue_;
 	std::map<double, cv::Vec3f> accBuffer_;
 	std::map<double, cv::Vec3f> gyroBuffer_;
+	UMutex imuMutex_;
 #endif
 };
 

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

@@ -45,11 +45,11 @@ class RTABMAP_CORE_EXPORT CameraStereoZed :
 {
 public:
 	static bool available();
-
+	static int sdkVersion();
 public:
 	CameraStereoZed(
 			int deviceId,
-			int resolution = 2, // 0=HD2K, 1=HD1080, 2=HD720, 3=VGA
+			int resolution = 6, // 0=HD2K, 1=HD1080, 2=HD1200, 3=HD720, 4=SVGA, 5=VGA, 6=AUTO
 			int quality = 1,    // 0=NONE, 1=PERFORMANCE, 2=QUALITY
 			int sensingMode = 0,// 0=STANDARD, 1=FILL
 			int confidenceThr = 100,
@@ -61,7 +61,7 @@ public:
 			int texturenessConfidenceThr = 90); // introduced with ZED SDK 3
 	CameraStereoZed(
 			const std::string & svoFilePath,
-			int quality = 1,    // 0=NONE, 1=PERFORMANCE, 2=QUALITY
+			int quality = 1,    // 0=NONE, 1=PERFORMANCE, 2=QUALITY, 3=NEURAL
 			int sensingMode = 0,// 0=STANDARD, 1=FILL
 			int confidenceThr = 100,
 			bool computeOdometry = false,

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

@@ -44,6 +44,8 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
 		pcl::IndicesPtr & obstaclesIndices,
 		pcl::IndicesPtr * flatObstacles) const
 {
+	UDEBUG("cloudIn=%dx%d indicesIn=%ld", cloudIn->width, cloudIn->height, indicesIn->size());
+
 	groundIndices.reset(new std::vector<int>);
 	obstaclesIndices.reset(new std::vector<int>);
 	if(flatObstacles)
@@ -54,6 +56,7 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
 	typename pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
 	pcl::IndicesPtr indices(new std::vector<int>);
 
+	UDEBUG("preVoxelFiltering=%d", preVoxelFiltering_?1:0);
 	if(preVoxelFiltering_)
 	{
 		// voxelize to grid cell size
@@ -127,6 +130,9 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
 			UDEBUG("flatObstaclesDetected=%d", flatObstaclesDetected_?1:0);
 			UDEBUG("maxGroundHeight=%f", maxGroundHeight_);
 			UDEBUG("groundNormalsUp=%f", groundNormalsUp_);
+			UDEBUG("labelUndergroundObstaclesAsGround=%d", labelUndergroundObstaclesAsGround_?1:0);
+			UDEBUG("viewPoint=%f,%f,%f", viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?pose.z():0));
+			UDEBUG("cloud=%dx%d indices=%ld", cloud->width, cloud->height, indices->size());
 			util3d::segmentObstaclesFromGround<PointT>(
 					cloud,
 					indices,
@@ -140,8 +146,8 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
 					maxGroundHeight_,
 					flatObstacles,
 					Eigen::Vector4f(viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?pose.z():0), 1),
-					groundNormalsUp_);
+					groundNormalsUp_,
-			UDEBUG("viewPoint=%f,%f,%f", viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?pose.z():0));
+					labelUndergroundObstaclesAsGround_);
 			//UWARN("Saving ground.pcd and obstacles.pcd");
 			//pcl::io::savePCDFile("ground.pcd", *cloud, *groundIndices);
 			//pcl::io::savePCDFile("obstacles.pcd", *cloud, *obstaclesIndices);
@@ -165,6 +171,42 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
 
 		UDEBUG("groundIndices=%d obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
 
+		if(!preVoxelFiltering_ && (!groundIndices->empty() || !obstaclesIndices->empty()))
+		{
+			// voxelize to grid cell size
+			typename pcl::PointCloud<PointT>::Ptr cloudWithTransform = cloud;
+			cloud.reset(new pcl::PointCloud<PointT>);
+			if(!groundIndices->empty())
+			{
+				*cloud += *util3d::voxelize(cloudWithTransform, groundIndices, cellSize_);
+				groundIndices->resize(cloud->size());
+				for(size_t i=0; i<groundIndices->size(); ++i)
+				{
+					groundIndices->at(i) = i;
+				}
+			}
+			if(!obstaclesIndices->empty())
+			{
+				int previousSize = cloud->size();
+				*cloud += *util3d::voxelize(cloudWithTransform, obstaclesIndices, cellSize_);
+				obstaclesIndices->resize(cloud->size()-previousSize);
+				for(size_t i=0; i<obstaclesIndices->size(); ++i)
+				{
+					obstaclesIndices->at(i) = previousSize+i;
+				}
+			}
+			if(flatObstacles && !(*flatObstacles)->empty())
+			{
+				int previousSize = cloud->size();
+				*cloud += *util3d::voxelize(cloudWithTransform, *flatObstacles, cellSize_);
+				(*flatObstacles)->resize(cloud->size()-previousSize);
+				for(size_t i=0; i<(*flatObstacles)->size(); ++i)
+				{
+					(*flatObstacles)->at(i) = previousSize+i;
+				}
+			}
+		}
+
 		// Do radius filtering after voxel filtering ( a lot faster)
 		if(noiseFilteringRadius_ > 0.0 && noiseFilteringMinNeighbors_ > 0)
 		{

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

@@ -50,6 +50,79 @@ typename pcl::PointCloud<PointT>::Ptr projectCloudOnXYPlane(
 	return output;
 }
 
+void clusterIndicesFloodfill(std::vector<int> & cluster,
+	float * visitedIndices,
+	int width,
+	int height,
+	float clusterRadius,
+	int currentIndex,
+	float previousHeight);
+
+/**
+ * @brief Cluster indices of an organized cloud
+ * 
+ * @tparam PointT 
+ * @param cloud 
+ * @param indices 
+ * @param minClusterSize 
+ * @param maxClusterSize 
+ * @param biggestClusterIndex 
+ * @return std::vector<pcl::IndicesPtr> 
+ */
+template<typename PointT>
+std::vector<pcl::IndicesPtr> clusterIndices(
+	const typename pcl::PointCloud<PointT>::Ptr & cloud,
+	const typename pcl::IndicesPtr & indices,
+	float clusterRadius,
+	int minClusterSize,
+	int maxClusterSize,
+	int * biggestClusterIndex)
+{
+	std::vector<pcl::IndicesPtr> clusters;
+	if(cloud->empty())
+	{
+		return clusters;
+	}
+
+	UASSERT(cloud->isOrganized());
+
+	cv::Mat visitedIndices = cv::Mat::zeros(cloud->height, cloud->width, CV_32FC1);
+	float * ptr = visitedIndices.ptr<float>();
+	// init search image
+	for(size_t i = 0; i<indices->size(); ++i)
+	{
+		ptr[indices->at(i)] = cloud->at(indices->at(i)).z;
+	}
+
+	int largestCluster = -1;
+	int largestClusterSize = 0;
+	int sum = 0;
+	for(size_t i = 0; i<indices->size(); ++i)
+	{
+		if(ptr[indices->at(i)] != 0.0f)
+		{
+			pcl::IndicesPtr cluster(new pcl::Indices());
+			clusterIndicesFloodfill(*cluster, ptr, visitedIndices.cols, visitedIndices.rows, clusterRadius, indices->at(i), ptr[indices->at(i)]);
+			if(cluster->size()>0 && (int)cluster->size()>=minClusterSize && (int)cluster->size()<=maxClusterSize)
+			{
+				clusters.push_back(cluster);
+				if((int)cluster->size() > largestClusterSize)
+				{
+					sum+=cluster->size();
+					largestCluster = clusters.size()-1;
+					largestClusterSize = cluster->size();
+				}
+			}
+		}
+	}
+	if(biggestClusterIndex)
+	{
+		*biggestClusterIndex = largestCluster;
+	}
+
+	return clusters;
+}
+
 template<typename PointT>
 void segmentObstaclesFromGround(
 		const typename pcl::PointCloud<PointT>::Ptr & cloud,
@@ -64,7 +137,8 @@ void segmentObstaclesFromGround(
 		float maxGroundHeight,
 		pcl::IndicesPtr * flatObstacles,
 		const Eigen::Vector4f & viewPoint,
-		float groundNormalsUp)
+		float groundNormalsUp,
+		bool labelUndergroundObstaclesAsGround)
 {
 	ground.reset(new std::vector<int>);
 	obstacles.reset(new std::vector<int>);
@@ -75,6 +149,8 @@ void segmentObstaclesFromGround(
 
 	if(cloud->size())
 	{
+		UDEBUG("Normal filtering.... cloud=%ld indices=%ld organized=%d",
+			cloud->size(), indices->size(), cloud->isOrganized()?1:0);
 		// Find the ground
 		pcl::IndicesPtr flatSurfaces = normalFiltering(
 				cloud,
@@ -84,22 +160,40 @@ void segmentObstaclesFromGround(
 				normalKSearch,
 				viewPoint,
 				groundNormalsUp);
+		UDEBUG("%ld points on flat surfaces (input indices = %ld, total cloud=%ld)",
+			flatSurfaces->size(), indices->size(), cloud->size());
 
+		Eigen::Vector4f biggestSurfaceMin,biggestSurfaceMax(0,0,0,0);
 		if(segmentFlatObstacles && flatSurfaces->size())
 		{
 			int biggestFlatSurfaceIndex;
-			std::vector<pcl::IndicesPtr> clusteredFlatSurfaces = extractClusters(
+
+			std::vector<pcl::IndicesPtr> clusteredFlatSurfaces;
+			if(cloud->isOrganized())
+			{
+				clusteredFlatSurfaces = clusterIndices<PointT>(
 					cloud,
 					flatSurfaces,
 					clusterRadius,
 					minClusterSize,
 					std::numeric_limits<int>::max(),
 					&biggestFlatSurfaceIndex);
+					UDEBUG("clusteredFlatSurfaces=%ld", clusteredFlatSurfaces.size());
+			}
+			else
+			{
+				clusteredFlatSurfaces = extractClusters(
+					cloud,
+					flatSurfaces,
+					clusterRadius,
+					minClusterSize,
+					std::numeric_limits<int>::max(),
+					&biggestFlatSurfaceIndex);
+			}
 
 			// cluster all surfaces for which the centroid is in the Z-range of the bigger surface
 			if(clusteredFlatSurfaces.size())
 			{
-				Eigen::Vector4f biggestSurfaceMin,biggestSurfaceMax;
 				if(maxGroundHeight != 0.0f)
 				{
 					// Search for biggest surface under max ground height
@@ -125,9 +219,12 @@ void segmentObstaclesFromGround(
 				if(biggestFlatSurfaceIndex>=0)
 				{
 					ground = clusteredFlatSurfaces.at(biggestFlatSurfaceIndex);
+					UDEBUG("Biggest flat surface size = %ld (%d%%) (z min=%f max=%f)",
+						ground->size(), 100*ground->size()/cloud->size(), biggestSurfaceMin[2], biggestSurfaceMax[2]);
 				}
 
-				if(!ground->empty() && (maxGroundHeight == 0.0f || biggestSurfaceMin[2] < maxGroundHeight))
+				if(!ground->empty() && 
+				  (maxGroundHeight == 0.0f || biggestSurfaceMin[2] < maxGroundHeight))
 				{
 					for(unsigned int i=0; i<clusteredFlatSurfaces.size(); ++i)
 					{
@@ -135,7 +232,7 @@ void segmentObstaclesFromGround(
 						{
 							Eigen::Vector4f centroid(0,0,0,1);
 							pcl::compute3DCentroid(*cloud, *clusteredFlatSurfaces.at(i), centroid);
-							if(maxGroundHeight==0.0f || centroid[2] <= maxGroundHeight || centroid[2] <= biggestSurfaceMax[2]) // epsilon
+							if(centroid[2] <= biggestSurfaceMax[2]) // relative to ground detected
 							{
 								ground = util3d::concatenate(ground, clusteredFlatSurfaces.at(i));
 							}
@@ -145,9 +242,46 @@ void segmentObstaclesFromGround(
 							}
 						}
 					}
+
+					int groundRatio = 100*ground->size()/cloud->size();
+					int minGroundRatio = 10;
+					if(minGroundRatio != 0 && groundRatio<minGroundRatio)
+					{
+						if(labelUndergroundObstaclesAsGround && maxGroundHeight!=0.0f)
+						{
+							// just do passthrough (e.g. reflective floor)
+							UWARN("Failed normal segmentation (ground ratio=%d%%, ground height=%f), fallback to passThrough (label underground as ground is true).",
+								groundRatio, !ground->empty()?biggestSurfaceMin[2]:0.0f);
+							// passthrough filter
+							ground = rtabmap::util3d::passThrough(cloud, indices, "z",
+									std::numeric_limits<int>::min(),
+									maxGroundHeight!=0.0f?maxGroundHeight:std::numeric_limits<int>::max());
+
+							pcl::IndicesPtr notObstacles = ground;
+							if(indices->size())
+							{
+								notObstacles = util3d::extractIndices(cloud, indices, true);
+								notObstacles = util3d::concatenate(notObstacles, ground);
+							}
+							obstacles = rtabmap::util3d::extractIndices(cloud, notObstacles, true);
+							return;
+						}
+						else
+						{
+							UWARN("Failed normal segmentation, ground surface is too small (ground ratio=%d%%, ground height=%f)!",
+								groundRatio, !ground->empty()?biggestSurfaceMin[2]:0.0f);
+							// reject ground!
+							ground.reset(new std::vector<int>);
+							if(flatObstacles)
+							{
+								*flatObstacles = flatSurfaces;
+							}
+						}
+					}
 				}
 				else
 				{
+					UWARN("Failed normal segmentation, could not detect the ground!");
 					// reject ground!
 					ground.reset(new std::vector<int>);
 					if(flatObstacles)
@@ -168,28 +302,49 @@ void segmentObstaclesFromGround(
 			pcl::IndicesPtr notObstacles = ground;
 			if(indices->size())
 			{
+				// This will ignore all points not in input indices for obstacles.
 				notObstacles = util3d::extractIndices(cloud, indices, true);
 				notObstacles = util3d::concatenate(notObstacles, ground);
 			}
-			pcl::IndicesPtr otherStuffIndices = util3d::extractIndices(cloud, notObstacles, true);
 
-			// If ground height is set, remove obstacles under it
+			// If ground height is set and if we label obstacles under it as ground
-			if(maxGroundHeight != 0.0f)
+			if(labelUndergroundObstaclesAsGround)
 			{
-				otherStuffIndices = rtabmap::util3d::passThrough(cloud, otherStuffIndices, "z", maxGroundHeight, std::numeric_limits<float>::max());
+				float max = biggestSurfaceMax[2];
+				if(maxGroundHeight > 0)
+				{
+					max += maxGroundHeight;
+				}
+
+				pcl::IndicesPtr otherStuffIndices = util3d::extractIndices(cloud, notObstacles, true);
+				pcl::IndicesPtr underground = rtabmap::util3d::passThrough(cloud, otherStuffIndices, "z", (float)std::numeric_limits<int>::min(), max);
+				if(!underground->empty())
+				{
+					ground = util3d::concatenate(ground, underground);
+					notObstacles = util3d::concatenate(underground, notObstacles);
+				}
 			}
 
+			pcl::IndicesPtr otherStuffIndices = util3d::extractIndices(cloud, notObstacles, true);
+
 			//Cluster remaining stuff (obstacles)
 			if(otherStuffIndices->size())
 			{
-				std::vector<pcl::IndicesPtr> clusteredObstaclesSurfaces = util3d::extractClusters(
+				if(minClusterSize>1)
-						cloud,
+				{
-						otherStuffIndices,
+					std::vector<pcl::IndicesPtr> clusteredObstaclesSurfaces = util3d::extractClusters(
-						clusterRadius,
+							cloud,
-						minClusterSize);
+							otherStuffIndices,
+							clusterRadius,
+							minClusterSize);
 
-				// merge indices
+					// merge indices
-				obstacles = util3d::concatenate(clusteredObstaclesSurfaces);
+					obstacles = util3d::concatenate(clusteredObstaclesSurfaces);
+				}
+				else
+				{
+					obstacles = otherStuffIndices;
+				}
 			}
 		}
 	}
@@ -208,7 +363,8 @@ void segmentObstaclesFromGround(
 		float maxGroundHeight,
 		pcl::IndicesPtr * flatObstacles,
 		const Eigen::Vector4f & viewPoint,
-		float groundNormalsUp)
+		float groundNormalsUp,
+		bool labelUndergroundObstaclesAsGround)
 {
 	pcl::IndicesPtr indices(new std::vector<int>);
 	segmentObstaclesFromGround<PointT>(
@@ -224,7 +380,8 @@ void segmentObstaclesFromGround(
 			maxGroundHeight,
 			flatObstacles,
 			viewPoint,
-			groundNormalsUp);
+			groundNormalsUp,
+			labelUndergroundObstaclesAsGround);
 }
 
 template<typename PointT>

corelib/include/rtabmap/core/util3d_mapping.h

@@ -157,7 +157,8 @@ void segmentObstaclesFromGround(
 		float maxGroundHeight = 0.0f,
 		pcl::IndicesPtr * flatObstacles = 0,
 		const Eigen::Vector4f & viewPoint = Eigen::Vector4f(0,0,100,0),
-		float groundNormalsUp = 0);
+		float groundNormalsUp = 0,
+		bool labelUndergroundObstaclesAsGround = false);
 template<typename PointT>
 void segmentObstaclesFromGround(
 		const typename pcl::PointCloud<PointT>::Ptr & cloud,
@@ -171,7 +172,8 @@ void segmentObstaclesFromGround(
 		float maxGroundHeight = 0.0f,
 		pcl::IndicesPtr * flatObstacles = 0,
 		const Eigen::Vector4f & viewPoint = Eigen::Vector4f(0,0,100,0),
-		float groundNormalsUp = 0);
+		float groundNormalsUp = 0,
+		bool labelUndergroundObstaclesAsGround = false);
 
 template<typename PointT>
 void occupancy2DFromGroundObstacles(

corelib/include/rtabmap/core/util3d_surface.h

@@ -381,6 +381,17 @@ pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_CORE_EXPORT computeFastOrganizedNormal
 		float searchRadius = 0.0f,
 		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
 
+pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_CORE_EXPORT computeFastOrganizedNormals(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		float maxDepthChangeFactor = 0.02f,
+		float normalSmoothingSize = 10.0f,
+		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
+pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_CORE_EXPORT computeFastOrganizedNormals(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float maxDepthChangeFactor = 0.02f,
+		float normalSmoothingSize = 10.0f,
+		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
 pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_CORE_EXPORT computeFastOrganizedNormals(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		float maxDepthChangeFactor = 0.02f,

corelib/src/Graph.cpp

@@ -902,7 +902,7 @@ void computeMaxGraphErrors(
 		float & maxAngularError,
 		const Link ** maxLinearErrorLink,
 		const Link ** maxAngularErrorLink,
-		bool for3DoF)
+		bool force3DoF)
 {
 	maxLinearErrorRatio = -1;
 	maxAngularErrorRatio = -1;
@@ -912,17 +912,44 @@ void computeMaxGraphErrors(
 	UDEBUG("poses=%d links=%d", (int)poses.size(), (int)links.size());
 	for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
 	{
-		// ignore links with high variance, priors and landmarks
+		// ignore priors
-		if(iter->second.transVariance() <= 1.0 && iter->second.from() != iter->second.to() && iter->second.type() != Link::kLandmark)
+		if(iter->second.from() != iter->second.to())
 		{
 			Transform t1 = uValue(poses, iter->second.from(), Transform());
 			Transform t2 = uValue(poses, iter->second.to(), Transform());
+
+			if( t1.isNull() ||
+				t2.isNull() ||
+				!t1.isInvertible() ||
+				!t2.isInvertible())
+			{
+				UWARN("Poses are null or not invertible, aborting optimized graph max error check! (Pose %d=%s Pose %d=%s)",
+					iter->second.from(),
+					t1.prettyPrint().c_str(),
+					iter->second.to(),
+					t2.prettyPrint().c_str());
+
+				if(maxLinearErrorLink)
+				{
+					*maxLinearErrorLink = 0;
+				}
+				if(maxAngularErrorLink)
+				{
+					*maxAngularErrorLink = 0;
+				}
+				maxLinearErrorRatio = -1;
+				maxAngularErrorRatio = -1;
+				maxLinearError = -1;
+				maxAngularError = -1;
+				return;
+			}
+
 			Transform t = t1.inverse()*t2;
 
 			float linearError = uMax3(
 					fabs(iter->second.transform().x() - t.x()),
 					fabs(iter->second.transform().y() - t.y()),
-					for3DoF?0:fabs(iter->second.transform().z() - t.z()));
+					force3DoF?0:fabs(iter->second.transform().z() - t.z()));
 			UASSERT(iter->second.transVariance(false)>0.0);
 			float stddevLinear = sqrt(iter->second.transVariance(false));
 			float linearErrorRatio = linearError/stddevLinear;
@@ -936,25 +963,30 @@ void computeMaxGraphErrors(
 				}
 			}
 
-			float opt_roll,opt_pitch,opt_yaw;
+			// For landmark links, don't compute angular error if it doesn't estimate orientation
-			float link_roll,link_pitch,link_yaw;
+			if(iter->second.type() != Link::kLandmark ||
-			t.getEulerAngles(opt_roll, opt_pitch, opt_yaw);
+				1.0 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) < 9999.0)
-			iter->second.transform().getEulerAngles(link_roll, link_pitch, link_yaw);
-			float angularError = uMax3(
-					for3DoF?0:fabs(opt_roll - link_roll),
-					for3DoF?0:fabs(opt_pitch - link_pitch),
-					fabs(opt_yaw - link_yaw));
-			angularError = angularError>M_PI?2*M_PI-angularError:angularError;
-			UASSERT(iter->second.rotVariance(false)>0.0);
-			float stddevAngular = sqrt(iter->second.rotVariance(false));
-			float angularErrorRatio = angularError/stddevAngular;
-			if(angularErrorRatio > maxAngularErrorRatio)
 			{
-				maxAngularError = angularError;
+				float opt_roll,opt_pitch,opt_yaw;
-				maxAngularErrorRatio = angularErrorRatio;
+				float link_roll,link_pitch,link_yaw;
-				if(maxAngularErrorLink)
+				t.getEulerAngles(opt_roll, opt_pitch, opt_yaw);
+				iter->second.transform().getEulerAngles(link_roll, link_pitch, link_yaw);
+				float angularError = uMax3(
+						force3DoF?0:fabs(opt_roll - link_roll),
+						force3DoF?0:fabs(opt_pitch - link_pitch),
+						fabs(opt_yaw - link_yaw));
+				angularError = angularError>M_PI?2*M_PI-angularError:angularError;
+				UASSERT(iter->second.rotVariance(false)>0.0);
+				float stddevAngular = sqrt(iter->second.rotVariance(false));
+				float angularErrorRatio = angularError/stddevAngular;
+				if(angularErrorRatio > maxAngularErrorRatio)
 				{
-					*maxAngularErrorLink = &iter->second;
+					maxAngularError = angularError;
+					maxAngularErrorRatio = angularErrorRatio;
+					if(maxAngularErrorLink)
+					{
+						*maxAngularErrorLink = &iter->second;
+					}
 				}
 			}
 		}

corelib/src/IMUThread.cpp

@@ -27,6 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/core/IMUThread.h"
 #include "rtabmap/core/IMU.h"
+#include "rtabmap/core/IMUFilter.h"
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UConversion.h>
@@ -38,13 +39,16 @@ IMUThread::IMUThread(int rate, const Transform & localTransform) :
 		rate_(rate),
 		localTransform_(localTransform),
 		captureDelay_(0.0),
-		previousStamp_(0.0)
+		previousStamp_(0.0),
+		_imuFilter(0),
+		_imuBaseFrameConversion(false)
 {
 }
 
 IMUThread::~IMUThread()
 {
 	imuFile_.close();
+	delete _imuFilter;
 }
 
 bool IMUThread::init(const std::string & path)
@@ -81,6 +85,19 @@ void IMUThread::setRate(int rate)
 	rate_ = rate;
 }
 
+void IMUThread::enableIMUFiltering(int filteringStrategy, const ParametersMap & parameters, bool baseFrameConversion)
+{
+	delete _imuFilter;
+	_imuFilter = IMUFilter::create((IMUFilter::Type)filteringStrategy, parameters);
+	_imuBaseFrameConversion = baseFrameConversion;
+}
+
+void IMUThread::disableIMUFiltering()
+{
+	delete _imuFilter;
+	_imuFilter = 0;
+}
+
 void IMUThread::mainLoopBegin()
 {
 	ULogger::registerCurrentThread("IMU");
@@ -141,6 +158,60 @@ void IMUThread::mainLoop()
 		previousStamp_ = stamp;
 
 		IMU imu(gyr, cv::Mat(), acc, cv::Mat(), localTransform_);
+
+		// IMU filtering
+		if(_imuFilter && !imu.empty())
+		{
+			if(imu.angularVelocity()[0] == 0 &&
+			   imu.angularVelocity()[1] == 0 &&
+			   imu.angularVelocity()[2] == 0 &&
+			   imu.linearAcceleration()[0] == 0 &&
+			   imu.linearAcceleration()[1] == 0 &&
+			   imu.linearAcceleration()[2] == 0)
+			{
+				UWARN("IMU's acc and gyr values are null! Please disable IMU filtering.");
+			}
+			else
+			{
+				// Transform IMU data in base_link to correctly initialize yaw
+				if(_imuBaseFrameConversion)
+				{
+					UASSERT(!imu.localTransform().isNull());
+					imu.convertToBaseFrame();
+
+				}
+				_imuFilter->update(
+						imu.angularVelocity()[0],
+						imu.angularVelocity()[1],
+						imu.angularVelocity()[2],
+						imu.linearAcceleration()[0],
+						imu.linearAcceleration()[1],
+						imu.linearAcceleration()[2],
+						stamp);
+				double qx,qy,qz,qw;
+				_imuFilter->getOrientation(qx,qy,qz,qw);
+
+				imu = IMU(
+						cv::Vec4d(qx,qy,qz,qw), cv::Mat::eye(3,3,CV_64FC1),
+						imu.angularVelocity(), imu.angularVelocityCovariance(),
+						imu.linearAcceleration(), imu.linearAccelerationCovariance(),
+						imu.localTransform());
+
+				UDEBUG("%f %f %f %f (gyro=%f %f %f, acc=%f %f %f, %fs)",
+							imu.orientation()[0],
+							imu.orientation()[1],
+							imu.orientation()[2],
+							imu.orientation()[3],
+							imu.angularVelocity()[0],
+							imu.angularVelocity()[1],
+							imu.angularVelocity()[2],
+							imu.linearAcceleration()[0],
+							imu.linearAcceleration()[1],
+							imu.linearAcceleration()[2],
+							stamp);
+			}
+		}
+
 		this->post(new IMUEvent(imu, stamp));
 	}
 	else if(!this->isKilled())

corelib/src/Memory.cpp

@@ -283,6 +283,10 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
                                           -landmarkId, inserted.first->second, landmarkSize.at<float>(0,0));
                                 }
                             }
+							else
+							{
+								UDEBUG("Caching landmark size %f for %d", landmarkSize.at<float>(0,0), -landmarkId);
+							}
                         }
 
                         std::map<int, std::set<int> >::iterator nter = _landmarksIndex.find(landmarkId);

corelib/src/OccupancyGrid.cpp

@@ -66,6 +66,7 @@ OccupancyGrid::OccupancyGrid(const ParametersMap & parameters) :
 	normalsSegmentation_(Parameters::defaultGridNormalsSegmentation()),
 	grid3D_(Parameters::defaultGrid3D()),
 	groundIsObstacle_(Parameters::defaultGridGroundIsObstacle()),
+	labelUndergroundObstaclesAsGround_(Parameters::defaultGridUndergroundIsGround()),
 	noiseFilteringRadius_(Parameters::defaultGridNoiseFilteringRadius()),
 	noiseFilteringMinNeighbors_(Parameters::defaultGridNoiseFilteringMinNeighbors()),
 	scan2dUnknownSpaceFilled_(Parameters::defaultGridScan2dUnknownSpaceFilled()),
@@ -128,6 +129,7 @@ void OccupancyGrid::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kGridNormalsSegmentation(), normalsSegmentation_);
 	Parameters::parse(parameters, Parameters::kGrid3D(), grid3D_);
 	Parameters::parse(parameters, Parameters::kGridGroundIsObstacle(), groundIsObstacle_);
+	Parameters::parse(parameters, Parameters::kGridUndergroundIsGround(), labelUndergroundObstaclesAsGround_);
 	Parameters::parse(parameters, Parameters::kGridNoiseFilteringRadius(), noiseFilteringRadius_);
 	Parameters::parse(parameters, Parameters::kGridNoiseFilteringMinNeighbors(), noiseFilteringMinNeighbors_);
 	Parameters::parse(parameters, Parameters::kGridScan2dUnknownSpaceFilled(), scan2dUnknownSpaceFilled_);

corelib/src/Odometry.cpp

@@ -324,6 +324,10 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 				imus_.erase(imus_.begin());
 			}
 		}
+		else
+		{
+			UWARN("Received IMU doesn't have orientation set! It is ignored.");
+		}
 	}
 
 

corelib/src/Rtabmap.cpp

@@ -147,6 +147,7 @@ Rtabmap::Rtabmap() :
 	_loopCovLimited(Parameters::defaultRGBDLoopCovLimited()),
 	_loopGPS(Parameters::defaultRtabmapLoopGPS()),
 	_maxOdomCacheSize(Parameters::defaultRGBDMaxOdomCacheSize()),
+	_localizationSmoothing(Parameters::defaultRGBDLocalizationSmoothing()),
 	_createGlobalScanMap(Parameters::defaultRGBDProximityGlobalScanMap()),
 	_markerPriorsLinearVariance(Parameters::defaultMarkerPriorsVarianceLinear()),
 	_markerPriorsAngularVariance(Parameters::defaultMarkerPriorsVarianceAngular()),
@@ -618,6 +619,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRGBDLoopCovLimited(), _loopCovLimited);
 	Parameters::parse(parameters, Parameters::kRtabmapLoopGPS(), _loopGPS);
 	Parameters::parse(parameters, Parameters::kRGBDMaxOdomCacheSize(), _maxOdomCacheSize);
+	Parameters::parse(parameters, Parameters::kRGBDLocalizationSmoothing(), _localizationSmoothing);
 	Parameters::parse(parameters, Parameters::kRGBDProximityGlobalScanMap(), _createGlobalScanMap);
 
 	Parameters::parse(parameters, Parameters::kMarkerPriorsVarianceLinear(), _markerPriorsLinearVariance);
@@ -3173,10 +3175,10 @@ bool Rtabmap::process(
 							&maxLinearLink,
 							&maxAngularLink,
 							_graphOptimizer->isSlam2d());
-					if(maxLinearLink == 0 && maxAngularLink==0 && _maxOdomCacheSize>0)
+					if(maxLinearLink == 0 && maxAngularLink==0)
 					{
-						UWARN("Could not compute graph errors! Wrong loop closures could be accepted!");
+						UWARN("Could not compute graph errors! Rejecting localization!");
-						optPoses = posesOut;
+						rejectLocalization = true;
 					}
 
 					if(maxLinearLink)
@@ -3287,10 +3289,10 @@ bool Rtabmap::process(
 									&maxLinearLink,
 									&maxAngularLink,
 									_graphOptimizer->isSlam2d());
-							if(maxLinearLink == 0 && maxAngularLink==0 && _maxOdomCacheSize>0)
+							if(maxLinearLink == 0 && maxAngularLink==0)
 							{
-								UWARN("Could not compute graph errors! Wrong loop closures could be accepted!");
+								UWARN("Could not compute graph errors! Rejecting localization!");
-								optPoses = posesOut;
+								rejectLocalization = true;
 							}
 
 							if(maxLinearLink)
@@ -3395,16 +3397,26 @@ bool Rtabmap::process(
 					Transform newOptPoseInv = optPoses.at(signature->id()).inverse();
 					for(std::multimap<int, Link>::iterator iter=localizationLinks.begin(); iter!=localizationLinks.end(); ++iter)
 					{
-						Transform newT = newOptPoseInv * optPoses.at(iter->first);
+						if(!_localizationSmoothing)
-						UDEBUG("Adjusted localization link %d->%d after optimization", iter->second.from(), iter->second.to());
+						{
-						UDEBUG("from %s", iter->second.transform().prettyPrint().c_str());
+							// Add original link without optimization
-						UDEBUG("  to %s", newT.prettyPrint().c_str());
+							UDEBUG("Adding new odom cache constraint %d->%d (%s)", 
-						iter->second.setTransform(newT);
+								iter->second.from(), iter->second.to(), iter->second.transform().prettyPrint().c_str());
-
+						}
-						// Update link in the referred signatures
+						else
-						if(iter->first > 0)
+						{
-							_memory->updateLink(iter->second, false);
+							// Adjust with optimized poses, this will smooth the localization
+							Transform newT = newOptPoseInv * optPoses.at(iter->first);
+							UDEBUG("Adjusted localization link %d->%d after optimization", iter->second.from(), iter->second.to());
+							UDEBUG("from %s", iter->second.transform().prettyPrint().c_str());
+							UDEBUG("  to %s", newT.prettyPrint().c_str());
+							iter->second.setTransform(newT);
 
+							// Update link in the referred signatures
+							if(iter->first > 0)
+								_memory->updateLink(iter->second, false);
+						}
+						
 						_odomCacheConstraints.insert(std::make_pair(signature->id(), iter->second));
 					}
 

corelib/src/camera/CameraDepthAI.cpp

@@ -57,7 +57,10 @@ CameraDepthAI::CameraDepthAI(
 	depthConfidence_(200),
 	resolution_(resolution),
 	imuFirmwareUpdate_(false),
-	imuPublished_(true)
+	imuPublished_(true),
+	publishInterIMU_(false),
+	dotProjectormA_(0.0),
+	floodLightmA_(200.0)
 #endif
 {
 #ifdef RTABMAP_DEPTHAI
@@ -106,6 +109,33 @@ void CameraDepthAI::setIMUPublished(bool published)
 #endif
 }
 
+void CameraDepthAI::publishInterIMU(bool enabled)
+{
+#ifdef RTABMAP_DEPTHAI
+	publishInterIMU_ = enabled;
+#else
+	UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
+#endif
+}
+
+void CameraDepthAI::setLaserDotBrightness(float dotProjectormA)
+{
+#ifdef RTABMAP_DEPTHAI
+	dotProjectormA_ = dotProjectormA;
+#else
+	UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
+#endif
+}
+
+void CameraDepthAI::setFloodLightBrightness(float floodLightmA)
+{
+#ifdef RTABMAP_DEPTHAI
+	floodLightmA_ = floodLightmA;
+#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("");
@@ -183,31 +213,29 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 	}
 
 	// StereoDepth
-	stereo->initialConfig.setConfidenceThreshold(depthConfidence_);
+	stereo->setDepthAlign(dai::StereoDepthProperties::DepthAlign::RECTIFIED_LEFT);
-	stereo->initialConfig.setLeftRightCheckThreshold(5);
+	stereo->setSubpixel(true);
-	stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout
+	stereo->setSubpixelFractionalBits(4);
-	stereo->setLeftRightCheck(true);
-	stereo->setSubpixel(false);
 	stereo->setExtendedDisparity(false);
+	stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout
+	stereo->initialConfig.setConfidenceThreshold(depthConfidence_);
+	stereo->initialConfig.setLeftRightCheck(true);
+	stereo->initialConfig.setLeftRightCheckThreshold(5);
+	stereo->initialConfig.setMedianFilter(dai::MedianFilter::KERNEL_5x5);
+	auto config = stereo->initialConfig.get();
+	config.costMatching.disparityWidth = dai::StereoDepthConfig::CostMatching::DisparityWidth::DISPARITY_64;
+	config.costMatching.enableCompanding = true;
+	stereo->initialConfig.set(config);
 
 	// Link plugins CAM -> STEREO -> XLINK
 	monoLeft->out.link(stereo->left);
 	monoRight->out.link(stereo->right);
 
+	stereo->rectifiedLeft.link(xoutLeft->input);
 	if(outputDepth_)
-	{
-		// Depth is registered to right image by default, so subscribe to right image when depth is used
-		if(outputDepth_)
-			stereo->rectifiedRight.link(xoutLeft->input);
-		else
-			stereo->rectifiedLeft.link(xoutLeft->input);
 		stereo->depth.link(xoutDepthOrRight->input);
-	}
 	else
-	{
-		stereo->rectifiedLeft.link(xoutLeft->input);
 		stereo->rectifiedRight.link(xoutDepthOrRight->input);
-	}
 
 	if(imuPublished_)
 	{
@@ -235,8 +263,7 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 	double fy = matrix[1][1];
 	double cx = matrix[0][2];
 	double cy = matrix[1][2];
-	matrix = calibHandler.getCameraExtrinsics(dai::CameraBoardSocket::RIGHT, dai::CameraBoardSocket::LEFT);
+	double baseline = calibHandler.getBaselineDistance(dai::CameraBoardSocket::RIGHT, dai::CameraBoardSocket::LEFT, false)/100.0;
-	double baseline = matrix[0][3]/100.0;
 	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);
 
@@ -249,12 +276,27 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 		//		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
+		auto eeprom = calibHandler.getEepromData();
-		imuLocalTransform_ = Transform(
+		if(eeprom.boardName == "OAK-D" ||
-				 0, 0, 1, 0,
+		   eeprom.boardName == "BW1098OBC")
-				 0, 1, 0, 0,
+		{
-				-1 ,0, 0, 0);
+			imuLocalTransform_ = Transform(
-		UINFO("IMU local transform = %s", imuLocalTransform_.prettyPrint().c_str());
+				 0, -1,  0,  0.0525,
+				 1,  0,  0,  0.0137,
+				 0,  0,  1,  0);
+		}
+		else if(eeprom.boardName == "DM9098")
+		{
+			imuLocalTransform_ = Transform(
+				 0,  1,  0,  0.0754,
+				 1,  0,  0,  0.0026,
+				 0,  0, -1, -0.007);
+		}
+		else
+		{
+			UWARN("Unknown boardName (%s)! Disabling IMU!", eeprom.boardName.c_str());
+			imuPublished_ = false;
+		}
 	}
 	else
 	{
@@ -263,10 +305,47 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 
 	if(imuPublished_)
 	{
-		imuQueue_ = device_->getOutputQueue("imu", 50, false);
+		imuLocalTransform_ = this->getLocalTransform() * imuLocalTransform_;
+		UINFO("IMU local transform = %s", imuLocalTransform_.prettyPrint().c_str());
+		device_->getOutputQueue("imu", 50, false)->addCallback([this](std::shared_ptr<dai::ADatatype> callback) {
+			if(dynamic_cast<dai::IMUData*>(callback.get()) != nullptr)
+			{
+				dai::IMUData* imuData = static_cast<dai::IMUData*>(callback.get());
+				auto imuPackets = imuData->packets;
+
+				for(auto& imuPacket : imuPackets)
+				{
+					auto& acceleroValues = imuPacket.acceleroMeter;
+					auto& gyroValues = imuPacket.gyroscope;
+					double accStamp = std::chrono::duration<double>(acceleroValues.getTimestampDevice().time_since_epoch()).count();
+					double gyroStamp = std::chrono::duration<double>(gyroValues.getTimestampDevice().time_since_epoch()).count();
+
+					if(publishInterIMU_)
+					{
+						IMU imu(cv::Vec3f(gyroValues.x, gyroValues.y, gyroValues.z), cv::Mat::eye(3,3,CV_64FC1),
+								cv::Vec3f(acceleroValues.x, acceleroValues.y, acceleroValues.z), cv::Mat::eye(3,3,CV_64FC1),
+								imuLocalTransform_);
+						UEventsManager::post(new IMUEvent(imu, (accStamp+gyroStamp)/2));
+					}
+					else
+					{
+						UScopeMutex lock(imuMutex_);
+						accBuffer_.emplace_hint(accBuffer_.end(), std::make_pair(accStamp, cv::Vec3f(acceleroValues.x, acceleroValues.y, acceleroValues.z)));
+						gyroBuffer_.emplace_hint(gyroBuffer_.end(), std::make_pair(gyroStamp, cv::Vec3f(gyroValues.x, gyroValues.y, gyroValues.z)));
+					}
+				}
+			}
+		});
+	}
+	leftQueue_ = device_->getOutputQueue("rectified_left", 8, false);
+	rightOrDepthQueue_ = device_->getOutputQueue(outputDepth_?"depth":"rectified_right", 8, false);
+
+	std::vector<std::tuple<std::string, int, int>> irDrivers = device_->getIrDrivers();
+	if(!irDrivers.empty())
+	{
+		device_->setIrLaserDotProjectorBrightness(dotProjectormA_);
+		device_->setIrFloodLightBrightness(floodLightmA_);
 	}
-	leftQueue_ = device_->getOutputQueue("rectified_left", 1, false);
-	rightOrDepthQueue_ = device_->getOutputQueue(outputDepth_?"depth":"rectified_right", 1, false);
 
 	uSleep(2000); // avoid bad frames on start
 
@@ -303,173 +382,67 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info)
 	auto rectifL = leftQueue_->get<dai::ImgFrame>();
 	auto rectifRightOrDepth = rightOrDepthQueue_->get<dai::ImgFrame>();
 
-	if(rectifL.get() && rectifRightOrDepth.get())
+	while(rectifL->getSequenceNum() < rectifRightOrDepth->getSequenceNum())
-	{
+		rectifL = leftQueue_->get<dai::ImgFrame>();
-		auto stampLeft = rectifL->getTimestamp().time_since_epoch().count();
+	while(rectifL->getSequenceNum() > rectifRightOrDepth->getSequenceNum())
-		auto stampRight = rectifRightOrDepth->getTimestamp().time_since_epoch().count();
+		rectifRightOrDepth = rightOrDepthQueue_->get<dai::ImgFrame>();
-		double stamp = double(stampLeft)/10e8;
-		left = rectifL->getCvFrame();
-		depthOrRight = rectifRightOrDepth->getCvFrame();
 
-		if(!left.empty() && !depthOrRight.empty())
+	double stamp = std::chrono::duration<double>(rectifL->getTimestampDevice(dai::CameraExposureOffset::MIDDLE).time_since_epoch()).count();
-		{
+	left = rectifL->getCvFrame();
-			if(depthOrRight.type() == CV_8UC1)
+	depthOrRight = rectifRightOrDepth->getCvFrame();
-			{
-				if(stereoModel_.isValidForRectification())
-				{
-					left = stereoModel_.left().rectifyImage(left);
-					depthOrRight = stereoModel_.right().rectifyImage(depthOrRight);
-				}
-				data = SensorData(left, depthOrRight, stereoModel_, this->getNextSeqID(), stamp);
-			}
-			else
-			{
-				data = SensorData(left, depthOrRight, stereoModel_.left(), this->getNextSeqID(), stamp);
-			}
 
-			if(fabs(double(stampLeft)/10e8 - double(stampRight)/10e8) >= 0.0001) //0.1 ms
+	if(depthOrRight.type() == CV_8UC1)
-			{
+		data = SensorData(left, depthOrRight, stereoModel_, this->getNextSeqID(), stamp);
-				UWARN("Frames are not synchronized! %f vs %f", double(stampLeft)/10e8, double(stampRight)/10e8);
-			}
-
-			//get imu
-			double stampStart = UTimer::now();
-			while(imuPublished_ && imuQueue_.get())
-			{
-				if(imuQueue_->has())
-				{
-					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;
-				}
-			}
-
-			cv::Vec3d acc, gyro;
-			bool valid = !accBuffer_.empty() && !gyroBuffer_.empty();
-			//acc
-			if(!accBuffer_.empty())
-			{
-				std::map<double, cv::Vec3f>::const_iterator iterB = accBuffer_.lower_bound(stamp);
-				std::map<double, cv::Vec3f>::const_iterator iterA = iterB;
-				if(iterA != accBuffer_.begin())
-				{
-					iterA = --iterA;
-				}
-				if(iterB == accBuffer_.end())
-				{
-					iterB = --iterB;
-				}
-				if(iterA == iterB && stamp == iterA->first)
-				{
-					acc[0] = iterA->second[0];
-					acc[1] = iterA->second[1];
-					acc[2] = iterA->second[2];
-				}
-				else if(stamp >= iterA->first && stamp <= iterB->first)
-				{
-					float t = (stamp-iterA->first) / (iterB->first-iterA->first);
-					acc[0] = iterA->second[0] + t*(iterB->second[0] - iterA->second[0]);
-					acc[1] = iterA->second[1] + t*(iterB->second[1] - iterA->second[1]);
-					acc[2] = iterA->second[2] + t*(iterB->second[2] - iterA->second[2]);
-				}
-				else
-				{
-					valid = false;
-					if(stamp < iterA->first)
-					{
-						UWARN("Could not find acc data to interpolate at image time %f (earliest is %f). Are sensors synchronized?", stamp, iterA->first);
-					}
-					else if(stamp > iterB->first)
-					{
-						UWARN("Could not find acc data to interpolate at image time %f (latest is %f). Are sensors synchronized?", stamp, iterB->first);
-					}
-					else
-					{
-						UWARN("Could not find acc data to interpolate at image time %f (between %f and %f). Are sensors synchronized?", stamp, iterA->first, iterB->first);
-					}
-				}
-			}
-			//gyro
-			if(!gyroBuffer_.empty())
-			{
-				std::map<double, cv::Vec3f>::const_iterator iterB = gyroBuffer_.lower_bound(stamp);
-				std::map<double, cv::Vec3f>::const_iterator iterA = iterB;
-				if(iterA != gyroBuffer_.begin())
-				{
-					iterA = --iterA;
-				}
-				if(iterB == gyroBuffer_.end())
-				{
-					iterB = --iterB;
-				}
-				if(iterA == iterB && stamp == iterA->first)
-				{
-					gyro[0] = iterA->second[0];
-					gyro[1] = iterA->second[1];
-					gyro[2] = iterA->second[2];
-				}
-				else if(stamp >= iterA->first && stamp <= iterB->first)
-				{
-					float t = (stamp-iterA->first) / (iterB->first-iterA->first);
-					gyro[0] = iterA->second[0] + t*(iterB->second[0] - iterA->second[0]);
-					gyro[1] = iterA->second[1] + t*(iterB->second[1] - iterA->second[1]);
-					gyro[2] = iterA->second[2] + t*(iterB->second[2] - iterA->second[2]);
-				}
-				else
-				{
-					valid = false;
-					if(stamp < iterA->first)
-					{
-						UWARN("Could not find gyro data to interpolate at image time %f (earliest is %f). Are sensors synchronized?", stamp, iterA->first);
-					}
-					else if(stamp > iterB->first)
-					{
-						UWARN("Could not find gyro data to interpolate at image time %f (latest is %f). Are sensors synchronized?", stamp, iterB->first);
-					}
-					else
-					{
-						UWARN("Could not find gyro data to interpolate at image time %f (between %f and %f). Are sensors synchronized?", stamp, iterA->first, iterB->first);
-					}
-				}
-			}
-
-			if(valid)
-			{
-				data.setIMU(IMU(gyro, cv::Mat::eye(3, 3, CV_64FC1), acc, cv::Mat::eye(3, 3, CV_64FC1), imuLocalTransform_));
-			}
-		}
-	}
 	else
+		data = SensorData(left, depthOrRight, stereoModel_.left(), this->getNextSeqID(), stamp);
+
+	if(imuPublished_ && !publishInterIMU_)
 	{
-		UWARN("Null images received!?");
+		cv::Vec3d acc, gyro;
+		std::map<double, cv::Vec3f>::const_iterator iterA, iterB;
+	
+		imuMutex_.lock();
+		while(accBuffer_.empty() || gyroBuffer_.empty() || accBuffer_.rbegin()->first < stamp || gyroBuffer_.rbegin()->first < stamp)
+		{
+			imuMutex_.unlock();
+			uSleep(1);
+			imuMutex_.lock();
+		}
+
+		//acc
+		iterB = accBuffer_.lower_bound(stamp);
+		iterA = iterB;
+		if(iterA != accBuffer_.begin())
+			iterA = --iterA;
+		if(iterA == iterB || stamp == iterB->first)
+		{
+			acc = iterB->second;
+		}
+		else if(stamp > iterA->first && stamp < iterB->first)
+		{
+			float t = (stamp-iterA->first) / (iterB->first-iterA->first);
+			acc = iterA->second + t*(iterB->second - iterA->second);
+		}
+		accBuffer_.erase(accBuffer_.begin(), iterB);
+
+		//gyro
+		iterB = gyroBuffer_.lower_bound(stamp);
+		iterA = iterB;
+		if(iterA != gyroBuffer_.begin())
+			iterA = --iterA;
+		if(iterA == iterB || stamp == iterB->first)
+		{
+			gyro = iterB->second;
+		}
+		else if(stamp > iterA->first && stamp < iterB->first)
+		{
+			float t = (stamp-iterA->first) / (iterB->first-iterA->first);
+			gyro = iterA->second + t*(iterB->second - iterA->second);
+		}
+		gyroBuffer_.erase(gyroBuffer_.begin(), iterB);
+
+		imuMutex_.unlock();
+		data.setIMU(IMU(gyro, cv::Mat::eye(3, 3, CV_64FC1), acc, cv::Mat::eye(3, 3, CV_64FC1), imuLocalTransform_));
 	}
 
 #else

corelib/src/camera/CameraStereoZed.cpp

@@ -240,6 +240,17 @@ bool CameraStereoZed::available()
 #endif
 }
 
+
+int CameraStereoZed::sdkVersion()
+{
+#ifdef RTABMAP_ZED
+	return ZED_SDK_MAJOR_VERSION;
+#else
+	return -1;
+#endif
+}
+
+
 CameraStereoZed::CameraStereoZed(
 		int deviceId,
 		int resolution,
@@ -274,6 +285,16 @@ CameraStereoZed::CameraStereoZed(
 {
 	UDEBUG("");
 #ifdef RTABMAP_ZED
+#if ZED_SDK_MAJOR_VERSION < 4
+	if(resolution_ == 3)
+	{
+		resolution_ = 2;
+	}
+	else if(resolution_ == 5)
+	{
+		resolution_ = 3;
+	}
+#endif
 #if ZED_SDK_MAJOR_VERSION < 3
 	UASSERT(resolution_ >= sl::RESOLUTION_HD2K && resolution_ <sl::RESOLUTION_LAST);
 	UASSERT(quality_ >= sl::DEPTH_MODE_NONE && quality_ <sl::DEPTH_MODE_LAST);
@@ -282,11 +303,15 @@ CameraStereoZed::CameraStereoZed(
 #else
     sl::RESOLUTION res = static_cast<sl::RESOLUTION>(resolution_);
     sl::DEPTH_MODE qual = static_cast<sl::DEPTH_MODE>(quality_);
-    sl::SENSING_MODE sens = static_cast<sl::SENSING_MODE>(sensingMode_);
 
     UASSERT(res >= sl::RESOLUTION::HD2K && res < sl::RESOLUTION::LAST);
     UASSERT(qual >= sl::DEPTH_MODE::NONE && qual < sl::DEPTH_MODE::LAST);
+#if ZED_SDK_MAJOR_VERSION < 4
+    sl::SENSING_MODE sens = static_cast<sl::SENSING_MODE>(sensingMode_);
     UASSERT(sens >= sl::SENSING_MODE::STANDARD && sens < sl::SENSING_MODE::LAST);
+#else
+    UASSERT(sensingMode_ >= 0 && sensingMode_ < 2);
+#endif
     UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100);
     UASSERT(texturenessConfidenceThr_ >= 0 && texturenessConfidenceThr_ <=100);
 #endif
@@ -334,11 +359,15 @@ CameraStereoZed::CameraStereoZed(
 #else
     sl::RESOLUTION res = static_cast<sl::RESOLUTION>(resolution_);
     sl::DEPTH_MODE qual = static_cast<sl::DEPTH_MODE>(quality_);
-    sl::SENSING_MODE sens = static_cast<sl::SENSING_MODE>(sensingMode_);
 
     UASSERT(res >= sl::RESOLUTION::HD2K && res < sl::RESOLUTION::LAST);
     UASSERT(qual >= sl::DEPTH_MODE::NONE && qual < sl::DEPTH_MODE::LAST);
+#if ZED_SDK_MAJOR_VERSION < 4
+    sl::SENSING_MODE sens = static_cast<sl::SENSING_MODE>(sensingMode_);
     UASSERT(sens >= sl::SENSING_MODE::STANDARD && sens < sl::SENSING_MODE::LAST);
+#else
+    UASSERT(sensingMode_ >= 0 && sensingMode_ < 2);
+#endif
     UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100);
     UASSERT(texturenessConfidenceThr_ >= 0 && texturenessConfidenceThr_ <=100);
 #endif
@@ -465,7 +494,11 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str
 	}
 
 	sl::CameraInformation infos = zed_->getCameraInformation();
+#if ZED_SDK_MAJOR_VERSION < 4
 	sl::CalibrationParameters *stereoParams = &(infos.calibration_parameters );
+#else
+	sl::CalibrationParameters *stereoParams = &(infos.camera_configuration.calibration_parameters );
+#endif
 	sl::Resolution res = stereoParams->left_cam.image_size;
 
 	stereoModel_ = StereoCameraModel(
@@ -473,7 +506,11 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str
 		stereoParams->left_cam.fy, 
 		stereoParams->left_cam.cx, 
 		stereoParams->left_cam.cy, 
+#if ZED_SDK_MAJOR_VERSION < 4
 		stereoParams->T[0],//baseline
+#else
+		stereoParams->getCameraBaseline(),
+#endif
 		this->getLocalTransform(),
 		cv::Size(res.width, res.height));
 
@@ -482,7 +519,11 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str
 			stereoParams->left_cam.fy,
 			stereoParams->left_cam.cx,
 			stereoParams->left_cam.cy,
+#if ZED_SDK_MAJOR_VERSION < 4
 			stereoParams->T[0],//baseline
+#else
+			stereoParams->getCameraBaseline(),
+#endif
 			(int)res.width,
 			(int)res.height,
 			this->getLocalTransform().prettyPrint().c_str());
@@ -493,11 +534,18 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str
     if(infos.camera_model != sl::MODEL::ZED)
 #endif
 	{
+#if ZED_SDK_MAJOR_VERSION < 4
 		imuLocalTransform_ = this->getLocalTransform() * zedPoseToTransform(infos.camera_imu_transform).inverse();
+#else
+		imuLocalTransform_ = this->getLocalTransform() * zedPoseToTransform(infos.sensors_configuration.camera_imu_transform).inverse();
+#endif
 		UINFO("IMU local transform: %s (imu2cam=%s))",
-				imuLocalTransform_.prettyPrint().c_str(),
+		imuLocalTransform_.prettyPrint().c_str(),
-				zedPoseToTransform(infos.camera_imu_transform).prettyPrint().c_str());
+#if ZED_SDK_MAJOR_VERSION < 4
-
+		zedPoseToTransform(infos.camera_imu_transform).prettyPrint().c_str());
+#else
+		zedPoseToTransform(infos.sensors_configuration.camera_imu_transform).prettyPrint().c_str());
+#endif
 		if(publishInterIMU_)
 		{
 			imuPublishingThread_ = new ZedIMUThread(200, zed_, imuLocalTransform_, true);
@@ -623,8 +671,10 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info)
 #ifdef RTABMAP_ZED
 #if ZED_SDK_MAJOR_VERSION < 3
 	sl::RuntimeParameters rparam((sl::SENSING_MODE)sensingMode_, quality_ > 0, quality_ > 0, sl::REFERENCE_FRAME_CAMERA);
-#else
+#elif ZED_SDK_MAJOR_VERSION < 4
     sl::RuntimeParameters rparam((sl::SENSING_MODE)sensingMode_, quality_ > 0, confidenceThr_, texturenessConfidenceThr_, sl::REFERENCE_FRAME::CAMERA);
+#else
+    sl::RuntimeParameters rparam(quality_ > 0, sensingMode_ == 1, confidenceThr_, texturenessConfidenceThr_, sl::REFERENCE_FRAME::CAMERA);
 #endif
 
 	if(zed_)

corelib/src/odometry/OdometryF2M.cpp

@@ -214,6 +214,11 @@ Transform OdometryF2M::computeTransform(
 	if(sba_ && sba_->gravitySigma() > 0.0f && !imus().empty())
 	{
 		imuT = Transform::getTransform(imus(), data.stamp());
+		if(data.imu().empty())
+		{
+			Eigen::Quaternionf q = imuT.getQuaternionf();
+			data.setIMU(IMU(cv::Vec4d(q.x(), q.y(), q.z(), q.w()), cv::Mat(), cv::Vec3d(), cv::Mat(), cv::Vec3d(), cv::Mat()));
+		}
 	}
 
 	RegistrationInfo regInfo;

corelib/src/optimizer/OptimizerGTSAM.cpp

@@ -527,7 +527,11 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 			{
 				float x,y,z,roll,pitch,yaw;
 				std::map<int, Transform> tmpPoses;
+#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3)
+				for(gtsam::Values::deref_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter)
+#else
 				for(gtsam::Values::const_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter)
+#endif
 				{
 					if(iter->value.dim() > 1)
 					{
@@ -630,7 +634,11 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 				optimizer->iterations(), optimizer->error(), graph.error(initialEstimate), graph.error(optimizer->values()), timer.ticks());
 
 		float x,y,z,roll,pitch,yaw;
+#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3)
+		for(gtsam::Values::deref_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter)
+#else
 		for(gtsam::Values::const_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter)
+#endif
 		{
 			if(iter->value.dim() > 1)
 			{

corelib/src/optimizer/gtsam/GravityFactor.h

@@ -63,15 +63,21 @@ public:
 
   /** vector of errors */
   Vector attitudeError(const Rot3& p,
-      OptionalJacobian<2,3> H = boost::none) const;
+#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3)
+      OptionalJacobian<2,3> H = {}) const;
+#else
+  	  OptionalJacobian<2,3> H = boost::none) const;
+#endif
 
   /** Serialization function */
+#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_MAJOR < 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR < 3)
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
     ar & boost::serialization::make_nvp("nZ_", const_cast<Unit3&>(nZ_));
     ar & boost::serialization::make_nvp("bRef_", const_cast<Unit3&>(bRef_));
   }
+#endif
 };
 
 /**
@@ -85,7 +91,11 @@ class Rot3GravityFactor: public NoiseModelFactor1<Rot3>, public GravityFactor {
 public:
 
   /// shorthand for a smart pointer to a factor
+#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3)
+  typedef std::shared_ptr<Rot3GravityFactor> shared_ptr;
+  #else
   typedef boost::shared_ptr<Rot3GravityFactor> shared_ptr;
+#endif
 
   /// Typedef to this class
   typedef Rot3GravityFactor This;
@@ -111,7 +121,11 @@ public:
 
   /// @return a deep copy of this factor
   virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3)
+    return std::static_pointer_cast<gtsam::NonlinearFactor>(
+#else
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
+#endif
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));
   }
 
@@ -124,7 +138,11 @@ public:
 
   /** vector of errors */
   virtual Vector evaluateError(const Rot3& nRb, //
-      boost::optional<Matrix&> H = boost::none) const {
+#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3)
+	OptionalMatrixType H = OptionalNone) const {
+#else
+	boost::optional<Matrix&> H = boost::none) const {
+#endif
     return attitudeError(nRb, H);
   }
   Unit3 nZ() const {
@@ -135,7 +153,7 @@ public:
   }
 
 private:
-
+#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_MAJOR < 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR < 3)
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
@@ -145,6 +163,7 @@ private:
     ar & boost::serialization::make_nvp("GravityFactor",
         boost::serialization::base_object<GravityFactor>(*this));
   }
+#endif
 
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
@@ -163,8 +182,11 @@ class Pose3GravityFactor: public NoiseModelFactor1<Pose3>,
 public:
 
   /// shorthand for a smart pointer to a factor
+#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3)
+  typedef std::shared_ptr<Pose3GravityFactor> shared_ptr;
+#else
   typedef boost::shared_ptr<Pose3GravityFactor> shared_ptr;
-
+#endif
   /// Typedef to this class
   typedef Pose3GravityFactor This;
 
@@ -189,7 +211,11 @@ public:
 
   /// @return a deep copy of this factor
   virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3)
+    return std::static_pointer_cast<gtsam::NonlinearFactor>(
+#else
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
+#endif
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));
   }
 
@@ -202,7 +228,11 @@ public:
 
   /** vector of errors */
   virtual Vector evaluateError(const Pose3& nTb, //
+#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3)
+      OptionalMatrixType H = OptionalNone) const {
+#else
       boost::optional<Matrix&> H = boost::none) const {
+#endif
     Vector e = attitudeError(nTb.rotation(), H);
     if (H) {
       Matrix H23 = *H;
@@ -219,7 +249,7 @@ public:
   }
 
 private:
-
+#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_MAJOR < 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR < 3)
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
@@ -229,7 +259,7 @@ private:
     ar & boost::serialization::make_nvp("GravityFactor",
         boost::serialization::base_object<GravityFactor>(*this));
   }
-
+#endif
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };

corelib/src/optimizer/gtsam/XYFactor.h

@@ -41,7 +41,12 @@ public:
   // error function
   // @param p    the pose in Pose2
   // @param H    the optional Jacobian matrix, which use boost optional and has default null pointer
-  gtsam::Vector evaluateError(const VALUE& p, boost::optional<gtsam::Matrix&> H = boost::none) const {
+  gtsam::Vector evaluateError(const VALUE& p,
+#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3)
+		  OptionalMatrixType H = OptionalNone) const {
+#else
+		  boost::optional<gtsam::Matrix&> H = boost::none) const {
+#endif
 
     // note that use boost optional like a pointer
     // only calculate jacobian matrix when non-null pointer exists

corelib/src/optimizer/gtsam/XYZFactor.h

@@ -41,14 +41,24 @@ public:
   // error function
   // @param p    the pose in Pose
   // @param H    the optional Jacobian matrix, which use boost optional and has default null pointer
-  gtsam::Vector evaluateError(const gtsam::Pose3& p, boost::optional<gtsam::Matrix&> H = boost::none) const {
+  gtsam::Vector evaluateError(const gtsam::Pose3& p,
+#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3)
+		  OptionalMatrixType H = OptionalNone) const {
+#else
+		  boost::optional<gtsam::Matrix&> H = boost::none) const {
+#endif
     if(H)
     {
 	  p.translation(H);
     }
     return (gtsam::Vector3() << p.x() - mx_, p.y() - my_, p.z() - mz_).finished();
   }
-  gtsam::Vector evaluateError(const gtsam::Point3& p, boost::optional<gtsam::Matrix&> H = boost::none) const {
+  gtsam::Vector evaluateError(const gtsam::Point3& p,
+#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3)
+		  OptionalMatrixType H = OptionalNone) const {
+#else
+		  boost::optional<gtsam::Matrix&> H = boost::none) const {
+#endif
     return (gtsam::Vector3() << p.x() - mx_, p.y() - my_, p.z() - mz_).finished();
   }
 };

corelib/src/optimizer/vertigo/gtsam/DerivedValue.h

@@ -72,9 +72,15 @@ public:
   /**
    * Clone this value (normal clone on the heap, delete with 'delete' operator)
    */
+#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3)
+  virtual std::shared_ptr<gtsam::Value> clone() const {
+    return std::make_shared<DERIVED>(static_cast<const DERIVED&>(*this));
+  }
+  #else
   virtual boost::shared_ptr<gtsam::Value> clone() const {
     return boost::make_shared<DERIVED>(static_cast<const DERIVED&>(*this));
   }
+#endif
 
   /// equals implementing generic Value interface
   virtual bool equals_(const gtsam::Value& p, double tol = 1e-9) const {

corelib/src/optimizer/vertigo/gtsam/betweenFactorSwitchable.h

@@ -30,9 +30,15 @@ namespace vertigo {
         betweenFactor(key1, key2, measured, model) {};
 
       gtsam::Vector evaluateError(const VALUE& p1, const VALUE& p2, const SwitchVariableLinear& s,
+#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3)
+		  OptionalMatrixType H1 = OptionalNone,
+		  OptionalMatrixType H2 = OptionalNone,
+		  OptionalMatrixType H3 = OptionalNone) const
+#else
           boost::optional<gtsam::Matrix&> H1 = boost::none,
-          boost::optional<gtsam::Matrix&> H2 =  boost::none,
+          boost::optional<gtsam::Matrix&> H2 = boost::none,
-          boost::optional<gtsam::Matrix&> H3 =  boost::none) const
+          boost::optional<gtsam::Matrix&> H3 = boost::none) const
+#endif
         {
 
           // calculate error
@@ -64,9 +70,15 @@ namespace vertigo {
         betweenFactor(key1, key2, measured, model) {};
 
       gtsam::Vector evaluateError(const VALUE& p1, const VALUE& p2, const SwitchVariableSigmoid& s,
+#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3)
+		  OptionalMatrixType H1 = OptionalNone,
+		  OptionalMatrixType H2 = OptionalNone,
+		  OptionalMatrixType H3 = OptionalNone) const
+#else
           boost::optional<gtsam::Matrix&> H1 = boost::none,
-          boost::optional<gtsam::Matrix&> H2 =  boost::none,
+          boost::optional<gtsam::Matrix&> H2 = boost::none,
-          boost::optional<gtsam::Matrix&> H3 =  boost::none) const
+          boost::optional<gtsam::Matrix&> H3 = boost::none) const
+#endif
       {
 
         // calculate error

corelib/src/optimizer/vertigo/gtsam/switchVariableLinear.h

@@ -76,8 +76,13 @@ namespace vertigo {
 
     /** between operation */
     inline SwitchVariableLinear between(const SwitchVariableLinear& l2,
+#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3)
+		OptionalMatrixType H1=OptionalNone,
+		OptionalMatrixType H2=OptionalNone) const {
+#else
         boost::optional<gtsam::Matrix&> H1=boost::none,
         boost::optional<gtsam::Matrix&> H2=boost::none) const {
+#endif
       if(H1) *H1 = -gtsam::Matrix::Identity(1, 1);
       if(H2) *H2 = gtsam::Matrix::Identity(1, 1);
       return SwitchVariableLinear(l2.value() - value());
@@ -116,11 +121,19 @@ template<> struct traits<vertigo::SwitchVariableLinear> {
   typedef OptionalJacobian<3, 3> ChartJacobian;
   typedef gtsam::Vector TangentVector;
   static TangentVector Local(const vertigo::SwitchVariableLinear& origin, const vertigo::SwitchVariableLinear& other,
-  ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
+#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3)
+      ChartJacobian Horigin = {}, ChartJacobian Hother = {}) {
+#else
+      ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
+#endif
     return origin.localCoordinates(other);
   }
   static vertigo::SwitchVariableLinear Retract(const vertigo::SwitchVariableLinear& g, const TangentVector& v,
-        ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
+#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3)
+      ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
+#else
+      ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
+#endif
       return g.retract(v);
     }
 };

corelib/src/optimizer/vertigo/gtsam/switchVariableSigmoid.h

@@ -76,8 +76,13 @@ namespace vertigo {
 
     /** between operation */
     inline SwitchVariableSigmoid between(const SwitchVariableSigmoid& l2,
+#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3)
+        OptionalMatrixType H1=OptionalNone,
+		OptionalMatrixType H2=OptionalNone) const {
+#else
         boost::optional<gtsam::Matrix&> H1=boost::none,
         boost::optional<gtsam::Matrix&> H2=boost::none) const {
+#endif
       if(H1) *H1 = -gtsam::Matrix::Identity(1, 1);
       if(H2) *H2 = gtsam::Matrix::Identity(1, 1);
       return SwitchVariableSigmoid(l2.value() - value());
@@ -117,11 +122,19 @@ template<> struct traits<vertigo::SwitchVariableSigmoid> {
   typedef OptionalJacobian<3, 3> ChartJacobian;
   typedef gtsam::Vector TangentVector;
   static TangentVector Local(const vertigo::SwitchVariableSigmoid& origin, const vertigo::SwitchVariableSigmoid& other,
-  ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
+#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3)
+	  ChartJacobian Horigin = {}, ChartJacobian Hother = {}) {
+#else
+	  ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
+#endif
     return origin.localCoordinates(other);
   }
   static vertigo::SwitchVariableSigmoid Retract(const vertigo::SwitchVariableSigmoid& g, const TangentVector& v,
+#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3)
+        ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
+#else
         ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
+#endif
       return g.retract(v);
     }
 };

corelib/src/util3d.cpp

@@ -372,34 +372,127 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
 	float depthCx = model.cx() * rgbToDepthFactorX;
 	float depthCy = model.cy() * rgbToDepthFactorY;
 
-	UDEBUG("depth=%dx%d fx=%f fy=%f cx=%f cy=%f (depth factors=%f %f) decimation=%d",
+	bool isMM = imageDepth.type() == CV_16UC1;
+
+	UDEBUG("depth=%dx%d (isMM=%d) fx=%f fy=%f cx=%f cy=%f (depth factors=%f %f) decimation=%d",
 			imageDepth.cols, imageDepth.rows,
+			isMM?1:0,
 			model.fx(), model.fy(), model.cx(), model.cy(),
 			rgbToDepthFactorX,
 			rgbToDepthFactorY,
 			decimation);
 
+	int decimationMode = 1;
 	int oi = 0;
-	for(int h = 0; h < imageDepth.rows && h/decimation < (int)cloud->height; h+=decimation)
+	if(isMM)
 	{
-		for(int w = 0; w < imageDepth.cols && w/decimation < (int)cloud->width; w+=decimation)
+		for(int h = 0; h < imageDepth.rows && h/decimation < (int)cloud->height; h+=decimation)
 		{
-			pcl::PointXYZ & pt = cloud->at((h/decimation)*cloud->width + (w/decimation));
+			const unsigned short * rowPtr = imageDepth.ptr<unsigned short>(h);
-
+			for(int w = 0; w < imageDepth.cols && w/decimation < (int)cloud->width; w+=decimation)
-			pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, depthCx, depthCy, depthFx, depthFy, false);
-			if(pcl::isFinite(ptXYZ) && ptXYZ.z>=minDepth && (maxDepth<=0.0f || ptXYZ.z <= maxDepth))
 			{
-				pt.x = ptXYZ.x;
+				pcl::PointXYZ & pt = cloud->at((h/decimation)*cloud->width + (w/decimation));
-				pt.y = ptXYZ.y;
+
-				pt.z = ptXYZ.z;
+				pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
-				if(validIndices)
+
+				if(decimationMode == 1 && decimation>1)
+				{
+					// project closest point
+					cv::Point2i closestPixel(w,h);
+					unsigned short closestDepthMM = 0;
+					for(int v = h; v < h + decimation; ++v)
+					{
+						const unsigned short * roiRowPtr = imageDepth.ptr<unsigned short>(v);
+						for(int u = w; u < w + decimation; ++u)
+						{
+							const unsigned short & depthMM = roiRowPtr[u];
+							if(depthMM > 0 && (depthMM < closestDepthMM || closestDepthMM == 0))
+							{
+								closestDepthMM = depthMM;
+								closestPixel.x = u;
+								closestPixel.y = v;
+							}
+						}
+					}
+					if(closestDepthMM > 0)
+					{
+						float depth = ((float)closestDepthMM)/1000.0f;
+						if(depth>=minDepth && (maxDepth<=0.0f || depth <= maxDepth))
+						{
+							// Fill in XYZ
+							pt.z = depth;
+							pt.x = ((float)closestPixel.x - depthCx) * pt.z / depthFx;
+							pt.y = ((float)closestPixel.y - depthCy) * pt.z / depthFy;
+						}
+					}
+				}
+				else if(rowPtr[w]>0)
+				{
+					float depth = ((float)rowPtr[w])/1000.0f;
+					if(depth>=minDepth && (maxDepth<=0.0f || depth <= maxDepth))
+					{
+						// Fill in XYZ
+						pt.z = depth;
+						pt.x = ((float)w - depthCx) * pt.z / depthFx;
+						pt.y = ((float)h - depthCy) * pt.z / depthFy;
+					}
+				}
+				if(pcl::isFinite(pt) && validIndices)
 				{
 					validIndices->at(oi++) = (h/decimation)*cloud->width + (w/decimation);
 				}
 			}
-			else
+		}
+	}
+	else
+	{
+		for(int h = 0; h < imageDepth.rows && h/decimation < (int)cloud->height; h+=decimation)
+		{
+			const float * rowPtr = imageDepth.ptr<float>(h);
+			for(int w = 0; w < imageDepth.cols && w/decimation < (int)cloud->width; w+=decimation)
 			{
+				pcl::PointXYZ & pt = cloud->at((h/decimation)*cloud->width + (w/decimation));
+
 				pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
+
+				if(decimationMode == 1 && decimation>1)
+				{
+					// project closest point
+					cv::Point2i closestPixel(w,h);
+					float closestDepth = 0.0f;
+					for(int v = h; v < h + decimation; ++v)
+					{
+						const float * roiRowPtr =  imageDepth.ptr<float>(v);
+						for(int u = w; u < w + decimation; ++u)
+						{
+							const float & depth = roiRowPtr[u];
+							if(depth > 0.0f && (depth < closestDepth || closestDepth == 0.0f))
+							{
+								closestDepth = depth;
+								closestPixel.x = u;
+								closestPixel.y = v;
+							}
+						}
+					}
+					if(closestDepth > 0.0f && closestDepth>=minDepth && (maxDepth<=0.0f || closestDepth <= maxDepth))
+					{
+						// Fill in XYZ
+						pt.z = closestDepth;
+						pt.x = ((float)closestPixel.x - depthCx) * pt.z / depthFx;
+						pt.y = ((float)closestPixel.y - depthCy) * pt.z / depthFy;
+					}
+				}
+				else if(rowPtr[w] > 0 && rowPtr[w]>=minDepth && (maxDepth<=0.0f || rowPtr[w] <= maxDepth))
+				{
+					// Fill in XYZ
+					pt.z = rowPtr[w];
+					pt.x = ((float)w - depthCx) * pt.z / depthFx;
+					pt.y = ((float)h - depthCy) * pt.z / depthFy;
+				}
+				if(pcl::isFinite(pt) && validIndices)
+				{
+					validIndices->at(oi++) = (h/decimation)*cloud->width + (w/decimation);
+				}
 			}
 		}
 	}

corelib/src/util3d_mapping.cpp

@@ -1026,6 +1026,36 @@ cv::Mat erodeMap(const cv::Mat & map)
 	return erodedMap;
 }
 
+void clusterIndicesFloodfill(std::vector<int> & cluster,
+	float * visitedIndices,
+	int width,
+	int height,
+	float clusterRadius,
+	int currentIndex,
+	float previousHeight)
+{
+	if(visitedIndices[currentIndex] == 0.0f || 
+		(clusterRadius>0.0f && fabs(visitedIndices[currentIndex]-previousHeight)>clusterRadius))
+	{
+		return;
+	}
+
+	int y = currentIndex / width;
+	int x = currentIndex - y*width;
+
+	if(x>=0 && x<width && y>=0 && y<height)
+	{
+		cluster.push_back(currentIndex);
+		float currentHeight = visitedIndices[currentIndex];
+		visitedIndices[currentIndex] = 0;
+
+		clusterIndicesFloodfill(cluster, visitedIndices, width, height, clusterRadius, (y+1)*width + x, currentHeight);
+		clusterIndicesFloodfill(cluster, visitedIndices, width, height, clusterRadius, (y-1)*width + x, currentHeight);
+		clusterIndicesFloodfill(cluster, visitedIndices, width, height, clusterRadius, y*width + x+1, currentHeight);
+		clusterIndicesFloodfill(cluster, visitedIndices, width, height, clusterRadius, y*width + x-1, currentHeight);
+	}
+}
+
 }
 
 }

corelib/src/util3d_surface.cpp

@@ -3033,6 +3033,65 @@ pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals2D(
 	return computeFastOrganizedNormals2DImpl<pcl::PointXYZI>(cloud, searchK, searchRadius, viewPoint);
 }
 
+template<typename PointT>
+pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormalsImpl(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float maxDepthChangeFactor,
+		float normalSmoothingSize,
+		const Eigen::Vector3f & viewPoint)
+{
+	UASSERT(cloud->isOrganized());
+
+	// Normal estimation
+	pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
+	pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
+#if PCL_VERSION_COMPARE(<, 1, 7, 0)
+	ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT);
+	ne.setBorderPolicy(ne.BORDER_POLICY_MIRROR);
+#else
+	ne.setNormalEstimationMethod (ne.SIMPLE_3D_GRADIENT);
+	ne.setBorderPolicy(ne.BORDER_POLICY_IGNORE);
+#endif
+	ne.setMaxDepthChangeFactor(maxDepthChangeFactor);
+	ne.setNormalSmoothingSize(normalSmoothingSize);
+	ne.setInputCloud(cloud);
+	// Commented: Keep the output normals size the same as the input cloud
+	//if(indices->size())
+	//{
+	//	ne.setIndices(indices);
+	//}
+
+	// create kdtree search tree (not used by IntegralImageNormalEstimation) to avoid 
+	// "[pcl::OrganizedNeighbor::radiusSearch] Input dataset is not from a projective device!"
+	// on clouds smaller than regular organized clouds from camera (640x480)
+	typename pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>);
+	ne.setSearchMethod(tree);
+
+	ne.setViewPoint(viewPoint[0], viewPoint[1], viewPoint[2]);
+	ne.compute(*normals);
+
+	return normals;
+}
+pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		float maxDepthChangeFactor,
+		float normalSmoothingSize,
+		const Eigen::Vector3f & viewPoint)
+{
+	pcl::IndicesPtr indices(new std::vector<int>);
+	return computeFastOrganizedNormals(cloud, indices, maxDepthChangeFactor, normalSmoothingSize, viewPoint);
+}
+pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float maxDepthChangeFactor,
+		float normalSmoothingSize,
+		const Eigen::Vector3f & viewPoint)
+{
+	return computeFastOrganizedNormalsImpl<pcl::PointXYZ>(cloud, indices, maxDepthChangeFactor, normalSmoothingSize, viewPoint);
+}
+
 pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		float maxDepthChangeFactor,
@@ -3049,36 +3108,7 @@ pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
 		float normalSmoothingSize,
 		const Eigen::Vector3f & viewPoint)
 {
-	UASSERT(cloud->isOrganized());
+	return computeFastOrganizedNormalsImpl<pcl::PointXYZRGB>(cloud, indices, maxDepthChangeFactor, normalSmoothingSize, viewPoint);
-
-	pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
-	if(indices->size())
-	{
-		tree->setInputCloud(cloud, indices);
-	}
-	else
-	{
-		tree->setInputCloud (cloud);
-	}
-
-	// Normal estimation
-	pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
-	pcl::IntegralImageNormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
-	ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT);
-	ne.setMaxDepthChangeFactor(maxDepthChangeFactor);
-	ne.setNormalSmoothingSize(normalSmoothingSize);
-	ne.setBorderPolicy(ne.BORDER_POLICY_MIRROR);
-	ne.setInputCloud(cloud);
-	// Commented: Keep the output normals size the same as the input cloud
-	//if(indices->size())
-	//{
-	//	ne.setIndices(indices);
-	//}
-	ne.setSearchMethod(tree);
-	ne.setViewPoint(viewPoint[0], viewPoint[1], viewPoint[2]);
-	ne.compute(*normals);
-
-	return normals;
 }
 
 float computeNormalsComplexity(

docker/bionic/Dockerfile

@@ -5,7 +5,7 @@ FROM ros:melodic-perception
 # Install build dependencies
 RUN apt-get update && \
     apt-get install -y git software-properties-common ros-melodic-rtabmap-ros && \
-    apt-get remove -y ros-melodic-rtabmap && \
+    apt-get remove -y ros-melodic-rtabmap* && \
     apt-get clean && rm -rf /var/lib/apt/lists/
 
 WORKDIR /root/
@@ -49,6 +49,9 @@ RUN git clone https://github.com/laurentkneip/opengv.git && \
     cd && \
     rm -r opengv
 
+# Will be used to read/store databases on host
+RUN mkdir -p /root/Documents/RTAB-Map
+
 RUN rm /bin/sh && ln -s /bin/bash /bin/sh
 
 # Copy current source code

docker/bionic/nvidia/Dockerfile

@@ -1,13 +0,0 @@
-FROM introlab3it/rtabmap:bionic
-
-# nvidia-container-runtime
-ENV NVIDIA_VISIBLE_DEVICES \
-    ${NVIDIA_VISIBLE_DEVICES:-all}
-ENV NVIDIA_DRIVER_CAPABILITIES \
-    ${NVIDIA_DRIVER_CAPABILITIES:+$NVIDIA_DRIVER_CAPABILITIES,}graphics
-
-# Will be used to read/store databases on host
-RUN mkdir -p /root/Documents/RTAB-Map
-
-# On Nvidia Jetpack, uncomment the following (https://github.com/introlab/rtabmap/issues/776):
-# ENV LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/lib/aarch64-linux-gnu/tegra

docker/focal-foxy/Dockerfile

@@ -1,10 +1,22 @@
 # Image: introlab3it/rtabmap:focal-foxy
 
-FROM osrf/ros:foxy-desktop
+FROM ubuntu:20.04
+
+RUN apt update && \
+    apt install software-properties-common -y && \
+    add-apt-repository universe && \
+    apt update && \
+    apt install curl -y && \
+    curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg && \
+    echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | tee /etc/apt/sources.list.d/ros2.list > /dev/null && \
+    apt-get clean && rm -rf /var/lib/apt/lists/
+
+ENV DEBIAN_FRONTEND=noninteractive
 
 # Install build dependencies
 RUN apt-get update && \
-    apt-get install -y git software-properties-common ros-foxy-rtabmap-ros && \
+    apt upgrade -y && \
+    apt-get install -y git ros-foxy-ros-base python3-argcomplete ros-dev-tools ros-foxy-rtabmap-ros && \
     apt-get remove -y ros-foxy-rtabmap && \
     apt-get clean && rm -rf /var/lib/apt/lists/
 
@@ -32,8 +44,6 @@ ARG TARGETPLATFORM
 ENV TARGETPLATFORM=${TARGETPLATFORM:-linux/amd64}
 RUN echo "I am building for $TARGETPLATFORM"
 
-ENV DEBIAN_FRONTEND=noninteractive
-
 # Azure Kinect DK
 # Taken from https://github.com/microsoft/Azure-Kinect-Sensor-SDK/issues/1190#issuecomment-822772494
 # K4A binaries on 20.04 not released yet, we should take those from 18.04
@@ -77,7 +87,7 @@ RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing libfreenect2
 
 # zed open capture
 RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing zed-open-capture..." && \
-    apt-get update && apt install libusb-1.0-0-dev libhidapi-libusb0 libhidapi-dev wget && \
+    apt-get update && apt install -y libusb-1.0-0-dev libhidapi-libusb0 libhidapi-dev wget && \
     apt-get clean && rm -rf /var/lib/apt/lists/ && \
     git clone https://github.com/stereolabs/zed-open-capture.git && \
     cd zed-open-capture && \
@@ -145,7 +155,7 @@ RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/
     rm -r AliceVision; fi
     
 #commit Aug 6 2020
-RUN apt-get update && apt install wget && \
+RUN apt-get update && apt install -y wget && \
     apt-get clean && rm -rf /var/lib/apt/lists/
 RUN git clone https://github.com/laurentkneip/opengv.git && \
     cd opengv && \
@@ -160,11 +170,28 @@ RUN git clone https://github.com/laurentkneip/opengv.git && \
     cd && \
     rm -r opengv
 
+#RUN git clone --branch 4.2.0 https://github.com/opencv/opencv.git && \
+#    git clone --branch 4.2.0 https://github.com/opencv/opencv_contrib.git && \
+#    cd opencv && \
+#    mkdir build && \
+#    cd build && \
+#    cmake -DBUILD_opencv_python3=OFF -DBUILD_opencv_python_bindings_generator=OFF -DBUILD_opencv_python_tests=OFF -DBUILD_PERF_TESTS=OFF -DBUILD_TESTS=OFF -DOPENCV_ENABLE_NONFREE=ON -DOPENCV_EXTRA_MODULES_PATH=/root/opencv_contrib/modules .. && \
+#    make -j$(nproc) && \
+#    make install && \
+#    cd ../.. && \
+#    rm -rf opencv opencv_contrib
+
+# Will be used to read/store databases on host
+RUN mkdir -p /root/Documents/RTAB-Map && chmod 777 /root/Documents/RTAB-Map
+
 RUN rm /bin/sh && ln -s /bin/bash /bin/sh
 
 # Copy current source code
 COPY . /root/rtabmap
 
+RUN cp /root/rtabmap/docker/focal-foxy/ros_entrypoint.sh /ros_entrypoint.sh
+ENTRYPOINT [ "/ros_entrypoint.sh" ]
+
 # Build RTAB-Map project
 RUN source /ros_entrypoint.sh && \
     cd rtabmap/build && \

docker/focal-foxy/nvidia/Dockerfile

@@ -1,13 +0,0 @@
-FROM introlab3it/rtabmap:focal-foxy
-
-# nvidia-container-runtime
-ENV NVIDIA_VISIBLE_DEVICES \
-    ${NVIDIA_VISIBLE_DEVICES:-all}
-ENV NVIDIA_DRIVER_CAPABILITIES \
-    ${NVIDIA_DRIVER_CAPABILITIES:+$NVIDIA_DRIVER_CAPABILITIES,}graphics
-    
-# Will be used to read/store databases on host
-RUN mkdir -p /root/Documents/RTAB-Map
-
-# On Nvidia Jetpack, uncomment the following (https://github.com/introlab/rtabmap/issues/776):
-# ENV LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/lib/aarch64-linux-gnu/tegra

docker/focal-foxy/ros_entrypoint.sh

@@ -0,0 +1,6 @@
+#!/bin/bash
+set -e
+
+# setup ros2 environment
+source "/opt/ros/foxy/setup.bash" --
+exec "$@"

docker/focal/Dockerfile

@@ -5,7 +5,7 @@ FROM ros:noetic-perception
 # Install build dependencies
 RUN apt-get update && \
     apt-get install -y git software-properties-common ros-noetic-rtabmap-ros && \
-    apt-get remove -y ros-noetic-rtabmap && \
+    apt-get remove -y ros-noetic-rtabmap* && \
     apt-get clean && rm -rf /var/lib/apt/lists/
 
 WORKDIR /root/
@@ -159,6 +159,20 @@ RUN git clone https://github.com/laurentkneip/opengv.git && \
     cd && \
     rm -r opengv
 
+RUN git clone --branch 4.2.0 https://github.com/opencv/opencv.git && \
+    git clone --branch 4.2.0 https://github.com/opencv/opencv_contrib.git && \
+    cd opencv && \
+    mkdir build && \
+    cd build && \
+    cmake -DBUILD_opencv_python3=OFF -DBUILD_opencv_python_bindings_generator=OFF -DBUILD_opencv_python_tests=OFF -DBUILD_PERF_TESTS=OFF -DBUILD_TESTS=OFF -DOPENCV_ENABLE_NONFREE=ON -DOPENCV_EXTRA_MODULES_PATH=/root/opencv_contrib/modules .. && \
+    make -j$(nproc) && \
+    make install && \
+    cd ../.. && \
+    rm -rf opencv opencv_contrib
+
+# Will be used to read/store databases on host
+RUN mkdir -p /root/Documents/RTAB-Map
+
 RUN rm /bin/sh && ln -s /bin/bash /bin/sh
 
 # Copy current source code

docker/focal/nvidia/Dockerfile

@@ -1,13 +0,0 @@
-FROM introlab3it/rtabmap:focal
-
-# nvidia-container-runtime
-ENV NVIDIA_VISIBLE_DEVICES \
-    ${NVIDIA_VISIBLE_DEVICES:-all}
-ENV NVIDIA_DRIVER_CAPABILITIES \
-    ${NVIDIA_DRIVER_CAPABILITIES:+$NVIDIA_DRIVER_CAPABILITIES,}graphics
-    
-# Will be used to read/store databases on host
-RUN mkdir -p /root/Documents/RTAB-Map
-
-# On Nvidia Jetpack, uncomment the following (https://github.com/introlab/rtabmap/issues/776):
-# ENV LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/lib/aarch64-linux-gnu/tegra

docker/jammy/Dockerfile

@@ -1,15 +1,28 @@
 # Image: introlab3it/rtabmap:jammy
 
-FROM osrf/ros:humble-desktop
+FROM ubuntu:22.04
+
+# Install ROS2
+RUN apt update && \
+    apt install software-properties-common -y && \
+    add-apt-repository universe && \
+    apt update && \
+    apt install curl -y && \
+    curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg && \
+    echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | tee /etc/apt/sources.list.d/ros2.list > /dev/null && \
+    apt-get clean && rm -rf /var/lib/apt/lists/
+
+ENV DEBIAN_FRONTEND=noninteractive
 
 # Install build dependencies
 RUN apt-get update && \
-    apt-get install -y git software-properties-common ros-humble-rtabmap-ros && \
+    apt upgrade -y && \
-    apt-get remove -y ros-humble-rtabmap && \
+    apt-get install -y git ros-humble-ros-base ros-dev-tools ros-humble-rtabmap-ros && \
+    apt-get remove -y ros-humble-rtabmap* && \
     apt-get clean && rm -rf /var/lib/apt/lists/
 
 WORKDIR /root/
-   
+
 # PDAL
 RUN apt-get update && apt-get install -y libpdal-dev && \
     apt-get clean && rm -rf /var/lib/apt/lists/
@@ -22,8 +35,6 @@ ARG TARGETPLATFORM
 ENV TARGETPLATFORM=${TARGETPLATFORM:-linux/amd64}
 RUN echo "I am building for $TARGETPLATFORM"
 
-ENV DEBIAN_FRONTEND=noninteractive
-
 # libfreenect2 
 RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing libfreenect2..." && \
     apt-get update && apt-get install -y mesa-utils xserver-xorg-video-all libusb-1.0-0-dev libturbojpeg0-dev libglfw3-dev && \
@@ -40,7 +51,7 @@ RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing libfreenect2
 
 # zed open capture
 RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing zed-open-capture..." && \
-    apt-get update && apt install libusb-1.0-0-dev libhidapi-libusb0 libhidapi-dev wget && \
+    apt-get update && apt install -y libusb-1.0-0-dev libhidapi-libusb0 libhidapi-dev wget && \
     apt-get clean && rm -rf /var/lib/apt/lists/ && \
     git clone https://github.com/stereolabs/zed-open-capture.git && \
     cd zed-open-capture && \
@@ -53,7 +64,7 @@ RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing zed-open-cap
     rm -r zed-open-capture; fi
     
 #commit Aug 6 2020
-RUN apt-get update && apt install wget && \
+RUN apt-get update && apt install -y wget && \
     apt-get clean && rm -rf /var/lib/apt/lists/
 RUN git clone https://github.com/laurentkneip/opengv.git && \
     cd opengv && \
@@ -68,11 +79,28 @@ RUN git clone https://github.com/laurentkneip/opengv.git && \
     cd && \
     rm -r opengv
 
+RUN git clone --branch 4.5.4 https://github.com/opencv/opencv.git && \
+    git clone --branch 4.5.4 https://github.com/opencv/opencv_contrib.git && \
+    cd opencv && \
+    mkdir build && \
+    cd build && \
+    cmake -DBUILD_opencv_python3=OFF -DBUILD_opencv_python_bindings_generator=OFF -DBUILD_opencv_python_tests=OFF -DBUILD_PERF_TESTS=OFF -DBUILD_TESTS=OFF -DOPENCV_ENABLE_NONFREE=ON -DOPENCV_EXTRA_MODULES_PATH=/root/opencv_contrib/modules .. && \
+    make -j$(nproc) && \
+    make install && \
+    cd ../.. && \
+    rm -rf opencv opencv_contrib
+
+# Will be used to read/store databases on host
+RUN mkdir -p /root/Documents/RTAB-Map && chmod 777 /root/Documents/RTAB-Map
+
 RUN rm /bin/sh && ln -s /bin/bash /bin/sh
 
 # Copy current source code
 COPY . /root/rtabmap
 
+RUN cp /root/rtabmap/docker/jammy/ros_entrypoint.sh /ros_entrypoint.sh
+ENTRYPOINT [ "/ros_entrypoint.sh" ]
+
 # ros2 seems not sourcing by default its multi-arch folders
 ENV LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/opt/ros/humble/lib/x86_64-linux-gnu:/opt/ros/humble/lib/aarch64-linux-gnu
 

docker/jammy/nvidia/Dockerfile

@@ -1,13 +0,0 @@
-FROM introlab3it/rtabmap:jammy
-
-# nvidia-container-runtime
-ENV NVIDIA_VISIBLE_DEVICES \
-    ${NVIDIA_VISIBLE_DEVICES:-all}
-ENV NVIDIA_DRIVER_CAPABILITIES \
-    ${NVIDIA_DRIVER_CAPABILITIES:+$NVIDIA_DRIVER_CAPABILITIES,}graphics
-    
-# Will be used to read/store databases on host
-RUN mkdir -p /root/Documents/RTAB-Map
-
-# On Nvidia Jetpack, uncomment the following (https://github.com/introlab/rtabmap/issues/776):
-# ENV LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/lib/aarch64-linux-gnu/tegra

docker/jammy/ros_entrypoint.sh

@@ -0,0 +1,6 @@
+#!/bin/bash
+set -e
+
+# setup ros2 environment
+source "/opt/ros/humble/setup.bash" --
+exec "$@"

docker/latest_deps/Dockerfile

@@ -4,7 +4,7 @@ FROM osrf/ros:humble-desktop
 # Install build dependencies
 RUN apt-get update && \
     apt-get install -y git software-properties-common ros-humble-rtabmap-ros libqt6* qt6* && \
-    apt-get remove -y ros-humble-rtabmap libpcl* libqt5* qt5* libvtk* libopencv* && \
+    apt-get remove -y ros-humble-rtabmap* ros-humble-libg2o libpcl* libqt5* qt5* libvtk* libopencv* && \
     apt-get clean && rm -rf /var/lib/apt/lists/
 
 WORKDIR /root/
@@ -42,12 +42,34 @@ RUN git clone https://github.com/opencv/opencv.git && \
     cd opencv && \
     mkdir build && \
     cd build && \
-    cmake -DBUILD_opencv_python3=OFF -DBUILD_opencv_python_bindings_generator=OFF -DBUILD_opencv_python_tests=OFF -DBUILD_PERF_TESTS=OFF -DBUILD_TESTS=OFF -DOPENCV_EXTRA_MODULES_PATH=/root/opencv_contrib/modules .. && \
+    cmake -DBUILD_opencv_python3=OFF -DBUILD_opencv_python_bindings_generator=OFF -DBUILD_opencv_python_tests=OFF -DBUILD_PERF_TESTS=OFF -DBUILD_TESTS=OFF -DOPENCV_ENABLE_NONFREE=ON -DOPENCV_EXTRA_MODULES_PATH=/root/opencv_contrib/modules .. && \
     make -j$(nproc) && \
     make install && \
     cd ../.. && \
     rm -rf opencv
 
+# Build latest gtsam
+RUN git clone https://github.com/borglab/gtsam.git && \
+ cd gtsam && \
+ mkdir build && \
+ cd build && \
+ cmake -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_STATIC_LIBRARY=OFF -DGTSAM_BUILD_UNSTABLE=OFF -DGTSAM_INSTALL_CPPUNILITE=OFF -DGTSAM_USE_SYSTEM_EIGEN=ON .. && \
+ cmake --build . --config Release --target install && \
+ cd ../.. && \
+ rm -rf gtsam
+
+# Build latest g2o
+RUN git clone https://github.com/RainerKuemmerle/g2o.git  && \
+ cd g2o && \
+ mkdir build && \
+ cd build && \
+ cmake -DBUILD_WITH_MARCH_NATIVE=OFF -DG2O_BUILD_APPS=OFF -DG2O_BUILD_EXAMPLES=OFF -DG2O_USE_OPENGL=OFF .. && \
+ cmake --build . --config Release --target install && \
+ cd ../.. && \
+ rm -rf g2o
+
+RUN mkdir -p /root/Documents/RTAB-Map
+
 # Copy current source code
 COPY . /root/rtabmap
 
@@ -61,9 +83,4 @@ RUN source /ros_entrypoint.sh && \
     rm -rf rtabmap && \
     ldconfig
 
-# nvidia-container-runtime
-ENV NVIDIA_VISIBLE_DEVICES \
-    ${NVIDIA_VISIBLE_DEVICES:-all}
-ENV NVIDIA_DRIVER_CAPABILITIES \
-    ${NVIDIA_DRIVER_CAPABILITIES:+$NVIDIA_DRIVER_CAPABILITIES,}graphics
 

guilib/include/rtabmap/gui/ExportCloudsDialog.h

@@ -153,6 +153,10 @@ private:
 	GainCompensator * _compensator;
 	const DBDriver * _dbDriver;
 	bool _scansHaveRGB;
+
+    bool saveOBJFile(const QString &path, pcl::TextureMesh::Ptr &mesh) const;
+    bool saveOBJFile(const QString &path, pcl::PolygonMesh &mesh) const;
+
 };
 
 }

guilib/src/CreateSimpleCalibrationDialog.cpp

@@ -175,7 +175,7 @@ void CreateSimpleCalibrationDialog::saveCalibration()
 					ui_->doubleSpinBox_fy->value(),
 					ui_->doubleSpinBox_cx->value(),
 					ui_->doubleSpinBox_cy->value(),
-					Transform::getIdentity(),
+					CameraModel::opticalRotation(),
 					0,
 					cv::Size(width, height));
 			UASSERT(modelLeft.isValidForProjection());

guilib/src/ExportCloudsDialog.cpp

@@ -2844,7 +2844,7 @@ bool ExportCloudsDialog::getExportedClouds(
 						}
 						if(!image.empty())
 						{
-
+
 							if(_ui->spinBox_camProjDecimation->value()>1)
 							{
 								image = util2d::decimate(image, _ui->spinBox_camProjDecimation->value());
@@ -4369,11 +4369,11 @@ void ExportCloudsDialog::saveMeshes(
 				}
 				else if(QFileInfo(path).suffix() == "obj")
 				{
-					success = pcl::io::saveOBJFile(path.toStdString(), *meshes.begin()->second) == 0;
+					success = saveOBJFile(path, *meshes.begin()->second);
 				}
 				else
 				{
-					UERROR("Extension not recognized! (%s) Should be (*.ply).", QFileInfo(path).suffix().toStdString().c_str());
+					UERROR("Extension not recognized! (%s) Should be (*.ply) or (*.obj).", QFileInfo(path).suffix().toStdString().c_str());
 				}
 				if(success)
 				{
@@ -4455,7 +4455,7 @@ void ExportCloudsDialog::saveMeshes(
 							}
 							else if(suffix == "obj")
 							{
-								success = pcl::io::saveOBJFile(pathFile.toStdString(), mesh) == 0;
+								success = saveOBJFile(pathFile, mesh);
 							}
 							else
 							{
@@ -4779,8 +4779,7 @@ void ExportCloudsDialog::saveTextureMeshes(
 					}
 				}
 
-				success = pcl::io::saveOBJFile(path.toStdString(), *mesh) == 0;
+                if(saveOBJFile(path, mesh))
-				if(success)
 				{
 					_progressDialog->incrementStep();
 					_progressDialog->appendText(tr("Saving the mesh (with %1 textures)... done.").arg(mesh->tex_materials.size()));
@@ -4976,7 +4975,7 @@ void ExportCloudsDialog::saveTextureMeshes(
 						bool success =false;
 						if(suffix == "obj")
 						{
-							success = pcl::io::saveOBJFile(pathFile.toStdString(), *mesh) == 0;
+							success = saveOBJFile(pathFile, mesh);
 						}
 						else
 						{
@@ -5010,4 +5009,28 @@ void ExportCloudsDialog::saveTextureMeshes(
 	}
 }
 
+    bool ExportCloudsDialog::saveOBJFile(const QString &path, pcl::TextureMesh::Ptr &mesh) const {
+#if PCL_VERSION_COMPARE(>=, 1, 13, 0)
+        mesh->tex_coord_indices = std::vector<std::vector<pcl::Vertices>>();
+        auto nr_meshes = static_cast<unsigned>(mesh->tex_polygons.size());
+        unsigned f_idx = 0;
+        for (unsigned m = 0; m < nr_meshes; m++) {
+            std::vector<pcl::Vertices> ci = mesh->tex_polygons[m];
+            for(std::size_t i = 0; i < ci.size(); i++) {
+                for (std::size_t j = 0; j < ci[i].vertices.size(); j++) {
+                    ci[i].vertices[j] = ci[i].vertices.size() * (i + f_idx) + j;
+                }
+            }
+            mesh->tex_coord_indices.push_back(ci);
+            f_idx += static_cast<unsigned>(mesh->tex_polygons[m].size());
+        }
+#endif
+        return pcl::io::saveOBJFile(path.toStdString(), *mesh) == 0;
+    }
+
+    bool ExportCloudsDialog::saveOBJFile(const QString &path, pcl::PolygonMesh &mesh) const {
+        return pcl::io::saveOBJFile(path.toStdString(), mesh) == 0;
+    }
+
+
 }

guilib/src/MainWindow.cpp

@@ -5843,27 +5843,20 @@ void MainWindow::startDetection()
 					_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcImages) &&
 				   !_preferencesDialog->getIMUPath().isEmpty())
 				{
-					if( odomStrategy != Odometry::kTypeOkvis &&
+					_imuThread = new IMUThread(_preferencesDialog->getIMURate(), _preferencesDialog->getIMULocalTransform());
-						odomStrategy != Odometry::kTypeMSCKF &&
+					if(_preferencesDialog->getIMUFilteringStrategy()>0)
-						odomStrategy != Odometry::kTypeVINS &&
+					{
-						odomStrategy != Odometry::kTypeOpenVINS)
+						_imuThread->enableIMUFiltering(_preferencesDialog->getIMUFilteringStrategy()-1, parameters, _preferencesDialog->getIMUFilteringBaseFrameConversion());
+					}
+					if(!_imuThread->init(_preferencesDialog->getIMUPath().toStdString()))
 					{
 						QMessageBox::warning(this, tr("Source IMU Path"),
-								tr("IMU path is set but odometry chosen doesn't support asynchronous IMU, ignoring IMU..."), QMessageBox::Ok);
+							tr("Initialization of IMU data has failed! Path=%1.").arg(_preferencesDialog->getIMUPath()), QMessageBox::Ok);
-					}
+						delete _camera;
-					else
+						_camera = 0;
-					{
+						delete _imuThread;
-						_imuThread = new IMUThread(_preferencesDialog->getIMURate(), _preferencesDialog->getIMULocalTransform());
+						_imuThread = 0;
-						if(!_imuThread->init(_preferencesDialog->getIMUPath().toStdString()))
+						return;
-						{
-							QMessageBox::warning(this, tr("Source IMU Path"),
-								tr("Initialization of IMU data has failed! Path=%1.").arg(_preferencesDialog->getIMUPath()), QMessageBox::Ok);
-							delete _camera;
-							_camera = 0;
-							delete _imuThread;
-							_imuThread = 0;
-							return;
-						}
 					}
 				}
 				Odometry * odom = Odometry::create(odomParameters);

guilib/src/ParametersToolBox.cpp

@@ -296,19 +296,14 @@ void ParametersToolBox::addParameter(
 	{
 		addParameter(layout, key.c_str(), QString::fromStdString(value));
 	}
-	else if(type.compare("int") == 0)
+	else if(type.compare("int") == 0 ||
+	        type.compare("uint") == 0 ||
+	        type.compare("unsigned int") == 0)
 	{
 		addParameter(layout, key.c_str(), uStr2Int(value));
 	}
-	else if(type.compare("uint") == 0)
+	else if(type.compare("double") == 0 ||
-	{
+	        type.compare("float") == 0)
-		addParameter(layout, key.c_str(), uStr2Int(value));
-	}
-	else if(type.compare("double") == 0)
-	{
-		addParameter(layout, key.c_str(), uStr2Double(value));
-	}
-	else if(type.compare("float") == 0)
 	{
 		addParameter(layout, key.c_str(), uStr2Double(value));
 	}
@@ -316,6 +311,10 @@ void ParametersToolBox::addParameter(
 	{
 		addParameter(layout, key.c_str(), uStr2Bool(value));
 	}
+	else
+	{
+	    UWARN("Not implemented type \"%s\" for parameter \"%s\". Parameter is not added to toolbox.", type.c_str(), key.c_str());
+	}
 }
 
 void ParametersToolBox::addParameter(QVBoxLayout * layout,

guilib/src/PreferencesDialog.cpp

@@ -380,6 +380,13 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 		_ui->comboBox_cameraStereo->setItemData(kSrcStereoZed - kSrcStereo, 0, Qt::UserRole - 1);
 		_ui->comboBox_odom_sensor->setItemData(2, 0, Qt::UserRole - 1);
 	}
+	else if(CameraStereoZed::sdkVersion() < 4)
+	{
+		_ui->comboBox_stereoZed_resolution->setItemData(2, 0, Qt::UserRole - 1);
+		_ui->comboBox_stereoZed_resolution->setItemData(4, 0, Qt::UserRole - 1);
+		_ui->comboBox_stereoZed_resolution->setItemData(6, 0, Qt::UserRole - 1);
+		_ui->comboBox_stereoZed_quality->setItemData(3, 0, Qt::UserRole - 1);
+	}
     if (!CameraStereoTara::available())
     {
         _ui->comboBox_cameraStereo->setItemData(kSrcStereoTara - kSrcStereo, 0, Qt::UserRole - 1);
@@ -800,6 +807,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	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->doubleSpinBox_depthai_laser_dot_brightness, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->doubleSpinBox_depthai_floodlight_brightness, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
 
 	connect(_ui->checkbox_rgbd_colorOnly, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->spinBox_source_imageDecimation, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
@@ -809,10 +818,10 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->pushButton_calibrate_simple, SIGNAL(clicked()), this, SLOT(calibrateSimple()));
 	connect(_ui->toolButton_openniOniPath, SIGNAL(clicked()), this, SLOT(selectSourceOniPath()));
 	connect(_ui->toolButton_openni2OniPath, SIGNAL(clicked()), this, SLOT(selectSourceOni2Path()));
-	connect(_ui->comboBox_k4a_rgb_resolution, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->comboBox_k4a_rgb_resolution, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
-	connect(_ui->comboBox_k4a_framerate, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->comboBox_k4a_framerate, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->comboBox_k4a_depth_resolution, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
-	connect(_ui->checkbox_k4a_irDepth, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->checkbox_k4a_irDepth, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->toolButton_k4a_mkv, SIGNAL(clicked()), this, SLOT(selectSourceMKVPath()));
 	connect(_ui->toolButton_source_distortionModel, SIGNAL(clicked()), this, SLOT(selectSourceDistortionModel()));
 	connect(_ui->toolButton_distortionModel, SIGNAL(clicked()), this, SLOT(visualizeDistortionModel()));
@@ -1139,6 +1148,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->checkbox_rgbd_createOccupancyGrid->setObjectName(Parameters::kRGBDCreateOccupancyGrid().c_str());
 	_ui->RGBDMarkerDetection->setObjectName(Parameters::kRGBDMarkerDetection().c_str());
 	_ui->spinBox_maxOdomCacheSize->setObjectName(Parameters::kRGBDMaxOdomCacheSize().c_str());
+	_ui->checkbox_localizationSmoothing->setObjectName(Parameters::kRGBDLocalizationSmoothing().c_str());
 
 	// Registration
 	_ui->reg_repeatOnce->setObjectName(Parameters::kRegRepeatOnce().c_str());
@@ -2006,9 +2016,9 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->lineEdit_rs2_jsonFile->clear();
 		_ui->lineEdit_openniOniPath->clear();
 		_ui->lineEdit_openni2OniPath->clear();
-		_ui->comboBox_k4a_rgb_resolution->setCurrentIndex(0);
+		_ui->comboBox_k4a_rgb_resolution->setCurrentIndex(0);
-		_ui->comboBox_k4a_framerate->setCurrentIndex(2);
+		_ui->comboBox_k4a_framerate->setCurrentIndex(2);
-		_ui->comboBox_k4a_depth_resolution->setCurrentIndex(2);
+		_ui->comboBox_k4a_depth_resolution->setCurrentIndex(2);
 		_ui->checkbox_k4a_irDepth->setChecked(false);
 		_ui->lineEdit_k4a_mkv->clear();
 		_ui->source_checkBox_useMKVStamps->setChecked(true);
@@ -2033,7 +2043,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->spinBox_stereo_right_device->setValue(-1);
 		_ui->spinBox_stereousbcam_streamWidth->setValue(0);
 		_ui->spinBox_stereousbcam_streamHeight->setValue(0);
-		_ui->comboBox_stereoZed_resolution->setCurrentIndex(2);
+		_ui->comboBox_stereoZed_resolution->setCurrentIndex(CameraStereoZed::sdkVersion()<4?3:6);
 		_ui->comboBox_stereoZed_quality->setCurrentIndex(1);
 		_ui->checkbox_stereoZed_selfCalibration->setChecked(true);
 		_ui->comboBox_stereoZed_sensingMode->setCurrentIndex(0);
@@ -2053,6 +2063,8 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->spinBox_depthai_confidence->setValue(200);
 		_ui->checkBox_depthai_imu_published->setChecked(true);
 		_ui->checkBox_depthai_imu_firmware_update->setChecked(false);
+		_ui->doubleSpinBox_depthai_laser_dot_brightness->setValue(0.0);
+		_ui->doubleSpinBox_depthai_floodlight_brightness->setValue(200.0);
 
 		_ui->checkBox_cameraImages_configForEachFrame->setChecked(false);
 		_ui->checkBox_cameraImages_timestamps->setChecked(false);
@@ -2457,9 +2469,9 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	settings.endGroup(); // K4W2
 
 	settings.beginGroup("K4A");
-	_ui->comboBox_k4a_rgb_resolution->setCurrentIndex(settings.value("rgb_resolution", _ui->comboBox_k4a_rgb_resolution->currentIndex()).toInt());
+	_ui->comboBox_k4a_rgb_resolution->setCurrentIndex(settings.value("rgb_resolution", _ui->comboBox_k4a_rgb_resolution->currentIndex()).toInt());
-	_ui->comboBox_k4a_framerate->setCurrentIndex(settings.value("framerate", _ui->comboBox_k4a_framerate->currentIndex()).toInt());
+	_ui->comboBox_k4a_framerate->setCurrentIndex(settings.value("framerate", _ui->comboBox_k4a_framerate->currentIndex()).toInt());
-	_ui->comboBox_k4a_depth_resolution->setCurrentIndex(settings.value("depth_resolution", _ui->comboBox_k4a_depth_resolution->currentIndex()).toInt());
+	_ui->comboBox_k4a_depth_resolution->setCurrentIndex(settings.value("depth_resolution", _ui->comboBox_k4a_depth_resolution->currentIndex()).toInt());
 	_ui->checkbox_k4a_irDepth->setChecked(settings.value("ir", _ui->checkbox_k4a_irDepth->isChecked()).toBool());
 	_ui->lineEdit_k4a_mkv->setText(settings.value("mkvPath", _ui->lineEdit_k4a_mkv->text()).toString());
 	_ui->source_checkBox_useMKVStamps->setChecked(settings.value("useMkvStamps", _ui->source_checkBox_useMKVStamps->isChecked()).toBool());
@@ -2537,6 +2549,8 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_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());
+	_ui->doubleSpinBox_depthai_laser_dot_brightness->setValue(settings.value("laser_dot_brightness", _ui->doubleSpinBox_depthai_laser_dot_brightness->value()).toDouble());
+	_ui->doubleSpinBox_depthai_floodlight_brightness->setValue(settings.value("floodlight_brightness", _ui->doubleSpinBox_depthai_floodlight_brightness->value()).toDouble());
 	settings.endGroup(); // DepthAI
 
 	settings.beginGroup("Images");
@@ -2984,9 +2998,9 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.endGroup(); // K4W2
 
 	settings.beginGroup("K4A");
-	settings.setValue("rgb_resolution", _ui->comboBox_k4a_rgb_resolution->currentIndex());
+	settings.setValue("rgb_resolution", _ui->comboBox_k4a_rgb_resolution->currentIndex());
-	settings.setValue("framerate", _ui->comboBox_k4a_framerate->currentIndex());
+	settings.setValue("framerate", _ui->comboBox_k4a_framerate->currentIndex());
-	settings.setValue("depth_resolution", _ui->comboBox_k4a_depth_resolution->currentIndex());
+	settings.setValue("depth_resolution", _ui->comboBox_k4a_depth_resolution->currentIndex());
 	settings.setValue("ir", _ui->checkbox_k4a_irDepth->isChecked());
 	settings.setValue("mkvPath", _ui->lineEdit_k4a_mkv->text());
 	settings.setValue("useMkvStamps", _ui->source_checkBox_useMKVStamps->isChecked());
@@ -3063,7 +3077,9 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	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.setValue("imu_firmware_update",   _ui->checkBox_depthai_imu_firmware_update->isChecked());
+	settings.setValue("laser_dot_brightness",  _ui->doubleSpinBox_depthai_laser_dot_brightness->value());
+	settings.setValue("floodlight_brightness", _ui->doubleSpinBox_depthai_floodlight_brightness->value());
 	settings.endGroup(); // DepthAI
 
 	settings.beginGroup("Images");
@@ -6039,9 +6055,9 @@ Camera * PreferencesDialog::createCamera(
 		}
 		
 		((CameraK4A*)camera)->setIRDepthFormat(_ui->checkbox_k4a_irDepth->isChecked());
-		((CameraK4A*)camera)->setPreferences(_ui->comboBox_k4a_rgb_resolution->currentIndex(),
+		((CameraK4A*)camera)->setPreferences(_ui->comboBox_k4a_rgb_resolution->currentIndex(),
-						     _ui->comboBox_k4a_framerate->currentIndex(),
+						     _ui->comboBox_k4a_framerate->currentIndex(),
-						     _ui->comboBox_k4a_depth_resolution->currentIndex());
+						     _ui->comboBox_k4a_depth_resolution->currentIndex());
 	}
 	else if (driver == kSrcRealSense)
 	{
@@ -6311,6 +6327,9 @@ Camera * PreferencesDialog::createCamera(
 		((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());
+		((CameraDepthAI*)camera)->publishInterIMU(_ui->checkbox_publishInterIMU->isChecked());
+		((CameraDepthAI*)camera)->setLaserDotBrightness(_ui->doubleSpinBox_depthai_laser_dot_brightness->value());
+		((CameraDepthAI*)camera)->setFloodLightBrightness(_ui->doubleSpinBox_depthai_floodlight_brightness->value());
 	}
 	else if(driver == kSrcUsbDevice)
 	{
@@ -6609,35 +6628,28 @@ void PreferencesDialog::testOdometry()
 		return;
 	}
 
+	ParametersMap parameters = this->getAllParameters();
 	IMUThread * imuThread = 0;
 	if((this->getSourceDriver() == kSrcStereoImages ||
 	   this->getSourceDriver() == kSrcRGBDImages ||
 	   this->getSourceDriver() == kSrcImages) &&
 	   !_ui->lineEdit_cameraImages_path_imu->text().isEmpty())
 	{
-		if(this->getOdomStrategy() != Odometry::kTypeOkvis &&
+		imuThread = new IMUThread(_ui->spinBox_cameraImages_max_imu_rate->value(), this->getIMULocalTransform());
-		   this->getOdomStrategy() != Odometry::kTypeMSCKF &&
+		if(getIMUFilteringStrategy()>0)
-		   this->getOdomStrategy() != Odometry::kTypeVINS &&
+		{
-		   this->getOdomStrategy() != Odometry::kTypeOpenVINS)
+			imuThread->enableIMUFiltering(getIMUFilteringStrategy()-1, parameters, getIMUFilteringBaseFrameConversion());
+		}
+		if(!imuThread->init(_ui->lineEdit_cameraImages_path_imu->text().toStdString()))
 		{
 			QMessageBox::warning(this, tr("Source IMU Path"),
-					tr("IMU path is set but odometry chosen doesn't support asynchronous IMU, ignoring IMU..."), QMessageBox::Ok);
+				tr("Initialization of IMU data has failed! Path=%1.").arg(_ui->lineEdit_cameraImages_path_imu->text()), QMessageBox::Ok);
-		}
+			delete camera;
-		else
+			delete imuThread;
-		{
+			return;
-			imuThread = new IMUThread(_ui->spinBox_cameraImages_max_imu_rate->value(), this->getIMULocalTransform());
-			if(!imuThread->init(_ui->lineEdit_cameraImages_path_imu->text().toStdString()))
-			{
-				QMessageBox::warning(this, tr("Source IMU Path"),
-					tr("Initialization of IMU data has failed! Path=%1.").arg(_ui->lineEdit_cameraImages_path_imu->text()), QMessageBox::Ok);
-				delete camera;
-				delete imuThread;
-				return;
-			}
 		}
 	}
 
-	ParametersMap parameters = this->getAllParameters();
 	if(getOdomRegistrationApproach() < 3)
 	{
 		uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), uNumber2Str(getOdomRegistrationApproach())));
@@ -7059,7 +7071,7 @@ void PreferencesDialog::calibrateOdomSensorExtrinsics()
 		return;
 	}
 
-
+
 	// 3 steps calibration: RGB -> IR -> Extrinsic
 	QMessageBox::StandardButton button = QMessageBox::question(this, tr("Calibration"),
 			tr("We will calibrate the extrinsics. Important: Make sure "

guilib/src/ui/preferencesDialog.ui

@@ -63,7 +63,7 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>0</y>
+            <y>-611</y>
             <width>756</width>
             <height>3657</height>
            </rect>
@@ -95,7 +95,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>22</number>
+                <number>12</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,0">
@@ -3188,7 +3188,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                     <item>
                      <widget class="QStackedWidget" name="stackedWidget_src">
                       <property name="currentIndex">
-                       <number>3</number>
+                       <number>1</number>
                       </property>
                       <widget class="QWidget" name="page_41">
                        <layout class="QVBoxLayout" name="verticalLayout_64">
@@ -4971,7 +4971,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            <item>
                             <widget class="QStackedWidget" name="stackedWidget_stereo">
                              <property name="currentIndex">
-                              <number>10</number>
+                              <number>4</number>
                              </property>
                              <widget class="QWidget" name="page_49">
                               <layout class="QVBoxLayout" name="verticalLayout_91"/>
@@ -5240,6 +5240,11 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                       <string>ULTRA</string>
                                      </property>
                                     </item>
+                                    <item>
+                                     <property name="text">
+                                      <string>NEURAL</string>
+                                     </property>
+                                    </item>
                                    </widget>
                                   </item>
                                   <item row="5" column="1">
@@ -5398,16 +5403,31 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                       <string>HD1080</string>
                                      </property>
                                     </item>
+                                    <item>
+                                     <property name="text">
+                                      <string>HD1200</string>
+                                     </property>
+                                    </item>
                                     <item>
                                      <property name="text">
                                       <string>HD720</string>
                                      </property>
                                     </item>
+                                    <item>
+                                     <property name="text">
+                                      <string>SVGA</string>
+                                     </property>
+                                    </item>
                                     <item>
                                      <property name="text">
                                       <string>VGA</string>
                                      </property>
                                     </item>
+                                    <item>
+                                     <property name="text">
+                                      <string>AUTO</string>
+                                     </property>
+                                    </item>
                                    </widget>
                                   </item>
                                  </layout>
@@ -5923,6 +5943,58 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
+                                  <item row="5" column="0">
+                                   <widget class="QDoubleSpinBox" name="doubleSpinBox_depthai_laser_dot_brightness">
+                                    <property name="suffix">
+                                     <string> mA</string>
+                                    </property>
+                                    <property name="maximum">
+                                     <double>1200.000000000000000</double>
+                                    </property>
+                                    <property name="value">
+                                     <double>0.000000000000000</double>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="5" column="1">
+                                   <widget class="QLabel" name="label_676">
+                                    <property name="text">
+                                     <string>Laser dot brightness.</string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
+                                    </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="6" column="0">
+                                   <widget class="QDoubleSpinBox" name="doubleSpinBox_depthai_floodlight_brightness">
+                                    <property name="suffix">
+                                     <string> mA</string>
+                                    </property>
+                                    <property name="maximum">
+                                     <double>1500.000000000000000</double>
+                                    </property>
+                                    <property name="value">
+                                     <double>200.000000000000000</double>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="6" column="1">
+                                   <widget class="QLabel" name="label_677">
+                                    <property name="text">
+                                     <string>Floodlight brightness.</string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
+                                    </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                    </property>
+                                   </widget>
+                                  </item>
                                  </layout>
                                 </widget>
                                </item>
@@ -11081,33 +11153,49 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        <string>Map Update</string>
                       </property>
                       <layout class="QGridLayout" name="gridLayout_47" columnstretch="0,1">
-                       <item row="8" column="1">
+                       <item row="5" column="0">
-                        <widget class="QLabel" name="label_scanMatching_14">
+                        <widget class="QDoubleSpinBox" name="rgdb_newMapOdomChange">
-                         <property name="text">
-                          <string>Use Identity matrix as guess when computing loop closure transform, otherwise no guess is used (assuming that registration strategy can deal with transformation estimation without guess).</string>
-                         </property>
-                         <property name="wordWrap">
-                          <bool>true</bool>
-                         </property>
-                         <property name="textInteractionFlags">
-                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="17" column="0">
-                        <widget class="QDoubleSpinBox" name="maxLocalizationDistance">
                          <property name="suffix">
                           <string> m</string>
                          </property>
+                         <property name="decimals">
+                          <number>1</number>
+                         </property>
+                         <property name="maximum">
+                          <double>99.000000000000000</double>
+                         </property>
+                         <property name="singleStep">
+                          <double>0.100000000000000</double>
+                         </property>
                          <property name="value">
                           <double>1.000000000000000</double>
                          </property>
                         </widget>
                        </item>
-                       <item row="1" column="1">
+                       <item row="2" column="0">
-                        <widget class="QLabel" name="label_152">
+                        <widget class="QDoubleSpinBox" name="rgdb_linearSpeedUpdate">
+                         <property name="suffix">
+                          <string> m/s</string>
+                         </property>
+                         <property name="decimals">
+                          <number>3</number>
+                         </property>
+                         <property name="singleStep">
+                          <double>0.100000000000000</double>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="8" column="0">
+                        <widget class="QCheckBox" name="loopClosure_identityGuess">
                          <property name="text">
-                          <string>Angular update: Minimum angular displacement to update the map. Note that Weight Update is done prior to this, so weights are still updated.</string>
+                          <string/>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="7" column="1">
+                        <widget class="QLabel" name="label_scanMatching_7">
+                         <property name="text">
+                          <string>Use odometry instead of IMU orientation to add gravity links to new nodes created. We assume that odometry is already aligned with gravity (e.g., we are using a VIO approach). Gravity constraints are used by graph optimization only if &quot;Optimizer/GravitySigma&quot; is not zero.</string>
                          </property>
                          <property name="wordWrap">
                           <bool>true</bool>
@@ -11117,6 +11205,58 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
+                       <item row="0" column="1">
+                        <widget class="QLabel" name="label_153">
+                         <property name="text">
+                          <string>Linear update: Minimum linear displacement to update the map. Note that Weight Update is done prior to this, so weights are still updated.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="5" column="1">
+                        <widget class="QLabel" name="label_163">
+                         <property name="text">
+                          <string>Odometry change detected that triggers a new map (0 means whatever the odometry change, the detector will still link the new pose in the current map). Also by default, when an odometry with Identity transformation is detected, a new map is automatically created. </string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="3" column="0">
+                        <widget class="QDoubleSpinBox" name="rgdb_angularSpeedUpdate">
+                         <property name="suffix">
+                          <string> rad/s</string>
+                         </property>
+                         <property name="decimals">
+                          <number>2</number>
+                         </property>
+                         <property name="maximum">
+                          <double>3.140000000000000</double>
+                         </property>
+                         <property name="singleStep">
+                          <double>0.100000000000000</double>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="6" column="0">
+                        <widget class="QCheckBox" name="odomScanHistory">
+                         <property name="text">
+                          <string/>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="14" column="0">
+                        <widget class="QSpinBox" name="spinBox_maxLocalLocationsRetrieved"/>
+                       </item>
                        <item row="1" column="0">
                         <widget class="QDoubleSpinBox" name="rgdb_angularUpdate">
                          <property name="suffix">
@@ -11133,10 +11273,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="12" column="0">
+                       <item row="11" column="1">
-                        <widget class="QCheckBox" name="memCovOffDiagIgnored">
+                        <widget class="QLabel" name="label_scanMatching_16">
                          <property name="text">
-                          <string/>
+                          <string>Inverted registration. On loop closure, do registration from the target to reference instead of reference to target.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
                          </property>
                         </widget>
                        </item>
@@ -11172,104 +11318,26 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="5" column="0">
+                       <item row="18" column="1">
-                        <widget class="QDoubleSpinBox" name="rgdb_newMapOdomChange">
+                        <widget class="QLabel" name="label_space2_5">
-                         <property name="suffix">
+                         <property name="text">
-                          <string> m</string>
+                          <string>Maximum odometry cache size. Used only in localization mode. This is used to get smoother localizations and to verify localization transforms (when maximum graph error is not null) to make sure we don't teleport to a location very similar to one we previously localized on. Set 0 to disable caching.</string>
                          </property>
-                         <property name="decimals">
+                         <property name="wordWrap">
-                          <number>1</number>
+                          <bool>true</bool>
                          </property>
-                         <property name="maximum">
+                         <property name="textInteractionFlags">
-                          <double>99.000000000000000</double>
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                         </property>
-                         <property name="singleStep">
-                          <double>0.100000000000000</double>
-                         </property>
-                         <property name="value">
-                          <double>1.000000000000000</double>
                          </property>
                         </widget>
                        </item>
-                       <item row="10" column="0">
+                       <item row="7" column="0">
-                        <widget class="QCheckBox" name="loopClosure_bunlde">
+                        <widget class="QCheckBox" name="odomGravity">
                          <property name="text">
                           <string/>
                          </property>
                         </widget>
                        </item>
-                       <item row="0" column="1">
-                        <widget class="QLabel" name="label_153">
-                         <property name="text">
-                          <string>Linear update: Minimum linear displacement to update the map. Note that Weight Update is done prior to this, so weights are still updated.</string>
-                         </property>
-                         <property name="wordWrap">
-                          <bool>true</bool>
-                         </property>
-                         <property name="textInteractionFlags">
-                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="2" column="1">
-                        <widget class="QLabel" name="label_432">
-                         <property name="text">
-                          <string>Maximum linear speed to update the map (0 means not limit).</string>
-                         </property>
-                         <property name="wordWrap">
-                          <bool>true</bool>
-                         </property>
-                         <property name="textInteractionFlags">
-                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="4" column="0">
-                        <widget class="QCheckBox" name="rgbd_savedLocalizationIgnored">
-                         <property name="text">
-                          <string/>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="2" column="0">
-                        <widget class="QDoubleSpinBox" name="rgdb_linearSpeedUpdate">
-                         <property name="suffix">
-                          <string> m/s</string>
-                         </property>
-                         <property name="decimals">
-                          <number>3</number>
-                         </property>
-                         <property name="singleStep">
-                          <double>0.100000000000000</double>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="0" column="0">
-                        <widget class="QDoubleSpinBox" name="rgdb_linearUpdate">
-                         <property name="suffix">
-                          <string> m</string>
-                         </property>
-                         <property name="decimals">
-                          <number>3</number>
-                         </property>
-                         <property name="singleStep">
-                          <double>0.100000000000000</double>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="5" column="1">
-                        <widget class="QLabel" name="label_163">
-                         <property name="text">
-                          <string>Odometry change detected that triggers a new map (0 means whatever the odometry change, the detector will still link the new pose in the current map). Also by default, when an odometry with Identity transformation is detected, a new map is automatically created. </string>
-                         </property>
-                         <property name="wordWrap">
-                          <bool>true</bool>
-                         </property>
-                         <property name="textInteractionFlags">
-                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                         </property>
-                        </widget>
-                       </item>
                        <item row="18" column="0">
                         <widget class="QSpinBox" name="spinBox_maxOdomCacheSize">
                          <property name="maximum">
@@ -11287,10 +11355,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="12" column="1">
+                       <item row="14" column="1">
-                        <widget class="QLabel" name="label_scanMatching_11">
+                        <widget class="QLabel" name="label_scanMatching_3">
                          <property name="text">
-                          <string>Ignore off diagonal values of the odometry covariance matrix.</string>
+                          <string>Maximum local locations retrieved (0=disabled) near the current pose in the local map or on the current planned path (those on the planned path have priority).</string>
                          </property>
                          <property name="wordWrap">
                           <bool>true</bool>
@@ -11300,6 +11368,33 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
+                       <item row="8" column="1">
+                        <widget class="QLabel" name="label_scanMatching_14">
+                         <property name="text">
+                          <string>Use Identity matrix as guess when computing loop closure transform, otherwise no guess is used (assuming that registration strategy can deal with transformation estimation without guess).</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="10" column="0">
+                        <widget class="QCheckBox" name="loopClosure_bunlde">
+                         <property name="text">
+                          <string/>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="11" column="0">
+                        <widget class="QCheckBox" name="loopClosure_invertedReg">
+                         <property name="text">
+                          <string/>
+                         </property>
+                        </widget>
+                       </item>
                        <item row="13" column="1">
                         <widget class="QLabel" name="label_scanMatching_12">
                          <property name="text">
@@ -11313,59 +11408,23 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="7" column="1">
+                       <item row="0" column="0">
-                        <widget class="QLabel" name="label_scanMatching_7">
+                        <widget class="QDoubleSpinBox" name="rgdb_linearUpdate">
-                         <property name="text">
-                          <string>Use odometry instead of IMU orientation to add gravity links to new nodes created. We assume that odometry is already aligned with gravity (e.g., we are using a VIO approach). Gravity constraints are used by graph optimization only if &quot;Optimizer/GravitySigma&quot; is not zero.</string>
-                         </property>
-                         <property name="wordWrap">
-                          <bool>true</bool>
-                         </property>
-                         <property name="textInteractionFlags">
-                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="9" column="1">
-                        <widget class="QLabel" name="label_scanMatching_9">
-                         <property name="text">
-                          <string>Re-extract visual features when computing loop closure transformations. Raw features are not saved in database.</string>
-                         </property>
-                         <property name="wordWrap">
-                          <bool>true</bool>
-                         </property>
-                         <property name="textInteractionFlags">
-                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="3" column="0">
-                        <widget class="QDoubleSpinBox" name="rgdb_angularSpeedUpdate">
                          <property name="suffix">
-                          <string> rad/s</string>
+                          <string> m</string>
                          </property>
                          <property name="decimals">
-                          <number>2</number>
+                          <number>3</number>
-                         </property>
-                         <property name="maximum">
-                          <double>3.140000000000000</double>
                          </property>
                          <property name="singleStep">
                           <double>0.100000000000000</double>
                          </property>
                         </widget>
                        </item>
-                       <item row="13" column="0">
+                       <item row="2" column="1">
-                        <widget class="QCheckBox" name="rgbd_loopCovLimited">
+                        <widget class="QLabel" name="label_432">
                          <property name="text">
-                          <string/>
+                          <string>Maximum linear speed to update the map (0 means not limit).</string>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="3" column="1">
-                        <widget class="QLabel" name="label_433">
-                         <property name="text">
-                          <string>Maximum angular speed to update the map (0 means not limit).</string>
                          </property>
                          <property name="wordWrap">
                           <bool>true</bool>
@@ -11375,13 +11434,24 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="14" column="0">
+                       <item row="9" column="0">
-                        <widget class="QSpinBox" name="spinBox_maxLocalLocationsRetrieved"/>
+                        <widget class="QCheckBox" name="loopClosure_reextract">
-                       </item>
-                       <item row="10" column="1">
-                        <widget class="QLabel" name="label_scanMatching_10">
                          <property name="text">
-                          <string>Do local bundle adjustment with neighborhood of the loop closure.</string>
+                          <string/>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="12" column="0">
+                        <widget class="QCheckBox" name="memCovOffDiagIgnored">
+                         <property name="text">
+                          <string/>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="6" column="1">
+                        <widget class="QLabel" name="label_scanMatching">
+                         <property name="text">
+                          <string>Neighbor link refining. When a new node is added to the graph, the transformation of its neighbor link (odometry) with the previous node is refined using ICP registration approach (laser scans required).</string>
                          </property>
                          <property name="wordWrap">
                           <bool>true</bool>
@@ -11404,20 +11474,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="6" column="0">
-                        <widget class="QCheckBox" name="odomScanHistory">
-                         <property name="text">
-                          <string/>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="7" column="0">
-                        <widget class="QCheckBox" name="odomGravity">
-                         <property name="text">
-                          <string/>
-                         </property>
-                        </widget>
-                       </item>
                        <item row="17" column="1">
                         <widget class="QLabel" name="label_space2_12">
                          <property name="text">
@@ -11431,39 +11487,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="14" column="1">
-                        <widget class="QLabel" name="label_scanMatching_3">
-                         <property name="text">
-                          <string>Maximum local locations retrieved (0=disabled) near the current pose in the local map or on the current planned path (those on the planned path have priority).</string>
-                         </property>
-                         <property name="wordWrap">
-                          <bool>true</bool>
-                         </property>
-                         <property name="textInteractionFlags">
-                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="9" column="0">
-                        <widget class="QCheckBox" name="loopClosure_reextract">
-                         <property name="text">
-                          <string/>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="18" column="1">
-                        <widget class="QLabel" name="label_space2_5">
-                         <property name="text">
-                          <string>Maximum odometry cache size. Used only in localization mode. This is used to get smoother localizations and to verify localization transforms (when maximum graph error is not null) to make sure we don't teleport to a location very similar to one we previously localized on. Set 0 to disable caching.</string>
-                         </property>
-                         <property name="wordWrap">
-                          <bool>true</bool>
-                         </property>
-                         <property name="textInteractionFlags">
-                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                         </property>
-                        </widget>
-                       </item>
                        <item row="15" column="1">
                         <widget class="QLabel" name="label_scanMatching_5">
                          <property name="text">
@@ -11477,17 +11500,40 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="8" column="0">
+                       <item row="1" column="1">
-                        <widget class="QCheckBox" name="loopClosure_identityGuess">
+                        <widget class="QLabel" name="label_152">
+                         <property name="text">
+                          <string>Angular update: Minimum angular displacement to update the map. Note that Weight Update is done prior to this, so weights are still updated.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="4" column="0">
+                        <widget class="QCheckBox" name="rgbd_savedLocalizationIgnored">
                          <property name="text">
                           <string/>
                          </property>
                         </widget>
                        </item>
-                       <item row="6" column="1">
+                       <item row="17" column="0">
-                        <widget class="QLabel" name="label_scanMatching">
+                        <widget class="QDoubleSpinBox" name="maxLocalizationDistance">
+                         <property name="suffix">
+                          <string> m</string>
+                         </property>
+                         <property name="value">
+                          <double>1.000000000000000</double>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="12" column="1">
+                        <widget class="QLabel" name="label_scanMatching_11">
                          <property name="text">
-                          <string>Neighbor link refining. When a new node is added to the graph, the transformation of its neighbor link (odometry) with the previous node is refined using ICP registration approach (laser scans required).</string>
+                          <string>Ignore off diagonal values of the odometry covariance matrix.</string>
                          </property>
                          <property name="wordWrap">
                           <bool>true</bool>
@@ -11497,10 +11543,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="11" column="1">
+                       <item row="10" column="1">
-                        <widget class="QLabel" name="label_scanMatching_16">
+                        <widget class="QLabel" name="label_scanMatching_10">
                          <property name="text">
-                          <string>Inverted registration. On loop closure, do registration from the target to reference instead of reference to target.</string>
+                          <string>Do local bundle adjustment with neighborhood of the loop closure.</string>
                          </property>
                          <property name="wordWrap">
                           <bool>true</bool>
@@ -11510,8 +11556,54 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="11" column="0">
+                       <item row="13" column="0">
-                        <widget class="QCheckBox" name="loopClosure_invertedReg">
+                        <widget class="QCheckBox" name="rgbd_loopCovLimited">
+                         <property name="text">
+                          <string/>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="9" column="1">
+                        <widget class="QLabel" name="label_scanMatching_9">
+                         <property name="text">
+                          <string>Re-extract visual features when computing loop closure transformations. Raw features are not saved in database.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="3" column="1">
+                        <widget class="QLabel" name="label_433">
+                         <property name="text">
+                          <string>Maximum angular speed to update the map (0 means not limit).</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="19" column="1">
+                        <widget class="QLabel" name="label_space2_18">
+                         <property name="text">
+                          <string>Localization smoothing. Used only in localization mode. Adjust localization constraints based on optimized odometry cache poses.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="19" column="0">
+                        <widget class="QCheckBox" name="checkbox_localizationSmoothing">
                          <property name="text">
                           <string/>
                          </property>