bump patch

We are shortly going to be removing octomap the package
from ROS 2 Rolling; that's because it's ABI conflicts with
the system package.  Instead, switch rtabmap to use the system
package, which should work fine.

Signed-off-by: Chris Lalancette <clalancette@gmail.com>

.devcontainer/android/devcontainer.json

@@ -0,0 +1,8 @@
+{
+    "image": "introlab3it/rtabmap:android-deps",
+    "customizations": {
+        "vscode": {
+          "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools", "vscjava.vscode-java-pack"]
+        }
+      }
+}

.devcontainer/bionic/devcontainer.json

@@ -0,0 +1,8 @@
+{
+    "image": "introlab3it/rtabmap:18.04",
+    "customizations": {
+        "vscode": {
+          "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools"]
+        }
+      }
+}

.devcontainer/focal/devcontainer.json

@@ -2,7 +2,7 @@
     "image": "introlab3it/rtabmap:20.04",
     "customizations": {
         "vscode": {
-          "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools", "vscjava.vscode-java-pack"]
+          "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools"]
         }
       }
 }

.devcontainer/jammy/devcontainer.json

@@ -0,0 +1,8 @@
+{
+    "image": "introlab3it/rtabmap:22.04",
+    "customizations": {
+        "vscode": {
+          "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools"]
+        }
+      }
+}

.devcontainer/noble/devcontainer.json

@@ -0,0 +1,8 @@
+{
+    "image": "introlab3it/rtabmap:24.04",
+    "customizations": {
+        "vscode": {
+          "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools"]
+        }
+      }
+}

.github/workflows/cmake-ros.yml

@@ -31,6 +31,9 @@ jobs:
           os: ubuntu-22.04
         - ros_distribution: 'iron'
           os: ubuntu-22.04
+        # Currently CI has some errors on setup-ros with this OS, disabling for now
+        #- ros_distribution: 'jazzy'
+        #  os: ubuntu-24.04
 
     steps:   
     - uses: ros-tooling/setup-ros@v0.6

.github/workflows/cmake.yml

@@ -18,7 +18,7 @@ jobs:
     strategy:
       fail-fast: false
       matrix:
-        os: [ubuntu-22.04, ubuntu-20.04]
+        os: [ubuntu-24.04, ubuntu-22.04, ubuntu-20.04]
 
     steps:    
     - name: Install dependencies

.github/workflows/docker.yml

@@ -12,7 +12,7 @@ jobs:
     strategy:
       fail-fast: false
       matrix:
-        docker_tag: [focal-deps, jammy-deps, jammy-iron-deps]
+        docker_tag: [focal-deps, jammy-deps, jammy-iron-deps, noble-deps]
         include:
         - docker_tag: focal-deps
           docker_tags: |
@@ -35,6 +35,13 @@ jobs:
           docker_platforms: |
             linux/amd64
           docker_path: 'jammy-iron/deps'
+        - docker_tag: noble-deps
+          docker_tags: |
+            introlab3it/rtabmap:noble-deps
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'noble/deps'
     
     steps:
       -
@@ -73,7 +80,7 @@ jobs:
     strategy:
       fail-fast: false
       matrix:
-        docker_tag: [bionic, focal, jammy, jammy-iron, android23, android24, android26, android30]
+        docker_tag: [bionic, focal, jammy, jammy-iron, noble, android23, android24, android26, android30]
         include:
         - docker_tag: bionic
           docker_tags: |
@@ -115,6 +122,16 @@ jobs:
           docker_platforms: |
             linux/amd64
           docker_path: 'jammy-iron'
+        - docker_tag: noble
+          docker_tags: |
+            introlab3it/rtabmap:noble
+            introlab3it/rtabmap:24.04
+          docker_args: |
+            NOT_USED=0
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'noble'
         - docker_tag: android23
           docker_tags: |
             introlab3it/rtabmap:android23

CMakeLists.txt

@@ -20,7 +20,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 21)
-SET(RTABMAP_PATCH_VERSION 5)
+SET(RTABMAP_PATCH_VERSION 6)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   
@@ -181,6 +181,7 @@ option(WITH_FREENECT      "Include Freenect support"             ON)
 option(WITH_FREENECT2     "Include Freenect2 support"            ON)
 option(WITH_K4W2          "Include Kinect for Windows v2 support" ON)
 option(WITH_K4A           "Include Kinect for Azure support"     ON)
+option(WITH_OPENNI        "Include OpenNI support"               ON)
 option(WITH_OPENNI2       "Include OpenNI2 support"              ON)
 option(WITH_DC1394        "Include dc1394 support"               ON)
 option(WITH_G2O           "Include g2o support"                  ON)
@@ -203,6 +204,7 @@ option(WITH_REALSENSE_SLAM "Include RealSenseSlam support"       ON)
 option(WITH_REALSENSE2    "Include RealSense support"            ON)
 option(WITH_MYNTEYE       "Include mynteye-s support"            ON)
 option(WITH_DEPTHAI       "Include depthai-core support"         OFF)
+option(WITH_XVSDK         "Include XVisio SDK support"           OFF)
 option(WITH_OCTOMAP       "Include OctoMap support"              ON)
 option(WITH_GRIDMAP       "Include GridMap support"              ON)
 option(WITH_CPUTSDF       "Include CPUTSDF support"              OFF)
@@ -662,6 +664,13 @@ IF(WITH_DEPTHAI)
     ENDIF(depthai_FOUND)
 ENDIF(WITH_DEPTHAI)
 
+IF(WITH_XVSDK)
+    FIND_PACKAGE(xvsdk QUIET)
+    IF(xvsdk_FOUND)
+       MESSAGE(STATUS "Found xvsdk (targets)")
+    ENDIF(xvsdk_FOUND)
+ENDIF(WITH_XVSDK)
+
 IF(WITH_OCTOMAP)
     FIND_PACKAGE(octomap QUIET)
     IF(octomap_FOUND)
@@ -956,6 +965,9 @@ ELSE()
       )
    ENDIF(WIN32)
 ENDIF()
+IF(NOT (OpenNI_FOUND AND WITH_OPENNI))
+   SET(OPENNI "//")
+ENDIF()
 IF(NOT OpenNI2_FOUND)
    SET(OPENNI2 "//")
 ENDIF()
@@ -1000,6 +1012,12 @@ IF(NOT depthai_FOUND)
 ELSE()
    SET(CONF_WITH_DEPTH_AI 1)
 ENDIF()
+IF(NOT xvsdk_FOUND)
+   SET(XVSDK "//")
+   SET(CONF_WITH_XVSDK 0)
+ELSE()
+   SET(CONF_WITH_XVSDK 1)
+ENDIF()
 IF(NOT octomap_FOUND)
    SET(OCTOMAP "//")
    SET(CONF_WITH_OCTOMAP 0)
@@ -1075,13 +1093,15 @@ IF(ANDROID)
    IF(BUILD_APP)
       ADD_SUBDIRECTORY( app )
    ENDIF(BUILD_APP)
-ELSEIF(Qt6_FOUND OR Qt5_FOUND OR (QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND))
-   ADD_SUBDIRECTORY( guilib )
-   IF(BUILD_APP)
-      ADD_SUBDIRECTORY( app )
-   ENDIF(BUILD_APP)
 ELSEIF(WITH_QT)
-   MESSAGE(WARNING "Qt not found, the GUI lib and the stand-alone application will not be compiled...")
+   IF(Qt6_FOUND OR Qt5_FOUND OR (QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND))
+      ADD_SUBDIRECTORY( guilib )
+      IF(BUILD_APP)
+         ADD_SUBDIRECTORY( app )
+      ENDIF(BUILD_APP)
+   ELSE()
+      MESSAGE(WARNING "Qt not found, the GUI lib and the stand-alone application will not be compiled...")
+   ENDIF()
 ENDIF()
 
 IF(BUILD_TOOLS)
@@ -1109,7 +1129,7 @@ ENDIF()
 ####
 add_library(rtabmap INTERFACE)
 add_library(rtabmap::rtabmap ALIAS rtabmap)
-IF(QT4_FOUND OR Qt5_FOUND OR Qt6_FOUND)
+IF(WITH_QT AND (QT4_FOUND OR Qt5_FOUND OR Qt6_FOUND))
    set(CONF_WITH_GUI ON)
    IF(QT4_FOUND)
       set(CONF_QT_VERSION 4)
@@ -1321,13 +1341,13 @@ IF(OpenCV_FOUND)
   ENDIF()
 ENDIF(OpenCV_FOUND)
 
-IF(QT4_FOUND)
+IF(WITH_QT AND QT4_FOUND)
 MESSAGE(STATUS "  With Qt4                  = YES (License: Open Source or Commercial)")
 MESSAGE(STATUS "  With VTK ${VTK_MAJOR_VERSION}.${VTK_MINOR_VERSION}              = YES (License: BSD)")
-ELSEIF(Qt5_FOUND)
+ELSEIF(WITH_QT AND Qt5_FOUND)
 MESSAGE(STATUS "  With Qt ${Qt5_VERSION}            = YES (License: Open Source or Commercial)")
 MESSAGE(STATUS "  With VTK ${VTK_MAJOR_VERSION}.${VTK_MINOR_VERSION}              = YES (License: BSD)")
-ELSEIF(Qt6_FOUND)
+ELSEIF(WITH_QT AND Qt6_FOUND)
 MESSAGE(STATUS "  With Qt ${Qt6_VERSION}            = YES (License: Open Source or Commercial)")
 MESSAGE(STATUS "  With VTK ${VTK_MAJOR_VERSION}.${VTK_MINOR_VERSION}              = YES (License: BSD)")
 
@@ -1531,6 +1551,14 @@ ELSE()
 MESSAGE(STATUS "  With Freenect             = NO (libfreenect not found)")
 ENDIF()
 
+IF(WITH_OPENNI AND OpenNI_FOUND)
+MESSAGE(STATUS "  With OpenNI               = YES (License: Apache v2)")
+ELSEIF(NOT WITH_OPENNI)
+MESSAGE(STATUS "  With OpenNI               = NO (WITH_OPENNI=OFF)")
+ELSE()
+MESSAGE(STATUS "  With OpenNI               = NO (OpenNI not found)")
+ENDIF()
+
 IF(OpenNI2_FOUND)
 MESSAGE(STATUS "  With OpenNI2              = YES (License: Apache v2)")
 ELSEIF(NOT WITH_OPENNI2)
@@ -1638,6 +1666,14 @@ ELSE()
 MESSAGE(STATUS "  With DepthAI              = NO (depthai-core not found)")
 ENDIF()
 
+IF(xvsdk_FOUND)
+MESSAGE(STATUS "  With XVisio SDK ${xvsdk_VERSION}     = YES")
+ELSEIF(NOT WITH_XVSDK)
+MESSAGE(STATUS "  With XVisio SDK           = NO (WITH_XVSDK=OFF)")
+ELSE()
+MESSAGE(STATUS "  With XVisio SDK           = NO (xvsdk not found)")
+ENDIF()
+
 MESSAGE(STATUS "")
 MESSAGE(STATUS " Odometry Approaches:")
 IF(loam_velodyne_FOUND)

RTABMapConfig.cmake.in

@@ -43,7 +43,11 @@ IF(@CONF_WITH_K4A@)
 ENDIF()
 
 IF(@CONF_WITH_DEPTH_AI@)
- find_dependency(depthai 2)
+  find_dependency(depthai 2.24)
+ENDIF()
+
+IF(@CONF_WITH_XVSDK@)
+  find_dependency(xvsdk)
 ENDIF()
 
 IF(@CONF_WITH_OCTOMAP@)
@@ -93,8 +97,7 @@ endforeach()
 include("${CMAKE_CURRENT_LIST_DIR}/RTABMapTargets.cmake")
 
 foreach(_comp ${RTABMap_FIND_COMPONENTS})
-  if (NOT ";${_RTABMap_supported_components};" MATCHES ";${_comp};")
+  if (NOT RTABMap_${_comp}_FOUND)
-    set(RTABMap_${_comp}_FOUND False)
     if(${RTABMap_FIND_REQUIRED_${_comp}})
       set(RTABMap_FOUND False)
       set(RTABMap_NOT_FOUND_MESSAGE "Unsupported or not found required component: ${_comp}")

Version.h.in

@@ -45,6 +45,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 @CERES@#define RTABMAP_CERES
 @MRPT@#define RTABMAP_MRPT
 @VERTIGO@#define RTABMAP_VERTIGO
+@OPENNI@#define RTABMAP_OPENNI
 @OPENNI2@#define RTABMAP_OPENNI2
 @FREENECT@#define RTABMAP_FREENECT
 @FREENECT2@#define RTABMAP_FREENECT2
@@ -68,6 +69,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 @REALSENSE2@#define RTABMAP_REALSENSE2
 @MYNTEYE@#define RTABMAP_MYNTEYE
 @DEPTHAI@#define RTABMAP_DEPTHAI
+@XVSDK@#define RTABMAP_XVSDK
 @OCTOMAP@#define RTABMAP_OCTOMAP
 @GRIDMAP@#define RTABMAP_GRIDMAP
 @CPUTSDF@#define RTABMAP_CPUTSDF

app/android/jni/RTABMapApp.cpp

@@ -135,6 +135,7 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapPublishLikelihood(), std::string("false")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapPublishPdf(), std::string("false")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapStartNewMapOnLoopClosure(), uBool2Str(!localizationMode_ && appendMode_)));
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDAggressiveLoopThr(), "0.0"));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemBinDataKept(), uBool2Str(!trajectoryMode_)));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), "10"));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), uBool2Str(!localizationMode_)));

corelib/include/rtabmap/core/CameraRGBD.h

@@ -37,4 +37,4 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/camera/CameraRealSense2.h>
 #include <rtabmap/core/camera/CameraRGBDImages.h>
 #include <rtabmap/core/camera/CameraK4A.h>
-
+#include <rtabmap/core/camera/CameraSeerSense.h>

corelib/include/rtabmap/core/DBReader.h

@@ -82,6 +82,7 @@ public:
 	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
 	virtual bool odomProvided() const {return !_odometryIgnored;}
+	virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.06);
 
 	const DBDriver * driver() const {return _dbDriver;}
 

corelib/include/rtabmap/core/Parameters.h

@@ -357,6 +357,7 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(RGBD, AngularUpdate,            float, 0.1,  "Minimum angular displacement (rad) to update the map. Rehearsal is done prior to this, so weights are still updated.");
     RTABMAP_PARAM(RGBD, LinearSpeedUpdate,        float, 0.0,  "Maximum linear speed (m/s) to update the map (0 means not limit).");
     RTABMAP_PARAM(RGBD, AngularSpeedUpdate,       float, 0.0,  "Maximum angular speed (rad/s) to update the map (0 means not limit).");
+    RTABMAP_PARAM(RGBD, AggressiveLoopThr,        float, 0.05, uFormat("Loop closure threshold used (overriding %s) when a new mapping session is not yet linked to a map of the highest loop closure hypothesis. In localization mode, this threshold is used when there are no loop closure constraints with any map in the cache (%s). In all cases, the goal is to aggressively loop on a previous map in the database. Only used when %s is enabled. Set 1 to disable.", kRtabmapLoopThr().c_str(), kRGBDMaxOdomCacheSize().c_str(), kRGBDEnabled().c_str()));
     RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0,    "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled).");
     RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd,     bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest node of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
     RTABMAP_PARAM(RGBD, OptimizeMaxError,         float, 3.0,   uFormat("Reject loop closures if optimization error ratio is greater than this value (0=disabled). Ratio is computed as absolute error over standard deviation of each link. This will help to detect when a wrong loop closure is added to the graph. Not compatible with \"%s\" if enabled.", kOptimizerRobust().c_str()));

corelib/include/rtabmap/core/Rtabmap.h

@@ -284,6 +284,7 @@ private:
 	unsigned int _maxMemoryAllowed; // signatures count in WM
 	float _loopThr;
 	float _loopRatio;
+	float _aggressiveLoopThr;
 	int _virtualPlaceLikelihoodRatio;
 	float _maxLoopClosureDistance;
 	bool _verifyLoopClosureHypothesis;

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

@@ -60,7 +60,7 @@ public:
 	void setExtendedDisparity(bool extendedDisparity);
 	void setSubpixelMode(bool enabled, int fractionalBits = 3);
 	void setCompanding(bool enabled, int width=96);
-	void setRectification(bool useSpecTranslation, float alphaScaling = 0.0f);
+	void setRectification(bool useSpecTranslation, float alphaScaling = 0.0f, bool enabled=true);
 	void setIMU(bool imuPublished, bool publishInterIMU);
 	void setIrIntensity(float dotIntensity = 0.0f, float floodIntensity = 0.0f);
 	void setDetectFeatures(int detectFeatures = 0);
@@ -90,6 +90,7 @@ private:
 	int compandingWidth_;
 	bool useSpecTranslation_;
 	float alphaScaling_;
+	bool imagesRectified_;
 	bool imuPublished_;
 	bool publishInterIMU_;
 	float dotIntensity_;

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

@@ -34,15 +34,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <pcl/pcl_config.h>
 
-#ifdef HAVE_OPENNI
+#ifdef RTABMAP_OPENNI
 #if __linux__ && __i386__ && __cplusplus >= 201103L
 #warning "Openni driver is not available on i386 when building with c++11 support"
-#else
+#endif
-#define RTABMAP_OPENNI
 #include <pcl/io/openni_camera/openni_depth_image.h>
 #include <pcl/io/openni_camera/openni_image.h>
 #endif
-#endif
 
 #include <boost/signals2/connection.hpp>
 
@@ -74,7 +72,7 @@ public:
 			float constant);
 #else
     void image_cb (
-    		const boost::shared_ptr<openni_wrapper::Image>& rgb,
+    	const boost::shared_ptr<openni_wrapper::Image>& rgb,
 			const boost::shared_ptr<openni_wrapper::DepthImage>& depth,
 			float constant);
 #endif

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

@@ -0,0 +1,52 @@
+#pragma once
+
+#include "rtabmap/core/Camera.h"
+#include "rtabmap/core/Version.h"
+#include "rtabmap/utilite/USemaphore.h"
+
+#ifdef RTABMAP_XVSDK
+#include <xv-sdk.h>
+#endif
+
+namespace rtabmap
+{
+
+class RTABMAP_CORE_EXPORT CameraSeerSense :
+	public Camera
+{
+public:
+	static bool available();
+
+public:
+	CameraSeerSense(
+		bool computeOdometry = false,
+		float imageRate = 0.0f,
+		const Transform & localTransform = Transform::getIdentity()
+	);
+	virtual ~CameraSeerSense();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+	virtual bool odomProvided() const;
+	virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.0);
+
+protected:
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
+
+private:
+#ifdef RTABMAP_XVSDK
+	CameraModel cameraModel_;
+	bool computeOdometry_;
+	int imuId_;
+	int tofId_;
+	std::shared_ptr<xv::Device> device_;
+	std::map<double, std::pair<cv::Vec3d, cv::Vec3d>> imuBuffer_;
+	std::pair<double, std::pair<cv::Mat, cv::Mat>> lastData_;
+	UMutex imuMutex_;
+	UMutex dataMutex_;
+	USemaphore dataReady_;
+#endif
+};
+
+} // namespace rtabmap

corelib/src/CMakeLists.txt

@@ -40,6 +40,7 @@ SET(SRC_FILES
     camera/CameraVideo.cpp
     camera/CameraMyntEye.cpp
     camera/CameraDepthAI.cpp
+	camera/CameraSeerSense.cpp
     
     EpipolarGeometry.cpp
     VisualWord.cpp
@@ -383,6 +384,13 @@ IF(depthai_FOUND)
 	)
 ENDIF(depthai_FOUND)
 
+IF(xvsdk_FOUND)
+	SET(PUBLIC_LIBRARIES
+		${PUBLIC_LIBRARIES}
+		${xvsdk_LIBRARIES}
+	)
+ENDIF(xvsdk_FOUND)
+
 IF(TARGET OpenMP::OpenMP_CXX)
 	SET(LIBRARIES
 		${LIBRARIES}

corelib/src/CameraModel.cpp

@@ -60,7 +60,7 @@ CameraModel::CameraModel(
 		localTransform_(localTransform)
 {
 	UASSERT(K_.empty() || (K_.rows == 3 && K_.cols == 3 && K_.type() == CV_64FC1));
-	UASSERT(D_.empty() || (D_.rows == 1 && (D_.cols == 4 || D_.cols == 5 || D_.cols == 6 || D_.cols == 8) && D_.type() == CV_64FC1));
+	UASSERT(D_.empty() || (D_.rows == 1 && (D_.cols == 4 || D_.cols == 5 || D_.cols == 6 || D_.cols == 8 || D_.cols == 12 || D_.cols == 14) && D_.type() == CV_64FC1));
 	UASSERT(R_.empty() || (R_.rows == 3 && R_.cols == 3 && R_.type() == CV_64FC1));
 	UASSERT(P_.empty() || (P_.rows == 3 && P_.cols == 4 && P_.type() == CV_64FC1));
 }
@@ -156,7 +156,7 @@ CameraModel::CameraModel(
 bool CameraModel::initRectificationMap()
 {
 	UASSERT(imageSize_.height > 0 && imageSize_.width > 0);
-	UASSERT(D_.rows == 1 && (D_.cols == 4 || D_.cols == 5 || D_.cols == 6 || D_.cols == 8));
+	UASSERT(D_.rows == 1 && (D_.cols == 4 || D_.cols == 5 || D_.cols == 6 || D_.cols == 8 || D_.cols == 12 || D_.cols == 14));
 	UASSERT(R_.rows == 3 && R_.cols == 3);
 	UASSERT(P_.rows == 3 && P_.cols == 4);
 	// init rectification map
@@ -279,7 +279,7 @@ bool CameraModel::load(const std::string & filePath)
 				std::vector<double> data;
 				n["data"] >> data;
 				UASSERT(rows*cols == (int)data.size());
-				UASSERT(rows == 1 && (cols == 4 || cols == 5 || cols == 8));
+				UASSERT(rows == 1 && (cols == 4 || cols == 5 || cols == 8 || cols == 12 || cols == 14));
 				D_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
 			}
 			else

corelib/src/DBReader.cpp

@@ -268,6 +268,12 @@ std::string DBReader::getSerial() const
 	return "DBReader";
 }
 
+bool DBReader::getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime)
+{
+	UERROR("DBReader only provides pose when capturing data, it cannot provide asynchronous pose.");
+	return false;
+}
+
 SensorData DBReader::captureImage(SensorCaptureInfo * info)
 {
 	SensorData data = this->getNextData(info);

corelib/src/MarkerDetector.cpp

@@ -182,7 +182,7 @@ std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
 		{
 			if(i==0)
 			{
-				*imageWithDetections = image.clone();
+				*imageWithDetections = cv::Mat(image.size(), subImageWithDetections.type());
 			}
 			if(!subImageWithDetections.empty())
 			{
@@ -363,7 +363,14 @@ std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
 
 	if(imageWithDetections)
 	{
-		image.copyTo(*imageWithDetections);
+		if(image.channels()==1)
+		{
+			cv::cvtColor(image, *imageWithDetections, cv::COLOR_GRAY2BGR);
+		}
+		else
+		{
+			image.copyTo(*imageWithDetections);
+		}
 		if(!ids.empty())
 		{
 			cv::aruco::drawDetectedMarkers(*imageWithDetections, corners, ids);

corelib/src/Odometry.cpp

@@ -491,7 +491,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 	}
 
 	// Ground alignment
-	if(_pose.isIdentity() && _alignWithGround)
+	if(_pose.x() == 0 && _pose.y() == 0 && _pose.z() == 0 && this->framesProcessed() == 0 && _alignWithGround)
 	{
 		if(data.depthOrRightRaw().empty())
 		{
@@ -507,6 +507,11 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 			if(indices->size())
 			{
 				cloud = util3d::voxelize(cloud, indices, 0.01);
+				if(!_pose.isIdentity())
+				{
+					// In case we are already aligned with gravity
+					cloud = util3d::transformPointCloud(cloud, _pose);
+				}
 				util3d::segmentObstaclesFromGround<pcl::PointXYZ>(cloud, ground, obstacles, 20, M_PI/4.0f, 0.02, 200, true);
 				if(ground->size())
 				{
@@ -535,11 +540,22 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 					//get rotation from z to n;
 					Eigen::Matrix3f R;
 					R = Eigen::Quaternionf().setFromTwoVectors(n,z);
-					Transform rotation(
+					if(_pose.r11() == 1.0f && _pose.r22() == 1.0f && _pose.r33() == 1.0f)
-							R(0,0), R(0,1), R(0,2), 0,
+					{
-							R(1,0), R(1,1), R(1,2), 0,
+						Transform rotation(
-							R(2,0), R(2,1), R(2,2), coefficients.values.at(3));
+								R(0,0), R(0,1), R(0,2), 0,
-					this->reset(rotation);
+								R(1,0), R(1,1), R(1,2), 0,
+								R(2,0), R(2,1), R(2,2), coefficients.values.at(3));
+						this->reset(rotation);
+					}
+					else
+					{
+						// Rotation is already set (e.g., from IMU/gravity), just update Z
+						UWARN("Rotation was already initialized, just offseting z to %f", coefficients.values.at(3));
+						Transform pose = _pose;
+						pose.z() = coefficients.values.at(3);
+						this->reset(pose);
+					}
 					success = true;
 				}
 			}

corelib/src/Parameters.cpp

@@ -718,6 +718,12 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
 				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
 #else
 				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With OpenNI:";
+#ifdef RTABMAP_OPENNI
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
 #endif
 				str = "With OpenNI2:";
 #ifdef RTABMAP_OPENNI2
@@ -802,6 +808,12 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
 				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
 #else
 				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With XVisio SDK:";
+#ifdef RTABMAP_XVSDK
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
 #endif
 				str = "With libpointmatcher:";
 #ifdef RTABMAP_POINTMATCHER

corelib/src/Rtabmap.cpp

@@ -103,6 +103,7 @@ Rtabmap::Rtabmap() :
 	_maxMemoryAllowed(Parameters::defaultRtabmapMemoryThr()), // 0=inf
 	_loopThr(Parameters::defaultRtabmapLoopThr()),
 	_loopRatio(Parameters::defaultRtabmapLoopRatio()),
+	_aggressiveLoopThr(Parameters::defaultRGBDAggressiveLoopThr()),
 	_virtualPlaceLikelihoodRatio(Parameters::defaultRtabmapVirtualPlaceLikelihoodRatio()),
 	_maxLoopClosureDistance(Parameters::defaultRGBDMaxLoopClosureDistance()),
 	_verifyLoopClosureHypothesis(Parameters::defaultVhEpEnabled()),
@@ -568,6 +569,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRtabmapMemoryThr(), _maxMemoryAllowed);
 	Parameters::parse(parameters, Parameters::kRtabmapLoopThr(), _loopThr);
 	Parameters::parse(parameters, Parameters::kRtabmapLoopRatio(), _loopRatio);
+	Parameters::parse(parameters, Parameters::kRGBDAggressiveLoopThr(), _aggressiveLoopThr);
 	Parameters::parse(parameters, Parameters::kRtabmapVirtualPlaceLikelihoodRatio(), _virtualPlaceLikelihoodRatio);
 
 	Parameters::parse(parameters, Parameters::kRGBDMaxLoopClosureDistance(), _maxLoopClosureDistance);
@@ -2104,21 +2106,26 @@ bool Rtabmap::process(
 			if(_highestHypothesis.first > 0)
 			{
 				float loopThr = _loopThr;
-				if((_startNewMapOnLoopClosure || !_memory->isIncremental()) &&
+				bool hasLoopClosureConstraints = false;
-					graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size() == 0 && // alone in the current map
+				for(std::multimap<int, Link>::iterator iter=_odomCacheConstraints.begin(); iter!=_odomCacheConstraints.end() && !hasLoopClosureConstraints; ++iter)
+				{
+					hasLoopClosureConstraints =
+							iter->second.type() == Link::kGlobalClosure ||
+							iter->second.type() == Link::kLocalSpaceClosure ||
+							iter->second.type() == Link::kLandmark;
+				}
+				if(	(( _memory->isIncremental() && !uContains(_optimizedPoses, _highestHypothesis.first)) || // not linked to previous map of that hypothesis
+					 (!_memory->isIncremental() && !hasLoopClosureConstraints)) && // not yet localized to any previous sessions
 					_memory->getWorkingMem().size()>1 && // should have an old map (beside virtual signature)
-					(int)_memory->getWorkingMem().size()<=_memory->getMaxStMemSize() &&
+					_rgbdSlamMode &&
-					_rgbdSlamMode)
+					loopThr > _aggressiveLoopThr)
 				{
-					// If the map is very small (under STM size) and we need to find
-					// a loop closure before continuing the map or localizing,
 					// use the best hypothesis directly.
-					loopThr = 0.0f;
+					UDEBUG("Using %s=%f", Parameters::kRGBDAggressiveLoopThr().c_str(), _aggressiveLoopThr);
+					loopThr = _aggressiveLoopThr;
 				}
 
 				// Loop closure Threshold
-				// When _loopThr=0, accept loop closure if the hypothesis is over
-				// the virtual (new) place hypothesis.
 				if(_highestHypothesis.second >= loopThr)
 				{
 					rejectedGlobalLoopClosure = true;
@@ -2666,7 +2673,7 @@ bool Rtabmap::process(
 				UDEBUG("got %d paths", (int)nearestPathsNotSorted.size());
 				// sort nearest paths by highest likelihood (if two have same likelihood, sort by id)
 				std::map<NearestPathKey, std::map<int, Transform> > nearestPaths;
-				Transform currentPoseInv = _optimizedPoses.at(signature->id());
+				Transform currentPoseInv = _optimizedPoses.at(signature->id()).inverse();
 				for(std::map<int, std::map<int, Transform> >::const_iterator iter=nearestPathsNotSorted.begin();iter!=nearestPathsNotSorted.end(); ++iter)
 				{
 					const std::map<int, Transform> & path = iter->second;

corelib/src/SensorCaptureThread.cpp

@@ -462,7 +462,11 @@ void SensorCaptureThread::mainLoop()
 
 		Transform pose;
 		cv::Mat covariance;
-		if(_odomSensor->getPose(data.stamp()+_poseTimeOffset, pose, covariance, _poseWaitTime>0?_poseWaitTime:0))
+		if(!info.odomPose.isNull() && _lidar == 0 && _odomSensor == _camera)
+		{
+			// Do nothing, we have already the pose
+		}
+		else if(_odomSensor->getPose(data.stamp()+_poseTimeOffset, pose, covariance, _poseWaitTime>0?_poseWaitTime:0))
 		{
 			info.odomPose = pose;
 			info.odomCovariance = covariance;

corelib/src/camera/CameraDepthAI.cpp

@@ -162,11 +162,12 @@ void CameraDepthAI::setCompanding(bool enabled, int width)
 #endif
 }
 
-void CameraDepthAI::setRectification(bool useSpecTranslation, float alphaScaling)
+void CameraDepthAI::setRectification(bool useSpecTranslation, float alphaScaling, bool enabled)
 {
 #ifdef RTABMAP_DEPTHAI
 	useSpecTranslation_ = useSpecTranslation;
 	alphaScaling_ = alphaScaling;
+	imagesRectified_ = enabled;
 #else
 	UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
 #endif
@@ -274,11 +275,15 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 	dai::Pipeline p;
 	auto monoLeft  = p.create<dai::node::MonoCamera>();
 	auto monoRight = p.create<dai::node::MonoCamera>();
-	auto stereo    = p.create<dai::node::StereoDepth>();
+	std::shared_ptr<dai::node::StereoDepth> stereo;
+	if(imagesRectified_)
+		stereo = p.create<dai::node::StereoDepth>();
 	std::shared_ptr<dai::node::Camera> colorCam;
 	if(outputMode_==2)
 	{
 		colorCam = p.create<dai::node::Camera>();
+		if(!imagesRectified_)
+			colorCam->setMeshSource(dai::CameraProperties::WarpMeshSource::NONE);
 		if(detectFeatures_)
 		{
 			UWARN("On-device feature detectors cannot be enabled on color camera input!");
@@ -340,31 +345,34 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 	}
 
 	// StereoDepth
-	if(outputMode_ == 2)
+	if(stereo.get())
-		stereo->setDepthAlign(dai::CameraBoardSocket::CAM_A);
+	{
-	else
+		if(outputMode_ == 2)
-		stereo->setDepthAlign(dai::StereoDepthProperties::DepthAlign::RECTIFIED_LEFT);
+			stereo->setDepthAlign(dai::CameraBoardSocket::CAM_A);
-	stereo->setExtendedDisparity(extendedDisparity_);
+		else
-	stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout
+			stereo->setDepthAlign(dai::StereoDepthProperties::DepthAlign::RECTIFIED_LEFT);
-	stereo->enableDistortionCorrection(true);
+		stereo->setExtendedDisparity(extendedDisparity_);
-	stereo->setDisparityToDepthUseSpecTranslation(useSpecTranslation_);
+		stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout
-	stereo->setDepthAlignmentUseSpecTranslation(useSpecTranslation_);
+		stereo->enableDistortionCorrection(true);
-	if(alphaScaling_ > -1.0f)
+		stereo->setDisparityToDepthUseSpecTranslation(useSpecTranslation_);
-		stereo->setAlphaScaling(alphaScaling_);
+		stereo->setDepthAlignmentUseSpecTranslation(useSpecTranslation_);
-	stereo->initialConfig.setConfidenceThreshold(confThreshold_);
+		if(alphaScaling_ > -1.0f)
-	stereo->initialConfig.setLeftRightCheck(lrcThreshold_>=0);
+			stereo->setAlphaScaling(alphaScaling_);
-	if(lrcThreshold_>=0)
+		stereo->initialConfig.setConfidenceThreshold(confThreshold_);
-		stereo->initialConfig.setLeftRightCheckThreshold(lrcThreshold_);
+		stereo->initialConfig.setLeftRightCheck(lrcThreshold_>=0);
-	stereo->initialConfig.setMedianFilter(dai::MedianFilter::KERNEL_7x7);
+		if(lrcThreshold_>=0)
-	auto config = stereo->initialConfig.get();
+			stereo->initialConfig.setLeftRightCheckThreshold(lrcThreshold_);
-	config.censusTransform.kernelSize = dai::StereoDepthConfig::CensusTransform::KernelSize::KERNEL_7x9;
+		stereo->initialConfig.setMedianFilter(dai::MedianFilter::KERNEL_7x7);
-	config.censusTransform.kernelMask = 0X2AA00AA805540155;
+		auto config = stereo->initialConfig.get();
-	config.postProcessing.brightnessFilter.maxBrightness = 255;
+		config.censusTransform.kernelSize = dai::StereoDepthConfig::CensusTransform::KernelSize::KERNEL_7x9;
-	stereo->initialConfig.set(config);
+		config.censusTransform.kernelMask = 0X2AA00AA805540155;
+		config.postProcessing.brightnessFilter.maxBrightness = 255;
+		stereo->initialConfig.set(config);
 
-	// Link plugins CAM -> STEREO -> XLINK
+		// Link plugins CAM -> STEREO -> XLINK
-	monoLeft->out.link(stereo->left);
+		monoLeft->out.link(stereo->left);
-	monoRight->out.link(stereo->right);
+		monoRight->out.link(stereo->right);
+	}
 
 	if(outputMode_ == 2)
 	{
@@ -386,7 +394,12 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 		depthOrRightEnc->setDefaultProfilePreset(monoRight->getFps(), dai::VideoEncoderProperties::Profile::MJPEG);
 		if(outputMode_ < 2)
 		{
-			stereo->rectifiedLeft.link(leftOrColorEnc->input);
+			if(imagesRectified_) {
+				stereo->rectifiedLeft.link(leftOrColorEnc->input);
+			}
+			else {
+				monoLeft->out.link(leftOrColorEnc->input);
+			}
 			leftOrColorEnc->bitstream.link(sync->inputs["left"]);
 		}
 		else
@@ -394,7 +407,7 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 			colorCam->video.link(leftOrColorEnc->input);
 			leftOrColorEnc->bitstream.link(sync->inputs["color"]);
 		}
-		if(outputMode_)
+		if(imagesRectified_ && outputMode_)
 		{
 			depthOrRightEnc->setQuality(100);
 			stereo->disparity.link(depthOrRightEnc->input);
@@ -402,23 +415,33 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 		}
 		else
 		{
-			stereo->rectifiedRight.link(depthOrRightEnc->input);
+			if(imagesRectified_) {
+				stereo->rectifiedRight.link(depthOrRightEnc->input);
+			}
+			else {
+				monoRight->out.link(depthOrRightEnc->input);
+			}
 			depthOrRightEnc->bitstream.link(sync->inputs["right"]);
 		}
 	}
 	else
 	{
-		stereo->setSubpixel(subpixelFractionalBits_>=3 && subpixelFractionalBits_<=5);
+		if(stereo.get()) {
-		if(subpixelFractionalBits_>=3 && subpixelFractionalBits_<=5)
+			stereo->setSubpixel(subpixelFractionalBits_>=3 && subpixelFractionalBits_<=5);
-			stereo->setSubpixelFractionalBits(subpixelFractionalBits_);
+			if(subpixelFractionalBits_>=3 && subpixelFractionalBits_<=5)
-		config = stereo->initialConfig.get();
+				stereo->setSubpixelFractionalBits(subpixelFractionalBits_);
-		config.costMatching.enableCompanding = compandingWidth_>0;
+			auto config = stereo->initialConfig.get();
-		if(compandingWidth_>0)
+			config.costMatching.enableCompanding = compandingWidth_>0;
-			config.costMatching.disparityWidth = compandingWidth_==64?dai::StereoDepthConfig::CostMatching::DisparityWidth::DISPARITY_64:dai::StereoDepthConfig::CostMatching::DisparityWidth::DISPARITY_96;
+			if(compandingWidth_>0)
-		stereo->initialConfig.set(config);
+				config.costMatching.disparityWidth = compandingWidth_==64?dai::StereoDepthConfig::CostMatching::DisparityWidth::DISPARITY_64:dai::StereoDepthConfig::CostMatching::DisparityWidth::DISPARITY_96;
+			stereo->initialConfig.set(config);
+		}
 		if(outputMode_ < 2)
 		{
-			stereo->rectifiedLeft.link(sync->inputs["left"]);
+			if(imagesRectified_)
+				stereo->rectifiedLeft.link(sync->inputs["left"]);
+			else
+				monoLeft->out.link(sync->inputs["left"]);
 		}
 		else
 		{
@@ -426,10 +449,15 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 			monoRight->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P);
 			colorCam->video.link(sync->inputs["color"]);
 		}
-		if(outputMode_)
+		if(imagesRectified_) {
-			stereo->depth.link(sync->inputs["depth"]);
+			if(outputMode_)
-		else
+				stereo->depth.link(sync->inputs["depth"]);
-			stereo->rectifiedRight.link(sync->inputs["right"]);
+			else
+				stereo->rectifiedRight.link(sync->inputs["right"]);
+		}
+		else {
+			monoRight->out.link(sync->inputs["right"]);
+		}
 	}
 
 	sync->setSyncThreshold(std::chrono::milliseconds(int(500 / monoLeft->getFps())));
@@ -460,7 +488,10 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 		auto cfg = gfttDetector->initialConfig.get();
 		cfg.featureMaintainer.minimumDistanceBetweenFeatures = minDistance_ * minDistance_;
 		gfttDetector->initialConfig.set(cfg);
-		stereo->rectifiedLeft.link(gfttDetector->inputImage);
+		if(stereo.get())
+			stereo->rectifiedLeft.link(gfttDetector->inputImage);
+		else
+			monoLeft->out.link(gfttDetector->inputImage);
 		gfttDetector->outputFeatures.link(sync->inputs["feat"]);
 	}
 	else if(detectFeatures_ >= 2)
@@ -472,13 +503,17 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 		neuralNetwork->setNumInferenceThreads(2);
 		neuralNetwork->setNumNCEPerInferenceThread(1);
 		neuralNetwork->input.setBlocking(false);
-		stereo->rectifiedLeft.link(manip->inputImage);
+		if(stereo.get())
+			stereo->rectifiedLeft.link(manip->inputImage);
+		else
+			monoLeft->out.link(manip->inputImage);
 		manip->out.link(neuralNetwork->input);
 		neuralNetwork->out.link(sync->inputs["feat"]);
 	}
 
 	device_.reset(new dai::Device(p, deviceToUse));
 
+	UINFO("Device serial: %s", device_->getMxId().c_str());
 	UINFO("Available camera sensors: ");
 	for(auto& sensor : device_->getCameraSensorNames()) {
 		UINFO("Socket: CAM_%c - %s", 'A'+(unsigned char)sensor.first, sensor.second.c_str());
@@ -487,6 +522,93 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 	UINFO("Loading eeprom calibration data");
 	dai::CalibrationHandler calibHandler = device_->readCalibration();
 
+	if(!calibrationFolder.empty() && !cameraName.empty() && imagesRectified_)
+	{
+		UINFO("Flashing camera...");
+		if(outputMode_ == 2)
+		{
+			stereoModel_.setName(cameraName, "rgb", "depth");
+		}
+		if(stereoModel_.load(calibrationFolder, cameraName, false))
+		{
+			std::vector<std::vector<float> > intrinsicsLeft(3);
+			std::vector<std::vector<float> > intrinsicsRight(3);
+			for(int row = 0; row<3; ++row)
+			{
+				intrinsicsLeft[row].resize(3);
+				intrinsicsRight[row].resize(3);
+				for(int col = 0; col<3; ++col)
+				{
+					intrinsicsLeft[row][col] = stereoModel_.left().K_raw().at<double>(row,col);
+					intrinsicsRight[row][col] = stereoModel_.right().K_raw().at<double>(row,col);
+				}
+			}
+
+			std::vector<float> distortionsLeft = stereoModel_.left().D_raw();
+			std::vector<float> distortionsRight = stereoModel_.right().D_raw();
+			std::vector<std::vector<float> > rotationMatrix(3);
+			for(int row = 0; row<3; ++row)
+			{
+				rotationMatrix[row].resize(3);
+				for(int col = 0; col<3; ++col)
+				{
+					rotationMatrix[row][col] = stereoModel_.stereoTransform()(row,col);
+				}
+			}
+
+			std::vector<float> translation(3);
+			translation[0] = stereoModel_.stereoTransform().x()*100.0f;
+			translation[1] = stereoModel_.stereoTransform().y()*100.0f;
+			translation[2] = stereoModel_.stereoTransform().z()*100.0f;
+
+			if(outputMode_ == 2)
+			{
+				// Only set RGB intrinsics
+				calibHandler.setCameraIntrinsics(dai::CameraBoardSocket::CAM_A, intrinsicsLeft, stereoModel_.left().imageWidth(), stereoModel_.left().imageHeight());
+				calibHandler.setDistortionCoefficients(dai::CameraBoardSocket::CAM_A, distortionsLeft);
+				std::vector<float> specTranslation = calibHandler.getCameraTranslationVector(dai::CameraBoardSocket::CAM_A, dai::CameraBoardSocket::CAM_C, true);
+				calibHandler.setCameraExtrinsics(dai::CameraBoardSocket::CAM_A, dai::CameraBoardSocket::CAM_C, rotationMatrix, translation, specTranslation);
+			}
+			else
+			{
+				calibHandler.setCameraIntrinsics(dai::CameraBoardSocket::CAM_B, intrinsicsLeft, stereoModel_.left().imageWidth(), stereoModel_.left().imageHeight());
+				calibHandler.setDistortionCoefficients(dai::CameraBoardSocket::CAM_B, distortionsLeft);
+				calibHandler.setCameraIntrinsics(dai::CameraBoardSocket::CAM_C, intrinsicsRight, stereoModel_.right().imageWidth(), stereoModel_.right().imageHeight());
+				calibHandler.setDistortionCoefficients(dai::CameraBoardSocket::CAM_C, distortionsRight);
+				std::vector<float> specTranslation = calibHandler.getCameraTranslationVector(dai::CameraBoardSocket::CAM_B, dai::CameraBoardSocket::CAM_C, true);
+				calibHandler.setCameraExtrinsics(dai::CameraBoardSocket::CAM_B, dai::CameraBoardSocket::CAM_C, rotationMatrix, translation, specTranslation);
+			}
+
+			try {
+				UINFO("Flashing camera with calibration from %s with camera name %s", calibrationFolder.c_str(), cameraName.c_str());
+				if(ULogger::level() <= ULogger::kInfo)
+				{
+					std::cout << "K left: " << stereoModel_.left().K_raw() << std::endl;
+					std::cout << "K right: " << stereoModel_.right().K_raw() << std::endl;
+					std::cout << "D left: " << stereoModel_.left().D_raw() << std::endl;
+					std::cout << "D right: " << stereoModel_.right().D_raw() << std::endl;
+					std::cout << "Extrinsics: " << stereoModel_.stereoTransform() << std::endl;
+					std::cout << "Expected K with rectification_alpha=0: " << stereoModel_.left().K()*(double(targetSize_.width)/double(stereoModel_.left().imageWidth())) << std::endl;
+				}
+				device_->flashCalibration2(calibHandler);
+				UINFO("Closing device...");
+				device_->close();
+				UINFO("Restarting pipeline...");
+				device_.reset(new dai::Device(p, deviceToUse));
+			}
+			catch(const std::runtime_error & e) {
+				UERROR("Failed flashing calibration: %s", e.what());
+			}
+		}
+		else
+		{
+			UERROR("Failed loading calibration from %s with camera name %s", calibrationFolder.c_str(), cameraName.c_str());
+		}
+
+		//Reload calibration
+		calibHandler = device_->readCalibration();
+	}
+
 	auto eeprom = calibHandler.getEepromData();
 	UINFO("Product name: %s, board name: %s", eeprom.productName.c_str(), eeprom.boardName.c_str());
 
@@ -512,12 +634,15 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 	double fy = newCameraMatrix.at<double>(1, 1);
 	double cx = newCameraMatrix.at<double>(0, 2);
 	double cy = newCameraMatrix.at<double>(1, 2);
-	double baseline = calibHandler.getBaselineDistance(dai::CameraBoardSocket::CAM_C, dai::CameraBoardSocket::CAM_B, useSpecTranslation_)/100.0;
+	UINFO("fx=%f fy=%f cx=%f cy=%f (target size = %dx%d)", fx, fy, cx, cy, targetSize_.width, targetSize_.height);
-	UINFO("fx=%f fy=%f cx=%f cy=%f baseline=%f", fx, fy, cx, cy, baseline);
+	if(outputMode_ == 2) {
-	if(outputMode_ == 2)
+		stereoModel_ = StereoCameraModel(device_->getDeviceName(), fx, fy, cx, cy, 0, this->getLocalTransform(), targetSize_);
-		stereoModel_ = StereoCameraModel(device_->getDeviceName(), fx, fy, cx, cy, baseline, this->getLocalTransform(), targetSize_);
+	}
-	else
+	else {
-		stereoModel_ = StereoCameraModel(device_->getDeviceName(), fx, fy, cx, cy, baseline, this->getLocalTransform()*Transform(-calibHandler.getBaselineDistance(dai::CameraBoardSocket::CAM_A)/100.0, 0, 0), targetSize_);
+		double baseline = calibHandler.getBaselineDistance(dai::CameraBoardSocket::CAM_C, dai::CameraBoardSocket::CAM_B, false)/100.0;
+		UINFO("baseline=%f", baseline);
+		stereoModel_ = StereoCameraModel(device_->getDeviceName(), fx, fy, cx, cy, outputMode_==0?baseline:0, this->getLocalTransform()*Transform(-calibHandler.getBaselineDistance(dai::CameraBoardSocket::CAM_A)/100.0, 0, 0), targetSize_);
+	}
 
 	if(imuPublished_)
 	{
@@ -624,7 +749,7 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 bool CameraDepthAI::isCalibrated() const
 {
 #ifdef RTABMAP_DEPTHAI
-	return stereoModel_.isValidForProjection();
+	return outputMode_ == 0?stereoModel_.isValidForProjection():stereoModel_.left().isValidForProjection();
 #else
 	return false;
 #endif
@@ -645,14 +770,14 @@ SensorData CameraDepthAI::captureImage(SensorCaptureInfo * info)
 
 	auto messageGroup = cameraQueue_->get<dai::MessageGroup>();
 	auto rectifLeftOrColor = messageGroup->get<dai::ImgFrame>(outputMode_<2?"left":"color");
-	auto rectifRightOrDepth = messageGroup->get<dai::ImgFrame>(outputMode_?"depth":"right");
+	auto rectifRightOrDepth = messageGroup->get<dai::ImgFrame>(imagesRectified_ && outputMode_?"depth":"right");
 
 	cv::Mat leftOrColor, depthOrRight;
 	if(device_->getDeviceInfo().protocol == X_LINK_TCP_IP || mxidOrName_.find(".") != std::string::npos)
 	{
 		leftOrColor = cv::imdecode(rectifLeftOrColor->getData(), cv::IMREAD_ANYCOLOR);
 		depthOrRight = cv::imdecode(rectifRightOrDepth->getData(), cv::IMREAD_GRAYSCALE);
-		if(outputMode_)
+		if(imagesRectified_ && outputMode_)
 		{
 			cv::Mat disp;
 			depthOrRight.convertTo(disp, CV_16UC1);
@@ -666,7 +791,7 @@ SensorData CameraDepthAI::captureImage(SensorCaptureInfo * info)
 	}
 
 	double stamp = std::chrono::duration<double>(rectifLeftOrColor->getTimestampDevice(dai::CameraExposureOffset::MIDDLE).time_since_epoch()).count();
-	if(outputMode_)
+	if(imagesRectified_ && outputMode_)
 		data = SensorData(leftOrColor, depthOrRight, stereoModel_.left(), this->getNextSeqID(), stamp);
 	else
 		data = SensorData(leftOrColor, depthOrRight, stereoModel_, this->getNextSeqID(), stamp);

corelib/src/camera/CameraSeerSense.cpp

@@ -0,0 +1,272 @@
+#include <rtabmap/core/camera/CameraSeerSense.h>
+#include <rtabmap/utilite/UEventsManager.h>
+
+namespace rtabmap {
+
+bool CameraSeerSense::available()
+{
+#ifdef RTABMAP_XVSDK
+	return true;
+#else
+	return false;
+#endif
+}
+
+CameraSeerSense::CameraSeerSense(bool computeOdometry, float imageRate, const Transform & localTransform) :
+	Camera(imageRate, localTransform)
+#ifdef RTABMAP_XVSDK
+	,
+	computeOdometry_(computeOdometry),
+	imuId_(0),
+	tofId_(0)
+#endif
+{
+#ifdef RTABMAP_XVSDK
+	xv::setLogLevel(xv::LogLevel(ULogger::level()+1));
+#endif
+}
+
+CameraSeerSense::~CameraSeerSense()
+{
+#ifdef RTABMAP_XVSDK
+	if(imuId_)
+		device_->imuSensor()->unregisterCallback(imuId_);
+	
+	if(tofId_)
+		device_->tofCamera()->unregisterColorDepthImageCallback(tofId_);
+
+	if(device_->slam())
+        device_->slam()->stop();
+
+    if(device_->imuSensor())
+		device_->imuSensor()->stop();
+
+    if(device_->colorCamera())
+		device_->colorCamera()->stop();
+
+    if(device_->tofCamera())
+		device_->tofCamera()->stop();
+
+	dataReady_.release();
+#endif
+}
+
+bool CameraSeerSense::init(const std::string & calibrationFolder, const std::string & cameraName)
+{
+	UDEBUG("");
+#ifdef RTABMAP_XVSDK
+	auto devices = xv::getDevices(3);
+	if(devices.empty())
+	{
+		UERROR("Timeout for SeerSense device detection.");
+		return false;
+	}
+	device_ = devices.begin()->second;
+
+	UASSERT(device_->imuSensor());
+	device_->imuSensor()->start();
+	imuId_ = device_->imuSensor()->registerCallback([this](const xv::Imu & xvImu) {
+		if(xvImu.hostTimestamp > 0)
+		{
+			if(isInterIMUPublishing())
+			{
+				IMU imu(cv::Vec3d(xvImu.gyro[0], xvImu.gyro[1], xvImu.gyro[2]), cv::Mat::eye(3,3,CV_64FC1),
+						cv::Vec3d(xvImu.accel[0], xvImu.accel[1], xvImu.accel[2]), cv::Mat::eye(3,3,CV_64FC1),
+						this->getLocalTransform());
+				this->postInterIMU(imu, xvImu.hostTimestamp);
+			}
+			else
+			{
+				UScopeMutex lock(imuMutex_);
+				imuBuffer_.emplace_hint(imuBuffer_.end(), xvImu.hostTimestamp,
+					std::make_pair(cv::Vec3d(xvImu.gyro[0], xvImu.gyro[1], xvImu.gyro[2]), cv::Vec3d(xvImu.accel[0], xvImu.accel[1], xvImu.accel[2])));
+			}
+		}
+	});
+
+	if(computeOdometry_)
+	{
+		UASSERT(device_->slam());
+		device_->slam()->start(xv::Slam::Mode::Mixed);
+	}
+
+	auto frameRate = xv::TofCamera::Framerate::FPS_30;
+	if(this->getImageRate() > 25)
+		frameRate = xv::TofCamera::Framerate::FPS_30;
+	else if(this->getImageRate() > 20)
+		frameRate = xv::TofCamera::Framerate::FPS_25;
+	else if(this->getImageRate() > 15)
+		frameRate = xv::TofCamera::Framerate::FPS_20;
+	else if(this->getImageRate() > 10)
+		frameRate = xv::TofCamera::Framerate::FPS_15;
+	else if(this->getImageRate() > 5)
+		frameRate = xv::TofCamera::Framerate::FPS_10;
+	else if(this->getImageRate() > 0)
+		frameRate = xv::TofCamera::Framerate::FPS_5;
+
+	UASSERT(device_->colorCamera());
+	device_->colorCamera()->setResolution(xv::ColorCamera::Resolution::RGB_640x480);
+	device_->colorCamera()->start();
+	UASSERT(device_->tofCamera());
+	device_->tofCamera()->setSonyTofSetting(xv::TofCamera::SonyTofLibMode::IQMIX_DF, xv::TofCamera::Resolution::QVGA, frameRate);
+	device_->tofCamera()->start();
+
+	UASSERT(!device_->tofCamera()->calibration().empty());
+	auto xvTofCalib = device_->tofCamera()->calibration()[0];
+	UASSERT(!xvTofCalib.pdcm.empty());
+	cv::Mat D(1, xvTofCalib.pdcm[0].distor.size(), CV_64FC1, xvTofCalib.pdcm[0].distor.begin());
+	cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
+	cv::Mat P = cv::Mat::eye(3, 4, CV_64FC1);
+	P.at<double>(0,0) = xvTofCalib.pdcm[0].fx;
+	P.at<double>(1,1) = xvTofCalib.pdcm[0].fy;
+	P.at<double>(0,2) = xvTofCalib.pdcm[0].u0;
+	P.at<double>(1,2) = xvTofCalib.pdcm[0].v0;
+	cv::Mat K = P.colRange(0, 3);
+	cameraModel_ = CameraModel(device_->id(), cv::Size(xvTofCalib.pdcm[0].w, xvTofCalib.pdcm[0].h), K, D, R, P,
+		this->getLocalTransform() * Transform(
+			xvTofCalib.pose.rotation()[0], xvTofCalib.pose.rotation()[1], xvTofCalib.pose.rotation()[2], xvTofCalib.pose.translation()[0],
+			xvTofCalib.pose.rotation()[3], xvTofCalib.pose.rotation()[4], xvTofCalib.pose.rotation()[5], xvTofCalib.pose.translation()[1],
+			xvTofCalib.pose.rotation()[6], xvTofCalib.pose.rotation()[7], xvTofCalib.pose.rotation()[8], xvTofCalib.pose.translation()[2]
+		)).scaled(0.5);
+	UASSERT(cameraModel_.isValidForRectification());
+	cameraModel_.initRectificationMap();
+
+	lastData_ = std::pair<double, std::pair<cv::Mat, cv::Mat>>();
+	tofId_ = device_->tofCamera()->registerColorDepthImageCallback([this](const xv::DepthColorImage & xvDepthColorImage) {
+		if(xvDepthColorImage.hostTimestamp > 0)
+		{
+			cv::Mat color = cv::Mat::zeros(cameraModel_.imageSize(), CV_8UC3);
+			color.forEach<cv::Vec3b>([&](cv::Vec3b& pixel, const int position[]) -> void {
+				const auto rgb = reinterpret_cast<std::uint8_t const *>(xvDepthColorImage.data.get() + (position[0]*cameraModel_.imageWidth()+position[1]) * 7);
+				pixel = cv::Vec3b(rgb[2], rgb[1], rgb[0]);
+			});
+			cv::Mat depth = cv::Mat::zeros(cameraModel_.imageSize(), CV_32FC1);
+			depth.forEach<float>([&](float &pixel, const int position[]) -> void {
+				pixel = *reinterpret_cast<float const *>(xvDepthColorImage.data.get() + (position[0]*cameraModel_.imageWidth()+position[1]) * 7 + 3);
+			});
+			UScopeMutex lock(dataMutex_);
+			bool notify = !lastData_.first;
+			lastData_ = std::make_pair(xvDepthColorImage.hostTimestamp, std::make_pair(color, depth));
+			if(notify)
+				dataReady_.release();
+		}
+	});
+
+	return true;
+#else
+	UERROR("CameraSeerSense: RTAB-Map is not built with XVisio SDK support!");
+#endif
+	return false;
+}
+
+bool CameraSeerSense::isCalibrated() const
+{
+	return true;
+}
+
+std::string CameraSeerSense::getSerial() const
+{
+#ifdef RTABMAP_XVSDK
+	return device_->id();
+#endif
+	return "";
+}
+
+bool CameraSeerSense::odomProvided() const
+{
+#ifdef RTABMAP_XVSDK
+	return computeOdometry_;
+#else
+	return false;
+#endif
+}
+
+bool CameraSeerSense::getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime)
+{
+#ifdef RTABMAP_XVSDK
+	xv::Pose xvPose;
+	if(computeOdometry_ && device_->slam()->getPoseAt(xvPose, stamp))
+	{
+		pose = this->getLocalTransform() *
+		Transform(
+			xvPose.transform().rotation()[0], xvPose.transform().rotation()[1], xvPose.transform().rotation()[2], xvPose.transform().translation()[0],
+			xvPose.transform().rotation()[3], xvPose.transform().rotation()[4], xvPose.transform().rotation()[5], xvPose.transform().translation()[1],
+			xvPose.transform().rotation()[6], xvPose.transform().rotation()[7], xvPose.transform().rotation()[8], xvPose.transform().translation()[2]) *
+		this->getLocalTransform().inverse();
+		covariance = cv::Mat::eye(6, 6, CV_64FC1) * 0.0005;
+		return true;
+	}
+#endif
+	return false;
+}
+
+SensorData CameraSeerSense::captureImage(SensorCaptureInfo * info)
+{
+	SensorData data;
+#ifdef RTABMAP_XVSDK
+	if(!dataReady_.acquire(1, 3000))
+	{
+		UERROR("Did not receive frame since 3 seconds...");
+		return data;
+	}
+
+	dataMutex_.lock();
+	data = SensorData(
+		cameraModel_.rectifyImage(lastData_.second.first, cv::INTER_CUBIC),
+		cameraModel_.rectifyImage(lastData_.second.second, cv::INTER_NEAREST),
+		cameraModel_, this->getNextSeqID(), lastData_.first);
+	lastData_ = std::pair<double, std::pair<cv::Mat, cv::Mat>>();
+	dataMutex_.unlock();
+
+	if(!isInterIMUPublishing())
+	{
+		cv::Vec3d gyro, acc;
+		std::map<double, std::pair<cv::Vec3d, cv::Vec3d>>::const_iterator iterA, iterB;
+
+		imuMutex_.lock();
+		while(imuBuffer_.empty() || imuBuffer_.rbegin()->first < data.stamp())
+		{
+			imuMutex_.unlock();
+			uSleep(1);
+			imuMutex_.lock();
+		}
+
+		iterB = imuBuffer_.lower_bound(data.stamp());
+		iterA = iterB;
+		if(iterA != imuBuffer_.begin())
+			iterA = --iterA;
+		if(iterA == iterB || data.stamp() == iterB->first)
+		{
+			gyro = iterB->second.first;
+			acc = iterB->second.second;
+		}
+		else if(data.stamp() > iterA->first && data.stamp() < iterB->first)
+		{
+			float t = (data.stamp()-iterA->first) / (iterB->first-iterA->first);
+			gyro = iterA->second.first + t*(iterB->second.first - iterA->second.first);
+			acc = iterA->second.second + t*(iterB->second.second - iterA->second.second);
+		}
+		imuBuffer_.erase(imuBuffer_.begin(), iterB);
+
+		imuMutex_.unlock();
+		data.setIMU(IMU(gyro, cv::Mat::eye(3, 3, CV_64FC1), acc, cv::Mat::eye(3, 3, CV_64FC1), this->getLocalTransform()));
+	}
+
+	xv::Pose xvPose;
+	if(computeOdometry_ && device_->slam()->getPoseAt(xvPose, data.stamp()))
+	{
+		info->odomPose = this->getLocalTransform() *
+		Transform(
+			xvPose.transform().rotation()[0], xvPose.transform().rotation()[1], xvPose.transform().rotation()[2], xvPose.transform().translation()[0],
+			xvPose.transform().rotation()[3], xvPose.transform().rotation()[4], xvPose.transform().rotation()[5], xvPose.transform().translation()[1],
+			xvPose.transform().rotation()[6], xvPose.transform().rotation()[7], xvPose.transform().rotation()[8], xvPose.transform().translation()[2]) *
+		this->getLocalTransform().inverse();
+		info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 0.0005;
+	}
+#else
+	UERROR("CameraSeerSense: RTAB-Map is not built with XVisio SDK support!");
+#endif
+	return data;
+}
+
+} // namespace rtabmap

corelib/src/global_map/OccupancyGrid.cpp

@@ -81,7 +81,10 @@ void OccupancyGrid::setMap(const cv::Mat & map, float xMin, float yMin, float ce
 		minValues_[0] = xMin;
 		minValues_[1] = yMin;
 		cellSize_ = cellSize;
-		addAssembledNode(poses.lower_bound(1)->first, poses.lower_bound(1)->second);
+		for(std::map<int, Transform>::const_iterator iter=poses.lower_bound(1); iter!=poses.end(); ++iter)
+		{
+			addAssembledNode(iter->first, iter->second);
+		}
 	}
 }
 

docker/README.md

@@ -0,0 +1,3 @@
+### Docker
+
+* Go to the [wiki](https://github.com/introlab/rtabmap/wiki/Installation#docker) for usage examples and how to build locally the images.

docker/bionic/android/rtabmap_apiXX/Dockerfile

@@ -4,10 +4,10 @@ FROM introlab3it/rtabmap:android-deps
 
 ARG API_VERSION=23
 
-WORKDIR /root/
-
 # Copy current source code
 COPY . /root/rtabmap-tango
 
-RUN /bin/bash -c "./rtabmap-tango/docker/bionic/android/rtabmap_apiXX/rtabmap.bash /opt/android $API_VERSION"
+WORKDIR /root/rtabmap-tango
+
+RUN /bin/bash -c "./docker/bionic/android/rtabmap_apiXX/rtabmap.bash /opt/android $API_VERSION"
 

docker/bionic/android/rtabmap_apiXX/rtabmap.bash

@@ -9,8 +9,6 @@ fi
 
 prefix=$1
 api=$2
-pwd=$(pwd)
-
 
 # tango
 wget 'https://docs.google.com/uc?authuser=0&id=12rHHkYM5k-UnQn-xGXs9JqYWhSXrgJr3&export=download' -O TangoSDK_Ikariotikos_C.zip
@@ -26,7 +24,7 @@ cp -r lib_tango_support_api/include/* $prefix/arm64-v8a/include/.
 cp -r lib_tango_support_api/lib/arm64-v8a/* $prefix/arm64-v8a/lib/.
 rm -r lib_tango_support_api
 wget 'https://docs.google.com/uc?authuser=0&id=1s5iPJ7xiridj9Jj--gCy2XiQFniheVm6&export=download' -O TangoSDK_Ikariotikos_Java.jar
-mv TangoSDK_Ikariotikos_Java.jar rtabmap-tango/app/android/libs/.
+mv TangoSDK_Ikariotikos_Java.jar app/android/libs/.
 
 # ARCore
 wget 'https://docs.google.com/uc?authuser=0&id=1VsibeqRYpS5pjmrG-vYTXyiPg8kbIfVN&export=download' -O arcore.zip
@@ -34,7 +32,7 @@ unzip -qq arcore.zip
 rm arcore.zip
 cp -r arcore1_18/include/* $prefix/arm64-v8a/include/.
 cp -r arcore1_18/arm64-v8a/* $prefix/arm64-v8a/lib/.
-cp arcore1_18/*.jar rtabmap-tango/app/android/libs/.
+cp arcore1_18/*.jar app/android/libs/.
 rm -r arcore1_18
 
 # AREngine
@@ -43,19 +41,18 @@ unzip -qq arengine.zip
 rm arengine.zip
 cp -r arengine/include/* $prefix/arm64-v8a/include/.
 cp -r arengine/arm64-v8a/* $prefix/arm64-v8a/lib/.
-cp arengine/*.jar rtabmap-tango/app/android/libs/.
+cp arengine/*.jar app/android/libs/.
 rm -r arengine
 
 # resource tool
-cd rtabmap-tango/build
+cd build
-$pwd/cmake-3.17.0-Linux-x86_64/bin/cmake -DANDROID_PREBUILD=ON ..
+/root/cmake-3.17.0-Linux-x86_64/bin/cmake -DANDROID_PREBUILD=ON ..
 make
-cd ../..
 
 # rtabmap
-mkdir rtabmap-tango/build/arm64-v8a
+mkdir arm64-v8a
-cd rtabmap-tango/build/arm64-v8a
+cd arm64-v8a
-$pwd/cmake-3.17.0-Linux-x86_64/bin/cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=$api -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a -DCMAKE_FIND_ROOT_PATH="$prefix/arm64-v8a/bin;$prefix/arm64-v8a;$prefix/arm64-v8a/share" -DBUILD_EXAMPLES=OFF -DBUILD_TOOLS=OFF -DOpenCV_DIR=$prefix/arm64-v8a/sdk/native/jni ../..
+/root/cmake-3.17.0-Linux-x86_64/bin/cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=$api -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a -DCMAKE_FIND_ROOT_PATH="$prefix/arm64-v8a/bin;$prefix/arm64-v8a;$prefix/arm64-v8a/share" -DBUILD_EXAMPLES=OFF -DBUILD_TOOLS=OFF -DOpenCV_DIR=$prefix/arm64-v8a/sdk/native/jni ../..
 make
 make clean
 

docker/noble/Dockerfile

@@ -0,0 +1,34 @@
+# Image: introlab3it/rtabmap:noble
+
+FROM introlab3it/rtabmap:noble-deps
+
+# OpenGV (issue seg fault on my computer, moved here from noble-deps to be built on ci)
+RUN git clone https://github.com/laurentkneip/opengv.git && \
+    cd opengv && \
+    git checkout 91f4b19c73450833a40e463ad3648aae80b3a7f3 && \
+    wget https://gist.githubusercontent.com/matlabbe/a412cf7c4627253874f81a00745a7fbb/raw/accc3acf465d1ffd0304a46b17741f62d4d354ef/opengv_disable_march_native.patch && \
+    git apply opengv_disable_march_native.patch && \
+    mkdir build && \
+    cd build && \
+    cmake -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTS=OFF .. && \
+    make -j$(nproc) && \
+    make install && \
+    cd && \
+    rm -r opengv
+
+# Will be used to read/store databases on host
+RUN mkdir -p /root/Documents/RTAB-Map && chmod 777 /root/Documents/RTAB-Map
+
+# Copy current source code
+COPY . /root/rtabmap
+
+# Build RTAB-Map project
+RUN source /ros_entrypoint.sh && \
+    cd rtabmap/build && \
+    cmake -DWITH_OPENGV=ON .. && \
+    make -j$(nproc) && \
+    make install && \
+    cd ../.. && \
+    rm -rf rtabmap && \
+    ldconfig
+

docker/noble/deps/Dockerfile

@@ -0,0 +1,119 @@
+# Image: introlab3it/rtabmap:noble-deps
+
+FROM ubuntu:24.04
+
+ARG TARGETPLATFORM
+ENV TARGETPLATFORM=${TARGETPLATFORM:-linux/amd64}
+RUN echo "I am building for $TARGETPLATFORM"
+
+ENV DEBIAN_FRONTEND=noninteractive
+
+# 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/
+
+# Install build dependencies
+RUN apt-get update && \
+    apt upgrade -y && \
+    apt-get install -y \
+      git \
+      wget \
+      libtbb-dev \
+      libproj-dev \
+      libpcl-dev \
+      ros-jazzy-ros-base \
+      ros-dev-tools \
+      ros-jazzy-cv-bridge \
+      ros-jazzy-image-geometry \
+      ros-jazzy-laser-geometry \
+      ros-jazzy-pcl-conversions \
+      ros-jazzy-rviz-common \
+      ros-jazzy-rviz-rendering \
+      ros-jazzy-rviz-default-plugins \
+      ros-jazzy-pcl-ros \
+      ros-jazzy-imu-filter-madgwick \
+      ros-jazzy-velodyne \
+      ros-jazzy-image-transport \
+      ros-jazzy-octomap-msgs \
+      ros-jazzy-libg2o \
+      ros-jazzy-libpointmatcher \
+      ros-jazzy-octomap \
+      ros-jazzy-qt-gui-cpp \
+      ros-jazzy-diagnostic-updater && \
+    apt-get clean && rm -rf /var/lib/apt/lists/
+
+WORKDIR /root/
+
+# GTSAM (we cannot use ros-jazzy-gtsam on Noble because it is linked to a different internal Eigen version than system one)
+RUN git clone https://github.com/borglab/gtsam.git && \
+    cd gtsam && \
+    git checkout 4.2.0 && \ 
+    mkdir build && \
+    cd build && \
+    cmake -DCMAKE_BUILD_TYPE=Release -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF -DGTSAM_USE_SYSTEM_EIGEN=ON .. && \
+    make -j$(nproc) && \
+    make install && \
+    cd && \
+    rm -r gtsam
+
+# 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 && \
+    apt-get clean && rm -rf /var/lib/apt/lists/ && \
+    git clone https://github.com/OpenKinect/libfreenect2 && \
+    cd libfreenect2 && \
+    mkdir build && \
+    cd build && \
+    cmake -DCMAKE_BUILD_TYPE=Release .. && \
+    make -j$(nproc) && \
+    make install && \
+    cd && \
+    rm -r libfreenect2; fi
+
+# zed open capture
+RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing zed-open-capture..." && \
+    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 && \
+    mkdir build && \
+    cd build && \
+    cmake -DCMAKE_BUILD_TYPE=Release .. && \
+    make -j$(nproc) && \
+    make install && \
+    cd && \
+    rm -r zed-open-capture; fi
+
+# OpenCV with all modules (same version than distro version to avoid conflicts with cv_bridge ros package)
+COPY ./docker/noble/deps/opencv_4_6_0_ffmpeg.patch .
+RUN git clone --branch 4.6.0 https://github.com/opencv/opencv.git && \
+    git clone --branch 4.6.0 https://github.com/opencv/opencv_contrib.git && \
+    mv opencv_4_6_0_ffmpeg.patch opencv/. && \
+    cd opencv && \
+    git apply opencv_4_6_0_ffmpeg.patch && \
+    mkdir build && \
+    cd build && \
+    cmake -DCMAKE_BUILD_TYPE=Release -DWITH_TBB=ON -DWITH_ADE=OFF -DWITH_OPENMP=ON -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
+
+RUN rm /bin/sh && ln -s /bin/bash /bin/sh
+
+COPY ./docker/noble/deps/ros_entrypoint.sh /ros_entrypoint.sh
+RUN chmod +x /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/jazzy/lib/x86_64-linux-gnu:/opt/ros/jazzy/lib/aarch64-linux-gnu
+
+# for jetson (https://github.com/introlab/rtabmap/issues/776)
+ENV LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/lib/aarch64-linux-gnu/tegra
+

docker/noble/deps/opencv_4_6_0_ffmpeg.patch

@@ -0,0 +1,12 @@
+diff --git a/modules/videoio/src/ffmpeg_codecs.hpp b/modules/videoio/src/ffmpeg_codecs.hpp
+index 61788e0345..faad2596ed 100644
+--- a/modules/videoio/src/ffmpeg_codecs.hpp
++++ b/modules/videoio/src/ffmpeg_codecs.hpp
+@@ -60,6 +60,7 @@ extern "C" {
+ #include <errno.h>
+ #endif
+ 
++#include <libavcodec/version.h>
+ #include <libavformat/avformat.h>
+ 
+ #ifdef __cplusplus

docker/noble/deps/ros_entrypoint.sh

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

guilib/include/rtabmap/gui/CalibrationDialog.h

@@ -39,7 +39,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap/utilite/UEventsHandler.h>
 
+#if defined(HAVE_OPENCV_ARUCO) || CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7)
+#include <opencv2/aruco/charuco.hpp>
+#define HAVE_CHARUCO
+#endif
+
 class Ui_calibrationDialog;
+class QTextStream;
 
 namespace rtabmap {
 
@@ -60,6 +66,7 @@ public:
 	int boardWidth() const;
 	int boardHeight() const;
 	double squareSize() const;
+	double markerLength() const;
 
 	void saveSettings(QSettings & settings, const QString & group = "") const;
 	void loadSettings(QSettings & settings, const QString & group = "");
@@ -68,20 +75,30 @@ public:
 	void setCameraName(const QString & name);
 	void setProgressVisibility(bool visible);
 	void setSwitchedImages(bool switched);
-	void setFisheyeImages(bool enabled);
+	void setFisheyeModel();
+	void setPlumbobModel();
+	void setRationalModel();
 	void setStereoMode(bool stereo, const QString & leftSuffix = "left", const QString & rightSuffix = "right");
 	void setSavingDirectory(const QString & savingDirectory) {savingDirectory_ = savingDirectory;}
 
-	StereoCameraModel stereoCalibration(const CameraModel & left, const CameraModel & right, bool ignoreStereoRectification) const;
+	StereoCameraModel stereoCalibration(const CameraModel & left, const CameraModel & right, bool ignoreStereoRectification, QTextStream * logStream = 0) const;
 
 public Q_SLOTS:
+
+	void setBoardType(int type);
 	void setBoardWidth(int width);
 	void setBoardHeight(int height);
 	void setSquareSize(double size);
+	void setMarkerDictionary(int dictionary);
+	void setMarkerLength(double length);
+	void setSubpixelRefinement(bool enabled);
+	void setSubpixelMaxError(double value);
+	void setCalibrationDataSaved(bool enabled);
 	void setExpectedStereoBaseline(double length);
 	void setMaxScale(int scale);
 
 	void processImages(const cv::Mat & imageLeft, const cv::Mat & imageRight, const QString & cameraName);
+	void generateBoard();
 	void calibrate();
 	void restart();
 	bool save();
@@ -95,6 +112,7 @@ protected:
 
 private:
 	float getArea(const std::vector<cv::Point2f> & corners, const cv::Size & boardSize);
+	float getSkew(const std::vector<cv::Point2f> & fourCorners);
 	float getSkew(const std::vector<cv::Point2f> & corners, const cv::Size & boardSize);
 
 	// x -> [0, 1] (left, right)
@@ -114,10 +132,25 @@ private:
 	QString cameraName_;
 	bool processingData_;
 	bool savedCalibration_;
+	int currentId_;
+	QString timestamp_;
+
+	std::vector<cv::Point3f> chessboardPoints_;
+	std::vector<int> chessboardPointIds_;
+#ifdef HAVE_CHARUCO
+	cv::Ptr<cv::aruco::Dictionary> markerDictionary_;
+	cv::Ptr<cv::aruco::DetectorParameters> arucoDetectorParams_;
+    cv::Ptr<cv::aruco::CharucoBoard> charucoBoard_;
+#endif
+
 
 	std::vector<std::vector<std::vector<cv::Point2f> > > imagePoints_;
+	std::vector<std::vector<std::vector<cv::Point3f> > > objectPoints_;
 	std::vector<std::vector<std::vector<float> > > imageParams_;
+	std::vector<std::vector<int > > imageIds_;
 	std::vector<std::vector<std::vector<cv::Point2f> > > stereoImagePoints_;
+	std::vector<std::vector<cv::Point3f> > stereoObjectPoints_;
+	std::vector<int> stereoImageIds_;
 	std::vector<cv::Size > imageSize_;
 	std::vector<rtabmap::CameraModel> models_;
 	rtabmap::StereoCameraModel stereoModel_;

guilib/include/rtabmap/gui/CameraViewer.h

@@ -44,6 +44,7 @@ namespace rtabmap {
 
 class ImageView;
 class CloudViewer;
+class MarkerDetector;
 
 class RTABMAP_GUI_EXPORT CameraViewer : public QDialog, public UEventsHandler
 {
@@ -70,6 +71,8 @@ private:
 	QLabel * imageSizeLabel_;
 	QCheckBox * showCloudCheckbox_;
 	QCheckBox * showScanCheckbox_;
+	QCheckBox * markerCheckbox_;
+	MarkerDetector * markerDetector_;
 };
 
 } /* namespace rtabmap */

guilib/include/rtabmap/gui/DatabaseViewer.h

@@ -201,7 +201,7 @@ private:
 	void updateCovariances(const QList<Link> & links);
 	void refineLinks(const QList<Link> & links);
 	void refineConstraint(int from, int to,  bool silent);
-	bool addConstraint(int from, int to, bool silent);
+	bool addConstraint(int from, int to, bool silent, bool silentlyUseOptimizedGraphAsGuess = false);
 	void exportPoses(int format);
 	void exportGPS(int format);
 

guilib/include/rtabmap/gui/MainWindow.h

@@ -192,6 +192,7 @@ protected Q_SLOTS:
 	void selectDepthAIOAKD();
 	void selectDepthAIOAKDLite();
 	void selectDepthAIOAKDPro();
+	void selectXvisioSeerSense();
 	void selectVLP16();
 	void dumpTheMemory();
 	void dumpThePrediction();

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -76,7 +76,8 @@ public:
 		kPanelCloudRendering = 2,
 		kPanelLogging = 4,
 		kPanelSource = 8,
-		kPanelAll = 15
+		kPanelCalibration = 16,
+		kPanelAll = 31
 	};
 	// TODO, tried to change the name of PANEL_FLAGS to PanelFlags... but signals/slots errors appeared...
 	Q_DECLARE_FLAGS(PANEL_FLAGS, PanelFlag);
@@ -96,6 +97,7 @@ public:
 		kSrcK4W2           = 8,
 		kSrcRealSense2     = 9,
 		kSrcK4A            = 10,
+		kSrcSeerSense      = 11,
 
 		kSrcStereo         = 100,
 		kSrcDC1394         = 100,
@@ -346,6 +348,7 @@ private Q_SLOTS:
 	void makeObsoleteCloudRenderingPanel();
 	void makeObsoleteLoggingPanel();
 	void makeObsoleteSourcePanel();
+	void makeObsoleteCalibrationPanel();
 	void clicked(const QModelIndex & current, const QModelIndex & previous);
 	void addParameter(int value);
 	void addParameter(bool value);

guilib/src/AboutDialog.cpp

@@ -136,6 +136,8 @@ AboutDialog::AboutDialog(QWidget * parent) :
 
 	_ui->label_freenect->setText(CameraFreenect::available()?"Yes":"No");
 	_ui->label_freenect_license->setEnabled(CameraFreenect::available());
+	_ui->label_openni->setText(CameraOpenni::available()?"Yes":"No");
+	_ui->label_openni_license->setEnabled(CameraOpenni::available());
 	_ui->label_openni2->setText(CameraOpenNI2::available()?"Yes":"No");
 	_ui->label_openni2_license->setEnabled(CameraOpenNI2::available());
 	_ui->label_freenect2->setText(CameraFreenect2::available()?"Yes":"No");
@@ -155,6 +157,7 @@ AboutDialog::AboutDialog(QWidget * parent) :
 	_ui->label_mynteye->setText(CameraMyntEye::available() ? "Yes" : "No");
 	_ui->label_depthai->setText(CameraDepthAI::available() ? "Yes" : "No");
 	_ui->label_depthai_license->setEnabled(CameraDepthAI::available());
+	_ui->label_xvsdk->setText(CameraSeerSense::available() ? "Yes" : "No");
 
 	_ui->label_toro->setText(Optimizer::isAvailable(Optimizer::kTypeTORO)?"Yes":"No");
 	_ui->label_toro_license->setEnabled(Optimizer::isAvailable(Optimizer::kTypeTORO)?true:false);

guilib/src/CameraViewer.cpp

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/util3d.h>
 #include <rtabmap/core/util2d.h>
 #include <rtabmap/core/util3d_filtering.h>
+#include <rtabmap/core/MarkerDetector.h>
 #include <rtabmap/gui/ImageView.h>
 #include <rtabmap/gui/CloudViewer.h>
 #include <rtabmap/utilite/UCv2Qt.h>
@@ -52,7 +53,8 @@ CameraViewer::CameraViewer(QWidget * parent, const ParametersMap & parameters) :
 	imageView_(new ImageView(this)),
 	cloudView_(new CloudViewer(this)),
 	processingImages_(false),
-	parameters_(parameters)
+	parameters_(parameters),
+	markerDetector_(0)
 {
 	qRegisterMetaType<rtabmap::SensorData>("rtabmap::SensorData");
 
@@ -79,6 +81,16 @@ CameraViewer::CameraViewer(QWidget * parent, const ParametersMap & parameters) :
 	showScanCheckbox_->setEnabled(false);
 	showScanCheckbox_->setChecked(true);
 
+	markerCheckbox_ = new QCheckBox("Detect markers", this);
+#ifdef HAVE_OPENCV_ARUCO
+	markerCheckbox_->setEnabled(true);
+	markerDetector_ = new MarkerDetector(parameters);
+#else
+	markerCheckbox_->setEnabled(false);
+	markerCheckbox_->setToolTip("Disabled: RTAB-Map is not built with OpenCV's aruco module.");
+#endif
+	markerCheckbox_->setChecked(false);
+
 	imageSizeLabel_ = new QLabel(this);
 
 	QDialogButtonBox * buttonBox = new QDialogButtonBox(this);
@@ -91,6 +103,7 @@ CameraViewer::CameraViewer(QWidget * parent, const ParametersMap & parameters) :
 	layout2->addWidget(decimationSpin_);
 	layout2->addWidget(showCloudCheckbox_);
 	layout2->addWidget(showScanCheckbox_);
+	layout2->addWidget(markerCheckbox_);
 	layout2->addWidget(imageSizeLabel_);
 	layout2->addStretch(1);
 	layout2->addWidget(buttonBox);
@@ -107,6 +120,7 @@ CameraViewer::CameraViewer(QWidget * parent, const ParametersMap & parameters) :
 CameraViewer::~CameraViewer()
 {
 	this->unregisterFromEventsManager();
+	delete markerDetector_;
 }
 
 void CameraViewer::setDecimation(int value)
@@ -119,9 +133,32 @@ void CameraViewer::showImage(const rtabmap::SensorData & data)
 	processingImages_ = true;
 	QString sizes;
 	imageView_->setVisible(!data.imageRaw().empty() || !data.imageRaw().empty());
+	std::map<int, MarkerInfo> detections;
 	if(!data.imageRaw().empty())
 	{
-		imageView_->setImage(uCvMat2QImage(data.imageRaw()));
+		std::vector<CameraModel> models;
+		if(markerCheckbox_->isEnabled() && markerCheckbox_->isChecked())
+		{
+			models = data.cameraModels();
+			if(models.empty())
+			{
+				for(size_t i=0; i<data.stereoCameraModels().size(); ++i)
+				{
+					models.push_back(data.stereoCameraModels()[i].left());
+				}
+			}
+		}
+			
+		if(!models.empty() && models[0].isValidForProjection())
+		{
+			cv::Mat imageWithDetections;
+			detections = markerDetector_->detect(data.imageRaw(), models, data.depthRaw(), std::map<int, float>(), &imageWithDetections);
+			imageView_->setImage(uCvMat2QImage(imageWithDetections));
+		}
+		else
+		{
+			imageView_->setImage(uCvMat2QImage(data.imageRaw()));
+		}
 		sizes.append(QString("Color=%1x%2").arg(data.imageRaw().cols).arg(data.imageRaw().rows));
 	}
 	if(!data.depthOrRightRaw().empty())
@@ -146,6 +183,28 @@ void CameraViewer::showImage(const rtabmap::SensorData & data)
 				showCloudCheckbox_->setEnabled(true);
 				cloudView_->addCloud("cloud", util3d::cloudFromSensorData(data, decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1, 0, 0, 0, parameters_));
 			}
+
+			// Add landmarks to 3D Map view
+#if PCL_VERSION_COMPARE(>=, 1, 7, 2)
+			cloudView_->removeAllCoordinates("landmark_");
+#endif
+			cloudView_->removeAllTexts();
+			if(!detections.empty())
+			{
+				for(std::map<int, MarkerInfo>::const_iterator iter=detections.begin(); iter!=detections.end(); ++iter)
+				{
+#if PCL_VERSION_COMPARE(>=, 1, 7, 2)
+					cloudView_->addOrUpdateCoordinate(uFormat("landmark_%d", iter->first), iter->second.pose(), iter->second.length(), false);
+#endif
+					std::string num = uNumber2Str(iter->first);
+					cloudView_->addOrUpdateText(
+							std::string("landmark_str_") + num,
+							num,
+							iter->second.pose(),
+							0.05,
+							Qt::yellow);
+				}
+			}
 		}
 	}
 

guilib/src/DatabaseViewer.cpp

@@ -468,6 +468,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	connect(ui_->spinBox_detectMore_iterations, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
 	connect(ui_->checkBox_detectMore_intraSession, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
 	connect(ui_->checkBox_detectMore_interSession, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
+	connect(ui_->checkBox_opt_graph_as_guess, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
 
 	connect(ui_->lineEdit_obstacleColor, SIGNAL(textChanged(const QString &)), this, SLOT(configModified()));
 	connect(ui_->lineEdit_groundColor, SIGNAL(textChanged(const QString &)), this, SLOT(configModified()));
@@ -635,6 +636,7 @@ void DatabaseViewer::readSettings()
 	ui_->spinBox_detectMore_iterations->setValue(settings.value("iterations", ui_->spinBox_detectMore_iterations->value()).toInt());
 	ui_->checkBox_detectMore_intraSession->setChecked(settings.value("intra_session", ui_->checkBox_detectMore_intraSession->isChecked()).toBool());
 	ui_->checkBox_detectMore_interSession->setChecked(settings.value("inter_session", ui_->checkBox_detectMore_interSession->isChecked()).toBool());
+	ui_->checkBox_opt_graph_as_guess->setChecked(settings.value("opt_graph_as_guess", ui_->checkBox_opt_graph_as_guess->isChecked()).toBool());
 	settings.endGroup();
 	settings.endGroup();
 
@@ -725,6 +727,7 @@ void DatabaseViewer::writeSettings()
 	settings.setValue("iterations", ui_->spinBox_detectMore_iterations->value());
 	settings.setValue("intra_session", ui_->checkBox_detectMore_intraSession->isChecked());
 	settings.setValue("inter_session", ui_->checkBox_detectMore_interSession->isChecked());
+	settings.setValue("opt_graph_as_guess", ui_->checkBox_opt_graph_as_guess->isChecked());
 	settings.endGroup();
 	settings.endGroup();
 
@@ -802,6 +805,7 @@ void DatabaseViewer::restoreDefaultSettings()
 	ui_->spinBox_detectMore_iterations->setValue(5);
 	ui_->checkBox_detectMore_intraSession->setChecked(true);
 	ui_->checkBox_detectMore_interSession->setChecked(true);
+	ui_->checkBox_opt_graph_as_guess->setChecked(true);
 }
 
 void DatabaseViewer::openDatabase()
@@ -1729,7 +1733,7 @@ void DatabaseViewer::updateIds()
 	UINFO("Loading all IDs...");
 	std::set<int> ids;
 	dbDriver_->getAllNodeIds(ids);
-#if QT_VERSION >= QT_VERSION_CHECK(6, 0, 0)
+#if QT_VERSION >= QT_VERSION_CHECK(5, 15, 3)
 	ids_ = QList<int>(ids.begin(), ids.end());
 #else
 	ids_ = QList<int>::fromStdList(std::list<int>(ids.begin(), ids.end()));
@@ -3960,7 +3964,7 @@ void DatabaseViewer::regenerateCurrentLocalMaps()
 	QSet<int> idsSet;
 	idsSet.insert(ids_.at(ui_->horizontalSlider_A->value()));
 	idsSet.insert(ids_.at(ui_->horizontalSlider_B->value()));
-#if QT_VERSION >= QT_VERSION_CHECK(6, 0, 0)
+#if QT_VERSION >= QT_VERSION_CHECK(5, 15, 3)
 	QList<int> ids(idsSet.begin(), idsSet.end());
 #else
 	QList<int> ids = idsSet.toList();
@@ -4215,6 +4219,7 @@ void DatabaseViewer::detectMoreLoopClosures()
 	std::pair<int, int> lastAdded(0,0);
 	bool intraSession = ui_->checkBox_detectMore_intraSession->isChecked();
 	bool interSession = ui_->checkBox_detectMore_interSession->isChecked();
+	bool useOptimizedGraphAsGuess = ui_->checkBox_opt_graph_as_guess->isChecked();
 	if(!interSession && !intraSession)
 	{
 		QMessageBox::warning(this, tr("Cannot detect more loop closures"), tr("Intra and inter session parameters are disabled! Enable one or both."));
@@ -4268,7 +4273,7 @@ void DatabaseViewer::detectMoreLoopClosures()
 						   delta.getNorm() >= ui_->doubleSpinBox_detectMore_radiusMin->value())
 						{
 							checkedLoopClosures.insert(std::make_pair(from, to));
-							if(addConstraint(from, to, true))
+							if(addConstraint(from, to, true, useOptimizedGraphAsGuess))
 							{
 								UINFO("Added new loop closure between %d and %d.", from, to);
 								++added;
@@ -8560,7 +8565,7 @@ void DatabaseViewer::addConstraint()
 	addConstraint(from, to, false);
 }
 
-bool DatabaseViewer::addConstraint(int from, int to, bool silent)
+bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool silentlyUseOptimizedGraphAsGuess)
 {
 	bool switchedIds = false;
 	if(from == to)
@@ -8728,7 +8733,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 								guessFromGraphRejected = true;
 							}
 						}
-						else
+						else if(silentlyUseOptimizedGraphAsGuess)
 						{
 							guess = fromIter->second.inverse() * toIter->second;
 						}

guilib/src/GuiLib.qrc

@@ -45,5 +45,6 @@
         <file>images/oakd_lite.png</file>
         <file>images/astra.png</file>
         <file>images/oakdpro.png</file>
+        <file>images/seer_sense_DS80.png</file>
     </qresource>
 </RCC>

guilib/src/MainWindow.cpp

@@ -469,8 +469,11 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
 	connect(_ui->actionDepthAI_oakd, SIGNAL(triggered()), this, SLOT(selectDepthAIOAKD()));
 	connect(_ui->actionDepthAI_oakdlite, SIGNAL(triggered()), this, SLOT(selectDepthAIOAKDLite()));
 	connect(_ui->actionDepthAI_oakdpro, SIGNAL(triggered()), this, SLOT(selectDepthAIOAKDPro()));
+	connect(_ui->actionXvisio_SeerSense, SIGNAL(triggered()), this, SLOT(selectXvisioSeerSense()));
 	connect(_ui->actionVelodyne_VLP_16, SIGNAL(triggered()), this, SLOT(selectVLP16()));
 	_ui->actionFreenect->setEnabled(CameraFreenect::available());
+	_ui->actionOpenNI_PCL->setEnabled(CameraOpenni::available());
+	_ui->actionOpenNI_PCL_ASUS->setEnabled(CameraOpenni::available());
 	_ui->actionOpenNI_CV->setEnabled(CameraOpenNICV::available());
 	_ui->actionOpenNI_CV_ASUS->setEnabled(CameraOpenNICV::available());
 	_ui->actionOpenNI2->setEnabled(CameraOpenNI2::available());
@@ -495,6 +498,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
     _ui->actionDepthAI_oakd->setEnabled(CameraDepthAI::available());
     _ui->actionDepthAI_oakdlite->setEnabled(CameraDepthAI::available());
     _ui->actionDepthAI_oakdpro->setEnabled(CameraDepthAI::available());
+	_ui->actionXvisio_SeerSense->setEnabled(CameraSeerSense::available());
 	this->updateSelectSourceMenu();
 
 	connect(_ui->actionPreferences, SIGNAL(triggered()), this, SLOT(openPreferences()));
@@ -5078,7 +5082,7 @@ void MainWindow::drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords
 			_lastId = (*refWords.rbegin()).first;
 		}
 		std::list<int> kpts = uKeysList(refWords);
-#if QT_VERSION >= QT_VERSION_CHECK(6, 0, 0)
+#if QT_VERSION >= QT_VERSION_CHECK(5, 15, 3)
 		_lastIds = QSet<int>(kpts.begin(), kpts.end());
 #else
 		_lastIds = QSet<int>::fromList(QList<int>::fromStdList(kpts));
@@ -5299,6 +5303,7 @@ void MainWindow::updateSelectSourceMenu()
 	_ui->actionDepthAI_oakd->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoDepthAI);
 	_ui->actionDepthAI_oakdlite->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoDepthAI);
 	_ui->actionDepthAI_oakdpro->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoDepthAI);
+	_ui->actionXvisio_SeerSense->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcSeerSense);
 	_ui->actionVelodyne_VLP_16->setChecked(_preferencesDialog->getLidarSourceDriver() == PreferencesDialog::kSrcLidarVLP16);
 }
 
@@ -5846,7 +5851,12 @@ void MainWindow::startDetection()
 		}
 	}
 
-	if(_preferencesDialog->getOdomSourceDriver() != PreferencesDialog::kSrcUndef)
+	if(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcDatabase &&
+	   camera && camera->odomProvided())
+	{
+		odomSensor = camera;
+	}
+	else if(_preferencesDialog->getOdomSourceDriver() != PreferencesDialog::kSrcUndef)
 	{
 		if(camera == 0 ||
 		   (_preferencesDialog->getOdomSourceDriver() != _preferencesDialog->getSourceDriver() &&
@@ -7228,6 +7238,11 @@ void MainWindow::selectDepthAIOAKDPro()
 	_preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcStereoDepthAI, 2); // variant 2=IMU+color
 }
 
+void MainWindow::selectXvisioSeerSense()
+{
+	_preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcSeerSense);
+}
+
 void MainWindow::selectVLP16()
 {
 	_preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcLidarVLP16);

guilib/src/PreferencesDialog.cpp

@@ -422,6 +422,11 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	{
 		_ui->comboBox_cameraStereo->setItemData(kSrcStereoDepthAI - kSrcStereo, 0, Qt::UserRole - 1);
 	}
+	if (!CameraSeerSense::available())
+	{
+		_ui->comboBox_cameraRGBD->setItemData(kSrcSeerSense - kSrcRGBD, 0, Qt::UserRole - 1);
+		_ui->comboBox_odom_sensor->setItemData(3, 0, Qt::UserRole - 1);
+	}
 	_ui->openni2_exposure->setEnabled(CameraOpenNI2::exposureGainAvailable());
 	_ui->openni2_gain->setEnabled(CameraOpenNI2::exposureGainAvailable());
 
@@ -1005,6 +1010,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 
 	// Create hypotheses
 	_ui->general_doubleSpinBox_hardThr->setObjectName(Parameters::kRtabmapLoopThr().c_str());
+	_ui->general_doubleSpinBox_agressiveThr->setObjectName(Parameters::kRGBDAggressiveLoopThr().c_str());
 	_ui->general_doubleSpinBox_loopRatio->setObjectName(Parameters::kRtabmapLoopRatio().c_str());
 	_ui->comboBox_virtualPlaceLikelihoodRatio->setObjectName(Parameters::kRtabmapVirtualPlaceLikelihoodRatio().c_str());
 	_ui->comboBox_globalDescriptorExtractor->setObjectName(Parameters::kMemGlobalDescriptorStrategy().c_str());
@@ -5187,9 +5193,16 @@ void PreferencesDialog::makeObsoleteLoggingPanel()
 
 void PreferencesDialog::makeObsoleteSourcePanel()
 {
+	ULOGGER_DEBUG("");
 	_obsoletePanels = _obsoletePanels | kPanelSource;
 }
 
+void PreferencesDialog::makeObsoleteCalibrationPanel()
+{
+	ULOGGER_DEBUG("");
+	_obsoletePanels = _obsoletePanels | kPanelCalibration;
+}
+
 QList<QGroupBox*> PreferencesDialog::getGroupBoxes()
 {
 	QList<QGroupBox*> boxes;
@@ -5620,7 +5633,8 @@ void PreferencesDialog::updateSourceGrpVisibility()
 			 _ui->comboBox_cameraRGBD->currentIndex() == kSrcRealSense - kSrcRGBD ||
 			 _ui->comboBox_cameraRGBD->currentIndex() == kSrcRGBDImages-kSrcRGBD ||
 			 _ui->comboBox_cameraRGBD->currentIndex() == kSrcOpenNI_PCL-kSrcRGBD ||
-			 _ui->comboBox_cameraRGBD->currentIndex() == kSrcRealSense2-kSrcRGBD));
+			 _ui->comboBox_cameraRGBD->currentIndex() == kSrcRealSense2-kSrcRGBD ||
+			 _ui->comboBox_cameraRGBD->currentIndex() == kSrcSeerSense-kSrcRGBD));
 	_ui->groupBox_openni2->setVisible(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcOpenNI2-kSrcRGBD);
 	_ui->groupBox_freenect2->setVisible(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcFreenect2-kSrcRGBD);
 	_ui->groupBox_k4w2->setVisible(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcK4W2 - kSrcRGBD);
@@ -5695,6 +5709,7 @@ void PreferencesDialog::updateSourceGrpVisibility()
 			(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcFreenect - kSrcRGBD) || //Kinect360
 			(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcK4A - kSrcRGBD) || //K4A
 			(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcRealSense2 - kSrcRGBD) || //D435i
+			(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcSeerSense - kSrcRGBD) ||
 			(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoRealSense2 - kSrcStereo) || //T265
 			(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoZed - kSrcStereo) || // ZEDm, ZED2
 			(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoMyntEye - kSrcStereo) || // MYNT EYE S
@@ -6234,6 +6249,11 @@ PreferencesDialog::Src PreferencesDialog::getOdomSourceDriver() const
 		//Zed SDK
 		return kSrcStereoZed;
 	}
+	else if(_ui->comboBox_odom_sensor->currentIndex() == 3)
+	{
+		//XVisio SDK
+		return kSrcSeerSense;
+	}
 	else if(_ui->comboBox_odom_sensor->currentIndex() != 0)
 	{
 		UERROR("Not implemented!");
@@ -6402,7 +6422,7 @@ Camera * PreferencesDialog::createCamera(
 		bool odomOnly,
 		bool odomSensorExtrinsicsCalib)
 {
-	if(odomOnly && !(driver == kSrcStereoRealSense2 || driver == kSrcStereoZed))
+	if(odomOnly && !(driver == kSrcStereoRealSense2 || driver == kSrcStereoZed || driver == kSrcSeerSense))
 	{
 		QMessageBox::warning(this, tr("Odometry Sensor"),
 				tr("Driver %1 cannot support odometry only mode.").arg(driver), QMessageBox::Ok);
@@ -6783,7 +6803,7 @@ Camera * PreferencesDialog::createCamera(
 		((CameraDepthAI*)camera)->setExtendedDisparity(_ui->checkBox_depthai_extended_disparity->isChecked());
 		((CameraDepthAI*)camera)->setSubpixelMode(_ui->comboBox_depthai_subpixel_fractional_bits->currentIndex()!=0, _ui->comboBox_depthai_subpixel_fractional_bits->currentIndex()==2?4:_ui->comboBox_depthai_subpixel_fractional_bits->currentIndex()==3?5:3);
 		((CameraDepthAI*)camera)->setCompanding(_ui->comboBox_depthai_disparity_companding->currentIndex()!=0, _ui->comboBox_depthai_disparity_companding->currentIndex()==1?64:96);
-		((CameraDepthAI*)camera)->setRectification(_ui->checkBox_depthai_use_spec_translation->isChecked(), _ui->doubleSpinBox_depthai_alpha_scaling->value());
+		((CameraDepthAI*)camera)->setRectification(_ui->checkBox_depthai_use_spec_translation->isChecked(), _ui->doubleSpinBox_depthai_alpha_scaling->value(), !useRawImages);
 		((CameraDepthAI*)camera)->setIMU(_ui->checkBox_depthai_imu_published->isChecked(), _ui->checkbox_publishInterIMU->isChecked());
 		((CameraDepthAI*)camera)->setIrIntensity(_ui->doubleSpinBox_depthai_dot_intensity->value(), _ui->doubleSpinBox_depthai_flood_intensity->value());
 		((CameraDepthAI*)camera)->setDetectFeatures(_ui->comboBox_depthai_detect_features->currentIndex());
@@ -6797,6 +6817,18 @@ Camera * PreferencesDialog::createCamera(
 			((CameraDepthAI*)camera)->setSuperPointDetector(_ui->doubleSpinBox_sptorch_threshold->value(), _ui->checkBox_sptorch_nms->isChecked(), _ui->spinBox_sptorch_minDistance->value());
 		}
 	}
+	else if (driver == kSrcSeerSense)
+	{
+		UDEBUG("SeerSense");
+		camera = new CameraSeerSense(
+			getOdomSourceDriver() == kSrcSeerSense || odomOnly,
+			this->getGeneralInputRate(),
+			this->getSourceLocalTransform());
+		camera->setInterIMUPublishing(
+			_ui->checkbox_publishInterIMU->isChecked(),
+			_ui->checkbox_publishInterIMU->isChecked() && getIMUFilteringStrategy()>0?
+					IMUFilter::create((IMUFilter::Type)(getIMUFilteringStrategy()-1), this->getAllParameters()):0);
+	}
 	else if(driver == kSrcUsbDevice)
 	{
 		camera = new CameraVideo(
@@ -7537,11 +7569,14 @@ void PreferencesDialog::calibrate()
 		}
 
 		bool freenect2 = driver == kSrcFreenect2;
-		bool fisheye = driver == kSrcStereoRealSense2;
+		bool rgbDepth = freenect2 || (driver==kSrcStereoDepthAI && _ui->comboBox_depthai_output_mode->currentIndex() == 2);
-		_calibrationDialog->setStereoMode(this->getSourceType() != kSrcRGB && driver != kSrcRealSense, freenect2?"rgb":"left", freenect2?"depth":"right"); // RGB+Depth or left+right
+		_calibrationDialog->setStereoMode(this->getSourceType() != kSrcRGB && driver != kSrcRealSense, rgbDepth?"rgb":"left", rgbDepth?"depth":"right"); // RGB+Depth or left+right
 		_calibrationDialog->setCameraName("");
 		_calibrationDialog->setSwitchedImages(freenect2);
-		_calibrationDialog->setFisheyeImages(fisheye);
+		if(driver == kSrcStereoRealSense2)
+			_calibrationDialog->setFisheyeModel();
+		if(driver == kSrcStereoDepthAI)
+			_calibrationDialog->setRationalModel();
 		_calibrationDialog->setSavingDirectory(this->getCameraInfoDir());
 		_calibrationDialog->registerToEventsManager();
 
@@ -7555,6 +7590,7 @@ void PreferencesDialog::calibrate()
 
 		cameraThread.join(true);
 	}
+	makeObsoleteCalibrationPanel();
 }
 
 void PreferencesDialog::calibrateSimple()

guilib/src/ui/DatabaseViewer.ui

@@ -61,7 +61,7 @@
           <rect>
            <x>0</x>
            <y>0</y>
-           <width>307</width>
+           <width>417</width>
            <height>389</height>
           </rect>
          </property>
@@ -392,7 +392,7 @@
           <rect>
            <x>0</x>
            <y>0</y>
-           <width>306</width>
+           <width>416</width>
            <height>389</height>
           </rect>
          </property>
@@ -1634,7 +1634,7 @@
      <item>
       <widget class="QToolBox" name="toolBox">
        <property name="currentIndex">
-        <number>1</number>
+        <number>2</number>
        </property>
        <widget class="QWidget" name="page_3">
         <property name="geometry">
@@ -2458,24 +2458,17 @@
         <property name="geometry">
          <rect>
           <x>0</x>
-          <y>0</y>
+          <y>-42</y>
-          <width>226</width>
+          <width>298</width>
-          <height>192</height>
+          <height>272</height>
          </rect>
         </property>
         <attribute name="label">
          <string>Detect more loop closures</string>
         </attribute>
         <layout class="QGridLayout" name="gridLayout_10" columnstretch="0,1">
-         <item row="4" column="1">
+         <item row="1" column="0">
-          <widget class="QLabel" name="label_32">
+          <widget class="QDoubleSpinBox" name="doubleSpinBox_detectMore_radiusMin">
-           <property name="text">
-            <string>Intra-session</string>
-           </property>
-          </widget>
-         </item>
-         <item row="0" column="0">
-          <widget class="QDoubleSpinBox" name="doubleSpinBox_detectMore_radius">
            <property name="suffix">
             <string> m</string>
            </property>
@@ -2489,10 +2482,37 @@
             <double>0.100000000000000</double>
            </property>
            <property name="value">
-            <double>1.000000000000000</double>
+            <double>0.000000000000000</double>
            </property>
           </widget>
          </item>
+         <item row="1" column="1">
+          <widget class="QLabel" name="label_36">
+           <property name="text">
+            <string>Radius Min</string>
+           </property>
+          </widget>
+         </item>
+         <item row="4" column="1">
+          <widget class="QLabel" name="label_32">
+           <property name="text">
+            <string>Intra-session</string>
+           </property>
+          </widget>
+         </item>
+         <item row="7" column="0">
+          <spacer name="verticalSpacer_5">
+           <property name="orientation">
+            <enum>Qt::Vertical</enum>
+           </property>
+           <property name="sizeHint" stdset="0">
+            <size>
+             <width>20</width>
+             <height>40</height>
+            </size>
+           </property>
+          </spacer>
+         </item>
          <item row="0" column="1">
           <widget class="QLabel" name="label_29">
            <property name="text">
@@ -2500,6 +2520,13 @@
            </property>
           </widget>
          </item>
+         <item row="2" column="1">
+          <widget class="QLabel" name="label_30">
+           <property name="text">
+            <string>Angle</string>
+           </property>
+          </widget>
+         </item>
          <item row="2" column="0">
           <widget class="QDoubleSpinBox" name="doubleSpinBox_detectMore_angle">
            <property name="suffix">
@@ -2519,10 +2546,22 @@
            </property>
           </widget>
          </item>
-         <item row="2" column="1">
+         <item row="0" column="0">
-          <widget class="QLabel" name="label_30">
+          <widget class="QDoubleSpinBox" name="doubleSpinBox_detectMore_radius">
-           <property name="text">
+           <property name="suffix">
-            <string>Angle</string>
+            <string> m</string>
+           </property>
+           <property name="decimals">
+            <number>2</number>
+           </property>
+           <property name="maximum">
+            <double>10.000000000000000</double>
+           </property>
+           <property name="singleStep">
+            <double>0.100000000000000</double>
+           </property>
+           <property name="value">
+            <double>1.000000000000000</double>
            </property>
           </widget>
          </item>
@@ -2539,6 +2578,13 @@
            </property>
           </widget>
          </item>
+         <item row="4" column="0">
+          <widget class="QCheckBox" name="checkBox_detectMore_intraSession">
+           <property name="text">
+            <string/>
+           </property>
+          </widget>
+         </item>
          <item row="3" column="1">
           <widget class="QLabel" name="label_31">
            <property name="text">
@@ -2546,19 +2592,6 @@
            </property>
           </widget>
          </item>
-         <item row="6" column="0">
-          <spacer name="verticalSpacer_5">
-           <property name="orientation">
-            <enum>Qt::Vertical</enum>
-           </property>
-           <property name="sizeHint" stdset="0">
-            <size>
-             <width>20</width>
-             <height>40</height>
-            </size>
-           </property>
-          </spacer>
-         </item>
          <item row="5" column="1">
           <widget class="QLabel" name="label_34">
            <property name="text">
@@ -2566,13 +2599,6 @@
            </property>
           </widget>
          </item>
-         <item row="4" column="0">
-          <widget class="QCheckBox" name="checkBox_detectMore_intraSession">
-           <property name="text">
-            <string/>
-           </property>
-          </widget>
-         </item>
          <item row="5" column="0">
           <widget class="QCheckBox" name="checkBox_detectMore_interSession">
            <property name="text">
@@ -2580,29 +2606,23 @@
            </property>
           </widget>
          </item>
-         <item row="1" column="1">
+         <item row="6" column="0">
-          <widget class="QLabel" name="label_36">
+          <widget class="QCheckBox" name="checkBox_opt_graph_as_guess">
            <property name="text">
-            <string>Radius Min</string>
+            <string/>
+           </property>
+           <property name="checked">
+            <bool>true</bool>
            </property>
           </widget>
          </item>
-         <item row="1" column="0">
+         <item row="6" column="1">
-          <widget class="QDoubleSpinBox" name="doubleSpinBox_detectMore_radiusMin">
+          <widget class="QLabel" name="label_37">
-           <property name="suffix">
+           <property name="text">
-            <string> m</string>
+            <string>Use optimized graph as guess</string>
            </property>
-           <property name="decimals">
+           <property name="wordWrap">
-            <number>2</number>
+            <bool>true</bool>
-           </property>
-           <property name="maximum">
-            <double>10.000000000000000</double>
-           </property>
-           <property name="singleStep">
-            <double>0.100000000000000</double>
-           </property>
-           <property name="value">
-            <double>0.000000000000000</double>
            </property>
           </widget>
          </item>
@@ -2613,8 +2633,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>185</width>
+          <width>432</width>
-          <height>485</height>
+          <height>196</height>
          </rect>
         </property>
         <attribute name="label">

guilib/src/ui/mainWindow.ui

@@ -229,6 +229,16 @@
        </property>
        <addaction name="actionOpenNI2_orbbec"/>
       </widget>
+      <widget class="QMenu" name="menuXvisio_SeerSense">
+       <property name="title">
+        <string>Xvisio SeerSense</string>
+       </property>
+       <property name="icon">
+        <iconset resource="../GuiLib.qrc">
+         <normaloff>:/images/seer_sense_DS80.png</normaloff>:/images/seer_sense_DS80.png</iconset>
+       </property>
+       <addaction name="actionXvisio_SeerSense"/>
+      </widget>
       <addaction name="menuKinect_for_Xbox_360"/>
       <addaction name="menuXtion_PRO_LIVE"/>
       <addaction name="menuOrbbec_Astra"/>
@@ -240,6 +250,7 @@
       <addaction name="menuRealSense_SR300"/>
       <addaction name="menuRealSense_D435"/>
       <addaction name="menuRealSense_L515"/>
+      <addaction name="menuXvisio_SeerSense"/>
      </widget>
      <widget class="QMenu" name="menuStereo_camera">
       <property name="title">
@@ -427,9 +438,6 @@
   </widget>
   <widget class="QStatusBar" name="statusbar"/>
   <widget class="QDockWidget" name="dockWidget_posterior">
-   <property name="features">
-    <set>QDockWidget::AllDockWidgetFeatures</set>
-   </property>
    <property name="windowTitle">
     <string>A posteriori PDF</string>
    </property>
@@ -1733,6 +1741,14 @@
     <string>Velodyne VLP-16</string>
    </property>
   </action>
+  <action name="actionXvisio_SeerSense">
+   <property name="checkable">
+    <bool>true</bool>
+   </property>
+   <property name="text">
+    <string>Xvisio</string>
+   </property>
+  </action>
  </widget>
  <customwidgets>
   <customwidget>

guilib/src/ui/preferencesDialog.ui

@@ -95,7 +95,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>11</number>
+                <number>7</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,0">
@@ -3424,7 +3424,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>1</number>
+                          <number>0</number>
                          </property>
                          <widget class="QWidget" name="page_41">
                           <layout class="QVBoxLayout" name="verticalLayout_64">
@@ -3515,6 +3515,11 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     <string>Kinect for Azure</string>
                                    </property>
                                   </item>
+                                  <item>
+                                   <property name="text">
+                                    <string>Xvisio SeerSense</string>
+                                   </property>
+                                  </item>
                                  </widget>
                                 </item>
                                 <item row="0" column="1">
@@ -3535,7 +3540,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                               <item>
                                <widget class="QStackedWidget" name="stackedWidget_rgbd">
                                 <property name="currentIndex">
-                                 <number>4</number>
+                                 <number>11</number>
                                 </property>
                                 <widget class="QWidget" name="page_32">
                                  <layout class="QVBoxLayout" name="verticalLayout_63">
@@ -3647,22 +3652,62 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     <property name="title">
                                      <string>OpenNI 2</string>
                                     </property>
-                                    <layout class="QGridLayout" name="gridLayout_54" columnstretch="0,0,1">
+                                    <layout class="QGridLayout" name="gridLayout_54" columnstretch="0,0,0">
-                                     <item row="0" column="2">
+                                     <item row="1" column="2">
-                                      <widget class="QLabel" name="label_231">
+                                      <widget class="QLabel" name="label_217">
                                        <property name="text">
-                                        <string>Path to a *.ONI file.</string>
+                                        <string>Auto white balance.</string>
                                        </property>
                                        <property name="wordWrap">
                                         <bool>true</bool>
                                        </property>
                                       </widget>
                                      </item>
-                                     <item row="0" column="1">
+                                     <item row="2" column="2">
-                                      <widget class="QLineEdit" name="lineEdit_openni2OniPath">
+                                      <widget class="QLabel" name="label_218">
+                                       <property name="text">
+                                        <string>Auto exposure.</string>
+                                       </property>
+                                       <property name="wordWrap">
+                                        <bool>true</bool>
+                                       </property>
+                                      </widget>
+                                     </item>
+                                     <item row="3" column="1">
+                                      <widget class="QSpinBox" name="openni2_exposure">
+                                       <property name="maximum">
+                                        <number>65535</number>
+                                       </property>
+                                      </widget>
+                                     </item>
+                                     <item row="5" column="1">
+                                      <widget class="QCheckBox" name="openni2_stampsIdsUsed">
                                        <property name="text">
                                         <string/>
                                        </property>
+                                       <property name="checked">
+                                        <bool>false</bool>
+                                       </property>
+                                      </widget>
+                                     </item>
+                                     <item row="9" column="2">
+                                      <widget class="QLabel" name="label_641">
+                                       <property name="text">
+                                        <string>Depth decimation.</string>
+                                       </property>
+                                       <property name="wordWrap">
+                                        <bool>true</bool>
+                                       </property>
+                                      </widget>
+                                     </item>
+                                     <item row="4" column="2">
+                                      <widget class="QLabel" name="label_220">
+                                       <property name="text">
+                                        <string>Gain.</string>
+                                       </property>
+                                       <property name="wordWrap">
+                                        <bool>true</bool>
+                                       </property>
                                       </widget>
                                      </item>
                                      <item row="0" column="0">
@@ -3672,10 +3717,10 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                        </property>
                                       </widget>
                                      </item>
-                                     <item row="1" column="2">
+                                     <item row="8" column="2">
-                                      <widget class="QLabel" name="label_217">
+                                      <widget class="QLabel" name="label_436">
                                        <property name="text">
-                                        <string>Auto white balance.</string>
+                                        <string>IR-Depth vertical shift. Positive toward up, negative toward down.</string>
                                        </property>
                                        <property name="wordWrap">
                                         <bool>true</bool>
@@ -3692,103 +3737,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                        </property>
                                       </widget>
                                      </item>
-                                     <item row="2" column="2">
-                                      <widget class="QLabel" name="label_218">
-                                       <property name="text">
-                                        <string>Auto exposure.</string>
-                                       </property>
-                                       <property name="wordWrap">
-                                        <bool>true</bool>
-                                       </property>
-                                      </widget>
-                                     </item>
-                                     <item row="2" column="1">
-                                      <widget class="QCheckBox" name="openni2_autoExposure">
-                                       <property name="text">
-                                        <string/>
-                                       </property>
-                                       <property name="checked">
-                                        <bool>true</bool>
-                                       </property>
-                                      </widget>
-                                     </item>
-                                     <item row="3" column="2">
-                                      <widget class="QLabel" name="label_219">
-                                       <property name="text">
-                                        <string>Exposure.</string>
-                                       </property>
-                                       <property name="wordWrap">
-                                        <bool>true</bool>
-                                       </property>
-                                      </widget>
-                                     </item>
-                                     <item row="3" column="1">
-                                      <widget class="QSpinBox" name="openni2_exposure">
-                                       <property name="maximum">
-                                        <number>65535</number>
-                                       </property>
-                                      </widget>
-                                     </item>
-                                     <item row="4" column="2">
-                                      <widget class="QLabel" name="label_220">
-                                       <property name="text">
-                                        <string>Gain.</string>
-                                       </property>
-                                       <property name="wordWrap">
-                                        <bool>true</bool>
-                                       </property>
-                                      </widget>
-                                     </item>
-                                     <item row="4" column="1">
-                                      <widget class="QSpinBox" name="openni2_gain">
-                                       <property name="maximum">
-                                        <number>1000</number>
-                                       </property>
-                                       <property name="value">
-                                        <number>100</number>
-                                       </property>
-                                      </widget>
-                                     </item>
-                                     <item row="5" column="2">
-                                      <widget class="QLabel" name="label_264">
-                                       <property name="text">
-                                        <string>Use timestamps and frame IDs from OpenNI2.</string>
-                                       </property>
-                                       <property name="wordWrap">
-                                        <bool>true</bool>
-                                       </property>
-                                      </widget>
-                                     </item>
-                                     <item row="5" column="1">
-                                      <widget class="QCheckBox" name="openni2_stampsIdsUsed">
-                                       <property name="text">
-                                        <string/>
-                                       </property>
-                                       <property name="checked">
-                                        <bool>false</bool>
-                                       </property>
-                                      </widget>
-                                     </item>
-                                     <item row="6" column="2">
-                                      <widget class="QLabel" name="label_223">
-                                       <property name="text">
-                                        <string>Mirroring.</string>
-                                       </property>
-                                       <property name="wordWrap">
-                                        <bool>true</bool>
-                                       </property>
-                                      </widget>
-                                     </item>
-                                     <item row="6" column="1">
-                                      <widget class="QCheckBox" name="openni2_mirroring">
-                                       <property name="text">
-                                        <string/>
-                                       </property>
-                                       <property name="checked">
-                                        <bool>false</bool>
-                                       </property>
-                                      </widget>
-                                     </item>
                                      <item row="7" column="2">
                                       <widget class="QLabel" name="label_435">
                                        <property name="text">
@@ -3799,29 +3747,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                        </property>
                                       </widget>
                                      </item>
-                                     <item row="7" column="1">
-                                      <widget class="QSpinBox" name="openni2_hshift">
-                                       <property name="suffix">
-                                        <string> pix</string>
-                                       </property>
-                                       <property name="minimum">
-                                        <number>-9999</number>
-                                       </property>
-                                       <property name="maximum">
-                                        <number>9999</number>
-                                       </property>
-                                      </widget>
-                                     </item>
-                                     <item row="8" column="2">
-                                      <widget class="QLabel" name="label_436">
-                                       <property name="text">
-                                        <string>IR-Depth vertical shift. Positive toward up, negative toward down.</string>
-                                       </property>
-                                       <property name="wordWrap">
-                                        <bool>true</bool>
-                                       </property>
-                                      </widget>
-                                     </item>
                                      <item row="8" column="1">
                                       <widget class="QSpinBox" name="openni2_vshift">
                                        <property name="suffix">
@@ -3835,16 +3760,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                        </property>
                                       </widget>
                                      </item>
-                                     <item row="9" column="2">
-                                      <widget class="QLabel" name="label_641">
-                                       <property name="text">
-                                        <string>Depth decimation.</string>
-                                       </property>
-                                       <property name="wordWrap">
-                                        <bool>true</bool>
-                                       </property>
-                                      </widget>
-                                     </item>
                                      <item row="9" column="1">
                                       <widget class="QSpinBox" name="openni2_depth_decimation">
                                        <property name="suffix">
@@ -3858,6 +3773,96 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                        </property>
                                       </widget>
                                      </item>
+                                     <item row="3" column="2">
+                                      <widget class="QLabel" name="label_219">
+                                       <property name="text">
+                                        <string>Exposure.</string>
+                                       </property>
+                                       <property name="wordWrap">
+                                        <bool>true</bool>
+                                       </property>
+                                      </widget>
+                                     </item>
+                                     <item row="5" column="2">
+                                      <widget class="QLabel" name="label_264">
+                                       <property name="text">
+                                        <string>Use timestamps and frame IDs from OpenNI2.</string>
+                                       </property>
+                                       <property name="wordWrap">
+                                        <bool>true</bool>
+                                       </property>
+                                      </widget>
+                                     </item>
+                                     <item row="6" column="2">
+                                      <widget class="QLabel" name="label_223">
+                                       <property name="text">
+                                        <string>Mirroring.</string>
+                                       </property>
+                                       <property name="wordWrap">
+                                        <bool>true</bool>
+                                       </property>
+                                      </widget>
+                                     </item>
+                                     <item row="4" column="1">
+                                      <widget class="QSpinBox" name="openni2_gain">
+                                       <property name="maximum">
+                                        <number>1000</number>
+                                       </property>
+                                       <property name="value">
+                                        <number>100</number>
+                                       </property>
+                                      </widget>
+                                     </item>
+                                     <item row="6" column="1">
+                                      <widget class="QCheckBox" name="openni2_mirroring">
+                                       <property name="text">
+                                        <string/>
+                                       </property>
+                                       <property name="checked">
+                                        <bool>false</bool>
+                                       </property>
+                                      </widget>
+                                     </item>
+                                     <item row="0" column="1">
+                                      <widget class="QLineEdit" name="lineEdit_openni2OniPath">
+                                       <property name="text">
+                                        <string/>
+                                       </property>
+                                      </widget>
+                                     </item>
+                                     <item row="0" column="2">
+                                      <widget class="QLabel" name="label_231">
+                                       <property name="text">
+                                        <string>Path to a *.ONI file.</string>
+                                       </property>
+                                       <property name="wordWrap">
+                                        <bool>true</bool>
+                                       </property>
+                                      </widget>
+                                     </item>
+                                     <item row="7" column="1">
+                                      <widget class="QSpinBox" name="openni2_hshift">
+                                       <property name="suffix">
+                                        <string> pix</string>
+                                       </property>
+                                       <property name="minimum">
+                                        <number>-9999</number>
+                                       </property>
+                                       <property name="maximum">
+                                        <number>9999</number>
+                                       </property>
+                                      </widget>
+                                     </item>
+                                     <item row="2" column="1">
+                                      <widget class="QCheckBox" name="openni2_autoExposure">
+                                       <property name="text">
+                                        <string/>
+                                       </property>
+                                       <property name="checked">
+                                        <bool>true</bool>
+                                       </property>
+                                      </widget>
+                                     </item>
                                     </layout>
                                    </widget>
                                   </item>
@@ -4914,6 +4919,23 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                   </item>
                                  </layout>
                                 </widget>
+                                <widget class="QWidget" name="page_99">
+                                 <layout class="QVBoxLayout" name="verticalLayout_180">
+                                  <item>
+                                   <spacer name="verticalSpacer_3">
+                                    <property name="orientation">
+                                     <enum>Qt::Vertical</enum>
+                                    </property>
+                                    <property name="sizeHint" stdset="0">
+                                     <size>
+                                      <width>20</width>
+                                      <height>40</height>
+                                     </size>
+                                    </property>
+                                   </spacer>
+                                  </item>
+                                 </layout>
+                                </widget>
                                </widget>
                               </item>
                              </layout>
@@ -8139,6 +8161,11 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                              <string>ZED sdk</string>
                             </property>
                            </item>
+                           <item>
+                            <property name="text">
+                             <string>SeerSense</string>
+                            </property>
+                           </item>
                           </widget>
                          </item>
                          <item row="0" column="1">
@@ -8736,7 +8763,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                          <property name="currentIndex">
                           <number>1</number>
                          </property>
-                         <widget class="QWidget" name="page_99">
+                         <widget class="QWidget" name="page_98">
                           <layout class="QVBoxLayout" name="verticalLayout_177">
                            <item>
                             <spacer name="verticalSpacer_83">
@@ -9511,6 +9538,45 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        <string>Thresholds</string>
                       </property>
                       <layout class="QGridLayout" name="gridLayout_44" columnstretch="0,1">
+                       <item row="1" column="1">
+                        <widget class="QLabel" name="label_maxWmSize">
+                         <property name="text">
+                          <string>Maximum signatures allowed in Working Memory (0 means inf).</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="1">
+                        <widget class="QLabel" name="label_96">
+                         <property name="text">
+                          <string>T_ratio : The loop closure hypothesis must be over T_ratio x lastHypothesisValue.</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="0">
+                        <widget class="QDoubleSpinBox" name="general_doubleSpinBox_hardThr">
+                         <property name="maximum">
+                          <double>1.000000000000000</double>
+                         </property>
+                         <property name="singleStep">
+                          <double>0.010000000000000</double>
+                         </property>
+                         <property name="value">
+                          <double>0.950000000000000</double>
+                         </property>
+                        </widget>
+                       </item>
                        <item row="0" column="0">
                         <widget class="QDoubleSpinBox" name="general_doubleSpinBox_timeThr">
                          <property name="suffix">
@@ -9553,32 +9619,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="1" column="1">
-                        <widget class="QLabel" name="label_maxWmSize">
-                         <property name="text">
-                          <string>Maximum signatures allowed in Working Memory (0 means inf).</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="0">
-                        <widget class="QDoubleSpinBox" name="general_doubleSpinBox_hardThr">
-                         <property name="maximum">
-                          <double>1.000000000000000</double>
-                         </property>
-                         <property name="singleStep">
-                          <double>0.010000000000000</double>
-                         </property>
-                         <property name="value">
-                          <double>0.950000000000000</double>
-                         </property>
-                        </widget>
-                       </item>
                        <item row="2" column="1">
                         <widget class="QLabel" name="label_93">
                          <property name="text">
@@ -9592,7 +9632,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="3" column="0">
+                       <item row="4" column="0">
                         <widget class="QDoubleSpinBox" name="general_doubleSpinBox_loopRatio">
                          <property name="maximum">
                           <double>1.000000000000000</double>
@@ -9606,9 +9646,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </widget>
                        </item>
                        <item row="3" column="1">
-                        <widget class="QLabel" name="label_96">
+                        <widget class="QLabel" name="label_332">
                          <property name="text">
-                          <string>T_ratio : The loop closure hypothesis must be over T_ratio x lastHypothesisValue.</string>
+                          <string>Loop closure threshold used (overriding T_loop) when a new mapping session is not yet linked to a map of the highest loop closure hypothesis. In localization mode, this threshold is used when there are no loop closure constraints with any map in the cache (see &quot;Max odometry cache size&quot; under RGB-D SLAM panel). In all cases, the goal is to aggressively loop on a previous map in the database that is not linked to current map. Only used when RGB-D SLAM mode is enabled. Set 1 to disable.</string>
                          </property>
                          <property name="wordWrap">
                           <bool>true</bool>
@@ -9618,6 +9658,19 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
+                       <item row="3" column="0">
+                        <widget class="QDoubleSpinBox" name="general_doubleSpinBox_agressiveThr">
+                         <property name="maximum">
+                          <double>1.000000000000000</double>
+                         </property>
+                         <property name="singleStep">
+                          <double>0.010000000000000</double>
+                         </property>
+                         <property name="value">
+                          <double>0.050000000000000</double>
+                         </property>
+                        </widget>
+                       </item>
                       </layout>
                      </widget>
                     </item>
@@ -10828,7 +10881,7 @@ generate the number of words requested.</string>
                           </widget>
                          </item>
                          <item row="6" column="1">
-                          <widget class="QLabel" name="label_755">
+                          <widget class="QLabel" name="label_7551">
                            <property name="text">
                             <string>If true, SSC (Suppression via Square Covering) is applied to limit keypoints.</string>
                            </property>
@@ -11887,7 +11940,7 @@ see Sqlite3 doc 'PRAGMA synchronous'.</string>
                        </widget>
                       </item>
                       <item row="4" column="1">
-                       <widget class="QLabel" name="label_755">
+                       <widget class="QLabel" name="label_757">
                         <property name="text">
                          <string>Likelihood ratio for VP.</string>
                         </property>

guilib/src/utilite/UPlot.cpp

@@ -789,7 +789,7 @@ void UPlotCurve::draw(QPainter * painter, const QRect & limits)
 				{
 					QPointF intersection;
 					QLineF::IntersectType type;
-#if QT_VERSION >= QT_VERSION_CHECK(6, 0, 0)
+#if QT_VERSION >= QT_VERSION_CHECK(5, 15, 3)
 					type = lineItem->line().intersects(QLineF(limits.topLeft(), limits.bottomLeft()), &intersection);
 #else
 					type = lineItem->line().intersect(QLineF(limits.topLeft(), limits.bottomLeft()), &intersection);
@@ -800,7 +800,7 @@ void UPlotCurve::draw(QPainter * painter, const QRect & limits)
 					}
 					else
 					{
-#if QT_VERSION >= QT_VERSION_CHECK(6, 0, 0)
+#if QT_VERSION >= QT_VERSION_CHECK(5, 15, 3)
 						type = lineItem->line().intersects(QLineF(limits.topLeft(), limits.topRight()), &intersection);
 #else
 						type = lineItem->line().intersect(QLineF(limits.topLeft(), limits.topRight()), &intersection);
@@ -811,7 +811,7 @@ void UPlotCurve::draw(QPainter * painter, const QRect & limits)
 						}
 						else
 						{
-#if QT_VERSION >= QT_VERSION_CHECK(6, 0, 0)
+#if QT_VERSION >= QT_VERSION_CHECK(5, 15, 3)
 							type = lineItem->line().intersects(QLineF(limits.bottomLeft(), limits.bottomRight()), &intersection);
 #else
 							type = lineItem->line().intersect(QLineF(limits.bottomLeft(), limits.bottomRight()), &intersection);
@@ -822,7 +822,7 @@ void UPlotCurve::draw(QPainter * painter, const QRect & limits)
 							}
 							else
 							{
-#if QT_VERSION >= QT_VERSION_CHECK(6, 0, 0)
+#if QT_VERSION >= QT_VERSION_CHECK(5, 15, 3)
 								type = lineItem->line().intersects(QLineF(limits.topRight(), limits.bottomRight()), &intersection);
 #else
 								type = lineItem->line().intersect(QLineF(limits.topRight(), limits.bottomRight()), &intersection);

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>rtabmap</name>
-  <version>0.21.5</version>
+  <version>0.21.6</version>
   <description>RTAB-Map's standalone library. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
   <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>
@@ -21,9 +21,9 @@
   <depend>libpointmatcher</depend> <!-- optional but recommended if lidar is used, also not available on 32 bits system, but rtabmap can be built without it -->
   <!-- <depend>libproj-dev</depend> needed due to error in vtk6 (kinetic)-->
   <depend>libsqlite3-dev</depend>
-  <depend>octomap</depend>
+  <depend>liboctomap-dev</depend>
-<!--  <depend>grid_map_core</depend> # not available on Rolling -->
+  <depend>grid_map_core</depend>
-  <depend>qtbase5-dev</depend>
+  <depend>qt_gui_cpp</depend> <!-- libqt4-dev or libqt5-dev -->
   <depend>zlib</depend>
 
   <export>

tools/Calibration/main.cpp

@@ -264,7 +264,7 @@ int main(int argc, char * argv[])
 		camera = new rtabmap::CameraRealSense2();
 		((rtabmap::CameraRealSense2*)camera)->setImagesRectified(false);
 		dialog.setStereoMode(true);
-		dialog.setFisheyeImages(true);
+		dialog.setFisheyeModel();
 	}
 	else
 	{

tools/CameraRGBD/main.cpp

@@ -65,6 +65,7 @@ void showUsage()
 			"                                     13=MYNT EYE S\n"
 			"                                     14=ZED Open Capture\n"
 			"                                     15=depthai-core\n"
+			"                                     16=XVSDK     (SeerSense)\n"
 			"  Options:\n"
 			"      -rate #.#                      Input rate Hz (default 0=inf)\n"
 			"      -device #                      Device ID (number or string)\n"
@@ -334,6 +335,15 @@ int main(int argc, char * argv[])
 		}
 		camera = new rtabmap::CameraDepthAI(deviceId);
 	}
+	else if (driver == 16)
+	{
+		if (!rtabmap::CameraSeerSense::available())
+		{
+			UERROR("Not built with XVisio SDK support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraSeerSense();
+	}
 	else
 	{
 		UFATAL("");

tools/Info/main.cpp

@@ -64,16 +64,6 @@ void showUsage()
 	exit(1);
 }
 
-std::string pad(const std::string & title, int padding = 20)
-{
-	int emptySize = padding - (int)title.size();
-	if(emptySize>0)
-	{
-		return title + std::string(emptySize, ' ');
-	}
-	return title;
-}
-
 int main(int argc, char * argv[])
 {
 	if(argc < 2)
@@ -205,7 +195,7 @@ int main(int argc, char * argv[])
 #else
 				printf("%s", COLOR_YELLOW);
 #endif
-				std::cout << (uFormat("%s%s  (%s=%s)\n", pad(iter->first + "=", padding).c_str(), iter->second.c_str(), otherDatabasePath.empty()?"default":otherDatabasePathName.c_str(), defaultValue.c_str()));
+				std::cout << (uFormat("%s%s  (%s=%s)\n", uPad(iter->first + "=", padding).c_str(), iter->second.c_str(), otherDatabasePath.empty()?"default":otherDatabasePathName.c_str(), defaultValue.c_str()));
 			}
 			else if(!diff)
 			{
@@ -215,7 +205,7 @@ int main(int argc, char * argv[])
 #else
 				printf("%s", COLOR_NORMAL);
 #endif
-				std::cout << (uFormat("%s%s\n", pad(iter->first + "=", padding).c_str(), iter->second.c_str()));
+				std::cout << (uFormat("%s%s\n", uPad(iter->first + "=", padding).c_str(), iter->second.c_str()));
 			}
 		}
 		else if(!defaultValueSet)
@@ -226,7 +216,7 @@ int main(int argc, char * argv[])
 #else
 			printf("%s", COLOR_RED);
 #endif
-			std::cout << (uFormat("%s%s  (%s=NA)\n", pad(iter->first + "=", padding).c_str(), iter->second.c_str(), otherDatabasePath.empty()?"default":otherDatabasePathName.c_str()));
+			std::cout << (uFormat("%s%s  (%s=NA)\n", uPad(iter->first + "=", padding).c_str(), iter->second.c_str(), otherDatabasePath.empty()?"default":otherDatabasePathName.c_str()));
 		}
 		else if(!diff)
 		{
@@ -236,7 +226,7 @@ int main(int argc, char * argv[])
 #else
 			printf("%s", COLOR_NORMAL);
 #endif
-			std::cout << (uFormat("%s%s\n", pad(iter->first + "=", padding).c_str(), iter->second.c_str()));
+			std::cout << (uFormat("%s%s\n", uPad(iter->first + "=", padding).c_str(), iter->second.c_str()));
 		}
 #ifdef _WIN32
 		SetConsoleTextAttribute(H,COLOR_NORMAL);
@@ -256,7 +246,7 @@ int main(int argc, char * argv[])
 #else
 			printf("%s", COLOR_RED);
 #endif
-			std::cout << (uFormat("%sNA  (%s=\"%s\")\n", pad(iter->first + "=", padding).c_str(), otherDatabasePath.empty()?"default":otherDatabasePathName.c_str(), iter->second.c_str()));
+			std::cout << (uFormat("%sNA  (%s=\"%s\")\n", uPad(iter->first + "=", padding).c_str(), otherDatabasePath.empty()?"default":otherDatabasePathName.c_str(), iter->second.c_str()));
 		
 #ifdef _WIN32
 			SetConsoleTextAttribute(H,COLOR_NORMAL);
@@ -340,9 +330,9 @@ int main(int argc, char * argv[])
 			previousStamp=s;
 			previousPose=p;
 		}
-		std::cout << (uFormat("%s%s\n", pad("Path:").c_str(), driver->getUrl().c_str()));
+		std::cout << (uFormat("%s%s\n", uPad("Path:").c_str(), driver->getUrl().c_str()));
-		std::cout << (uFormat("%s%s\n", pad("Version:").c_str(), driver->getDatabaseVersion().c_str()));
+		std::cout << (uFormat("%s%s\n", uPad("Version:").c_str(), driver->getDatabaseVersion().c_str()));
-		std::cout << (uFormat("%s%d\n", pad("Sessions:").c_str(), sessions));
+		std::cout << (uFormat("%s%d\n", uPad("Sessions:").c_str(), sessions));
 		std::multimap<int, Link> links;
 		driver->getAllLinks(links, true, true);
 		bool reducedGraph = false;
@@ -363,11 +353,11 @@ int main(int argc, char * argv[])
 		}
 		if(reducedGraph)
 		{
-			std::cout << (uFormat("%s%f m (approx. as graph has been reduced)\n", pad("Total odom:").c_str(), infoTotalOdom));
+			std::cout << (uFormat("%s%f m (approx. as graph has been reduced)\n", uPad("Total odom:").c_str(), infoTotalOdom));
 		}
 		else
 		{
-			std::cout << (uFormat("%s%f m\n", pad("Total odometry length:").c_str(), infoTotalOdom));
+			std::cout << (uFormat("%s%f m\n", uPad("Total odometry length:").c_str(), infoTotalOdom));
 		}
 
 		std::stringstream sessionsInOptGraphStr;
@@ -400,14 +390,14 @@ int main(int argc, char * argv[])
 			}
 		}
 
-		std::cout << (uFormat("%s%fs\n", pad("Total time:").c_str(), infoTotalTime));
+		std::cout << (uFormat("%s%fs\n", uPad("Total time:").c_str(), infoTotalTime));
-		std::cout << (uFormat("%s%d nodes and %d words (dim=%d type=%s)\n", pad("LTM:").c_str(), (int)ids.size(), driver->getTotalDictionarySize(), wordsDim, wordsType==CV_8UC1?"8U":wordsType==CV_32FC1?"32F":uNumber2Str(wordsType).c_str()));
+		std::cout << (uFormat("%s%d nodes and %d words (dim=%d type=%s)\n", uPad("LTM:").c_str(), (int)ids.size(), driver->getTotalDictionarySize(), wordsDim, wordsType==CV_8UC1?"8U":wordsType==CV_32FC1?"32F":uNumber2Str(wordsType).c_str()));
-		std::cout << (uFormat("%s%d nodes and %d words\n", pad("WM:").c_str(), driver->getLastNodesSize(), driver->getLastDictionarySize()));
+		std::cout << (uFormat("%s%d nodes and %d words\n", uPad("WM:").c_str(), driver->getLastNodesSize(), driver->getLastDictionarySize()));
-		std::cout << (uFormat("%s%d poses and %d links\n", pad("Global graph:").c_str(), odomPoses, links.size()));
+		std::cout << (uFormat("%s%d poses and %d links\n", uPad("Global graph:").c_str(), odomPoses, links.size()));
-		std::cout << (uFormat("%s%d poses (x=%d->%d, y=%d->%d, z=%d->%d)\n", pad("Optimized graph:").c_str(), (int)optimizedPoses.size(), links.size(), (int)min[0], (int)max[0], (int)min[1], (int)max[1], min[2], (int)max[2]));
+		std::cout << (uFormat("%s%d poses (x=%d->%d, y=%d->%d, z=%d->%d)\n", uPad("Optimized graph:").c_str(), (int)optimizedPoses.size(), links.size(), (int)min[0], (int)max[0], (int)min[1], (int)max[1], min[2], (int)max[2]));
-		std::cout << (uFormat("%s%d/%d [%s]\n", pad("Maps in graph:").c_str(), (int)mapsLinkedToLastGraph.size(), sessions, sessionsInOptGraphStr.str().c_str()));
+		std::cout << (uFormat("%s%d/%d [%s]\n", uPad("Maps in graph:").c_str(), (int)mapsLinkedToLastGraph.size(), sessions, sessionsInOptGraphStr.str().c_str()));
-		std::cout << (uFormat("%s%d poses\n", pad("Ground truth:").c_str(), gtPoses));
+		std::cout << (uFormat("%s%d poses\n", uPad("Ground truth:").c_str(), gtPoses));
-		std::cout << (uFormat("%s%d poses\n", pad("GPS:").c_str(), gpsValues));
+		std::cout << (uFormat("%s%d poses\n", uPad("GPS:").c_str(), gpsValues));
 		std::cout << (uFormat("Links:\n"));
 		for(size_t i=0; i<linkTypes.size(); ++i)
 		{
@@ -419,7 +409,7 @@ int main(int argc, char * argv[])
 				std = std::sqrt(std);
 			}
 			std::cout << (uFormat("%s%d\t(length avg: %.2fm, std: %.2fm, max: %.2fm)\n", 
-				pad(uFormat("  %s:", Link::typeName((Link::Type)i).c_str())).c_str(),
+				uPad(uFormat("  %s:", Link::typeName((Link::Type)i).c_str())).c_str(),
 				linkTypes[i],
 				avg,
 				std,
@@ -429,42 +419,42 @@ int main(int argc, char * argv[])
 		long total = 0;
 		long dbSize = UFile::length(driver->getUrl());
 		long mem = dbSize;
-		std::cout << (uFormat("%s%d %s\n", pad("Database size:").c_str(), mem>1000000?mem/1000000:mem>1000?mem/1000:mem, mem>1000000?"MB":mem>1000?"KB":"Bytes"));
+		std::cout << (uFormat("%s%d %s\n", uPad("Database size:").c_str(), mem>1000000?mem/1000000:mem>1000?mem/1000:mem, mem>1000000?"MB":mem>1000?"KB":"Bytes"));
 		mem = driver->getNodesMemoryUsed();
 		total+=mem;
-		std::cout << (uFormat("%s%d %s\t(%.2f%%)\n", pad("Nodes size:").c_str(), mem>1000000?mem/1000000:mem>1000?mem/1000:mem, mem>1000000?"MB":mem>1000?"KB":"Bytes", dbSize>0?double(mem)/double(dbSize)*100.0:0.0));
+		std::cout << (uFormat("%s%d %s\t(%.2f%%)\n", uPad("Nodes size:").c_str(), mem>1000000?mem/1000000:mem>1000?mem/1000:mem, mem>1000000?"MB":mem>1000?"KB":"Bytes", dbSize>0?double(mem)/double(dbSize)*100.0:0.0));
 		mem = driver->getLinksMemoryUsed();
 		total+=mem;
-		std::cout << (uFormat("%s%d %s\t(%.2f%%)\n", pad("Links size:").c_str(), mem>1000000?mem/1000000:mem>1000?mem/1000:mem, mem>1000000?"MB":mem>1000?"KB":"Bytes", dbSize>0?double(mem)/double(dbSize)*100.0:0.0));
+		std::cout << (uFormat("%s%d %s\t(%.2f%%)\n", uPad("Links size:").c_str(), mem>1000000?mem/1000000:mem>1000?mem/1000:mem, mem>1000000?"MB":mem>1000?"KB":"Bytes", dbSize>0?double(mem)/double(dbSize)*100.0:0.0));
 		mem = driver->getImagesMemoryUsed();
 		total+=mem;
-		std::cout << (uFormat("%s%d %s\t(%.2f%%)\n", pad("RGB Images size:").c_str(), mem>1000000?mem/1000000:mem>1000?mem/1000:mem, mem>1000000?"MB":mem>1000?"KB":"Bytes", dbSize>0?double(mem)/double(dbSize)*100.0:0.0));
+		std::cout << (uFormat("%s%d %s\t(%.2f%%)\n", uPad("RGB Images size:").c_str(), mem>1000000?mem/1000000:mem>1000?mem/1000:mem, mem>1000000?"MB":mem>1000?"KB":"Bytes", dbSize>0?double(mem)/double(dbSize)*100.0:0.0));
 		mem = driver->getDepthImagesMemoryUsed();
 		total+=mem;
-		std::cout << (uFormat("%s%d %s\t(%.2f%%)\n", pad("Depth Images size:").c_str(), mem>1000000?mem/1000000:mem>1000?mem/1000:mem, mem>1000000?"MB":mem>1000?"KB":"Bytes", dbSize>0?double(mem)/double(dbSize)*100.0:0.0));
+		std::cout << (uFormat("%s%d %s\t(%.2f%%)\n", uPad("Depth Images size:").c_str(), mem>1000000?mem/1000000:mem>1000?mem/1000:mem, mem>1000000?"MB":mem>1000?"KB":"Bytes", dbSize>0?double(mem)/double(dbSize)*100.0:0.0));
 		mem = driver->getCalibrationsMemoryUsed();
 		total+=mem;
-		std::cout << (uFormat("%s%d %s\t(%.2f%%)\n", pad("Calibrations size:").c_str(), mem>1000000?mem/1000000:mem>1000?mem/1000:mem, mem>1000000?"MB":mem>1000?"KB":"Bytes", dbSize>0?double(mem)/double(dbSize)*100.0:0.0));
+		std::cout << (uFormat("%s%d %s\t(%.2f%%)\n", uPad("Calibrations size:").c_str(), mem>1000000?mem/1000000:mem>1000?mem/1000:mem, mem>1000000?"MB":mem>1000?"KB":"Bytes", dbSize>0?double(mem)/double(dbSize)*100.0:0.0));
 		mem = driver->getGridsMemoryUsed();
 		total+=mem;
-		std::cout << (uFormat("%s%d %s\t(%.2f%%)\n", pad("Grids size:").c_str(), mem>1000000?mem/1000000:mem>1000?mem/1000:mem, mem>1000000?"MB":mem>1000?"KB":"Bytes", dbSize>0?double(mem)/double(dbSize)*100.0:0.0));
+		std::cout << (uFormat("%s%d %s\t(%.2f%%)\n", uPad("Grids size:").c_str(), mem>1000000?mem/1000000:mem>1000?mem/1000:mem, mem>1000000?"MB":mem>1000?"KB":"Bytes", dbSize>0?double(mem)/double(dbSize)*100.0:0.0));
 		mem = driver->getLaserScansMemoryUsed();
 		total+=mem;
-		std::cout << (uFormat("%s%d %s\t(%.2f%%)\n", pad("Scans size:").c_str(), mem>1000000?mem/1000000:mem>1000?mem/1000:mem, mem>1000000?"MB":mem>1000?"KB":"Bytes", dbSize>0?double(mem)/double(dbSize)*100.0:0.0));
+		std::cout << (uFormat("%s%d %s\t(%.2f%%)\n", uPad("Scans size:").c_str(), mem>1000000?mem/1000000:mem>1000?mem/1000:mem, mem>1000000?"MB":mem>1000?"KB":"Bytes", dbSize>0?double(mem)/double(dbSize)*100.0:0.0));
 		mem = driver->getUserDataMemoryUsed();
 		total+=mem;
-		std::cout << (uFormat("%s%d %s\t(%.2f%%)\n", pad("User data size:").c_str(), mem>1000000?mem/1000000:mem>1000?mem/1000:mem, mem>1000000?"MB":mem>1000?"KB":"Bytes", dbSize>0?double(mem)/double(dbSize)*100.0:0.0));
+		std::cout << (uFormat("%s%d %s\t(%.2f%%)\n", uPad("User data size:").c_str(), mem>1000000?mem/1000000:mem>1000?mem/1000:mem, mem>1000000?"MB":mem>1000?"KB":"Bytes", dbSize>0?double(mem)/double(dbSize)*100.0:0.0));
 		mem = driver->getWordsMemoryUsed();
 		total+=mem;
-		std::cout << (uFormat("%s%d %s\t(%.2f%%)\n", pad("Dictionary size:").c_str(), mem>1000000?mem/1000000:mem>1000?mem/1000:mem, mem>1000000?"MB":mem>1000?"KB":"Bytes", dbSize>0?double(mem)/double(dbSize)*100.0:0.0));
+		std::cout << (uFormat("%s%d %s\t(%.2f%%)\n", uPad("Dictionary size:").c_str(), mem>1000000?mem/1000000:mem>1000?mem/1000:mem, mem>1000000?"MB":mem>1000?"KB":"Bytes", dbSize>0?double(mem)/double(dbSize)*100.0:0.0));
 		mem = driver->getFeaturesMemoryUsed();
 		total+=mem;
-		std::cout << (uFormat("%s%d %s\t(%.2f%%)\n", pad("Features size:").c_str(), mem>1000000?mem/1000000:mem>1000?mem/1000:mem, mem>1000000?"MB":mem>1000?"KB":"Bytes", dbSize>0?double(mem)/double(dbSize)*100.0:0.0));
+		std::cout << (uFormat("%s%d %s\t(%.2f%%)\n", uPad("Features size:").c_str(), mem>1000000?mem/1000000:mem>1000?mem/1000:mem, mem>1000000?"MB":mem>1000?"KB":"Bytes", dbSize>0?double(mem)/double(dbSize)*100.0:0.0));
 		mem = driver->getStatisticsMemoryUsed();
 		total+=mem;
-		std::cout << (uFormat("%s%d %s\t(%.2f%%)\n", pad("Statistics size:").c_str(), mem>1000000?mem/1000000:mem>1000?mem/1000:mem, mem>1000000?"MB":mem>1000?"KB":"Bytes", dbSize>0?double(mem)/double(dbSize)*100.0:0.0));
+		std::cout << (uFormat("%s%d %s\t(%.2f%%)\n", uPad("Statistics size:").c_str(), mem>1000000?mem/1000000:mem>1000?mem/1000:mem, mem>1000000?"MB":mem>1000?"KB":"Bytes", dbSize>0?double(mem)/double(dbSize)*100.0:0.0));
 		mem = dbSize - total;
-		std::cout << (uFormat("%s%d %s\t(%.2f%%)\n", pad("Other (indexing, unused):").c_str(), mem>1000000?mem/1000000:mem>1000?mem/1000:mem, mem>1000000?"MB":mem>1000?"KB":"Bytes", dbSize>0?double(mem)/double(dbSize)*100.0:0.0));
+		std::cout << (uFormat("%s%d %s\t(%.2f%%)\n", uPad("Other (indexing, unused):").c_str(), mem>1000000?mem/1000000:mem>1000?mem/1000:mem, mem>1000000?"MB":mem>1000?"KB":"Bytes", dbSize>0?double(mem)/double(dbSize)*100.0:0.0));
 		std::cout << ("\n");
 	}
 

tools/Report/CMakeLists.txt

@@ -1,10 +1,10 @@
 
 set(LIBRARIES rtabmap_core)
 
-IF(QT4_FOUND OR Qt5_FOUND)
+IF(WITH_QT AND (QT4_FOUND OR Qt5_FOUND))
     ADD_DEFINITIONS("-DWITH_QT")
     set(LIBRARIES ${LIBRARIES} rtabmap_gui)
-ENDIF(QT4_FOUND OR Qt5_FOUND)
+ENDIF(WITH_QT AND (QT4_FOUND OR Qt5_FOUND))
 
 ADD_EXECUTABLE(report main.cpp)
   

tools/Report/main.cpp

@@ -83,9 +83,9 @@ void showUsage()
 			"    --report           Export all evaluation statistics values in report.txt \n"
 			"    --loc [#]          Show localization statistics for each \"Statistic/Id\" per\n"
 			"                       session. Optionally set number 1=min,2=max,4=mean,8=stddev,16=total,32=nonnull%%\n"
-			"                       to show cumulative results on console.\n"
+			"                       to show cumulative results on console (it is a mask, \n"
-			"    --loc_delay #      Delay to split sessions for localization statistics (default 60 seconds)\n"
+			"                       we can combine those numbers, e.g., 63 for all) \n"
-			"                       (it is a mask, we can combine those numbers, e.g., 63 for all) \n"
+			"    --loc_delay #      Delay to split sessions for localization statistics (default 60 seconds).\n"
 			"    --ignore_inter_nodes  Ignore intermediate poses and statistics.\n"
 			"    --udebug           Show debug log.\n"
 			"    --help,-h          Show usage\n\n");

tools/Reprocess/main.cpp

@@ -600,7 +600,7 @@ int main(int argc, char * argv[])
 		printf("Custom parameters:\n");
 		for(ParametersMap::iterator iter=customParameters.begin(); iter!=customParameters.end(); ++iter)
 		{
-			printf("  %s\t= %s\n", iter->first.c_str(), iter->second.c_str());
+			printf("  %s %s\n", uPad(iter->first+" =", 25).c_str(), iter->second.c_str());
 		}
 	}
 

utilite/include/rtabmap/utilite/UStl.h

@@ -809,4 +809,14 @@ inline int uCompareVersion(const std::string & version, int major, int minor=-1,
 	return -1;
 }
 
+inline std::string uPad(const std::string & title, int padding = 20)
+{
+	int emptySize = padding - (int)title.size();
+	if(emptySize>0)
+	{
+		return title + std::string(emptySize, ' ');
+	}
+	return title;
+}
+
 #endif /* USTL_H */